rt_fifos.c

实时fifo先进先出队列

/*
COPYRIGHT (C) 1999  Paolo Mantegazza ([e-mail])

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
*/

/* 
ACKNOWLEDGEMENTS: 
- nice proc file contributed by Steve Papacharalambous ([e-mail]);
*/

/* 
ACKNOWLEDGEMENT NOTE: besides naming conventions and the idea of a fifo handler
function, the only remaining code from RTL original fifos, as written and 
copyrighted by Michael Barabanov, should be the function "check_blocked" 
(modified to suite my style). However I like to remark that I owe to that code 
my first understanding of Linux devices drivers.
*/

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/mm.h>
#include <linux/malloc.h>
#include <linux/poll.h>
#include <linux/tty_driver.h>
#include <linux/console.h>
#include <linux/config.h>

#include <asm/io.h>

#if CONFIG_PROC_FS && RTAI_PROC
#include <linux/stat.h>
#include <linux/proc_fs.h>
#include <rtai_proc_fs.h>
static int rtai_proc_fifo_register(void);
static void rtai_proc_fifo_unregister(void);
#endif

#include "rtai_fifos.h"
#include "rt_compat.h"

typedef struct lx_queue {
	struct lx_queue *prev;
	struct lx_queue *next;
	struct lx_task_struct *task;
} F_QUEUE;

typedef struct lx_semaphore {
	int free;
	F_QUEUE queue;
} F_SEM;

typedef struct lx_task_struct {
	F_QUEUE queue;
	int priority;
	struct wait_queue *waitq;
	struct wait_queue linux_task;
	int blocked;
} LX_TASK;

typedef struct lx_mailbox {
	F_SEM sndsem, rcvsem;
	LX_TASK *waiting_task;
	char *bufadr;
	int size, fbyte, avbs, frbs;
	spinlock_t buflock;
} F_MBX;

typedef struct rt_fifo_struct {
	F_MBX mbx;		// MUST BE THE FIRST!
	int opncnt;
	int malloc_type;
	int pol_asyn_pended;
	struct wait_queue *pollq;
	struct fasync_struct *asynq;
	int (*handler)(unsigned int arg);
	F_SEM sem;
#if RTAI_RTF_NAMED
	char name[RTF_NAMELEN+1];
#endif
} FIFO;

static int fifo_srq, async_sig;
static spinlock_t rtf_lock = SPIN_LOCK_UNLOCKED;
#if RTAI_RTF_NAMED
static spinlock_t rtf_name_lock = SPIN_LOCK_UNLOCKED;
#endif

#define MAX_FIFOS 64
static FIFO fifo[MAX_FIFOS] = {{{{0}}}};

#define MAXREQS 64	// KEEP IT A POWER OF 2!!!
static struct { int in, out; struct wait_queue **waitq[MAXREQS]; } taskq;
static struct { int in, out; FIFO *fifo[MAXREQS]; } pol_asyn_q;

static int do_nothing(unsigned int arg) { return 0; }

static inline int check_blocked(sigset_t signal, sigset_t blocked)
{
	int i = 0;
	do {
		if ( signal.sig[i] & ~blocked.sig[i] ) {
			return 1;
		}
	} while (++i < _NSIG_WORDS);
	return 0;
}

static inline void mbx_sem_signal(F_SEM *sem, FIFO *fifop)
{
	unsigned long flags;
	LX_TASK *task;

	rtf_save_flags_and_cli(flags);
	if ((task = (sem->queue.next)->task)) {
		sem->queue.next = task->queue.next;
		(task->queue.next)->prev = &(sem->queue);
		task->blocked = 0;
		if ((task->linux_task.task)->state == TASK_INTERRUPTIBLE) {
			taskq.waitq[taskq.in] = &(task->waitq);
			taskq.in = (taskq.in + 1) & (MAXREQS - 1);
			rtf_pend_srq(fifo_srq);
		}
	} else {
		sem->free = 1;
		if (fifop && !(fifop->pol_asyn_pended) &&
		    (((F_MBX *)fifop)->avbs || ((F_MBX *)fifop)->frbs) &&
		    (fifop->pollq != WAIT_QUEUE_HEAD(&(fifop->pollq)) || 
								fifop->asynq)) {
			fifop->pol_asyn_pended = 1;
			pol_asyn_q.fifo[pol_asyn_q.in] = fifop;
			pol_asyn_q.in = (pol_asyn_q.in + 1) & (MAXREQS - 1);
			rtf_pend_srq(fifo_srq);
		}
	}
	rtf_restore_flags(flags);
}

