static void mcedev_remove(struct pci_dev *dev)
{
int i;
// Walk through the list of device table entries and clean up any allocated resources.
MPRINTK(0x01, KERN_DEBUG "mcedev_remove: %i active devices found\n", mcedev_found);
if (mcedev_found > 0) {
for (i = 0; i < MAXNUM_MCE_DEV; i++) {
MPRINTK(0x01, KERN_DEBUG "mcedev_remove: device table slot %i ... ", i);
if ((mcemu_device_table[i] != NULL) && (dev == mcemu_device_table[i]->dev)) {
MPRINTK(0x01, "struct pci_dev matches\n");
if (mcemu_device_table[i]->cleanup != NULL) {
mcemu_device_table[i]->cleanup(mcemu_device_table[i]);
}
else
MPRINTK(0x01, KERN_DEBUG "... not calling cleanup (is NULL!!)\n");
down(&mcedev_sem);
pci_disable_device(mcemu_device_table[i]->dev);
kfree(mcemu_device_table[i]);
mcemu_device_table[i] = NULL;
mcedev_found--;
up(&mcedev_sem);
}
else MPRINTK(0x01, "no match\n");
}
}
}
void mcedev_print_resources(struct mcedev_data *mcedev) {
int i;
for (i = 0; i < MAXNUM_PCI_BAR; i++) {
MPRINTK(0x02, KERN_DEBUG "BAR %i (%s) at 0x%08lx, size 0x%08lx, flags 0x%08lx\n",
i, mcedev->dev->resource[i].name, mcedev->bar[i].start,
mcedev->bar[i].size, mcedev->bar[i].pci_flags);
}
MPRINTK(0x02, KERN_DEBUG "IRQ: %i\n", mcedev->irq);
}
static void
print_mem (struct mem *m)
{
int i;
MPRINTK("mem:\n");
MPRINTK(" check=%X, other = %X\n", m->check, m->other);
MPRINTK(" len=%d buff:\n " , m->len);
for (i = 0; i < 10; i++)
{
MPRINTK ("0x%02X ",m->buff[i]);
}
MPRINTK ("\n");
}
static void *
smalloc (unsigned long size)
{
struct mem *head, *tail;
static unsigned short count;
int i;
int sum;
unsigned char *ptr;
MPRINTK ("smalloc (size = %d)\n",size);
head = malloc (size + 2*sizeof (*head));
if (head == NULL) return (NULL);
tail = (struct mem *)((unsigned char *)(head+1) + size);
head->other = tail;
tail->other = head;
tail->len = size;
head->len = size;
for (i = 0; i < 10; i++)
{
tail->buff[i]=count++;
head->buff[i]=count;
}
ptr = (unsigned char *)head;
head->check = 0;
sum = 0;
for (i = 0; i < sizeof (*head); i ++)
{
sum+= ptr[i];
}
head->check = ~sum;
ptr = (unsigned char *)tail;
tail->check = 0;
sum = 0;
for (i = 0; i < sizeof (*head); i ++)
{
sum+= ptr[i];
}
tail->check = ~sum;
MPRINTK ("head = %X:\n", head);
print_mem(head);
MPRINTK ("tail = %X:\n", tail);
print_mem(tail);
return (head+1);
}
int map_to_kernelmem(unsigned long phys_addr, unsigned long size, void **target, char *desc) {
MPRINTK(0x02, KERN_DEBUG "map_to_kernelmem: %s for access by the kernel (0x%08lx, 0x%08lx) ...\n",
desc, phys_addr, size);
*target = ioremap_nocache(phys_addr, size);
if (target) {
MPRINTK(0x02, KERN_DEBUG "map_to_kernelmem: ... Success, mapped to 0x%08lx\n", (unsigned long) *target);
}
else {
MPRINTK(0x02, KERN_DEBUG "map_to_kernelmem: ... Failed!\n");
return -ENODEV;
}
return 0;
}
void
sock_wfree (volatile struct sock *sk, void *mem, unsigned long size)
{
MPRINTK ("sock_wfree (sk=%X, mem=%X, size=%d)\n",sk, mem, size);
if (sk)
{
sk->wmem_alloc -= size;
sfree(mem,size);
/* in case it might be waiting for more memory. */
if (!sk->dead && sk->wmem_alloc > SK_WMEM_MAX/2) wake_up(sk->sleep);
if (sk->destroy && sk->wmem_alloc == 0 && sk->rmem_alloc == 0)
{
MPRINTK ("recovered lost memory, destroying sock = %X\n",sk);
delete_timer ((struct timer *)&sk->time_wait);
free_s ((void *)sk, sizeof (*sk));
}
}
else
{
free_s (mem, size);
}
}
void *
sock_rmalloc(volatile struct sock *sk, unsigned long size, int force)
{
struct mem *tmp;
if (sk )
{
if (sk->rmem_alloc + size >= SK_RMEM_MAX && !force)
{
MPRINTK ("sock_rmalloc(%X,%d,%d) returning NULL\n",sk,size,force);
return (NULL);
}
cli();
sk->rmem_alloc+= size;
sti();
}
if (sk)
tmp = smalloc (size);
else
tmp = malloc (size);
MPRINTK ("sock_rmalloc(%X,%d,%d) returning %X\n",sk, size, force, tmp);
return (tmp);
}
static int __init mcedev_init(void)
{
int result = 0;
dev_t mcedev_dev;
printk(KERN_INFO "mcedev Id: %s, Version %s\n", MCEIDSTRING, MODVERSTRING);
if (mcedev_major) {
mcedev_dev = MKDEV(mcedev_major, 0);
MPRINTK(MCEDBG_GLOBAL, KERN_DEBUG "mcedev: registering %i minors for major %i\n",
(MAXNUM_MCE_DEV + 1) * (1U << MINOR_FUNCTIONBITS), mcedev_major);
result = register_chrdev_region(mcedev_dev, (MAXNUM_MCE_DEV + 1) * (1U << MINOR_FUNCTIONBITS), "mcedev");
if (result != 0) {
printk(KERN_ERR "mcedev: register_chrdev_region() failed: %i, Trying alloc\n", result);
mcedev_major = 0;
}
}
if (mcedev_major == 0) {
result = alloc_chrdev_region(&mcedev_dev, 0,(MAXNUM_MCE_DEV + 1) * (1U << MINOR_FUNCTIONBITS), "mcedev");
if (result != 0) {
printk(KERN_ERR "mcedev: alloc_chrdev_region() failed: %i\n", result);
return -ENODEV;
}
mcedev_major = MAJOR(mcedev_dev);
MPRINTK(MCEDBG_GLOBAL, KERN_DEBUG "mcedev: succesfully allocated %i minors for major: %i\n",
(MAXNUM_MCE_DEV + 1) * (1U << MINOR_FUNCTIONBITS), mcedev_major);
}
mcedev_class = class_create(THIS_MODULE, "mcedev");
if (IS_ERR(mcedev_class)) {
printk(KERN_ERR "mcedev: Error creating mcedev class.\n");
return -ENODEV;
}
return pci_register_driver(&pci_driver);
}
int allocate_device_node(int devid, int devcount, struct file_operations *myfops, struct cdev *cdev) {
int err;
// TODO: Add check to make sure devcount doesn't spill over into next MINOR_DEVNUM space
cdev_init(cdev, myfops);
cdev->ops = myfops;
cdev->owner = THIS_MODULE;
err = cdev_add(cdev, devid, devcount);
if (err)
printk(KERN_INFO "Error %d adding device: 0x%08x (%d:%d(%d:%d))\n",
err, devid, MAJOR(devid), MINOR(devid),
MINOR_DEVNUM(MINOR(cdev->dev)), MINOR_FUNCTION(MINOR(cdev->dev)));
else
MPRINTK(0x02, KERN_DEBUG "cdev add ok (%i) device: 0x%08x (%d:%d:(%d:%d)), count: %d\n",
err, cdev->dev, MAJOR(cdev->dev), MINOR(cdev->dev),
MINOR_DEVNUM(MINOR(cdev->dev)), MINOR_FUNCTION(MINOR(cdev->dev)), cdev->count);
return err;
}
void
sock_rfree (volatile struct sock *sk, void *mem, unsigned long size)
{
MPRINTK ("sock_rfree (sk=%X, mem=%X, size=%d)\n",sk, mem, size);
if (sk)
{
sk->rmem_alloc -= size;
sfree(mem,size);
if (sk->destroy && sk->wmem_alloc == 0 && sk->rmem_alloc == 0)
{
delete_timer ((struct timer *)&sk->time_wait);
free_s ((void *)sk, sizeof (*sk));
}
}
else
{
free_s (mem, size);
}
}
void
sfree (void *data, unsigned long len)
{
int i;
int sum;
int csum;
unsigned char *ptr;
int bad = 0;
struct mem *head, *tail;
MPRINTK ("sfree(data=%X, len = %d)\n", data, len);
head = data;
head--;
tail = (struct mem *)((unsigned char *)(head+1) + len);
print_mem (head);
print_mem (tail);
if (head->other != tail)
{
MPRINTK ("sfree: head->other != tail:\n");
bad = 1;
}
if (tail->other != head)
{
MPRINTK ("sfree: tail->other != head:\n");
bad =1 ;
}
if (head ->len != len)
{
MPRINTK ("sfree: head->len != len");
bad = 1;
}
if (tail ->len != len)
{
MPRINTK ("sfree: tail->len != len");
bad = 1;
}
csum = head->check;
ptr = (unsigned char *)head;
head->check = 0;
sum = 0;
for (i = 0; i < sizeof (*head); i ++)
{
sum+= ptr[i];
}
if (csum != ~sum)
{
MPRINTK ("sfree: head failed checksum\n");
bad = 1;
}
csum = tail->check;
ptr = (unsigned char *)tail;
tail->check = 0;
sum = 0;
for (i = 0; i < sizeof (*head); i ++)
{
sum+= ptr[i];
}
if (csum != ~sum)
{
MPRINTK ("sfree: tail failed checksum\n");
bad = 1;
}
if (!bad)
free_s (head, len+2*sizeof (*head));
else
schedule();
}
static int mcedev_probe(struct pci_dev *dev, const struct pci_device_id *id)
{
int result, index;
int (*startup)(struct mcedev_data *mcedev, int dev_found, int major);
struct mcedev_data *mce_dev; // = &mcemu_devices[mcedev_found];
// Print information
printk(KERN_INFO "mcedev_probe found device: VID 0x%04x, DID 0x%04x, SVID 0x%04x, SDID 0x%04x, BUS 0x%02x, DEVFN 0x%02x.%01i \n",
dev->vendor, dev->device, dev->subsystem_vendor, dev->subsystem_device,
dev->bus->number, PCI_SLOT(dev->devfn), PCI_FUNC(dev->devfn));
// enable PCI device - needed to access it further on
result = pci_enable_device(dev);
if (result) {
pci_disable_device(dev);
return -ENODEV;
}
// Allocate memory for device/driver data
mce_dev = kmalloc(sizeof(*mce_dev), GFP_KERNEL);
if (!mce_dev) {
printk(KERN_INFO "mcedev_probe: Out of memory ... \n");
pci_disable_device(dev);
return -ENOMEM;
}
// fill initial data structures
mce_dev->dev = dev;
mce_dev->specific_data = NULL;
mce_dev->mcedev_type = MCEDEV_TYPE_UNDEFINED;
mce_dev->mcedev_class = mcedev_class;
// Get mutex on device table
down(&mcedev_sem);
// More devices already registered than we can handle? ... Should never happen. ;-)
if (mcedev_found >= MAXNUM_MCE_DEV) {
printk(KERN_INFO "mcedev_probe: reached maximum number of supported devices (%i), not initializing device\n",
mcedev_found);
up(&mcedev_sem);
pci_disable_device(dev);
kfree(mce_dev);
return -ENODEV;
}
// This is our slot in table
index = mcedev_found;
// Table clean?
if (mcemu_device_table[index] != NULL) {
MPRINTK(0x01, KERN_DEBUG "mcedev_probe: WARNING: table entry %i is not NULL!\n", index);
}
// Insert our structure into table
mcemu_device_table[index] = mce_dev;
MPRINTK(0x01, KERN_DEBUG "mcedev_probe: Added mce_dev into slot %i of device table\n", index);
// Call driver initialization function
if (id->driver_data != 0) {
startup = (void *) id->driver_data;
}
else {
startup = mcedev_no_init;
}
result = startup(mce_dev, index, mcedev_major);
MPRINTK(0x01, KERN_DEBUG "mcedev_probe: startup result = %i\n", result);
if (result <= 0) {
printk(KERN_INFO "mcedev_probe: device specific startup failed (%i)\n", result);
mcemu_device_table[index] = NULL;
up(&mcedev_sem);
pci_disable_device(dev);
kfree(mce_dev);
return -ENODEV;
}
// Increment total and release semaphore, our structure is now in global table
mcedev_found++;
up(&mcedev_sem);
return 0;
}
