tbx.c

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

/*
 * Copyright (C) 2001  G.M. Bertani <[e-mail]>
 * Copyright (C) 2002 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.
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <rtai_schedcore.h>

MODULE_LICENSE("GPL");

/*++++++++++++++++++++++++++++ TYPED MAILBOXES ++++++++++++++++++++++++++++++*/ 

#define CHK_TBX_MAGIC \
do { \
	if (tbx->magic != RT_TBX_MAGIC) {  \
		return -EINVAL; \
	} \
} while (0);

#define TBX_MOD_SIZE(indx) ((indx) < tbx->size ? (indx) : (indx) - tbx->size)

#define TBX_REVERSE_MOD_SIZE(indx) ((indx) >= 0 ? (indx) : tbx->size + (indx))

static inline void tbx_smx_signal(TBX* tbx, SEM *smx)
{
	unsigned long flags;
	RT_TASK *task;

	flags = rt_global_save_flags_and_cli();
	if ((task = (smx->queue.next)->task)) {
        	tbx->waiting_nr--;
		dequeue_blocked(task);
		rem_timed_task(task);
		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
			enq_ready_task(task);
			RT_SCHEDULE(task, hard_cpu_id());
		}
	} else {
		smx->count = 1;
	}
	rt_global_restore_flags(flags);
}

static inline int tbx_smx_wait(TBX* tbx, SEM *smx, RT_TASK *rt_current)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
    	if (!(smx->count)) {
		tbx->waiting_nr++;
	        rt_current->state |= RT_SCHED_SEMAPHORE;
                rem_ready_current(rt_current);
                enqueue_blocked(rt_current, &smx->queue, smx->qtype);
		rt_schedule();
	} else {
		smx->count = 0;
	}
	rt_global_restore_flags(flags);
	return (int)(rt_current->blocked_on);
}

static inline int tbx_smx_wait_until(TBX *tbx, SEM *smx, RTIME time, RT_TASK *rt_current)
{
	int timed = 0;
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (!(smx->count)) {
		tbx->waiting_nr++;
		rt_current->blocked_on = &smx->queue;
		rt_current->resume_time = time;
		rt_current->state |= (RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED);
		rem_ready_current(rt_current);
		enqueue_blocked(rt_current, &smx->queue, smx->qtype);
		enq_timed_task(rt_current);
		rt_schedule();
        	if (rt_current->blocked_on) {
                        dequeue_blocked(rt_current);
			timed = 1;
			tbx->waiting_nr--;
		}
	} else {
		smx->count = 0;
	}
	rt_global_restore_flags(flags);
	return timed;
}

static inline int tbx_wait_room(TBX *tbx, int *fravbs, int msgsize, RT_TASK *rt_current)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if ((*fravbs) < msgsize) {
		tbx->waiting_nr++;
		rt_current->suspdepth = 1;
		rt_current->state |= RT_SCHED_MBXSUSP;
		rem_ready_current(rt_current);
		rt_current->blocked_on = SOMETHING;
		tbx->waiting_task = rt_current;
		rt_schedule();
	}
	rt_global_restore_flags(flags);
	return (int)(rt_current->blocked_on);
}

static inline int tbx_check_room(TBX *tbx, int *fravbs, int msgsize)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (((*fravbs) < msgsize) || !tbx->sndsmx.count) {
		rt_global_restore_flags(flags);
		return 0;    
	}
	tbx->sndsmx.count = 0;
	rt_global_restore_flags(flags);
	return msgsize;
}

static inline int tbx_wait_room_until(TBX *tbx, int *fravbs, int msgsize, RTIME time, RT_TASK *rt_current)
{
	int timed = 0;
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if ((*fravbs) < msgsize) {
	        tbx->waiting_nr++;
		rt_current->blocked_on = SOMETHING;
		rt_current->resume_time = time;
		rt_current->state |= (RT_SCHED_MBXSUSP | RT_SCHED_DELAYED);
		rem_ready_current(rt_current);
		tbx->waiting_task = rt_current;
		enq_timed_task(rt_current);
		rt_schedule();
		if (rt_current->blocked_on) {
			tbx->waiting_nr--;
			rt_current->blocked_on = NOTHING;
			tbx->waiting_task = NOTHING;
			timed = 1;
		}
	}
	rt_global_restore_flags(flags);
	return timed;
}

static inline unsigned char tbx_check_msg(TBX *tbx, int *fravbs, int msgsize, unsigned char* type)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (tbx->rcvsmx.count == 0 || (*fravbs) < msgsize ) {
		rt_global_restore_flags(flags);
		return 0;
	}
	tbx->rcvsmx.count = 0;
	if (*tbx->bcbadr == TYPE_BROADCAST) {
		*type = TYPE_BROADCAST;
	} else {
	        *type = *(tbx->bufadr + tbx->fbyte);
		tbx->fbyte = TBX_MOD_SIZE(tbx->fbyte + sizeof(*type));
		tbx->frbs += sizeof(*type);
		tbx->avbs -= sizeof(*type);
	}        
	rt_global_restore_flags(flags);
	return *type;
}

