fifos.c

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

		if (msg_size) {
			while ((n = msg_size - mbx->frbs) > 0) {
				if ((tocpy = mbx->size - mbx->fbyte) > n) {
					tocpy = n;
				}
				if (tocpy > mbx->avbs) {
					tocpy = mbx->avbs;
				}
				rtf_spin_lock_irqsave(flags, mbx->buflock);
				mbx->frbs  += tocpy;
				mbx->avbs  -= tocpy;
				rtf_spin_unlock_irqrestore(flags, mbx->buflock);
				mbx->fbyte = MOD_SIZE(mbx->fbyte + tocpy);
			}
		}		
	}
	return 0;
}

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

	while (msg_size > 0 && mbx->avbs) {
		if ((tocpy = mbx->size - mbx->fbyte) > msg_size) {
			tocpy = msg_size;
		}
		if (tocpy > mbx->avbs) {
			tocpy = mbx->avbs;
		}
		if (lnx) {
			copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		} else {
			memcpy(*msg, mbx->bufadr + mbx->fbyte, 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);
		msg_size  -= tocpy;
		*msg      += tocpy;
	}
	return msg_size;
}

static inline int mbx_evdrp(F_MBX *mbx, char **msg, int msg_size, int lnx)
{
	int tocpy, fbyte, avbs;

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

static inline void mbx_sem_init(F_SEM *sem, int value)
{
	sem->free  = value;
	sem->qtype = 0;
	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);
		taskq.task[taskq.in] = task->task;
		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->lbyte = mbx->avbs = 0;
#ifdef CONFIG_SMP
        mbx->buflock.lock = 0;
#endif
	spin_lock_init(&(mbx->buflock));
}