static inline void mbx_signal(F_MBX *mbx)
{
	unsigned long flags;
	LX_TASK *task;

	rtf_save_flags_and_cli(flags);
	if ((task = mbx->waiting_task)) {
		mbx->waiting_task = 0;
		task->blocked = 0;
		if ((task->linux_task.task)->state == TASK_INTERRUPTIBLE) {
			taskq.waitq[taskq.in] = &(task->waitq);
			taskq.in = (taskq.in + 1) & (MAXREQS - 1);
			rtf_pend_srq(fifo_srq);
		}
	}
	rtf_restore_flags(flags);
}

static inline int mbx_sem_wait_if(F_SEM *sem)
{
	unsigned long flags;
	int free;
	rtf_save_flags_and_cli(flags);
	if ((free = sem->free)) {
		sem->free = 0;
	}
	rtf_restore_flags(flags);
	return free;
}

static inline int mbx_sem_wait(F_SEM *sem)
{
	unsigned long flags;
	LX_TASK task;
	F_QUEUE *q;

	task.queue.task = &task;
	task.priority = 1000000 - current->rt_priority;
	task.blocked = 1;
	task.waitq = 0;
	task.linux_task.task = current;
	__add_wait_queue(&task.waitq, &task.linux_task);

	rtf_save_flags_and_cli(flags);
	if (!(sem->free)) {
		q = &(sem->queue);
		while ((q = q->next) != &(sem->queue) &&
			 (q->task)->priority <= task.priority);
		task.queue.prev = q->prev;
		task.queue.next = q;
		(q->prev)->next = &(task.queue);
		q->prev = &(task.queue);
		current->state = TASK_INTERRUPTIBLE;
		rtf_restore_flags(flags);
		schedule();
		rtf_save_flags_and_cli(flags);
		if (task.blocked) { 
			(task.queue.prev)->next = task.queue.next;
			(task.queue.next)->prev = task.queue.prev;
			if (!((sem->queue.next)->task)) {
				sem->free = 1;
			}
		}
		rtf_restore_flags(flags);
		if (check_blocked(current->signal, current->blocked)) {
			return -ERESTARTSYS;
		}
	} else {
		sem->free = 0;
		rtf_restore_flags(flags);
		task.blocked = 0;
	}
	return task.blocked;
}

static inline int mbx_wait(F_MBX *mbx, int *fravbs)
{
	unsigned long flags;
	LX_TASK task;

	task.blocked = 1;
	task.waitq = 0;
	task.linux_task.task = current;
	__add_wait_queue(&task.waitq, &task.linux_task);

	rtf_save_flags_and_cli(flags);
	if (!(*fravbs)) {
		mbx->waiting_task = &task;
		current->state = TASK_INTERRUPTIBLE;
		rtf_restore_flags(flags);
		schedule();
		mbx->waiting_task = 0;
		if (check_blocked(current->signal, current->blocked)) {
			if (task.blocked) {
				if( fravbs == &mbx->avbs ) {
					mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
				} else {
					mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
				}
			}
			return -ERESTARTSYS;
		}
	} else {
		rtf_restore_flags(flags);
		task.blocked = 0;
	}
	return task.blocked;
}

static inline int mbx_sem_wait_timed(F_SEM *sem, int delay)
{
	unsigned long flags;
	LX_TASK task;
	F_QUEUE *q;

	task.queue.task = &task;
	task.priority = 1000000 - current->rt_priority;
	task.blocked = 1;
	task.waitq = 0;
	task.linux_task.task = current;
	__add_wait_queue(&task.waitq, &task.linux_task);

	rtf_save_flags_and_cli(flags);
	if (!(sem->free)) {
		q = &(sem->queue);
		while ((q = q->next) != &(sem->queue) &&
			 (q->task)->priority <= task.priority);
		task.queue.prev = q->prev;
		task.queue.next = q;
		(q->prev)->next = &(task.queue);
		q->prev = &(task.queue);
		current->state = TASK_INTERRUPTIBLE;
		rtf_restore_flags(flags);
		schedule_timeout(delay);
		rtf_save_flags_and_cli(flags);
		if (task.blocked) { 
			(task.queue.prev)->next = task.queue.next;
			(task.queue.next)->prev = task.queue.prev;
			if (!((sem->queue.next)->task)) {
				sem->free = 1;
			}
		}
		rtf_restore_flags(flags);
		if (check_blocked(current->signal, current->blocked)) {
			return -ERESTARTSYS;
		}
	} else {
		sem->free = 0;
		rtf_restore_flags(flags);
		task.blocked = 0;
	}
	return task.blocked;
}

