rt_fifos.c

实时fifo先进先出队列

#define VALID_FIFO	if (minor >= MAX_FIFOS) { return -ENODEV; } \
			if (!(fifo[minor].opncnt)) { return -EINVAL; }

int rtf_reset(unsigned int minor)
{
	F_MBX *mbx;
	VALID_FIFO;
	if (mbx_delete(mbx = &(fifo[minor].mbx))) {
		return -EFAULT;
	}
	mbx_init(mbx, mbx->size, mbx->bufadr);
	return 0;
}

int rtf_resize(unsigned int minor, int size)
{
	void *bufadr, *msg;
	int malloc_type;
	F_MBX *mbx;
	
	VALID_FIFO;
	malloc_type = fifo[minor].malloc_type;
	if (size <= PAGE_SIZE*32) {
		if (!(bufadr = kmalloc(size, GFP_KERNEL))) {
			return -ENOMEM;
		}
		fifo[minor].malloc_type = 'k';
	} else {
		if (!(bufadr = vmalloc(size))) {
			return -ENOMEM;
		}
		fifo[minor].malloc_type = 'v';
	}
	msg = bufadr;
	mbx = &(fifo[minor].mbx);
	mbx->avbs = 1000000000 - mbx_get(mbx, (char **)&msg, 1000000000, 0);
	if (malloc_type == 'k') {
		kfree(fifo[minor].mbx.bufadr); 
	} else {
		vfree(fifo[minor].mbx.bufadr); 
	}
	mbx->bufadr = bufadr;
	mbx->size = size > mbx->size ? size : mbx->size;
	mbx->fbyte = 0;
	mbx->frbs = mbx->size - mbx->avbs;
	return size;
}

int rtf_create(unsigned int minor, int size)
{
	void *buf;
	unsigned long flags;

	if (minor >= MAX_FIFOS) {
		return -ENODEV;
	}
	rtf_save_flags_and_cli(flags);
	if (!(fifo[minor].opncnt)) {
		if (size <= PAGE_SIZE*32) {
			if (!(buf = kmalloc(size, GFP_KERNEL))) {
				rtf_restore_flags(flags);
				return -ENOMEM;
			}
			fifo[minor].malloc_type = 'k';
		} else {
			if (!(buf = vmalloc(size))) {
				rtf_restore_flags(flags);
				return -ENOMEM;
			}
			fifo[minor].malloc_type = 'v';
		}
		fifo[minor].handler = do_nothing;
		mbx_init(&(fifo[minor].mbx), size, buf);
		mbx_sem_init(&(fifo[minor].sem), 0);
		fifo[minor].pol_asyn_pended = 0;
		fifo[minor].asynq = 0;
#if RTAI_RTF_NAMED
		fifo[minor].name[0] = 0;
#endif
	} else {
		if (size > fifo[minor].mbx.size) {
			rtf_resize(minor, size);
		}
	}
	MOD_INC_USE_COUNT;
	(fifo[minor].opncnt)++;
	rtf_restore_flags(flags);
	return 0;
}

int rtf_destroy(unsigned int minor)
{
	VALID_FIFO;
	MOD_DEC_USE_COUNT;
	(fifo[minor].opncnt)--;
	if(!(fifo[minor].opncnt)) {
		if (fifo[minor].malloc_type == 'k') {
			kfree(fifo[minor].mbx.bufadr); 
		} else {
			vfree(fifo[minor].mbx.bufadr); 
		}
		fifo[minor].handler = do_nothing;
		mbx_delete(&(fifo[minor].mbx));
		fifo[minor].pol_asyn_pended = 0;
		fifo[minor].asynq = 0;
#if RTAI_RTF_NAMED
		fifo[minor].name[0] = 0;
#endif
	}
	return fifo[minor].opncnt;
}

int rtf_create_handler(unsigned int minor, int (*handler) (unsigned int fifo))
{
	if (minor >= MAX_FIFOS || !handler) {
		return -EINVAL;
	}
	fifo[minor].handler = handler;
	return 0;
}

int rtf_put(unsigned int minor, void *buf, int count)
{
	VALID_FIFO;
	count -= mbx_send_wp(&(fifo[minor].mbx), buf, count, 0);
	return count;
}

int rtf_get(unsigned int minor, void *buf, int count)
{
	VALID_FIFO;
	count -= mbx_receive_wp(&(fifo[minor].mbx), buf, count, 0);
	return count;
}

int rtf_sem_init(unsigned int minor, int value)
{
	VALID_FIFO;
	mbx_sem_init(&(fifo[minor].sem), value);
	return 0;
}

int rtf_sem_post(unsigned int minor)
{
	VALID_FIFO;
	mbx_sem_signal(&(fifo[minor].sem), 0);
	return 0;
}

