scull.c

linux device drive II 的示例移植到2.6.x内核

/*
	linux device drivers 
	scull modified to linux-2.6.x
	
	zhengjie
	[e-mail]
*/
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/module.h>
#include <linux/errno.h>
#include <linux/devfs_fs_kernel.h>     /* devfs_mk_dir() */
#include <linux/slab.h>
#include <linux/device.h>              /* class_simple_create() */
#include <linux/moduleparam.h>         /* module_param() */
#include <asm/uaccess.h>               /* access_ok() */
#include <linux/sched.h>               /* capable() */
#include <linux/capability.h>          /* CAP_SYS_ADMIN */
#include "scull.h"

static int scull_major;

int scull_quantum = SCULL_QUANTUM;
int scull_qset    = SCULL_QSET;


typedef struct scull_dev {
	void    **data;
	struct  scull_dev *next;
	int	quantum;
	int 	qset;
	size_t  size;
	struct  semaphore sem;
} scull_dev;


struct scull_dev *scull_devices;

struct class_simple *scull_class;

struct file_operations  scull_fops = 
{
	.owner	  = THIS_MODULE,
	.llseek   = scull_llseek,
	.read     = scull_read,
	.write    = scull_write,
	.ioctl    = scull_ioctl,
	.open     = scull_open,
	.release  = scull_release,
};

static int scull_trim(struct scull_dev *dev)
{
	int qset = dev->qset;
	int i;
	struct scull_dev *next, *dptr;
	
	for(dptr=dev; dptr; dptr=next){
		if(dptr->data){
			for(i=0; i<qset; i++){
				if(dptr->data[i])
					kfree(dptr->data[i]);
				kfree(dptr->data);
				dptr->data = NULL;	
			}
		}
		next = dptr->next;
		if(dptr != dev)	kfree(dptr);
	}
	dev->size = 0;
	dev->qset = SCULL_QSET;
	dev->quantum = SCULL_QUANTUM;
	dev->next = NULL;
	return 0;
}

static struct scull_dev* scull_follow(struct scull_dev *dev, int n)
{
    while (n--) {
        if (!dev->next) {
            dev->next = kmalloc(sizeof(scull_dev), GFP_KERNEL);
            memset(dev->next, 0, sizeof(scull_dev));
        }
        dev = dev->next;
        continue;
    }
    return dev;
}


static ssize_t scull_read(struct file *filp, char __user *buf,
			  size_t count, loff_t *ppos)
{
	struct scull_dev *dev, *dptr;
	ssize_t ret = 0;
	int qnum, qset, qtotal;
	int item, s_pos, q_pos, rest;

	dev = filp->private_data;
	qnum = dev->quantum;
	qset = dev->qset;
 	qtotal = qnum*qset;
	
	if(down_interruptible(&dev->sem))
		return -ERESTARTSYS;
	if(*ppos > dev->size)
		goto out;
	if(*ppos + count > dev->size)
		count = dev->size - *ppos;
	item  = (long)*ppos / qtotal;
	rest  = (long)*ppos % qtotal;
	s_pos = rest / qnum;
	q_pos = rest % qnum;	

	dptr = scull_follow(dev, item);
	
	if(!dptr->data)
		goto out;
	if(!dptr->data[s_pos])
		goto out;
	if(count > qtotal - q_pos)
		count = qtotal - q_pos;
	if(copy_to_user(buf, dptr->data[s_pos] + q_pos , count)){
		ret = -EFAULT;
		goto out;
	}
	
	*ppos += count;
	ret = count;
	
out:
	up(&dev->sem);
	return ret;  

}



static ssize_t scull_write(struct file *filp, const __user char *buf,
			   size_t count, loff_t *ppos )
{
	
	struct scull_dev *dev;
	struct scull_dev *dptr;
	ssize_t ret = 0;
	int qnum, qset, qtotal;
	int item, rest, s_pos, q_pos;
	

	dev = filp->private_data;
	qnum = dev->quantum;
	qset = dev->qset;
	qtotal = qnum*qset;
	
	if(down_interruptible(&dev->sem))
		return -ERESTARTSYS;
	item = (long)*ppos / qtotal;
	rest = (long)*ppos % qtotal;
	s_pos = rest / qnum;
	q_pos = rest % qnum;
	
	dptr = scull_follow(dev, item);
	
	if(!dptr->data){
		dptr->data = kmalloc(qset*sizeof(char), GFP_KERNEL);
		if(!dptr->data)
			goto out;
	}
	if(!dptr->data[s_pos]){
		dptr->data[s_pos] = kmalloc(qnum*sizeof(char), GFP_KERNEL);
		if(!dptr->data[s_pos])
			goto out;
	}
	if(count > qnum - q_pos)
		count = qnum - q_pos;

	if(copy_from_user(dptr->data[s_pos] + q_pos, buf, count)){
		ret = -EFAULT;
		goto out;	
	}
	*ppos += count;
	ret = count;

out:
	up(&dev->sem);
	return ret;
}