static inline int mbx_wait_timed(F_MBX *mbx, int *fravbs, int delay)
{
	unsigned long flags;
	LX_TASK task;

	task.blocked = 1;
	task.waitq = 0;
	task.linux_task.task = current;
	__add_wait_queue(&task.waitq, &task.linux_task);

	rtf_save_flags_and_cli(flags);
	if (!(*fravbs)) {
		mbx->waiting_task = &task;
		current->state = TASK_INTERRUPTIBLE;
		rtf_restore_flags(flags);
		schedule_timeout(delay);
		mbx->waiting_task = 0;
		if (check_blocked(current->signal, current->blocked)) {
			if (task.blocked) {
				if (fravbs == &mbx->avbs) {
					mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
				} else {
					mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
				}
			}
			return -ERESTARTSYS;
		}
	} else {
		rtf_restore_flags(flags);
		task.blocked = 0;
	}
	return task.blocked;
}

#define MOD_SIZE(indx) ((indx) < mbx->size ? (indx) : (indx) - mbx->size)

static inline int mbx_put(F_MBX *mbx, char **msg, int msg_size, int lnx)
{
	int tocpy, last_byte;
	unsigned long flags;

	rtf_spin_lock_irqsave(flags, mbx->buflock);
	while (mbx->frbs && msg_size > 0) {
		last_byte = MOD_SIZE(mbx->fbyte + mbx->avbs);
		if ((tocpy = mbx->size - last_byte) > msg_size) {
			tocpy = msg_size;
		}
		if (tocpy > mbx->frbs) {
			tocpy = mbx->frbs;
		}
		rtf_spin_unlock_irqrestore(flags, mbx->buflock);
		if (lnx) {
			copy_from_user(mbx->bufadr + last_byte, *msg, tocpy);
		} else {
			memcpy(mbx->bufadr + last_byte, *msg, tocpy);
		}
		msg_size  -= tocpy;
		*msg      += tocpy;
		rtf_spin_lock_irqsave(flags, mbx->buflock);
		mbx->frbs -= tocpy;
		mbx->avbs += tocpy;
	}
	rtf_spin_unlock_irqrestore(flags, mbx->buflock);
	return msg_size;
}

static inline int mbx_get(F_MBX *mbx, char **msg, int msg_size, int lnx)
{
	int tocpy;
	unsigned long flags;

	rtf_spin_lock_irqsave(flags, mbx->buflock);
	while (mbx->avbs && msg_size > 0) {
		if ((tocpy = mbx->size - mbx->fbyte) > msg_size) {
			tocpy = msg_size;
		}
		if (tocpy > mbx->avbs) {
			tocpy = mbx->avbs;
		}
		rtf_spin_unlock_irqrestore(flags, mbx->buflock);
		if (lnx) {
			copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		} else {
			memcpy(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		}
		msg_size  -= tocpy;
		*msg      += tocpy;
		rtf_spin_lock_irqsave(flags, mbx->buflock);
		mbx->fbyte = MOD_SIZE(mbx->fbyte + tocpy);
		mbx->frbs += tocpy;
		mbx->avbs -= tocpy;
	}
	rtf_spin_unlock_irqrestore(flags, mbx->buflock);
	return msg_size;
}

static inline void mbx_sem_init(F_SEM *sem, int value)
{
	sem->free = value;
	sem->queue.prev = &(sem->queue);
	sem->queue.next = &(sem->queue);
	sem->queue.task = 0;
}

static inline int mbx_sem_delete(F_SEM *sem)
{
	unsigned long flags;
	LX_TASK *task;

	rtf_save_flags_and_cli(flags);
	while ((task = (sem->queue.next)->task)) {
		sem->queue.next = task->queue.next;
		(task->queue.next)->prev = &(sem->queue);
		if ((task->linux_task.task)->state == TASK_INTERRUPTIBLE) {
			taskq.waitq[taskq.in] = &(task->waitq);
			taskq.in = (taskq.in + 1) & (MAXREQS - 1);
			rtf_pend_srq(fifo_srq);
		}
	}
	rtf_restore_flags(flags);
	return 0;
}

static inline void mbx_init(F_MBX *mbx, int size, char *bufadr)
{
	mbx_sem_init(&(mbx->sndsem), 1);
	mbx_sem_init(&(mbx->rcvsem), 1);
	mbx->waiting_task = 0;
	mbx->bufadr = bufadr;
	mbx->size = mbx->frbs = size;
	mbx->fbyte = mbx->avbs = 0;
	spin_lock_init(&(mbx->buflock));
#if __SMP__
        mbx->buflock.lock = 0;
#endif
}

static inline int mbx_delete(F_MBX *mbx)
{
	if (mbx_sem_delete(&(mbx->sndsem)) || mbx_sem_delete(&(mbx->rcvsem))) {
		return -EFAULT;
	}
	return 0;
}

static inline int mbx_send(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	if (mbx_sem_wait(&(mbx->sndsem))) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait(mbx, &mbx->frbs)) {
			return msg_size;
		}
		msg_size = mbx_put(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
	}
	mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
	return 0;
}

