fifos.c

rtai-3.1-test3的源代码(Real-Time Application Interface )

				if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'r')) < 0) {
					return handler_ret;
				}
				return args.count;
			}
			return 0;
		}
		case READ_IF: {
			struct { char *buf; int count; } args;
			int handler_ret;
			copy_from_user(&args, (void *)arg, sizeof(args));
			args.count -= mbx_receive_if(&(fifop->mbx), args.buf, args.count, 1);
			if (args.count) {
				inode->i_atime = CURRENT_TIME;
				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));
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_WRITE_TIMED, args.count, DELAY(args.delay));
			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 WRITE_IF: {
			struct { char *buf; int count, delay; } args;
			int handler_ret;
			copy_from_user(&args, (void *)arg, sizeof(args));
			if (args.count) {
				args.count -= mbx_send_wp(&(fifop->mbx), args.buf, args.count, 1);
				if (!args.count) {
					return -EAGAIN;
				}
			}
			inode->i_ctime = inode->i_mtime = CURRENT_TIME;
			if ((handler_ret = ((int (*)(int, ...))(fifo[minor].handler))(minor, 'w')) < 0) {
				return handler_ret;
			}
			return args.count;
		}
		case OVRWRITE: {
			struct { char *buf; int count; } args;
			copy_from_user(&args, (void *)arg, sizeof(args));
			return mbx_ovrwr_send(&(fifop->mbx), (char **)&args.buf, args.count, 1);
		}
		case RTF_SEM_INIT: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_INIT, minor, arg);
			mbx_sem_init(&(fifop->sem), arg);
			return 0;
		}
		case RTF_SEM_WAIT: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_WAIT, minor, 0);
			return mbx_sem_wait(&(fifop->sem));
		}
		case RTF_SEM_TRYWAIT: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_TRY_WAIT, minor, 0);
			return mbx_sem_wait_if(&(fifop->sem));
		}
		case RTF_SEM_TIMED_WAIT: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_TIMED_WAIT, minor, DELAY(arg));
			return mbx_sem_wait_timed(&(fifop->sem), DELAY(arg));
		}
		case RTF_SEM_POST: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_POST, minor, 0);
			mbx_sem_signal(&(fifop->sem), 0);
			return 0;
		}
		case RTF_SEM_DESTROY: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_DESTROY, minor, 0);
			mbx_sem_delete(&(fifop->sem));
			return 0;
		}
		case SET_ASYNC_SIG: {
			TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SET_ASYNC_SIG, arg, 0);
			async_sig = arg;
			return 0;
		}
		case FIONREAD: {
			return put_user(fifo[minor].mbx.avbs, (int *)arg);
	        }
		/* 
		 * 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_NAMED_CREATE: {
			struct { char name[RTF_NAMELEN+1]; int size; } args;

			copy_from_user(&args, (void *)arg, sizeof(args));
			return rtf_named_create(args.name, args.size);
	        }
		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);
		}
	        case TCGETS:
		        /* Keep isatty() probing silent */
		        return -ENOTTY;

		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);
	TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_POLL, minor, 0);
	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)
{
	TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_LLSEEK, MINOR((filp->f_dentry->d_inode)->i_rdev), offset);

	return rtf_reset(MINOR((filp->f_dentry->d_inode)->i_rdev));
}

static struct file_operations rtf_fops =
{
	owner:		THIS_MODULE,
	llseek:		rtf_llseek,
	read:		rtf_read,
	write:		rtf_write,
	poll:		rtf_poll,
	ioctl:		rtf_ioctl,
	open:		rtf_open,
	release:	rtf_release,
	fasync:		rtf_fasync,
};

#ifdef CONFIG_DEVFS_FS
#include <linux/devfs_fs_kernel.h>
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
static devfs_handle_t devfs_handle;
#endif
#endif

static int MaxFifos = MAX_FIFOS;
MODULE_PARM(MaxFifos, "i");