static inline int mbx_delete(F_MBX *mbx)
{
	mbx_signal(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)) {
			mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
			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 inline int mbx_send_if(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 >= msg_size)) {
		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)) {
			mbx_sem_signal(&(mbx->sndsem), (FIFO *)mbx);
			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)) {
			mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
			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)) {
			mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
			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 inline int mbx_receive_if(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	unsigned long flags;

	rtf_save_flags_and_cli(flags);
	if (mbx->rcvsem.free && (mbx->frbs >= msg_size)) {
		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)) {
			mbx_sem_signal(&(mbx->rcvsem), (FIFO *)mbx);
			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_ovrwr_send(F_MBX *mbx, void *msg, int msg_size, int lnx)
{
	unsigned long flags;

	rtf_save_flags_and_cli(flags);
	if (mbx->sndsem.free) {
		mbx->sndsem.free = 0;
		rtf_restore_flags(flags);
		msg_size = mbx_ovrwr_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 void rtf_sysrq_handler(void)
{
	FIFO *fifop;
	while (taskq.out != taskq.in) {
		if (taskq.task[taskq.out]->state == TASK_INTERRUPTIBLE) {
			wake_up_process(taskq.task[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 (waitqueue_active(&(fifop = pol_asyn_q.fifo[pol_asyn_q.out])->pollq)) {
			wake_up_interruptible(&(fifop->pollq));
		}
		if (fifop->asynq) { 
			kill_fasync(&fifop->asynq, async_sig, POLL_IN);
		}
		pol_asyn_q.out = (pol_asyn_q.out + 1) & (MAXREQS - 1);
	}
	set_tsk_need_resched(current);
}

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

/**
 * @ingroup fifos_ipc
 * Reset a real-time FIFO
 *
 * rtf_reset resets RT-FIFO @a fd_fifo by setting its buffer pointers to zero,
 * so that any existing data is discarded and the fifo started anew like at its
 * creations.   It can be used both in kernel and user space.
 *
 * @param minor is a file descriptor returned by standard UNIX open in user
 * space while it is directly the chosen fifo number in kernel space.
 *
 * @retval 0 on succes.
 * @retval ENODEV if @a fd_fifo is greater than or equal to RTF_NO.
 * @retval EINVAL if @a fd_fifo refers to a not opened fifo.
 * @retval EFAULT if the operation was unsuccessful.
 */
int rtf_reset(unsigned int minor)
{
	int semval;
	F_MBX *mbx;
	
	VALID_FIFO;

	TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_RESET, minor, 0);

	mbx = &(fifo[minor].mbx);
	if (!mbx_sem_wait(&(mbx->rcvsem))) {
		while (!(semval = mbx_sem_wait_if(&mbx->sndsem)) && !mbx->waiting_task) {
			current->state = TASK_INTERRUPTIBLE;
			schedule_timeout(1);
		}
	} else {
		return -EBADF;
	}
	mbx->frbs = mbx->size;
	mbx->fbyte = mbx->lbyte = mbx->avbs = 0;
	if (semval) {
		mbx_sem_signal(&mbx->sndsem, (FIFO *)mbx);
	} else {
		mbx_signal(mbx);
	}
	mbx_sem_signal(&mbx->rcvsem, (FIFO *)mbx);
	return 0;
}


/**
 * @ingroup fifos_ipc
 * Resize a real-time FIFO
 *
 * rtf_resize modifies the real-time fifo fifo, previously created with,
 * rtf_create(), to have a new size of @a size. Any data in the fifo is
 * discarded.
 *
 * @param minor is a file descriptor returned by standard UNIX open in user
 * space while it is directly the chosen fifo number in kernel space.
 *
 * @param size is the requested new size.
 *
 * @retval size on success.
 * @retval -ENODEV if @a fifo is greater than or equal to RTF_NO.
 * @retval -EINVAL if @a fifo refers to a not opened fifo.
 * @retval -ENOMEM if @a size bytes could not be allocated for the RT-FIFO. Fifo
 * @retval -EBUSY if @a size is smaller than actual content
 * size is unchanged.
 */
int rtf_resize(unsigned int minor, int size)
{
	void *oldbuf, *newbuf;
	int old_malloc_type, new_malloc_type, semval;
	F_MBX *mbx;
	
	VALID_FIFO;

	TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_RESIZE, minor, size);

	mbx = &(fifo[minor].mbx);
	if (!size) {
		return -EINVAL;
	}
	if (size <= PAGE_SIZE*32) {
		if (!(newbuf = kmalloc(size, GFP_KERNEL))) {
			return -ENOMEM;
		}
		new_malloc_type = 'k';
	} else {
		if (!(newbuf = vmalloc(size))) {
			return -ENOMEM;
		}
		new_malloc_type = 'v';
	}
	if (!mbx_sem_wait(&(mbx->rcvsem))) {
		while (!(semval = mbx_sem_wait_if(&mbx->sndsem)) && !mbx->waiting_task) {
			current->state = TASK_INTERRUPTIBLE;
			schedule_timeout(1);
		}
	} else {
		return -EBADF;
	}
	if (size < mbx->avbs) {
		if (semval) {
			mbx_sem_signal(&mbx->sndsem, (FIFO *)mbx);
		}
		mbx_sem_signal(&mbx->rcvsem, (FIFO *)mbx);
		new_malloc_type == 'k' ? kfree(newbuf) : vfree(newbuf);
		return -EBUSY;
	}
	oldbuf = newbuf;
	mbx->frbs = mbx_get(mbx, (char **)(&oldbuf), size, 0);
	mbx->avbs = mbx->lbyte = size - mbx->frbs;
	mbx->fbyte = 0;
	oldbuf = mbx->bufadr;
	mbx->bufadr = newbuf;
	old_malloc_type = fifo[minor].malloc_type;
	fifo[minor].malloc_type = new_malloc_type;
	if (semval) {
		mbx->size = size;
		mbx_sem_signal(&mbx->sndsem, (FIFO *)mbx);
	} else if (size > mbx->size) {
		mbx->size = size;
		mbx_signal(mbx);
	}
	mbx_sem_signal(&mbx->rcvsem, (FIFO *)mbx);
	old_malloc_type == 'k' ? kfree(oldbuf) : vfree(oldbuf);
	return size;
}


/**
 * @ingroup fifos_ipc
 * Create a real-time FIFO
 *
 * rtf_create creates a real-time fifo (RT-FIFO) of initial size @a size and
 * assigns it the identifier @a fifo.  It must be used only in kernel space.
 *
 * @param minor is a positive integer that identifies the fifo on further
 * operations. It has to be less than RTF_NO.
 *
 * @param size is the requested size for the fifo. 
 * 
 * @a fifo may refer to an existing RT-FIFO. In this case the size is adjusted
 * if necessary.
 *
 * The RT-FIFO is a character based mechanism to communicate among real-time
 * tasks and ordinary Linux processes. The rtf_* functions are used by the
 * real-time tasks; Linux processes use standard character device access
 * functions such as read, write, and select.
 *
 * If this function finds an existing fifo of lower size it resizes it to the
 * larger new size. Note that the same condition apply to the standard Linux
 * device open, except that when it does not find any already existing fifo it
 * creates it with a default size of 1K bytes.
 *
 * It must be remarked that practically any fifo size can be asked for. In
 * fact if @a size is within the constraint allowed by kmalloc such a function
 * is used, otherwise vmalloc is called, thus allowing any size that can fit
 * into the available core memory.
 *
 * Multiple calls of this function are allowed, a counter is kept internally to
 * track their number, and avoid destroying/closing a fifo that is still used.
 *
 * @retval size on success
 * @retval ENODEV if fifo is greater than or equal to RTF_NO
 * @retval ENOMEM if the necessary size could not be allocated for the RT-FIFO.
 *
 */
int rtf_create(unsigned int minor, int size)
{
	void *buf;

	if (minor >= MAX_FIFOS) {