static inline int mbx_send_wp(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	unsigned long flags;

	rtf_save_flags_and_cli(flags);
	if (mbx->sndsem.free && mbx->frbs) {
		mbx->sndsem.free = 0;
		rtf_restore_flags(flags);
		msg_size = mbx_put(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
		mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
	} else {
		rtf_restore_flags(flags);
	}
	return msg_size;
}

static int mbx_send_timed(F_MBX *mbx, void *msg, int msg_size, int delay, int lnx)
{
	if (mbx_sem_wait_timed(&(mbx->sndsem), delay)) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait_timed(mbx, &(mbx->frbs), delay)) {
			return msg_size;
		}
		msg_size = mbx_put(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
	}
	mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
	return 0;
}

static inline int mbx_receive(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	if (mbx_sem_wait(&(mbx->rcvsem))) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait(mbx, &mbx->avbs)) {
			return msg_size;
		}
		msg_size = mbx_get(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
	}
	mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
	return 0;
}

static inline int mbx_receive_wjo(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	if (mbx_sem_wait(&(mbx->rcvsem))) {
		return msg_size;
	}
	if (msg_size) {
		if (mbx_wait(mbx, &mbx->avbs)) {
			return msg_size;
		}
		msg_size = mbx_get(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
	}
	mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
	return msg_size;
}

static inline int mbx_receive_wp(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	unsigned long flags;

	rtf_save_flags_and_cli(flags);
	if (mbx->rcvsem.free && mbx->avbs) {
		mbx->rcvsem.free = 0;
		rtf_restore_flags(flags);
		msg_size = mbx_get(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
		mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
	} else {
		rtf_restore_flags(flags);
	}
	return msg_size;
}

static int mbx_receive_timed(F_MBX *mbx, void *msg, int msg_size, int delay, int lnx)
{
	if (mbx_sem_wait_timed(&(mbx->rcvsem), delay)) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait_timed(mbx, &(mbx->avbs), delay)) {
			return msg_size;
		}
		msg_size = mbx_get(mbx, (char **)(&msg), msg_size, lnx);
		mbx_signal(mbx);
	}
	mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
	return 0;
}

#ifdef CONFIG_RTL
void rtf_sysrq_handler(int irq, void *dev_id, struct pt_regs *p)
#else
void rtf_sysrq_handler(void)
#endif
{
	FIFO *fifop;
	while (taskq.out != taskq.in) {
		wake_up_interruptible(taskq.waitq[taskq.out]);
		taskq.out = (taskq.out + 1) & (MAXREQS - 1);
	}
	while (pol_asyn_q.out != pol_asyn_q.in) {
		fifop = pol_asyn_q.fifo[pol_asyn_q.out];
		fifop->pol_asyn_pended = 0;
		if ((fifop = pol_asyn_q.fifo[pol_asyn_q.out])->pollq) {
			wake_up_interruptible(&(fifop->pollq));
		}
		if (fifop->asynq) { 
			kill_fasync(fifop->asynq, async_sig);
		}
		pol_asyn_q.out = (pol_asyn_q.out + 1) & (MAXREQS - 1);
	}
}