dhahelper.c

来自「linux下的MPEG1」· C语言 代码 · 共 1,240 行 · 第 1/3 页

	if (copy_from_user(&mem, arg, sizeof(dhahelper_vmi_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	nitems = mem.length / PAGE_SIZE;
	if(mem.length % PAGE_SIZE) nitems++;
	addr = mem.virtaddr;
	for(i=0;i<nitems;i++)
	{
	    unsigned long result;
	    result = uvirt_to_pa((unsigned long)addr);
	    if (copy_to_user(&mem.realaddr[i], &result, sizeof(unsigned long)))
	    {
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy to userspace\n");
		return -EFAULT;
	    }
	    addr += PAGE_SIZE;
	}
	return 0;
}

static int dhahelper_virt_to_bus(dhahelper_vmi_t *arg)
{
	dhahelper_vmi_t mem;
	unsigned long i,nitems;
	char *addr;
	if (copy_from_user(&mem, arg, sizeof(dhahelper_vmi_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	nitems = mem.length / PAGE_SIZE;
	if(mem.length % PAGE_SIZE) nitems++;
	addr = mem.virtaddr;
	for(i=0;i<nitems;i++)
	{
	    unsigned long result;
	    result = uvirt_to_bus((unsigned long)addr);
	    if (copy_to_user(&mem.realaddr[i], &result, sizeof(unsigned long)))
	    {
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy to userspace\n");
		return -EFAULT;
	    }
	    addr += PAGE_SIZE;
	}
	return 0;
}


static int dhahelper_alloc_pa(dhahelper_mem_t *arg)
{
	dhahelper_mem_t mem;
	if (copy_from_user(&mem, arg, sizeof(dhahelper_mem_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	mem.addr = rvmalloc(mem.length);
	if (copy_to_user(arg, &mem, sizeof(dhahelper_mem_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy to userspace\n");
		return -EFAULT;
	}
	return 0;
}

static int dhahelper_free_pa(dhahelper_mem_t *arg)
{
	dhahelper_mem_t mem;
	if (copy_from_user(&mem, arg, sizeof(dhahelper_mem_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	rvfree(mem.addr,mem.length);
	return 0;
}

static int dhahelper_lock_mem(dhahelper_mem_t *arg)
{
	dhahelper_mem_t mem;
	int retval;
	unsigned long i,nitems,addr;
	if (copy_from_user(&mem, arg, sizeof(dhahelper_mem_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	nitems = mem.length / PAGE_SIZE;
	if(mem.length % PAGE_SIZE) nitems++;
	addr = (unsigned long)mem.addr;
	for(i=0;i<nitems;i++)
	{
	    retval = pag_lock((unsigned long)addr);
	    if(retval)
	    {
		unsigned long j;
		addr = (unsigned long)mem.addr;
		for(j=0;j<i;j++)
		{
		    pag_unlock(addr);
		    addr +=  PAGE_SIZE;
		}
		return retval;
	    }
	    addr += PAGE_SIZE;
	}
	return 0;
}

static int dhahelper_unlock_mem(dhahelper_mem_t *arg)
{
	dhahelper_mem_t mem;
	int retval;
	unsigned long i,nitems,addr;
	if (copy_from_user(&mem, arg, sizeof(dhahelper_mem_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	nitems = mem.length / PAGE_SIZE;
	if(mem.length % PAGE_SIZE) nitems++;
	addr = (unsigned long)mem.addr;
	for(i=0;i<nitems;i++)
	{
	    retval = pag_unlock((unsigned long)addr);
	    if(retval) return retval;
	    addr += PAGE_SIZE;
	}
	return 0;
}

static struct dha_irq {
    spinlock_t lock;
    unsigned long flags;
    int handled;
    int rcvd;
    volatile u32 *ack_addr;
    u32 ack_data;
    struct pci_dev *dev;
    wait_queue_head_t wait;
    unsigned long count;
} dha_irqs[256];

static irqreturn_t dhahelper_irq_handler(int irq, void *dev_id,
					 struct pt_regs *regs)
{
    spin_lock_irqsave(&dha_irqs[irq].lock, dha_irqs[irq].flags);
    if(dha_irqs[irq].handled){
	dha_irqs[irq].rcvd = 1;
	dha_irqs[irq].count++;
	if(dha_irqs[irq].ack_addr){
	    *dha_irqs[irq].ack_addr = dha_irqs[irq].ack_data;
	    mb();
	}
	wake_up_interruptible(&dha_irqs[irq].wait);
    }
    spin_unlock_irqrestore(&dha_irqs[irq].lock, dha_irqs[irq].flags);
    irqreturn(0);
}

static int dhahelper_install_irq(dhahelper_irq_t *arg)
{
    dhahelper_irq_t my_irq;
    struct pci_dev *pci;
    long rlen;
    int retval;
    long ack_addr;
    int irqn;

    if (copy_from_user(&my_irq, arg, sizeof(dhahelper_irq_t)))
    {
	if (dhahelper_verbosity > 0)
	    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
	return -EFAULT;
    }

    if(!(pci = pci_find_slot(my_irq.bus, PCI_DEVFN(my_irq.dev, my_irq.func))))
	return -EINVAL;

    rlen = pci_resource_len(pci, my_irq.ack_region);
    if(my_irq.ack_offset > rlen - 4)
	return -EINVAL;

    irqn = pci->irq;

    spin_lock_irqsave(&dha_irqs[irqn].lock,
		      dha_irqs[irqn].flags);

    if(dha_irqs[irqn].handled){
	retval = -EBUSY;
	goto fail;
    }

    if(my_irq.ack_region >= 0){
	ack_addr = pci_resource_start(pci, my_irq.ack_region);
	ack_addr += my_irq.ack_offset;
#ifdef CONFIG_ALPHA
	ack_addr += ((struct pci_controller *) pci->sysdata)->dense_mem_base;
#endif
	/* FIXME:  Other architectures */

	dha_irqs[irqn].ack_addr = phys_to_virt(ack_addr);
	dha_irqs[irqn].ack_data = my_irq.ack_data;
    } else {
	dha_irqs[irqn].ack_addr = 0;
    }

    dha_irqs[irqn].lock = SPIN_LOCK_UNLOCKED;
    dha_irqs[irqn].flags = 0;
    dha_irqs[irqn].rcvd = 0;
    dha_irqs[irqn].dev = pci;
    init_waitqueue_head(&dha_irqs[irqn].wait);
    dha_irqs[irqn].count = 0;

    retval = request_irq(irqn, dhahelper_irq_handler,
			 SA_SHIRQ, "dhahelper", pci);

    if(retval < 0)
	goto fail;

    copy_to_user(&arg->num, &irqn, sizeof(irqn));

    dha_irqs[irqn].handled = 1;

out:
    spin_unlock_irqrestore(&dha_irqs[irqn].lock,
			   dha_irqs[irqn].flags);
    return retval;

fail:
    if(retval == -EINVAL){
	printk("dhahelper: bad irq number or handler\n");
    } else if(retval == -EBUSY){
	printk("dhahelper: IRQ %u busy\n", irqn);
    } else {
	printk("dhahelper: Could not install irq handler...\n");
    }
    printk("dhahelper: Perhaps you need to let your BIOS assign an IRQ to your video card\n");
    goto out;
}

static int dhahelper_free_irq(dhahelper_irq_t *arg)
{
	dhahelper_irq_t irq;
	struct pci_dev *pci;
	int irqn;

	if (copy_from_user(&irq, arg, sizeof(dhahelper_irq_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}

	pci = pci_find_slot(irq.bus, PCI_DEVFN(irq.dev, irq.func));
	if(!pci)
	    return -EINVAL;

	irqn = pci->irq;

	spin_lock_irqsave(&dha_irqs[irqn].lock, dha_irqs[irqn].flags);
	if(dha_irqs[irqn].handled) {
		free_irq(irqn, pci);
		dha_irqs[irqn].handled = 0;
		printk("IRQ %i: %li\n", irqn, dha_irqs[irqn].count);
	}
	spin_unlock_irqrestore(&dha_irqs[irqn].lock, dha_irqs[irqn].flags);
	return 0;
}

static int dhahelper_ack_irq(dhahelper_irq_t *arg)
{
	dhahelper_irq_t irq;
	int retval = 0;
	DECLARE_WAITQUEUE(wait, current);
	if (copy_from_user(&irq, arg, sizeof(dhahelper_irq_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	if(irq.num > 255) return -EINVAL;
	if(!dha_irqs[irq.num].handled) return -ESRCH;
	add_wait_queue(&dha_irqs[irq.num].wait, &wait);
	set_current_state(TASK_INTERRUPTIBLE);
	for(;;){
		int r;
		spin_lock_irqsave(&dha_irqs[irq.num].lock,
				  dha_irqs[irq.num].flags);
		r = dha_irqs[irq.num].rcvd;
		spin_unlock_irqrestore(&dha_irqs[irq.num].lock,
				       dha_irqs[irq.num].flags);

		if(r){
			dha_irqs[irq.num].rcvd = 0;
			break;
		}

		if(signal_pending(current)){
			retval = -ERESTARTSYS;
			break;
		}

		schedule();
	}
	set_current_state(TASK_RUNNING);
	remove_wait_queue(&dha_irqs[irq.num].wait, &wait);	
	return retval;
}

static int dhahelper_cpu_flush(dhahelper_cpu_flush_t *arg)
{
	dhahelper_cpu_flush_t my_l2;
	if (copy_from_user(&my_l2, arg, sizeof(dhahelper_cpu_flush_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
#if defined(__i386__)
	/* WBINVD writes all modified cache lines back to main memory */
	if(boot_cpu_data.x86 > 3) { __asm __volatile("wbinvd":::"memory"); }
#else
	/* FIXME!!!*/
	mb(); /* declared in "asm/system.h" */
#endif
	return 0;
}

static struct pci_dev *pdev = NULL;
static int dhahelper_pci_find(dhahelper_pci_device_t *arg)
{
	dhahelper_pci_device_t this_dev;
	pdev = pci_find_device(PCI_ANY_ID, PCI_ANY_ID, pdev);
	if(pdev)
	{
	    this_dev.bus = pdev->bus->number;
	    this_dev.card = PCI_SLOT(pdev->devfn);
	    this_dev.func = PCI_FUNC(pdev->devfn);
	    this_dev.vendor = pdev->vendor;
	    this_dev.device = pdev->device;
	    this_dev.base0 = pci_resource_start (pdev, 0);
	    this_dev.base1 = pci_resource_start (pdev, 1);
	    this_dev.base2 = pci_resource_start (pdev, 2);
	    pci_read_config_dword(pdev, pdev->rom_base_reg, (u32*)&this_dev.baserom);
	    this_dev.base3 = pci_resource_start (pdev, 3);
	    this_dev.base4 = pci_resource_start (pdev, 4);
	    this_dev.base5 = pci_resource_start (pdev, 5);
	    pci_read_config_byte(pdev, PCI_INTERRUPT_LINE, &this_dev.irq);
	    pci_read_config_byte(pdev, PCI_INTERRUPT_PIN, &this_dev.ipin);
	    pci_read_config_byte(pdev, PCI_MIN_GNT, &this_dev.gnt);
	    pci_read_config_byte(pdev, PCI_MAX_LAT, &this_dev.lat);
	}
	else memset(&this_dev,0,sizeof(dhahelper_pci_device_t));
	if (copy_to_user(arg, &this_dev, sizeof(dhahelper_pci_device_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy to userspace\n");
		return -EFAULT;
	}
	return pdev?0:-ENODATA;
}

static int dhahelper_pci_config(dhahelper_pci_config_t *arg)
{
    dhahelper_pci_config_t op;
    struct pci_dev *pdev;
	if (copy_from_user(&op, arg, sizeof(dhahelper_pci_config_t)))
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: failed copy from userspace\n");
		return -EFAULT;
	}
	pdev = pci_find_slot(op.bus,PCI_DEVFN(op.dev,op.func));
	if(!pdev)
	{
		if (dhahelper_verbosity > 0)
		    printk(KERN_ERR "dhahelper: can't identify device\n");
		return -EFAULT;
	}
	switch(op.operation)
	{
	    case PCI_OP_READ:
		switch(op.size)
		{
		    case 1:
			pci_read_config_byte(pdev,op.cmd,(u8*)&op.ret);
			break;
		    case 2:
			pci_read_config_word(pdev,op.cmd,(u16*)&op.ret);
			break;
		    case 4:
			pci_read_config_dword(pdev,op.cmd,(u32*)&op.ret);
			break;
		    default:
			if (dhahelper_verbosity > 0)
			    printk(KERN_ERR "dhahelper: wrong size of pci operation: %u \n",op.size);
			return -EFAULT;
		}
	    case PCI_OP_WRITE:
		switch(op.size)
		{
		    case 1: