rtl_fifo.c

fsmlabs的real time linux的内核

/*
 * (C) Finite State Machine Labs Inc. 1995-2000 <[e-mail]>
 *
 * Released under the terms of GPL 2.
 * Open RTLinux makes use of a patented process described in
 * US Patent 5,995,745. Use of this process is governed
 * by the Open RTLinux Patent License which can be obtained from
 * www.fsmlabs.com/PATENT or by sending email to
 * [e-mail]
 */
/*
 * Includes a tiny bit of code from Linux fs/pipe.c copyright (C) Linus Torvalds.
 *
 */

#include <linux/module.h>
#include <linux/major.h>
#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <linux/poll.h>
#include <linux/smp.h>
#include <linux/vmalloc.h>

#include <asm/uaccess.h>
#include <asm/system.h>
#include <asm/irq.h>

#include <rtl_conf.h>
#include <rtl_sync.h>
#include <rtl_fifo.h>
#include <rtl_core.h>
#include <rtl.h>

MODULE_LICENSE("GPL v2");
MODULE_AUTHOR("FSMLabs Inc.");
MODULE_DESCRIPTION("RTLinux FIFOs");

struct rt_fifo_struct {
	int allocated;
	int bidirectional;
	int user_open;
	struct task_struct *opener;
	char *base;
	int bufsize;
	int start;
	int len;
	spinlock_t fifo_spinlock;
	int (*user_handler) (unsigned int fifo);
	int (*rt_handler) (unsigned int fifo);
	int (*user_ioctl)(unsigned int fifo, unsigned int cmd, unsigned long arg);
	struct module *creator;
#if LINUX_VERSION_CODE >= 0x020300
	wait_queue_head_t wait;
#else
	struct wait_queue *wait;
#endif
};

struct rt_fifo_struct rtl_fifos[RTF_MAX_FIFO];
static int rtl_fifo_to_wakeup[RTF_MAX_FIFO] = {0,}; 
static int rtl_fifo_irq = 0; 

#ifdef CONFIG_RTFPREALLOC
#define PREALLOC_BUFFERS CONFIG_NRTFBUFF
#define PREALLOC_SIZE CONFIG_SIZE_RTFBUFF
static int fifo_buffer_control[PREALLOC_BUFFERS]={0,};
static char fifo_buffer[PREALLOC_BUFFERS*PREALLOC_SIZE];
static char * get_prealloc(void)
{
	int i; 
	for(i=0; i < PREALLOC_BUFFERS; i++){
		if(!test_and_set_bit(0,&fifo_buffer_control[i])){
			return &fifo_buffer[i*PREALLOC_SIZE];
		}
	}
		return 0;
}

static int free_prealloc(char *b)
{
	int i;
	for(i = 0; i < PREALLOC_BUFFERS; i++){
		if(b == &fifo_buffer[i*PREALLOC_SIZE])
		{
			clear_bit(0, &fifo_buffer_control[i]);
			return 1;
		}
	}
	return 0;
}

static int find_prealloc(char *b)
{
	int i;
	for(i = 0; i < PREALLOC_BUFFERS; i++){
		if(b == &fifo_buffer[i*PREALLOC_SIZE])
		{
			return 1;
		}
	}
	return 0;
}
#else
#define get_prealloc() 0
#define free_prealloc(x) 0
#define find_prealloc(x) 0
#define  PREALLOC_SIZE 8192 // used for O_CREATE in posix open even if no prealloc
#endif

#define RTF_ADDR(minor)		(&rtl_fifos[minor])

#define RTF_BI(minor)		(RTF_ADDR(minor)->bidirectional)
#define RTF_ALLOCATED(minor)		(RTF_ADDR(minor)->allocated)
#define RTF_USER_OPEN(minor)	(RTF_ADDR(minor)->user_open)
#define RTF_OPENER(minor)	(RTF_ADDR(minor)->opener)
#define RTF_BASE(minor)		(RTF_ADDR(minor)->base)
#define RTF_SPIN(minor)		(RTF_ADDR(minor)->fifo_spinlock)
#define RTF_BUF(minor)		(RTF_ADDR(minor)->bufsize)
#define RTF_START(minor)	(RTF_ADDR(minor)->start)
#define RTF_HANDLER(minor)	(RTF_ADDR(minor)->user_handler)
#define RTF_RT_HANDLER(minor)	(RTF_ADDR(minor)->rt_handler)
#define RTF_USER_IOCTL(minor)	(RTF_ADDR(minor)->user_ioctl)
#define RTF_LEN(minor)		(RTF_ADDR(minor)->len)
#define RTF_FREE(minor)		(RTF_BUF(minor) - RTF_LEN(minor))
#define RTF_WAIT(minor)		(RTF_ADDR(minor)->wait)

