scullc

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/moduleparam.h>
#include <linux/kdev_t.h>
#include <linux/fs.h>
#include <linux/cdev.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/uaccess.h>
#include "scullc.h"

MODULE_LICENSE("Dual BSD/GPL");
MODULE_DESCRIPTION("STARTING DEVICE DRIVERS");

static int major = SCULLC_MAJOR;
static int minor = SCULLC_MINOR;
static int count = SCULLC_COUNT;
static int scullc_qset =    SCULLC_QSET;
static int scullc_quantum = SCULLC_QUANTUM;

module_param(major,int,0);
module_param(minor,int,0);

struct scull_qset {
    void **data;
    struct scull_qset *next;
};

struct scull_dev {
    struct scull_qset *data;
    int quantum;
    int qset;
    unsigned long size;
    unsigned int access_key;
    struct semaphore sem;
    struct cdev cdev;
};

struct scull_dev *scullc_devices;

int scull_trim(struct scull_dev*);

int scull_open(struct inode *inode, struct file *filp)
{
    struct scull_dev *dev;

    dev = container_of(inode->i_cdev,struct scull_dev, cdev);
    filp->private_data = dev;

    if ((filp->f_flags & O_ACCMODE) == O_WRONLY)
        return 0;
        //scull_trim(dev);
    return 0;
}

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

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

ssize_t scull_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos)
{
    struct scull_dev *dev = filp->private_data;
    struct scull_qset *dptr;
    int quantum = dev->quantum, qset = dev->qset;
    int itemsize = quantum * qset;
    int item, s_pos, q_pos, rest;
    ssize_t retval = 0;

    if (down_interruptible(&dev->sem))
        return -ERESTARTSYS;
    if (*f_pos >= dev->size)
        goto out;
    if (*f_pos + count > dev->size)
        count = dev->size - *f_pos;

    item = (long)*f_pos / itemsize;
    rest = (long)*f_pos % itemsize;
    s_pos = rest / quantum; q_pos = rest % quantum;

    dptr = scull_follow(dev,item);

    if (dptr == NULL || !dptr->data || !dptr->data[s_pos])
        goto out;

    if (count > quantum - q_pos)
        count = quantum - q_pos;

    if (copy_to_user(buf,dptr->data[s_pos] + q_pos,count)) {
        retval = -EFAULT;
        goto out;
    }
    *f_pos += count;
    retval = count;

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

ssize_t scull_write(struct file *filp, const char __user *buf,
        size_t count, loff_t *f_pos)
{
    struct scull_dev *dev = filp->private_data;
    struct scull_qset *dptr;
    int quantum = dev->quantum, qset = dev->qset;
    int itemsize = quantum * qset;
    int item, s_pos, q_pos, rest;
    ssize_t retval = -ENOMEM;

    if (down_interruptible(&dev->sem))
        return -ERESTARTSYS;

    item = (long)*f_pos / itemsize;
    rest = (long)*f_pos % itemsize;
    s_pos = rest / quantum; q_pos = rest % quantum;

    dptr = scull_follow(dev,item);

    if (dptr == NULL)
        goto out;
    if (!dptr->data) {
        dptr->data = kmalloc(qset * sizeof(char *), GFP_KERNEL);
        if (!dptr->data)
            goto out;
        memset(dptr->data,0,qset * sizeof(char*));
    }
    if (!dptr->data[s_pos]) {
        dptr->data[s_pos] = kmalloc(quantum,GFP_KERNEL);
        if (!dptr->data[s_pos])
            goto out;
    }
    if (count > quantum - q_pos)
        count = quantum - q_pos;

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

    if (dev->size < *f_pos)
        dev->size = *f_pos;

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

struct file_operations scull_fops = {
    .owner   = THIS_MODULE,
    .open    = scull_open,
    .release = scull_release,
};

/*
int scull_trim(struct scull_dev *dev)
{
    struct scull_qset *next, *dptr;
    int qset = dev->qset;
    int i;

    for (dptr = dev->data; dptr; dptr = next) {
        if (dptr->data) {
            for (i = 0; i < qset; i++)
                kfree(dptr->data[i]);
            kfree(dptr->data);
            dptr->data = NULL;
        }
        next = dptr->next;
        kfree(dptr);
    }
    dev->size = 0;
    dev->quantum = scullc_quantum;
    dev->qset = scullc_qset;
    dev->data = NULL;
    return 0;
}*/

static void scull_setup_cdev(struct scull_dev *dev, int index)
{
    int err, devno = MKDEV(major, minor + index);

    cdev_init(&dev->cdev,&scull_fops);
    dev->cdev.owner = THIS_MODULE;
    dev->cdev.ops = &scull_fops;
    err = cdev_add(&dev->cdev,devno,1);
    if (err)
        printk(KERN_NOTICE "Error %d adding scull%d",err,index);
}

static int initt(void)
{
    int result, i;
    dev_t dev;

    if (major) {
        dev = MKDEV(major,minor);
        printk(KERN_WARNING "Manually registering device\n");
        result = register_chrdev_region(dev,count,"scullc");
    } else {
        printk(KERN_WARNING "Automatically registering device\n");
        alloc_chrdev_region(&dev,0,count,"scullc");
        result = major = MAJOR(dev);
    }

    if (result < 0) {
        printk(KERN_CRIT "Failed obtaining major number\n");
        return result;
    }

    printk(KERN_WARNING "Major is %d\n",major);

    scullc_devices = kmalloc(sizeof(struct scull_dev)*count,GFP_KERNEL);

    printk(KERN_CRIT "Scullc_devices is %s",
        (scullc_devices) ? "NONNULL" : "NULL");
    printk(KERN_CRIT "scullc_devices @%p",scullc_devices);

    if (!scullc_devices)
        goto fail;

    memset(scullc_devices,0,sizeof(struct scull_dev)*count);
    for (i = 0; i < count; i++) {
        scullc_devices[i].quantum = scullc_quantum;
        scullc_devices[i].qset = scullc_qset;
        scull_setup_cdev(scullc_devices + i, i);
    }

    return 0;

    fail:
        unregister_chrdev_region(dev,count);
        return result;
}

static void exitt(void) {
    int i;

    printk(KERN_WARNING "ON EXIT\n");

    for (i = 0; i < count; i++) {
        cdev_del(&scullc_devices[i].cdev);
    }

    unregister_chrdev_region(major,count);
}

module_init(initt);
module_exit(exitt);