mbx.c

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

/** 
 * @file
 * Mailbox functions.
 * @author Paolo Mantegazza
 *
 * @note Copyright (C) 1999-2003 Paolo Mantegazza
 * <[e-mail]> 
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * @ingroup mbx
 */

/**
 * @ingroup sched
 * @defgroup mbx Mailbox functions
 *@{*/

#include <asm/uaccess.h>

#include <rtai_schedcore.h>

MODULE_LICENSE("GPL");

/* +++++++++++++++++++++++++++++ MAIL BOXES ++++++++++++++++++++++++++++++++ */

static inline void mbx_signal(MBX *mbx)
{
	unsigned long flags;
	RT_TASK *task;
	int tosched;

	flags = rt_global_save_flags_and_cli();
	if ((task = mbx->waiting_task)) {
		rem_timed_task(task);
		mbx->waiting_task = NOTHING;
		task->prio_passed_to = NOTHING;
		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_MBXSUSP | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
			enq_ready_task(task);
			if (mbx->sndsem.type <= 0) {
				RT_SCHEDULE(task, hard_cpu_id());
				rt_global_restore_flags(flags);
				return;
			}
			tosched = 1;
			goto res;
		}
	}
	tosched = 0;
res:	if (mbx->sndsem.type > 0) {
		DECLARE_RT_CURRENT;
		int sched;
		ASSIGN_RT_CURRENT;
		mbx->owndby = 0;
		if (rt_current->owndres & SEMHLF) {
			--rt_current->owndres;
		}
		if (!rt_current->owndres) {
			sched = renq_current(rt_current, rt_current->base_priority);
		} else if (!(rt_current->owndres & SEMHLF)) {
			int priority;
			sched = renq_current(rt_current, rt_current->base_priority > (priority = ((rt_current->msg_queue.next)->task)->priority) ? priority : rt_current->base_priority);
		} else {
			sched = 0;
		}
		if (rt_current->suspdepth) {
			if (rt_current->suspdepth > 0) {
				rt_current->state |= RT_SCHED_SUSPENDED;
				rem_ready_current(rt_current);
                        	sched = 1;
			} else {
				rt_task_delete(rt_current);
			}
		}
		if (sched) {
			if (tosched) {
				RT_SCHEDULE_BOTH(task, cpuid);
			} else {
				rt_schedule();
			}
		} else if (tosched) {
			RT_SCHEDULE(task, cpuid);
		}
	}
	rt_global_restore_flags(flags);
}

#define RT_MBX_MAGIC 0x3ad46e9b

static inline int mbx_wait(MBX *mbx, int *fravbs, RT_TASK *rt_current)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (!(*fravbs)) {
		unsigned long schedmap;
		if (mbx->sndsem.type > 0) {
			schedmap = pass_prio(mbx->owndby, rt_current);
		} else {
			schedmap = 0;
		}
		rt_current->state |= RT_SCHED_MBXSUSP;
		rem_ready_current(rt_current);
		mbx->waiting_task = rt_current;
		RT_SCHEDULE_MAP_BOTH(schedmap);
		if (mbx->waiting_task == rt_current || mbx->magic != RT_MBX_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return -1;
		}
	}
	if (mbx->sndsem.type > 0) {
		(mbx->owndby = rt_current)->owndres++;
	}
	rt_global_restore_flags(flags);
	return 0;
}

static inline int mbx_wait_until(MBX *mbx, int *fravbs, RTIME time, RT_TASK *rt_current)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (!(*fravbs)) {
		mbx->waiting_task = rt_current;
		if ((rt_current->resume_time = time) > rt_smp_time_h[hard_cpu_id()]) {
			unsigned long schedmap;
			if (mbx->sndsem.type > 0) {
				schedmap = pass_prio(mbx->owndby, rt_current);
			} else {
				schedmap = 0;
			}
			rt_current->state |= (RT_SCHED_MBXSUSP | RT_SCHED_DELAYED);
			rem_ready_current(rt_current);
			enq_timed_task(rt_current);
			RT_SCHEDULE_MAP_BOTH(schedmap);
		}
		if (mbx->magic != RT_MBX_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return -1;
		}
		if (mbx->waiting_task == rt_current) {
			mbx->waiting_task = NOTHING;
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return -1;
		}
	}
	if (mbx->sndsem.type > 0) {
		(mbx->owndby = rt_current)->owndres++;
	}
	rt_global_restore_flags(flags);
	return 0;
}

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