#define RTF_WRAP(minor,pos)	((pos) < RTF_BUF(minor)? (pos) : (pos) - RTF_BUF(minor))
#define RTF_END(minor)		RTF_WRAP(minor, RTF_START(minor)+RTF_LEN(minor))
#define RTF_EMPTY(minor)	(RTF_LEN(minor)==0)
#define RTF_FULL(minor)		(RTF_FREE(minor)==0)
#define RTF_MAX_RCHUNK(minor)	(RTF_BUF(minor) - RTF_START(minor))
#define RTF_MAX_WCHUNK(minor)	(RTF_BUF(minor) - RTF_END(minor))

#define RTL_SLEEP_POS 1
#define RTL_NEEDS_WAKE_POS 2

static int default_handler (unsigned int fifo)
{
	return 0;
}

static void rtf_wake_up(void *p)
{
	struct rt_fifo_struct *fifo_ptr = (struct rt_fifo_struct *) p;
	wake_up_interruptible(&(fifo_ptr->wait));
	current->need_resched = 1;
}

static void fifo_wake_sleepers(int );

/* These are for use in the init and exit code of real-time modules
   DO NOT call these from a RT task  */

int rtf_resize(unsigned int minor, int size)
{
	void *mem=0;
	void *old;
	rtl_irqstate_t interrupt_state;

	if (size <= 0) {
		return -EINVAL;
	}

	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if(size == PREALLOC_SIZE){
		mem=get_prealloc();
	}
	
	if(!mem){
 		if (!rtl_rt_system_is_idle()) {
 			return -EINVAL;
 		}
		mem = vmalloc(size);
	}
	if (!mem) {
		return -ENOMEM;
	}
	memset(mem, 0, size);
	old = RTF_BASE(minor);


	rtl_spin_lock_irqsave(&RTF_SPIN(minor), interrupt_state);
	RTF_BASE(minor) = mem;
	RTF_BUF(minor) = size;
	RTF_START(minor) = 0;
	RTF_LEN(minor) = 0;
	rtl_spin_unlock_irqrestore(&RTF_SPIN(minor), interrupt_state);

	if (RTF_ALLOCATED(minor) && old && !free_prealloc(old)){
		vfree(old);
	}
	return 0;
}

extern int rtf_link_user_ioctl (unsigned int minor,
		int (*handler)(unsigned int fifo, unsigned int cmd, unsigned long arg))
{
	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	RTF_USER_IOCTL(minor) = handler;
	return 0;
}