static inline int tbx_wait_msg(TBX *tbx, int *fravbs, int msgsize, unsigned char*type, RT_TASK *rt_current)
{
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (*tbx->bcbadr == TYPE_BROADCAST) {
		*type = TYPE_BROADCAST;
	} else {
		if ((*fravbs) < msgsize ) {
			tbx->waiting_nr++;
			rt_current->state |= RT_SCHED_MBXSUSP;
	                rem_ready_current(rt_current);
			rt_current->blocked_on = SOMETHING;
			tbx->waiting_task = rt_current;
			rt_schedule();
        	}
	        *type = *(tbx->bufadr + tbx->fbyte);
        	tbx->fbyte = TBX_MOD_SIZE(tbx->fbyte + sizeof(*type));
	        tbx->frbs += sizeof(*type);
        	tbx->avbs -= sizeof(*type);
	}        
	rt_global_restore_flags(flags);
	return (int)(rt_current->blocked_on);
}

static inline int tbx_wait_msg_until(TBX *tbx, int *fravbs, int msgsize, RTIME time, unsigned char *type, RT_TASK *rt_current)
{
	int timed = 0;
	unsigned long flags;

	flags = rt_global_save_flags_and_cli();
	if (*tbx->bcbadr == TYPE_BROADCAST) {
		*type = TYPE_BROADCAST;
	} else {
	        *type = *(tbx->bufadr + tbx->fbyte);
        	if ((*fravbs) < msgsize && *tbx->bcbadr == TYPE_NONE) {
			tbx->waiting_nr++;
        		rt_current->blocked_on = SOMETHING;
			rt_current->resume_time = time;
			rt_current->state |= (RT_SCHED_MBXSUSP | RT_SCHED_DELAYED);
                        rem_ready_current(rt_current);
			tbx->waiting_task = rt_current;
                        enq_timed_task(rt_current);
			rt_schedule();
			if (rt_current->blocked_on) {
				tbx->waiting_nr--;
				rt_current->blocked_on = NOTHING;
				tbx->waiting_task = NOTHING;
				timed = 1;
				*type = TYPE_NONE;
				rt_global_restore_flags(flags);
				return timed;
			}
	        }
        	tbx->fbyte = TBX_MOD_SIZE(tbx->fbyte + sizeof(*type));
	        tbx->frbs += sizeof(*type);
        	tbx->avbs -= sizeof(*type);
	}
	rt_global_restore_flags(flags);
	return timed;
}

static inline void tbx_signal(TBX *tbx)
{
	unsigned long flags;
	RT_TASK *task;

	flags = rt_global_save_flags_and_cli();
	if ((task = tbx->waiting_task)) {
		tbx->waiting_nr--;
                rem_timed_task(task);
        	task->blocked_on = NOTHING;
		tbx->waiting_task = NOTHING;
		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_MBXSUSP | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
                        enq_ready_task(task);
			rt_schedule();
		}
	}
	rt_global_restore_flags(flags);
}

static inline int tbxput(TBX *tbx, char **msg, int msg_size, unsigned char type)
{
	int tocpy, last_byte;
	unsigned long flags;
	int msgpacksize;
            
	msgpacksize = msg_size + sizeof(type); 
	flags = rt_global_save_flags_and_cli();
	while (tbx->frbs && msgpacksize > 0) {
        	last_byte = TBX_MOD_SIZE(tbx->fbyte + tbx->avbs);
		tocpy = tbx->size - last_byte;
		if (tocpy > msgpacksize) {
			tocpy = msgpacksize;
		}
//		rt_global_restore_flags(flags);
		if (type != TYPE_NONE) {
			tocpy = sizeof(type);
			*(tbx->bufadr + last_byte) = type;
			msgpacksize -= tocpy;
			type = TYPE_NONE;
	        } else {
			memcpy(tbx->bufadr + last_byte, *msg, tocpy);
			msgpacksize -= tocpy;
			*msg += tocpy;
		}
//		flags = rt_global_save_flags_and_cli();
        	tbx->frbs -= tocpy;
		tbx->avbs += tocpy;
	}    
	rt_global_restore_flags(flags);
	return msgpacksize;
}