static inline int mbxput(MBX *mbx, char **msg, int msg_size, int space)
{
	unsigned long flags;
	int tocpy;

	while (msg_size > 0 && mbx->frbs) {
		if ((tocpy = mbx->size - mbx->lbyte) > msg_size) {
			tocpy = msg_size;
		}
		if (tocpy > mbx->frbs) {
			tocpy = mbx->frbs;
		}
		if (space) {
			memcpy(mbx->bufadr + mbx->lbyte, *msg, tocpy);
		} else {
			copy_from_user(mbx->bufadr + mbx->lbyte, *msg, tocpy);
		}
		flags = rt_spin_lock_irqsave(&(mbx->lock));
		mbx->frbs -= tocpy;
		mbx->avbs += tocpy;
		rt_spin_unlock_irqrestore(flags, &(mbx->lock));
		msg_size -= tocpy;
		*msg     += tocpy;
		mbx->lbyte = MOD_SIZE(mbx->lbyte + tocpy);
	}
	return msg_size;
}

static inline int mbxovrwrput(MBX *mbx, char **msg, int msg_size, int space)
{
	unsigned long flags;
	int tocpy,n;

	if ((n = msg_size - mbx->size) > 0) {
		*msg += n;
		msg_size -= n;
	}		
	while (msg_size > 0) {
		if (mbx->frbs) {	
			if ((tocpy = mbx->size - mbx->lbyte) > msg_size) {
				tocpy = msg_size;
			}
			if (tocpy > mbx->frbs) {
				tocpy = mbx->frbs;
			}
			if (space) {
				memcpy(mbx->bufadr + mbx->lbyte, *msg, tocpy);
			} else {
				copy_from_user(mbx->bufadr + mbx->lbyte, *msg, tocpy);
			}
	        	flags = rt_spin_lock_irqsave(&(mbx->lock));
			mbx->frbs -= tocpy;
			mbx->avbs += tocpy;
        		rt_spin_unlock_irqrestore(flags, &(mbx->lock));
			msg_size -= tocpy;
			*msg     += tocpy;
			mbx->lbyte = MOD_SIZE(mbx->lbyte + tocpy);
		}	
		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;
				}
	        		flags = rt_spin_lock_irqsave(&(mbx->lock));
				mbx->frbs  += tocpy;
				mbx->avbs  -= tocpy;
        			rt_spin_unlock_irqrestore(flags, &(mbx->lock));
				mbx->fbyte = MOD_SIZE(mbx->fbyte + tocpy);
			}
		}		
	}
	return 0;
}

static inline int mbxget(MBX *mbx, char **msg, int msg_size, int space)
{
	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 (space) {
			memcpy(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		} else {
			copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		}
		flags = rt_spin_lock_irqsave(&(mbx->lock));
		mbx->frbs  += tocpy;
		mbx->avbs  -= tocpy;
		rt_spin_unlock_irqrestore(flags, &(mbx->lock));
		msg_size -= tocpy;
		*msg     += tocpy;
		mbx->fbyte = MOD_SIZE(mbx->fbyte + tocpy);
	}
	return msg_size;
}

static inline int mbxevdrp(MBX *mbx, char **msg, int msg_size, int space)
{
	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 (space) {
			memcpy(*msg, mbx->bufadr + fbyte, tocpy);
		} else {
			copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		}
		avbs     -= tocpy;
		msg_size -= tocpy;
		*msg     += tocpy;
		fbyte = MOD_SIZE(fbyte + tocpy);
	}
	return msg_size;
}


/**
 * @brief Receives bytes as many as possible leaving the message
 * available for another receive.
 *
 * rt_mbx_evdrp receives at most @e msg_size of bytes of message
 * from the mailbox @e mbx and then returns immediately.
 * Does what rt_mbx_receive_wp does while keeping the message in the mailbox buffer.
 * Useful if one needs to just preview the mailbox content, without actually
 * receiving it.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg points to a buffer provided by the caller.
 *
 * @param msg_size corresponds to the size of the message to be received.
 *
 * @return The number of bytes not received is returned.
 */
int _rt_mbx_evdrp(MBX *mbx, void *msg, int msg_size, int space)
{
	return mbxevdrp(mbx, (char **)(&msg), msg_size, space);
}


#define CHK_MBX_MAGIC { if (mbx->magic != RT_MBX_MAGIC) { return -EINVAL; } }


/**
 * @brief Initializes a fully typed mailbox queueing tasks
 * according to the specified type.
 *
 * rt_typed_mbx_init initializes a mailbox of size @e size. @e mbx must
 * point to a user allocated MBX structure. Tasks are queued in FIFO
 * order (FIFO_Q), priority order (PRIO_Q) or resource order (RES_Q).
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param size corresponds to the size of the mailbox.
 *
 * @param type corresponds to the queueing policy: FIFO_Q, PRIO_Q or RES_Q.
 *
 * @return On success 0 is returned. On failure, a special value is
 * returned as indicated below:
 * - @b ENOMEM: Space could not be allocated for the mailbox buffer.
 *
 * See also: notes under rt_mbx_init().
 */