int rtf_sem_trywait(unsigned int minor)
{
	VALID_FIFO;
	return mbx_sem_wait_if(&(fifo[minor].sem));
}

int rtf_sem_delete(unsigned int minor)
{
	VALID_FIFO;
	return mbx_sem_delete(&(fifo[minor].sem));
}

static int rtf_open(struct inode *inode, struct file *filp)
{
#define DEFAULT_SIZE 1000
	return rtf_create(MINOR(inode->i_rdev), DEFAULT_SIZE);
}

static int rtf_fasync(int fd, struct file *filp, int mode)
{	
	int minor;
	minor = MINOR((filp->f_dentry->d_inode)->i_rdev);
	return fasync_helper(fd, filp, mode, &(fifo[minor].asynq));
	if (!mode) {
		fifo[minor].asynq = 0;
	}
}

static int rtf_release(struct inode *inode, struct file *filp)
{
	int minor;
	minor = MINOR(inode->i_rdev);
	wake_up_interruptible(&(fifo[minor].pollq));
	rtf_fasync(-1, filp, 0);
	return rtf_destroy(minor);
}

static ssize_t rtf_read(struct file *filp, char *buf, size_t count, loff_t* ppos)
{
	struct inode *inode = filp->f_dentry->d_inode;
	unsigned int minor = MINOR(inode->i_rdev);
	int handler_ret;

	if (filp->f_flags & O_NONBLOCK) {
		count -= mbx_receive_wp(&(fifo[minor].mbx), buf, count, 1);
		if (!count) {
			return -EAGAIN;
		}
	} else {
		count -= mbx_receive_wjo(&(fifo[minor].mbx), buf, count, 1);
	}

	if (count) {
		inode->i_atime = CURRENT_TIME;
//		if ((handler_ret = (fifo[minor].handler)(minor)) < 0) {
		if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'r')) < 0) {
			return handler_ret;
		}
		return count;
	}
	return 0;

	return count;
}

static ssize_t rtf_write(struct file *filp, const char *buf, size_t count, loff_t* ppos)
{
	struct inode *inode = filp->f_dentry->d_inode;
	unsigned int minor = MINOR(inode->i_rdev);
	int handler_ret;

	if (filp->f_flags & O_NONBLOCK) {
		count -= mbx_send_wp(&(fifo[minor].mbx), (char *)buf, count, 1);
		if (!count) {
			return -EAGAIN;
		}
	} else {
		count -= mbx_send(&(fifo[minor].mbx), (char *)buf, count, 1);
	}

	inode->i_ctime = inode->i_mtime = CURRENT_TIME;
//	if ((handler_ret = (fifo[minor].handler)(minor)) < 0) {
	if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'w')) < 0) {
		return handler_ret;
	}

	return count;
}

#define DELAY(x) (((x)*HZ + 500)/1000)

static int rtf_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
{	
	unsigned int minor;
	FIFO *fifop;

	fifop = fifo + (minor = MINOR(inode->i_rdev));
	switch(cmd) {
		case RESET: {
			return rtf_reset(minor);
		}
		case RESIZE: {
			return rtf_resize(minor, arg);
		}
		case SUSPEND_TIMED: {
			current->state = TASK_INTERRUPTIBLE;
			schedule_timeout(DELAY(arg));
			if (check_blocked(current->signal, current->blocked)) {
				return -ERESTARTSYS;
			}
			return 0;
		}
		case OPEN_SIZED: {
			return rtf_create(minor, arg);
		}
		case READ_ALL_AT_ONCE: {
			struct { char *buf; int count; } args;
			int handler_ret;
			copy_from_user(&args, (void *)arg, sizeof(args));
			args.count -= mbx_receive(&(fifop->mbx), args.buf, args.count, 1);
			if (args.count) {
				inode->i_atime = CURRENT_TIME;
//				if ((handler_ret = (fifop->handler)(minor)) < 0) {
				if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'r')) < 0) {
					return handler_ret;
				}
				return args.count;
			}
			return 0;
		}
		case READ_TIMED: {
			struct { char *buf; int count, delay; } args;
			int handler_ret;
			copy_from_user(&args, (void *)arg, sizeof(args));
			if (!args.delay) {
				args.count -= mbx_receive_wp(&(fifop->mbx), args.buf, args.count, 1);
				if (!args.count) {
					return -EAGAIN;
				}
			} else {
				args.count -= mbx_receive_timed(&(fifop->mbx), args.buf, args.count, DELAY(args.delay), 1);
			}
			if (args.count) {
				inode->i_atime = CURRENT_TIME;
//				if ((handler_ret = (fifop->handler)(minor)) < 0) {
				if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'r')) < 0) {
					return handler_ret;
				}
				return args.count;
			}
			return 0;
		}
		case WRITE_TIMED: {
			struct { char *buf; int count, delay; } args;
			int handler_ret;
			copy_from_user(&args, (void *)arg, sizeof(args));
			if (!args.delay) {
				args.count -= mbx_send_wp(&(fifop->mbx), args.buf, args.count, 1);
				if (!args.count) {
					return -EAGAIN;
				}
			} else {
				args.count -= mbx_send_timed(&(fifop->mbx), args.buf, args.count, DELAY(args.delay), 1);
			}
			inode->i_ctime = inode->i_mtime = CURRENT_TIME;
//			if ((handler_ret = (fifop->handler)(minor)) < 0) {
			if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'w')) < 0) {
				return handler_ret;
			}
			return args.count;
		}
		case RTF_SEM_INIT: {
			mbx_sem_init(&(fifop->sem), arg);
			return 0;
		}
		case RTF_SEM_WAIT: {
			return mbx_sem_wait(&(fifop->sem));
		}
		case RTF_SEM_TRYWAIT: {
			return mbx_sem_wait_if(&(fifop->sem));
		}
		case RTF_SEM_TIMED_WAIT: {
			return mbx_sem_wait_timed(&(fifop->sem), DELAY(arg));
		}
		case RTF_SEM_POST: {
			mbx_sem_signal(&(fifop->sem), 0);
			return 0;
		}
		case RTF_SEM_DESTROY: {
			mbx_sem_delete(&(fifop->sem));
			return 0;
		}
		case SET_ASYNC_SIG: {
			async_sig = arg;
			return 0;
		}
		case FIONREAD: {
			return put_user(fifo[minor].mbx.avbs, (int *)arg);
	        }