static struct rt_fun_entry rtai_fifos_fun[] = {
	[_CREATE]       = { 0, rtf_create },
	[_DESTROY]      = { 0, rtf_destroy },
	[_PUT]          = { 0, rtf_put },
	[_GET]          = { 0, rtf_get },
	[_RESET]        = { 0, rtf_reset },
	[_RESIZE]       = { 0, rtf_resize },
	[_SEM_INIT]     = { 0, rtf_sem_init },
	[_SEM_DESTRY]   = { 0, rtf_sem_destroy },
	[_SEM_POST]     = { 0, rtf_sem_post },
	[_SEM_TRY]      = { 0, rtf_sem_trywait },
	[_CREATE_NAMED] = { 0, rtf_create_named },
	[_GETBY_NAME]   = { 0, rtf_getfifobyname },
	[_OVERWRITE]    = { 0, rtf_ovrwr_put },
	[_PUT_IF]       = { 0, rtf_put_if },
	[_GET_IF]       = { 0, rtf_get_if }
};

static int register_lxrt_fifos_support(void)
{
	RT_TASK *rt_linux_tasks[NR_RT_CPUS];
	rt_base_linux_task = rt_get_base_linux_task(rt_linux_tasks);
	if(rt_base_linux_task->task_trap_handler[0]) {
		if(((int (*)(void *, int))rt_base_linux_task->task_trap_handler[0])(rtai_fifos_fun, FUN_FIFOS_LXRT_INDX)) {
			printk("LXRT EXTENSION SLOT FOR FIFOS (%d) ALREADY USED\n", FUN_FIFOS_LXRT_INDX);
			return -EACCES;
		}
	}
	return(0);
}

static void unregister_lxrt_fifos_support(void)
{
	if(rt_base_linux_task->task_trap_handler[1]) {
		((int (*)(void *, int))rt_base_linux_task->task_trap_handler[1])(rtai_fifos_fun, FUN_FIFOS_LXRT_INDX);
	}
}

int __rtai_fifos_init(void)
{
	int minor;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) || !CONFIG_DEVFS_FS
	if (register_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo",&rtf_fops)) {
#else /* LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) && CONFIG_DEVFS_FS */
	if (devfs_register_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo",&rtf_fops)) {
#endif  /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) || !CONFIG_DEVFS_FS */
		printk("RTAI-FIFO: cannot register major %d.\n", RTAI_FIFOS_MAJOR);
		return -EIO;
	}

#ifdef CONFIG_DEVFS_FS

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
	for (minor = 0; minor < MAX_FIFOS; minor++)
	    if (devfs_mk_cdev(MKDEV(RTAI_FIFOS_MAJOR,minor),
			      S_IFCHR|S_IRUSR|S_IWUSR,
			      "rtf/%u",
			      minor) < 0)
		{
		printk("RTAI-FIFO: cannot create devfs entry rtf/%u.\n",minor);
		devfs_remove("rtf");
		unregister_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo");
		return -EIO;
		}
#else /* LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) */
	devfs_handle = devfs_mk_dir(NULL, "rtf", NULL);

	if(!devfs_handle)
	    {
	    printk("RTAI-FIFO: cannot create devfs dir entry.\n");
	    return -EIO;
	    }

	devfs_register_series(devfs_handle,
			      "%u",
			      MAX_FIFOS,
			      DEVFS_FL_DEFAULT,
			      RTAI_FIFOS_MAJOR,
			      0,
			      S_IFCHR|S_IRUGO|S_IWUGO,
			      &rtf_fops,
			      NULL);
#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) */

#endif /* CONFIG_DEVFS_FS */

	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;

	if (!(fifo = (FIFO *)kmalloc(MaxFifos*sizeof(FIFO), GFP_KERNEL))) {
		printk("RTAI-FIFO: cannot allocate memory for FIFOS structure.\n");
		return -ENOSPC;
	}
	memset(fifo, 0, MaxFifos*sizeof(FIFO));

	for (minor = 0; minor < MAX_FIFOS; minor++) {
		fifo[minor].opncnt = fifo[minor].pol_asyn_pended = 0;
		init_waitqueue_head(&fifo[minor].pollq);
		fifo[minor].asynq = 0;;
		mbx_sem_init(&(fifo[minor].sem), 0);
	}
#ifdef CONFIG_PROC_FS
	rtai_proc_fifo_register();
#endif
	return register_lxrt_fifos_support();
}