int rt_typed_mbx_init(MBX *mbx, int size, int type)
{
	if (!(mbx->bufadr = sched_malloc(size))) { 
		return -ENOMEM;
	}
	rt_typed_sem_init(&(mbx->sndsem), 1, type & 3 ? type : BIN_SEM | type);
	rt_typed_sem_init(&(mbx->rcvsem), 1, type & 3 ? type : BIN_SEM | type);
	mbx->magic = RT_MBX_MAGIC;
	mbx->size = mbx->frbs = size;
	mbx->waiting_task = mbx->owndby = 0;
	mbx->fbyte = mbx->lbyte = mbx->avbs = 0;
        spin_lock_init(&(mbx->lock));
	return 0;
}


/**
 * @brief Initializes a mailbox.
 *
 * rt_mbx_init initializes a mailbox of size @e size. @e mbx must
 * point to a user allocated MBX structure.
 * Using mailboxes is a flexible method for inter task
 * communications. Tasks are allowed to send arbitrarily sized
 * messages by using any mailbox buffer size. There is even no need to
 * use a buffer sized at least as the largest message you envisage,
 * even if efficiency is likely to suffer from such a
 * decision. However if you expect a message larger than the average
 * message size very rarely you can use a smaller buffer without much
 * loss of efficiency. In such a way you can set up your own mailbox
 * usage protocol, e.g. using fix sized messages with a buffer that is
 * an integer multiple of such a size guarantees maximum efficiency by
 * having each message sent/received atomically to/from the
 * mailbox. Multiple senders and receivers are allowed and each will
 * get the service it requires in turn, according to its priority. 
 * Thus mailboxes provide a flexible mechanism to allow you to freely
 * implement your own policy.
 *
 * rt_mbx_init is equivalent to rt_typed_mbx_init(mbx, size, PRIO_Q).
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param size corresponds to the size of the mailbox.
 *
 * @return On success 0 is returned. On failure, a special value is
 * returned as indicated below:
 * - @b ENOMEM: Space could not be allocated for the mailbox buffer.
 *
 * See also: notes under rt_typed_mbx_init().
 */
int rt_mbx_init(MBX *mbx, int size)
{
	return rt_typed_mbx_init(mbx, size, PRIO_Q);
}


/**
 *
 * @brief Deletes a mailbox.
 * 
 * rt_mbx_delete removes a mailbox previously created with rt_mbx_init().
 *
 * @param mbx is the pointer to the structure used in the corresponding call
 * to rt_mbx_init.
 *
 * @return 0 is returned on success. On failure, a negative value is
 * returned as described below:
 * - @b EINVAL: @e mbx points to an invalid mailbox.
 * - @b EFAULT: mailbox data were found in an invalid state.
 */
int rt_mbx_delete(MBX *mbx)
{
	CHK_MBX_MAGIC;
	mbx->magic = 0;
	if (rt_sem_delete(&mbx->sndsem) || rt_sem_delete(&mbx->rcvsem)) {
		return -EFAULT;
	}
	while (mbx->waiting_task) {
		mbx_signal(mbx);
	}
	sched_free(mbx->bufadr); 
	return 0;
}


/**
 * @brief Sends a message unconditionally.
 *
 * rt_mbx_send sends a message @e msg of @e msg_size bytes to the
 * mailbox @e mbx. The caller will be blocked until the whole message
 * is copied into the mailbox or an error occurs. Even if the message
 * can be sent in a single shot, the sending task can be blocked if
 * there is a task of higher priority waiting to receive from the
 * mailbox.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg corresponds to the message to be sent.
 *
 * @param msg_size is the size of the message.
 *
 * @return On success, 0 is returned.
 * On failure a value is returned as described below:
 * - the number of bytes not received: an error is occured 
 *   in the queueing of all sending tasks.
 * - @b EINVAL: mbx points to an invalid mailbox.
 */
int _rt_mbx_send(MBX *mbx, void *msg, int msg_size, int space)
{
	RT_TASK *rt_current = RT_CURRENT;

	CHK_MBX_MAGIC;
	if (rt_sem_wait(&mbx->sndsem) > 1) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait(mbx, &mbx->frbs, rt_current)) {
			rt_sem_signal(&mbx->sndsem);
			return msg_size;
		}
		msg_size = mbxput(mbx, (char **)(&msg), msg_size, space);
		mbx_signal(mbx);
	}
	rt_sem_signal(&mbx->sndsem);
	return 0;
}


/**
 * @brief Sends as many bytes as possible without blocking the calling task.
 *
 * rt_mbx_send_wp atomically sends as many bytes of message @e msg as
 * possible to the mailbox @e mbx then returns immediately.
 *
 * @param mbx is a pointer to a user allocated mailbox structure.
 *
 * @param msg corresponds to the message to be sent.
 * 
 * @param msg_size is the size of the message.
 *
 * @return On success, the number of unsent bytes is returned. On
 * failure a negative value is returned as described below:
 * - @b EINVAL: @e mbx points to an invalid mailbox.
 */
int _rt_mbx_send_wp(MBX *mbx, void *msg, int msg_size, int space)
{
	unsigned long flags;
	RT_TASK *rt_current = RT_CURRENT;