#if RTAI_RTF_NAMED
		/* 
		 * Support for named FIFOS : Ian Soanes ([e-mail])
		 * Based on ideas from Stuart Hughes and David Schleef
		 */
		case RTF_GET_N_FIFOS: {
			return MAX_FIFOS;
		}
		case RTF_GET_FIFO_INFO: {
			struct rt_fifo_get_info_struct req;
			int i, n;

			copy_from_user(&req, (void *)arg, sizeof(req));
			for ( i = req.fifo, n = 0; 
			      i < MAX_FIFOS && n < req.n; 
			      i++, n++
			      ) {
				struct rt_fifo_info_struct info;

				info.fifo_number = i;
				info.size        = fifo[i].mbx.size;
				info.opncnt      = fifo[i].opncnt;
				strncpy(info.name, fifo[i].name, RTF_NAMELEN+1);
				copy_to_user(req.ptr + n, &info, sizeof(info));
			}
			return n;
		}
		case RTF_CREATE_NAMED: {
			char name[RTF_NAMELEN+1];

			copy_from_user(name, (void *)arg, RTF_NAMELEN+1);
			return rtf_create_named(name);
	        }
		case RTF_NAME_LOOKUP: {
			char name[RTF_NAMELEN+1];

			copy_from_user(name, (void *)arg, RTF_NAMELEN+1);
			return rtf_getfifobyname(name);
		}
#endif // RTAI_RTF_NAMED
		default : {
			printk("RTAI-FIFO: cmd %d is not implemented\n", cmd);
			return -EINVAL;
	}
	}
	return 0;
}

static unsigned int rtf_poll(struct file *filp, poll_table *wait)
{
	unsigned int retval, minor;

	retval = 0;
	minor = MINOR((filp->f_dentry->d_inode)->i_rdev);
	poll_wait(filp, &(fifo[minor].pollq), wait);
	if (fifo[minor].mbx.avbs) {
		retval |= POLLIN | POLLRDNORM;
	}
	if (fifo[minor].mbx.frbs) {
		retval |= POLLOUT | POLLWRNORM;
	}
	return retval;
}

static loff_t rtf_llseek(struct file *filp, loff_t offset, int origin)
{
	return rtf_reset(MINOR((filp->f_dentry->d_inode)->i_rdev));
}

static struct file_operations rtf_fops =
{
	rtf_llseek, rtf_read, rtf_write, NULL, rtf_poll, rtf_ioctl, NULL,
	rtf_open, NULL, rtf_release, NULL, rtf_fasync, NULL, NULL, NULL
};

int init_module(void)
{
	int minor;

	if (register_chrdev(RTAI_MAJOR, "rtai_fifo", &rtf_fops)) {
		printk("RTAI-FIFO: cannot register major %d.\n", RTAI_MAJOR);
		return -EIO;
	}
	if ((fifo_srq = rtf_request_srq(rtf_sysrq_handler)) < 0) {
		printk("RTAI-FIFO: no srq available in rtai.\n");
		return fifo_srq;
	}
	taskq.in = taskq.out = pol_asyn_q.in = pol_asyn_q.out = 0;
	async_sig = SIGIO;
	for (minor = 0; minor < MAX_FIFOS; minor++) {
		fifo[minor].opncnt = fifo[minor].pol_asyn_pended = 0;
		init_waitqueue(&fifo[minor].pollq);
		fifo[minor].asynq = 0;;
		mbx_sem_init(&(fifo[minor].sem), 0);
	}
#if CONFIG_PROC_FS && RTAI_PROC
	rtai_proc_fifo_register();
#endif
	return 0;
}