void __rtai_fifos_exit(void)
{
	unregister_lxrt_fifos_support();

#if CONFIG_DEVFS_FS
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
	devfs_remove("rtf");
	unregister_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo");
#else /* LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) */
	devfs_unregister(devfs_handle);
	devfs_unregister_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo");
#endif  /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) || !CONFIG_DEVFS_FS */
#else /* !CONFIG_DEVFS_FS */
	unregister_chrdev(RTAI_FIFOS_MAJOR,"rtai_fifo");
#endif /* CONFIG_DEVFS_FS */

	if (rtf_free_srq(fifo_srq) < 0) {
		printk("RTAI-FIFO: rtai srq %d illegal or already free.\n", fifo_srq);
	}
#ifdef CONFIG_PROC_FS
        rtai_proc_fifo_unregister();
#endif
	kfree(fifo);
}

#ifndef CONFIG_RTAI_FIFOS_BUILTIN
module_init(__rtai_fifos_init);
module_exit(__rtai_fifos_exit);
#endif /* !CONFIG_RTAI_FIFOS_BUILTIN */

#ifdef CONFIG_PROC_FS
/* ----------------------< proc filesystem section >----------------------*/

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

	len = sprintf(buf, "RTAI Real Time fifos status.\n\n" );
	if (len > LIMIT) {
		return(len);
	}
	len += sprintf(buf + len, "Maximum number of FIFOS %d.\n\n", MaxFifos);
	if (len > LIMIT) {
		return(len);
	}
	len += sprintf(buf+len, "fifo No  Open Cnt  Buff Size  handler  malloc type");
	if (len > LIMIT) {
		return(len);
	}
	len += sprintf(buf+len, " Name\n----------------");
	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 %-10p %-12s", i,
                        	        fifo[i].opncnt, fifo[i].mbx.size,
					fifo[i].handler,
					fifo[i].malloc_type == 'v'
					    ? "vmalloc" : "kmalloc" 
					);
			if (len > LIMIT) {
				return(len);
			}
			len += sprintf(buf+len, "%s\n", fifo[i].name);
			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 int rtai_proc_fifo_register(void) 
{
        struct proc_dir_entry *proc_fifo_ent;
        proc_fifo_ent = create_proc_entry("fifos", S_IFREG|S_IRUGO|S_IWUSR, 
								rtai_proc_root);
        if (!proc_fifo_ent) {
                printk("Unable to initialize /proc/rtai/fifos\n");
                return(-1);
        }
        proc_fifo_ent->read_proc = rtai_read_fifos;
	return 0;
}

static void rtai_proc_fifo_unregister(void) 
{
	remove_proc_entry("fifos", rtai_proc_root);
}

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

/* 
 * Support for named FIFOS : Ian Soanes ([e-mail])
 * Based on ideas from Stuart Hughes and David Schleef
 */
int rtf_named_create(const char *name, int size)
{
	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)) {
			break;
		} else if (!fifo[minor].opncnt && !fifo[minor].name[0]) {
		        strncpy(fifo[minor].name, name, RTF_NAMELEN + 1);
			rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
		        if ((err = rtf_create(minor, size)) < 0) {
		        	fifo[minor].name[0] = 0;
			        return err;
			}
			return minor;
		}
	}
	rtf_spin_unlock_irqrestore(flags, rtf_name_lock);
	return -EBUSY;
}

int rtf_create_named(const char *name)
{
	return rtf_named_create(name, DEFAULT_SIZE);
}

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;
}

#ifdef CONFIG_KBUILD
EXPORT_SYMBOL(rtf_create);
EXPORT_SYMBOL(rtf_create_handler);
EXPORT_SYMBOL(rtf_create_named);
EXPORT_SYMBOL(rtf_destroy);
EXPORT_SYMBOL(rtf_evdrp);
EXPORT_SYMBOL(rtf_get);
EXPORT_SYMBOL(rtf_get_if);
EXPORT_SYMBOL(rtf_getfifobyname);
EXPORT_SYMBOL(rtf_ovrwr_put);
EXPORT_SYMBOL(rtf_put);
EXPORT_SYMBOL(rtf_put_if);
EXPORT_SYMBOL(rtf_reset);
EXPORT_SYMBOL(rtf_resize);
EXPORT_SYMBOL(rtf_sem_destroy);
EXPORT_SYMBOL(rtf_sem_init);
EXPORT_SYMBOL(rtf_sem_post);
EXPORT_SYMBOL(rtf_sem_trywait);
EXPORT_SYMBOL(rtf_named_create);
#endif /* CONFIG_KBUILD */