extern int rtf_make_user_pair (unsigned int fifo_get, unsigned int fifo_put)
{
	if (fifo_get >= RTF_MAX_FIFO || fifo_put >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if (!RTF_ALLOCATED(fifo_get) || !RTF_ALLOCATED(fifo_put)) {
		return -EINVAL;
	}
	RTF_BI(fifo_get) = (fifo_put - fifo_get);
	RTF_BI(fifo_put) = -(fifo_put - fifo_get);
	return 0;
}


int __rtf_create(unsigned int minor, int size, struct module *creator)
{
	int ret;

	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if (RTF_ALLOCATED(minor)) {
		return -EBUSY;
	}
	spin_lock_init(&RTF_SPIN(minor));
	RTF_BI(minor) = 0;
	if ((ret = rtf_resize(minor, size)) < 0) {
		return -ENOMEM;
	}
	RTF_ADDR(minor)->creator = creator;
	RTF_USER_OPEN(minor) = 0;
	RTF_OPENER(minor) = 0;
	RTF_HANDLER(minor) = &default_handler;
	RTF_RT_HANDLER(minor) = &default_handler;
	RTF_USER_IOCTL(minor) = 0;

#if LINUX_VERSION_CODE >= 0x020300
	init_waitqueue_head(&RTF_WAIT(minor));
#else
	init_waitqueue (&RTF_WAIT(minor));
#endif
	RTF_ALLOCATED(minor) = 1;
	return 0;
}


int rtf_destroy(unsigned int minor)
{
	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if (RTF_USER_OPEN(minor)) {
		return -EINVAL;
	}
	if (!RTF_ALLOCATED(minor)) {
		return -EINVAL;
	}
	RTF_ADDR(minor)->creator = 0;
	RTF_HANDLER(minor) = &default_handler;
	RTF_RT_HANDLER(minor) = &default_handler;
	if (!free_prealloc(RTF_BASE(minor))){
		vfree(RTF_BASE(minor));
	}
	RTF_ALLOCATED(minor) = 0;
	return 0;
}


int rtf_create_handler(unsigned int minor, int (*handler) (unsigned int fifo))
{
	if (minor >= RTF_MAX_FIFO || !RTF_ALLOCATED(minor) || !handler) {
		return -EINVAL;
	}
	RTF_HANDLER(minor) = handler;
	return 0;
}


extern int rtf_create_rt_handler(unsigned int minor,
		int (*handler)(unsigned int fifo))
{
	if (minor >= RTF_MAX_FIFO || !RTF_ALLOCATED(minor) || !handler) {
		return -EINVAL;
	}
	RTF_RT_HANDLER(minor) = handler;
	return 0;
}


/* these can be called from RT tasks and interrupt handlers */

int rtf_isempty(unsigned int minor)
{
	return RTF_LEN(minor) == 0;
}

int rtf_isused(unsigned int minor)
{
	return RTF_USER_OPEN(minor) != 0;
}

int rtf_flush(unsigned int minor)
{
	rtl_irqstate_t interrupt_state;
	rtl_spin_lock_irqsave(&RTF_SPIN(minor), interrupt_state);
	RTF_LEN(minor) = 0;
	rtl_spin_unlock_irqrestore(&RTF_SPIN(minor), interrupt_state);
	return 0;
}

int rtf_put(unsigned int minor, void *buf, int count)
{
	rtl_irqstate_t interrupt_state;
	int chars = 0, free = 0, written = 0;
	char *pipebuf;

	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if (!RTF_ALLOCATED(minor))
		return -EINVAL;

	rtl_spin_lock_irqsave(&RTF_SPIN(minor), interrupt_state);
	if (RTF_FREE(minor) < count) {
		rtl_spin_unlock_irqrestore(&RTF_SPIN(minor), interrupt_state);
		return -ENOSPC;
	}
	while (count > 0 && (free = RTF_FREE(minor))) {
		chars = RTF_MAX_WCHUNK(minor);
		if (chars > count)
			chars = count;
		if (chars > free)
			chars = free;
		pipebuf = RTF_BASE(minor) + RTF_END(minor);
		written += chars;
		RTF_LEN(minor) += chars;
		count -= chars;
		memcpy(pipebuf, buf, chars);
		buf += chars;
	}
	rtl_spin_unlock_irqrestore(&RTF_SPIN(minor), interrupt_state);

	(*RTF_RT_HANDLER(minor))(minor);

	if (RTF_USER_OPEN(minor)) {
		fifo_wake_sleepers(minor - (RTF_BI(minor) < 0));
	}
	return written;
}


int rtf_get(unsigned int minor, void *buf, int count)
{
	rtl_irqstate_t interrupt_state;
	int chars = 0, size = 0, read = 0;
	char *pipebuf;

	if (minor >= RTF_MAX_FIFO) {
		return -ENODEV;
	}
	if (!RTF_ALLOCATED(minor))
		return -EINVAL;

	rtl_spin_lock_irqsave(&RTF_SPIN(minor), interrupt_state);
	while (count > 0 && (size = RTF_LEN(minor))) {
		chars = RTF_MAX_RCHUNK(minor);
		if (chars > count)
			chars = count;
		if (chars > size)
			chars = size;

		read += chars;
		pipebuf = RTF_BASE(minor) + RTF_START(minor);
		RTF_START(minor) += chars;
		RTF_START(minor) = RTF_WRAP(minor, RTF_START(minor));
		RTF_LEN(minor) -= chars;
		count -= chars;
		memcpy(buf, pipebuf, chars);
		buf += chars;
	}
	rtl_spin_unlock_irqrestore(&RTF_SPIN(minor), interrupt_state);

	(*RTF_RT_HANDLER(minor))(minor);