static int scull_ioctl(struct inode *inode, struct file *flip,
			unsigned int cmd, unsigned long arg)
{
	int err = 0;
	int ret = 0;
        char __user *argp = (char __user *) arg;
	
	if(_IOC_TYPE(cmd) != SCULL_IOC_MAGIC) return -ENOTTY;
	if(_IOC_NR(cmd) > SCULL_IOC_MAXNR) return -ENOTTY;		
	
//	if(_IOC_DIR(cmd) & _IOC_READ)
//		err = access_ok(VERIFY_WRITE, (void *)argp, _IOC_SIZE(cmd));
//		printk("err=%d\n", err);
//	if(_IOC_DIR(cmd) & _IOC_WRITE)
//		err = access_ok(VERIFY_READ, (void *)argp, _IOC_SIZE(cmd));
//		printk("err=%d\n", err);
//	if(!err) return -EFAULT;
	switch(cmd){
	case SCULL_IOCRESET:
		scull_quantum = SCULL_QUANTUM;
		scull_qset    = SCULL_QSET;
		break;
//	case SCULL_IOCSQUANTUM:
//		if(!capable(CAP_SYS_ADMIN))
//			return -EPERM;		
//		ret = __get_user(scull_quantum, argp);
//		break;
//	case SCULL_IOCSQSET:
//		break;
	case SCULL_IOCTQUANTUM:
		if(!capable(CAP_SYS_ADMIN))
			return -EPERM;
		ret = arg;
		break;
	case SCULL_IOCTQSET:
		printk("<3>Hello, Kernel-2.6.10_scull!\n");
		break;
//	case SCULL_IOCGQUANTUM:
//		ret = __put_user(scull_quantum, argp);
//		break;
//	case SCULL_IOCGQSET:
//		break;
	case SCULL_IOCQQUANTUM:
		return scull_quantum;
		break;
	case SCULL_IOCQQSET:
		break;
//	case SCULL_IOCXQUANTUM:
//		if(!capable(CAP_SYS_ADMIN))
//			return -EPERM;
//		tmp = scull_quantum;
//		ret = __get_user(scull_quantum, argp);
//		if(ret == 0)
//			ret = __put_user(scull_quantum, argp);
//		break;
//	case SCULL_IOCXQSET:
//		break;
//	case SCULL_IOCHQUANTUM:
//		if(!capable(CAP_SYS_ADMIN))
//			return -EPERM;
//		tmp = scull_quantum;
//		scull_quantum = argp;
//		return tmp;
//		break;
	case SCULL_IOCHQSET:
		break;
	default:
		return -ENOTTY;	
	}
return ret;
}			

static int scull_open(struct inode *inode, struct file *filp)
{
	struct scull_dev *dev;
	unsigned int minor = iminor(inode);
	
	if(minor > 2)
		return -ENXIO;
	dev = (struct scull_dev *)filp->private_data;
	if(!dev){
		dev = scull_devices;
		filp->private_data = dev;
	}
	if((filp->f_flags & O_ACCMODE) == O_WRONLY){
		if(down_interruptible(&dev->sem)){
			return -ERESTARTSYS;
		}
		scull_trim(dev);
		up(&dev->sem);
	}
	return 0;
}

static int scull_release(struct inode *inode, struct file *filp)
{
	return 0;
}

static loff_t scull_llseek(struct file *filp, loff_t off, int whence)
{
	scull_dev *dev = filp->private_data;
	loff_t newpos;

	switch(whence){
	    case 0:    /* SEEK_SET */
		newpos = off; 
		break;
	    case 1:    /* SEEK_CUR */
		newpos = filp->f_pos + off;
		break;
	    case 2:    /* SEEK_END */
		newpos = dev->size +off;
		break;
	    default:
		return -EINVAL;
	}
	if(newpos < 0) return -EINVAL;
	filp->f_pos = newpos;
	return newpos;	
}



static int __init scull_init_module(void)
{
	int result = 0, tmp;
	
	scull_major = register_chrdev(0, "scull", &scull_fops);
	
	if(scull_major < 0){        
		printk(KERN_ERR"Scull - cannot register device\n");
		return scull_major;
	}
        if(scull_major == 0){
		printk(KERN_ERR"Scull major didnt register into kernel\n");
		return scull_major;
	}

        scull_class = class_simple_create(THIS_MODULE, "scull");
        if(IS_ERR(scull_class)){
                result = PTR_ERR(scull_class);
                goto fail_malloc;
        }
        class_simple_device_add(scull_class, MKDEV(scull_major,0), NULL, "scull");

        /* Create scull devfs */
	devfs_mk_dir("scull");
 	tmp = devfs_mk_cdev(MKDEV(scull_major, 0), 
			S_IFCHR | S_IRUSR | S_IWUSR, "scull/0");
	if(tmp)
		goto out;
	scull_devices = kmalloc(sizeof(struct scull_dev), GFP_KERNEL);
	if(!scull_devices){
		result = -ENOMEM;
		goto fail_malloc;
	}	
	memset(scull_devices, 0, sizeof(struct scull_dev));
	scull_devices->quantum = SCULL_QUANTUM;
	scull_devices->qset    = SCULL_QSET;
	sema_init(&scull_devices->sem, 1);
	goto out;

fail_malloc:
	unregister_chrdev(scull_major, "scull");
	return 	result;
out: 	return result;
}

static void __exit scull_cleanup_module(void)
{
	kfree(scull_devices);
	class_simple_device_remove(MKDEV(scull_major, 0));
	class_simple_destroy(scull_class);
	unregister_chrdev(scull_major, "scull");
	devfs_remove("scull");
}

module_init(scull_init_module);
module_exit(scull_cleanup_module);
MODULE_LICENSE("GPL");