void cleanup_module(void)
{
#ifdef CONFIG_RTL
	rtf_free_srq(fifo_srq);
#else
	if (rtf_free_srq(fifo_srq) < 0) {
		printk("RTAI-FIFO: rtai srq %d illegal or already free.\n", fifo_srq);
	}
#endif
#if CONFIG_PROC_FS && RTAI_PROC
        rtai_proc_fifo_unregister();
#endif
	unregister_chrdev(RTAI_MAJOR, "rtai_fifo");
}

#if CONFIG_PROC_FS && RTAI_PROC
/* ----------------------< proc filesystem section >----------------------*/

static int rtai_read_fifos(char* buf, char** start, off_t offset, int len, int unused)
{
	int i;

	len = sprintf(buf, "RTAI Real Time fifos status.\n\n" );
	if (len > LIMIT) {
		return(len);
	}
	len += sprintf(buf+len, "fifo No  Open Cnt  Buff Size  malloc type");
	if (len > LIMIT) {
		return(len);
	}
#ifdef RTAI_RTF_NAMED
	len += sprintf(buf+len, " Name\n----------------");
#else
	len += sprintf(buf+len, "\n");
#endif
	if (len > LIMIT) {
		return(len);
	}
	len += sprintf(buf+len, "-----------------------------------------\n");
	if (len > LIMIT) {
		return(len);
	}
/*
 * Display the status of all open RT fifos.
 */
	for (i = 0; i < MAX_FIFOS; i++) {
		if (fifo[i].opncnt > 0) {
			len += sprintf(	buf+len, "%-8d %-9d %-10d %-12s", i,
                        	        fifo[i].opncnt, fifo[i].mbx.size,
					fifo[i].malloc_type == 'v'
					    ? "vmalloc" : "kmalloc" 
					);
			if (len > LIMIT) {
				return(len);
			}
#ifdef RTAI_RTF_NAMED
			len += sprintf(buf+len, "%s\n", fifo[i].name);
#else
			len += sprintf(buf+len, "\n");
#endif
			if (len > LIMIT) {
				return(len);
			}
		} /* End if - fifo is open. */
	} /* End for loop - loop for all fifos. */
	return len;

}  /* End function - rtai_read_fifos */

static struct proc_dir_entry *proc_rtai_fifo;

static int rtai_proc_fifo_register(void) 
{
	proc_rtai_fifo = create_proc_entry("fifos", 0, proc_rtai);
	proc_rtai_fifo->get_info = rtai_read_fifos;

	return 0;
}  /* End function - rtai_proc_fifo_register */

static void rtai_proc_fifo_unregister(void) 
{
	remove_proc_entry("fifos", proc_rtai);
}  /* End function - rtai_proc_fifo_unregister */

/* ------------------< end of proc filesystem section >------------------*/
#endif // RTAI_PROC

#if RTAI_RTF_NAMED
/* 
 * Support for named FIFOS : Ian Soanes ([e-mail])
 * Based on ideas from Stuart Hughes and David Schleef
 */
int rtf_create_named(const char *name)
{
	int minor, err;
	unsigned long flags;

	if (strlen(name) > RTF_NAMELEN) {
	    	return -EINVAL;
	}
	rtf_spin_lock_irqsave(flags, rtf_name_lock);
	for (minor = 0; minor < MAX_FIFOS; minor++) {
	    	if (!strncmp(name, fifo[minor].name, RTF_NAMELEN)) {
			rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
		        return -EBUSY;
		}
		else if (!fifo[minor].opncnt && !fifo[minor].name[0]) {
		        if ((err = rtf_create(minor, DEFAULT_SIZE)) < 0) {
				rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
			        return err;
			}
		        strncpy(fifo[minor].name, name, RTF_NAMELEN+1);
			rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
			return minor;
		}
	}
	rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
	return -EBUSY;
}

int rtf_getfifobyname(const char *name)
{
    	int minor;

	if (strlen(name) > RTF_NAMELEN) {
	    	return -EINVAL;
	}
	for (minor = 0; minor < MAX_FIFOS; minor++) {
	    	if ( fifo[minor].opncnt && 
		     !strncmp(name, fifo[minor].name, RTF_NAMELEN)
		     ) {
		    	return minor;
		}
	}
	return -ENODEV;
}
#endif // RTAI_RTF_NAMED