static inline int tbxget(TBX *tbx, char **msg, int msg_size)
{
	int tocpy;
	unsigned long flags;
    
	flags = rt_global_save_flags_and_cli();
	while (tbx->avbs && msg_size > 0) {
		tocpy = tbx->size - tbx->fbyte;
		if (tocpy > msg_size) {
			tocpy = msg_size;
		}
		if (tocpy > tbx->avbs) {
			tocpy = tbx->avbs;
		}
//		rt_global_restore_flags(flags);
	        memcpy(*msg, tbx->bufadr + tbx->fbyte, tocpy);
        	msg_size  -= tocpy;
	        *msg      += tocpy;
//		flags = rt_global_save_flags_and_cli();
	        tbx->fbyte = TBX_MOD_SIZE(tbx->fbyte + tocpy);
        	tbx->frbs += tocpy;
	        tbx->avbs -= tocpy;
	}
	rt_global_restore_flags(flags);
	return msg_size;
}

static inline int tbxbackput(TBX *tbx, char **msg, int msg_size, unsigned char type)
{
	int tocpy = 0, first_byte = -1;
	unsigned long flags;
	int msgpacksize;
        
	msgpacksize = msg_size + sizeof(type); 
	flags = rt_global_save_flags_and_cli();
	while (tbx->frbs && msgpacksize > 0) {
        	if (first_byte == -1) {
			tbx->fbyte = TBX_REVERSE_MOD_SIZE(tbx->fbyte - msgpacksize);
			first_byte = tbx->fbyte;
	        } else {
        		first_byte = TBX_MOD_SIZE(first_byte + tocpy);
		}
		tocpy = tbx->size - first_byte;
		if (tocpy > msgpacksize) {
			tocpy = msgpacksize;
		}
//		rt_global_restore_flags(flags);
		if (type != TYPE_NONE) {
			tocpy = sizeof(type);
			*(tbx->bufadr + first_byte) = type;
			msgpacksize -= tocpy;
			type = TYPE_NONE;
	        } else {
        		memcpy(tbx->bufadr + first_byte, *msg, tocpy);
			msgpacksize -= tocpy;
			*msg += tocpy;
		}
//		flags = rt_global_save_flags_and_cli();
		tbx->frbs -= tocpy;
		tbx->avbs += tocpy;
	}    
	rt_global_restore_flags(flags);
	return msgpacksize;
}

static inline void tbx_receive_core(TBX *tbx, void *msg, int *msg_size, unsigned char type, unsigned char *lock)
{
	char *temp;

	temp = msg;    
	if (type == TYPE_BROADCAST) {
		if (*tbx->bcbadr == TYPE_NONE) {
			tbxget(tbx, (char **)(&temp), *msg_size);
			if (tbx->waiting_nr > 0) {
				memcpy((char*)(tbx->bcbadr+sizeof(type)), msg, *msg_size);
				*tbx->bcbadr = TYPE_BROADCAST;
				*lock = 1;
			} else {
				rt_sem_broadcast(&(tbx->bcbsmx));
			}
			*msg_size = 0;
		} else {
			memcpy(msg, (char*)(tbx->bcbadr+sizeof(type)), *msg_size);
			*msg_size = 0;                
			if (tbx->waiting_nr == 0) {
				*tbx->bcbadr = TYPE_NONE;
				rt_sem_broadcast(&(tbx->bcbsmx));
            		} else {
				*lock = 1;
			}
		}
	} else {
		*msg_size = tbxget(tbx, (char **)(&temp), *msg_size);
	}
}

int rt_tbx_init(TBX *tbx, int size, int flags)
{
	if (!(tbx->bufadr = sched_malloc(size))) { 
		return -ENOMEM;
	}
	if (!(tbx->bcbadr = sched_malloc(size))) { 
		sched_free(tbx->bufadr);
		return -ENOMEM;
	}
	*tbx->bcbadr = TYPE_NONE;
	memset(tbx->bufadr, 0, size);
	memset(tbx->bcbadr, 0, size);
	rt_typed_sem_init(&(tbx->sndsmx), 1, CNT_SEM | flags);
	rt_typed_sem_init(&(tbx->rcvsmx), 1, CNT_SEM | flags);
	rt_typed_sem_init(&(tbx->bcbsmx), 1, BIN_SEM | flags);
	tbx->magic = RT_TBX_MAGIC;
	tbx->size = tbx->frbs = size;
	tbx->waiting_task = 0;
	tbx->waiting_nr = 0;
	spin_lock_init(&(tbx->buflock));
	tbx->fbyte = tbx->avbs = 0;
	return 0;
}

int rt_tbx_delete(TBX *tbx)
{
	CHK_TBX_MAGIC;
	tbx->magic = 0;
	if (rt_sem_delete(&(tbx->sndsmx)) || rt_sem_delete(&(tbx->rcvsmx))|| rt_sem_delete(&(tbx->bcbsmx))) {
		return -EFAULT;
	}
	while (tbx->waiting_task) {
		tbx_signal(tbx);