343 lines
9.1 KiB
C
343 lines
9.1 KiB
C
#include <linux/module.h>
|
|
#include <linux/init.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/sched.h>
|
|
#include <linux/cdev.h>
|
|
#include <linux/fs.h>
|
|
#include <linux/wait.h>
|
|
#include <linux/errno.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/uaccess.h>
|
|
#include <linux/device.h>
|
|
|
|
#include "char_dev.h"
|
|
|
|
|
|
static dev_t devno = 0;
|
|
static uint32_t my_fifo_major = MY_FIFO_MAJOR;
|
|
static uint32_t my_fifo_minor = 0;
|
|
|
|
static struct class *myclass;
|
|
|
|
|
|
static int my_fifo_size = MY_FIFO_SIZE;
|
|
module_param(my_fifo_size, int, S_IRUGO);
|
|
|
|
|
|
struct my_fifo_dev{
|
|
wait_queue_head_t readq, writeq;
|
|
char *fifo_begain,*fifo_end;
|
|
uint32_t fifo_size;
|
|
char *rp,*wp;
|
|
uint32_t nreaders,nwriters;
|
|
struct semaphore sem;
|
|
struct cdev my_cdev;
|
|
};
|
|
|
|
static struct my_fifo_dev *my_fifo_device_p = NULL;
|
|
|
|
/* 打开设备 */
|
|
static int my_fifo_open(struct inode *inode, struct file *filp)
|
|
{
|
|
struct my_fifo_dev *my_fifo_dev = NULL;
|
|
|
|
my_fifo_dev = container_of(inode->i_cdev, struct my_fifo_dev, my_cdev);
|
|
filp->private_data = my_fifo_dev;
|
|
|
|
//printk(KERN_INFO "[ KERN_INFO ] my_fifo_dev addr = 0x%x\n",my_fifo_dev);
|
|
|
|
|
|
if (down_interruptible(&my_fifo_dev->sem))
|
|
return -ERESTARTSYS;
|
|
|
|
if(!my_fifo_dev->fifo_begain){
|
|
//printk(KERN_INFO "[ KERN_INFO ] fifo is empty, kmalloc it ...\n");
|
|
my_fifo_dev->fifo_begain = kmalloc(my_fifo_size, GFP_KERNEL);
|
|
if(!my_fifo_dev->fifo_begain){
|
|
up(&my_fifo_dev->sem);
|
|
return -ENOMEM;
|
|
}
|
|
|
|
my_fifo_dev->fifo_size = my_fifo_size;
|
|
my_fifo_dev->fifo_end = my_fifo_dev->fifo_begain + my_fifo_size;
|
|
my_fifo_dev->rp = my_fifo_dev->wp = my_fifo_dev->fifo_begain;
|
|
}
|
|
|
|
//printk(KERN_INFO "[ KERN_INFO ] open...rp:0x%p,wp:0x%p\n",my_fifo_dev->rp,my_fifo_dev->wp);
|
|
|
|
|
|
/* use f_mode,not f_flags: it's cleaner (fs/open.c tells why) */
|
|
if (filp->f_mode & FMODE_READ)
|
|
my_fifo_dev->nreaders++;
|
|
if (filp->f_mode & FMODE_WRITE)
|
|
my_fifo_dev->nwriters++;
|
|
up(&my_fifo_dev->sem);
|
|
|
|
|
|
return nonseekable_open(inode,filp);
|
|
}
|
|
|
|
|
|
static ssize_t my_fifo_read(struct file *filp, char __user *buf, size_t cnt, loff_t *offt)
|
|
{
|
|
struct my_fifo_dev *my_fifo_dev = filp->private_data;
|
|
uint32_t read_cont = 0;
|
|
|
|
if (!(filp->f_mode & FMODE_READ)){
|
|
printk(KERN_ERR "[ KERN_ERR ] read is not permitted\n");
|
|
return -EPERM;
|
|
}
|
|
|
|
if(down_interruptible(&my_fifo_dev->sem)){
|
|
printk(KERN_ERR "[ KERN_ERR ] down_interruptible err\n");
|
|
return -ERESTARTSYS;
|
|
}
|
|
|
|
while(my_fifo_dev->rp == my_fifo_dev->wp){
|
|
up(&my_fifo_dev->sem);
|
|
|
|
if(filp->f_flags & O_NONBLOCK)
|
|
return -EAGAIN;
|
|
|
|
//printk(KERN_INFO "[ KERN_INFO ] rp:0x%p,wp:0x%p\n",my_fifo_dev->rp,my_fifo_dev->wp);
|
|
//printk(KERN_INFO "[ KERN_INFO ] waiting...\n");
|
|
|
|
if(wait_event_interruptible(my_fifo_dev->readq, (my_fifo_dev->rp != my_fifo_dev->wp)))
|
|
return -ERESTARTSYS;
|
|
|
|
//printk(KERN_INFO "[ KERN_INFO ] wake up!\n");
|
|
|
|
if(down_interruptible(&my_fifo_dev->sem))
|
|
return -ERESTARTSYS;
|
|
}
|
|
|
|
if(my_fifo_dev->rp < my_fifo_dev->wp){
|
|
read_cont = min(my_fifo_dev->wp - my_fifo_dev->rp, cnt);
|
|
}
|
|
else{
|
|
read_cont = min(my_fifo_dev->fifo_end - my_fifo_dev->rp , cnt);
|
|
}
|
|
|
|
//printk(KERN_INFO "[ KERN_INFO ] read %d bytes\n",read_cont);
|
|
|
|
if(copy_to_user(buf, my_fifo_dev->rp, read_cont)){
|
|
up(&my_fifo_dev->sem);
|
|
return -EFAULT;
|
|
}
|
|
|
|
my_fifo_dev->rp += read_cont;
|
|
|
|
if(my_fifo_dev->rp == my_fifo_dev->fifo_end)
|
|
my_fifo_dev->rp = my_fifo_dev->fifo_begain;
|
|
|
|
up(&my_fifo_dev->sem);
|
|
|
|
wake_up_interruptible(&my_fifo_dev->writeq);
|
|
|
|
|
|
return read_cont;
|
|
}
|
|
|
|
/* How much space is free? */
|
|
static int spacefree(struct my_fifo_dev *dev)
|
|
{
|
|
if (dev->rp == dev->wp)
|
|
return dev->fifo_size - 1;
|
|
return ((dev->rp + dev->fifo_size - dev->wp) % dev->fifo_size) - 1;
|
|
}
|
|
|
|
|
|
/* Wait for space for writing; caller must hold device semaphore. On
|
|
* error the semaphore will be released before returning. */
|
|
static int getwritespace(struct my_fifo_dev *dev, struct file *filp)
|
|
{
|
|
while (spacefree(dev) == 0) { /* full */
|
|
DEFINE_WAIT(wait);
|
|
|
|
up(&dev->sem);
|
|
if (filp->f_flags & O_NONBLOCK)
|
|
return -EAGAIN;
|
|
prepare_to_wait(&dev->writeq, &wait, TASK_INTERRUPTIBLE);
|
|
if (spacefree(dev) == 0){
|
|
//printk(KERN_INFO "[ KERN_INFO ] rp:0x%p,wp:0x%p\n",dev->rp,dev->wp);
|
|
//printk(KERN_INFO "[ KERN_INFO ] waiting...\n");
|
|
schedule();
|
|
//printk(KERN_INFO "[ KERN_INFO ] wake up!\n");
|
|
}
|
|
finish_wait(&dev->writeq, &wait);
|
|
if (signal_pending(current)){
|
|
printk(KERN_ERR "[ KERN_ERR ] signal_pending err\n");
|
|
return -ERESTARTSYS; /* signal: tell the fs layer to handle it */
|
|
}
|
|
if (down_interruptible(&dev->sem)){
|
|
printk(KERN_ERR "[ KERN_ERR ] down_interruptible err\n");
|
|
return -ERESTARTSYS;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
static ssize_t my_fifo_write(struct file *filp,const char __user *buf,size_t cnt, loff_t *offt)
|
|
{
|
|
struct my_fifo_dev *my_fifo_dev = filp->private_data;
|
|
uint32_t write_cont = 0;
|
|
uint32_t ret = 0;
|
|
|
|
if (!(filp->f_mode & FMODE_WRITE)){
|
|
printk(KERN_ERR "[ KERN_ERR ] write is not permitted\n");
|
|
return -EPERM;
|
|
}
|
|
|
|
if(down_interruptible(&my_fifo_dev->sem)){
|
|
printk(KERN_ERR "[ KERN_ERR ] down_interruptible err\n");
|
|
return -ERESTARTSYS;
|
|
}
|
|
|
|
ret = getwritespace(my_fifo_dev,filp);
|
|
if(ret){
|
|
printk(KERN_ERR "[ KERN_ERR ] getwritespace failed ret = %d\n",ret);
|
|
return ret;
|
|
}
|
|
|
|
write_cont = min(spacefree(my_fifo_dev),cnt);
|
|
if(my_fifo_dev->rp <= my_fifo_dev->wp)
|
|
write_cont = min(write_cont, (my_fifo_dev->fifo_end - my_fifo_dev->wp));
|
|
else
|
|
write_cont = min(write_cont, (my_fifo_dev->rp - my_fifo_dev->wp - 1));
|
|
|
|
if (copy_from_user(my_fifo_dev->wp, buf, write_cont)) {
|
|
up (&my_fifo_dev->sem);
|
|
return -EFAULT;
|
|
}
|
|
|
|
my_fifo_dev->wp += write_cont;
|
|
if (my_fifo_dev->wp == my_fifo_dev->fifo_end)
|
|
my_fifo_dev->wp = my_fifo_dev->fifo_begain; /* wrapped */
|
|
up(&my_fifo_dev->sem);
|
|
|
|
wake_up_interruptible(&my_fifo_dev->readq);
|
|
|
|
return write_cont;
|
|
}
|
|
static int my_fifo_release(struct inode *inode, struct file *filp)
|
|
{
|
|
struct my_fifo_dev *my_fifo_dev = NULL;
|
|
|
|
my_fifo_dev = filp->private_data;
|
|
|
|
down(&my_fifo_dev->sem);
|
|
if (filp->f_mode & FMODE_READ)
|
|
my_fifo_dev->nreaders--;
|
|
if (filp->f_mode & FMODE_WRITE)
|
|
my_fifo_dev->nwriters--;
|
|
if (my_fifo_dev->nreaders + my_fifo_dev->nwriters == 0) {
|
|
kfree(my_fifo_dev->fifo_begain);
|
|
my_fifo_dev->fifo_begain = NULL;
|
|
}
|
|
up(&my_fifo_dev->sem);
|
|
return 0;
|
|
|
|
}
|
|
|
|
static struct file_operations my_fifo_fops = {
|
|
.owner = THIS_MODULE,
|
|
.open = my_fifo_open,
|
|
.read = my_fifo_read,
|
|
.write = my_fifo_write,
|
|
.release = my_fifo_release,
|
|
};
|
|
|
|
|
|
static int __init my_fifo_init(void)
|
|
{
|
|
uint32_t ret = 0;
|
|
printk(KERN_INFO "[ KERN_INFO ] my fifo Module Init start\n");
|
|
|
|
if(my_fifo_major){
|
|
devno = MKDEV(my_fifo_major,my_fifo_minor);
|
|
ret = register_chrdev_region(devno, 1, "my_fifo");
|
|
}
|
|
else{
|
|
|
|
ret = alloc_chrdev_region(&devno, 0, 1, "my_fifo");
|
|
my_fifo_major = MAJOR(devno);
|
|
}
|
|
if(ret < 0){
|
|
printk(KERN_ERR "[ KERN_ERR ] chrdev alloc region failed\n");
|
|
goto devno_fail;
|
|
}
|
|
|
|
printk(KERN_INFO "[ KERN_INFO ] devno is %u(0x%x)(major:%u,minor:%u)\n",devno,devno,my_fifo_major,my_fifo_minor);
|
|
|
|
my_fifo_device_p = kmalloc(sizeof(struct my_fifo_dev), GFP_KERNEL);
|
|
if(!my_fifo_device_p){
|
|
printk(KERN_ERR "[ KERN_ERR ] kmalloc failed!\n");
|
|
goto kmalloc_fail;
|
|
}
|
|
memset(my_fifo_device_p, 0, sizeof(struct my_fifo_dev));
|
|
|
|
init_waitqueue_head(&(my_fifo_device_p->readq));
|
|
init_waitqueue_head(&(my_fifo_device_p->writeq));
|
|
sema_init(&my_fifo_device_p->sem,1);
|
|
|
|
cdev_init(&my_fifo_device_p->my_cdev, &my_fifo_fops);
|
|
my_fifo_device_p->my_cdev.owner = THIS_MODULE;
|
|
ret = cdev_add (&my_fifo_device_p->my_cdev, devno, 1);
|
|
|
|
if(ret){
|
|
printk(KERN_ERR "[ KERN_ERR ] cdev_add failed!\n");
|
|
goto cdev_add_fail;
|
|
}
|
|
|
|
myclass = class_create(THIS_MODULE, "myclass");
|
|
if(IS_ERR(myclass))
|
|
{
|
|
printk(KERN_ERR" create class faild!/n");
|
|
ret = -EBUSY;
|
|
goto class_create_fail;
|
|
}
|
|
|
|
device_create(myclass,NULL,MKDEV(my_fifo_major,my_fifo_minor),NULL,"myfifo");
|
|
|
|
return 0;
|
|
|
|
class_create_fail:
|
|
cdev_del(&my_fifo_device_p->my_cdev);
|
|
cdev_add_fail:
|
|
kfree(my_fifo_device_p);
|
|
kmalloc_fail:
|
|
unregister_chrdev_region(devno, 1);
|
|
devno_fail:
|
|
return ret;
|
|
}
|
|
|
|
static void __exit my_fifo_exit(void)
|
|
{
|
|
|
|
device_destroy(myclass,MKDEV(my_fifo_major,my_fifo_minor));
|
|
|
|
class_destroy(myclass);
|
|
|
|
cdev_del(&my_fifo_device_p->my_cdev);
|
|
|
|
if(my_fifo_device_p->fifo_begain != NULL)
|
|
kfree(my_fifo_device_p->fifo_begain);
|
|
|
|
kfree(my_fifo_device_p);
|
|
|
|
if(!devno)
|
|
unregister_chrdev_region(devno, 1);
|
|
|
|
printk("[ default ] my_fifo Module Exit\n");
|
|
}
|
|
|
|
module_init(my_fifo_init);
|
|
module_exit(my_fifo_exit);
|
|
|
|
|
|
MODULE_LICENSE("GPL");
|
|
MODULE_AUTHOR("liuchao");
|
|
|