rtai_sched.c

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

			}
		} else if (task->state & RPC) {
			enqueue_blocked(task, &rt_current->ret_queue, 0);
			task->state = (task->state & ~(RPC | DELAYED)) | RETURN;
		}
	} else {
		rt_current->ret_queue.task = SOMETHING;
		rt_current->state |= RECEIVE;
		rem_ready_current();
		rt_current->msg_queue.task = task != rt_current ? task : (RT_TASK *)0;
		rt_schedule();
		*msg = rt_current->msg;
	}
	if (rt_current->ret_queue.task) {
		rt_current->ret_queue.task = NOTHING;
		task = (RT_TASK *)0;
	} else {
		task = rt_current->msg_queue.task;
	}
	rt_current->msg_queue.task = rt_current;
	hard_restore_flags(flags);
	if (task && (struct proxy_t *)task->stack_bottom) {
		if (((struct proxy_t *)task->stack_bottom)->receiver == rt_current) {
			rt_return(task, 0);
		}
	}
	return task;
}


RT_TASK *rt_receive_if(RT_TASK *task, unsigned int *msg)
{
	unsigned long flags;

	if (task && task->magic != RT_TASK_MAGIC) {
		return MSG_ERR;
	}

	if(!task) { TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_RECV_IF, 0, 0, 0); }
	else { TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_RECV_IF, task->tid, 0, 0); }

	hard_save_flags_and_cli(flags);

	if (!task) task = (rt_current->msg_queue.next)->task;
	if ((task->state & (SEND | RPC)) && task->msg_queue.task == rt_current) {
		dequeue_blocked(task);
		rem_timed_task(task);
		*msg = task->msg;
		rt_current->msg_queue.task = task;
		if (task->state & SEND) {
			task->msg_queue.task = task;
			if (task->state != READY && (task->state &= ~(SEND | DELAYED)) == READY) {
				enq_ready_task(task);
				rt_schedule();
			}
		} else if (task->state & RPC) {
			enqueue_blocked(task, &rt_current->ret_queue, 0);
			task->state = (task->state & ~(RPC | DELAYED)) | RETURN;
		}
		if (rt_current->ret_queue.task) {
			rt_current->ret_queue.task = NOTHING;
			task = (RT_TASK *)0;
		} else {
			task = rt_current->msg_queue.task;
		}
		rt_current->msg_queue.task = rt_current;
	} else {
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	if (task && (struct proxy_t *)task->stack_bottom) {
		if (((struct proxy_t *)task->stack_bottom)->receiver == rt_current) {
			rt_return(task, 0);
		}
	}
	return task;
}


RT_TASK *rt_receive_until(RT_TASK *task, unsigned int *msg, RTIME time)
{
	unsigned long flags;

	if (task && task->magic != RT_TASK_MAGIC) {
		return MSG_ERR;
	}

	if(!task) { TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_RECV_UNTIL, 0, 0, time); }
	else { TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_RECV_UNTIL, task->tid, 0, time); }

	hard_save_flags_and_cli(flags);

	if (!task) task = (rt_current->msg_queue.next)->task;
	if ((task->state & (SEND | RPC)) && task->msg_queue.task == rt_current) {
		dequeue_blocked(task);
		rem_timed_task(task);
		*msg = task->msg;
		rt_current->msg_queue.task = task;
		if (task->state & SEND) {
			task->msg_queue.task = task;
			if (task->state != READY && (task->state &= ~(SEND | DELAYED)) == READY) {
				enq_ready_task(task);
				rt_schedule();
			}
		} else if (task->state & RPC) {
			enqueue_blocked(task, &rt_current->ret_queue, 0);
			task->state = (task->state & ~(RPC | DELAYED)) | RETURN;
		}
	} else {
		rt_current->ret_queue.task = SOMETHING;
		if ((rt_current->resume_time = time) > rt_time_h) {
			rt_current->state |= (RECEIVE | DELAYED);
			rem_ready_current();
			rt_current->msg_queue.task = task != rt_current ? task : (RT_TASK *)0;
			enq_timed_task(rt_current);
			rt_schedule();
			*msg = rt_current->msg;
		}
	}
	if (rt_current->ret_queue.task) {
		rt_current->ret_queue.task = NOTHING;
		task = (RT_TASK *)0;
	} else {
		task = rt_current->msg_queue.task;
	}
	rt_current->msg_queue.task = rt_current;
	hard_restore_flags(flags);
	if (task && (struct proxy_t *)task->stack_bottom) {
		if (((struct proxy_t *)task->stack_bottom)->receiver == rt_current) {
			rt_return(task, 0);
		}
	}
	return task;
}


RT_TASK *rt_receive_timed(RT_TASK *task, unsigned int *msg, RTIME delay)
{
	return rt_receive_until(task, msg, (oneshot_timer ? rdtsc(): rt_times.tick_time) + delay);
}

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

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

	hard_save_flags_and_cli(flags);
	if ((task = mbx->waiting_task)) {
		rem_timed_task(task);
		mbx->waiting_task = NOTHING;
		task->prio_passed_to = NOTHING;
		if (task->state != READY && (task->state &= ~(MBXSUSP | DELAYED)) == READY) {
			enq_ready_task(task);
			if (mbx->sndsem.type <= 0) {
				rt_schedule();
				hard_restore_flags(flags);
				return;
			}
		}
	}
	if (mbx->sndsem.type > 0) {
		int sched;
		mbx->owndby = 0;
		if (rt_current->owndres & SEMHLF) {
			--rt_current->owndres;
		}
		if (!rt_current->owndres) {
			sched = renq_ready_task(rt_current, rt_current->base_priority);
		} else if (!(rt_current->owndres & SEMHLF)) {
			int priority;
			sched = renq_ready_task(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 |= SUSPENDED;
				rem_ready_current();
				sched = 1;
			} else {
				rt_task_delete(rt_current);
			}	
		}	
		if (sched) {
			rt_schedule();
		}	
	}
	hard_restore_flags(flags);
}

#define RT_MBX_MAGIC 0x3ad46e9b

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

	hard_save_flags_and_cli(flags);
	if (!(*fravbs)) {
		if (mbx->sndsem.type > 0) {
			pass_prio(mbx->owndby, rt_current);
		}
		rt_current->state |= MBXSUSP;
		rem_ready_current();
		mbx->waiting_task = rt_current;
		rt_schedule();
		if (mbx->waiting_task == rt_current || mbx->magic != RT_MBX_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			hard_restore_flags(flags);
			return -1;
		}
	}
	if (mbx->sndsem.type > 0) {
		(mbx->owndby = rt_current)->owndres++;
	}
	hard_restore_flags(flags);
	return 0;
}

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

	hard_save_flags_and_cli(flags);
	if (!(*fravbs)) {
		mbx->waiting_task = rt_current;
		if ((rt_current->resume_time = time) > rt_time_h) {
			if (mbx->sndsem.type > 0) {
				pass_prio(mbx->owndby, rt_current);
			}
			rt_current->state |= (MBXSUSP | DELAYED);
			rem_ready_current();
			enq_timed_task(rt_current);
			rt_schedule();
		}
		if (mbx->magic != RT_MBX_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			hard_restore_flags(flags);
			return -1;
		}
		if (mbx->waiting_task == rt_current) {
			mbx->waiting_task = NOTHING;
			rt_current->prio_passed_to = NOTHING;
			hard_restore_flags(flags);
			return -1;
		}
	}
	if (mbx->sndsem.type > 0) {
		(mbx->owndby = rt_current)->owndres++;
	}
	hard_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)
{
	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;
		}
		memcpy(mbx->bufadr + mbx->lbyte, *msg, tocpy);
		hard_save_flags_and_cli(flags);
		mbx->frbs -= tocpy;
		mbx->avbs += tocpy;
		hard_restore_flags(flags);
		msg_size -= tocpy;
		*msg     += tocpy;
		mbx->lbyte = MOD_SIZE(mbx->lbyte + tocpy);
	}
	return msg_size;
}

static inline int mbxget(MBX *mbx, char **msg, int msg_size)
{
	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;
		}
		memcpy(*msg, mbx->bufadr + mbx->fbyte, tocpy);
		hard_save_flags_and_cli(flags);
		mbx->frbs  += tocpy;
		mbx->avbs  -= tocpy;
		hard_restore_flags(flags);
		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 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;
		}
		memcpy(*msg, mbx->bufadr + fbyte, tocpy);
		avbs     -= tocpy;
		msg_size -= tocpy;
		*msg     += tocpy;
		fbyte = MOD_SIZE(fbyte + tocpy);
	}
	return msg_size;
}

int rt_mbx_evdrp(MBX *mbx, void *msg, int msg_size)
{
	return mbxevdrp(mbx, (char **)(&msg), msg_size);
}

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

int rt_typed_mbx_init(MBX *mbx, int size, int type)
{
	if (!(mbx->bufadr = sched_malloc(size))) { 
		return -ENOMEM;
	}
	TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_INIT, mbx, size, 0);
	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;
	return 0;
}


int rt_mbx_init(MBX *mbx, int size)
{
	return rt_typed_mbx_init(mbx, size, PRIO_Q);
}


int rt_mbx_delete(MBX *mbx)
{
	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_DELETE, mbx, 0, 0);

	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;
}


int rt_mbx_send(MBX *mbx, void *msg, int msg_size)
{
	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_SEND, mbx, msg_size, 0);

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


int rt_mbx_send_wp(MBX *mbx, void *msg, int msg_size)
{
	unsigned long flags;

	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_SEND_WP, mbx, msg_size, 0);

	hard_save_flags_and_cli(flags);
	if (mbx->sndsem.count && mbx->frbs) {
		mbx->sndsem.count = 0;
		if (mbx->sndsem.type > 0) {
			(mbx->sndsem.owndby = mbx->owndby = rt_current)->owndres += 2;
		}
		hard_restore_flags(flags);
		msg_size = mbxput(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
		rt_sem_signal(&mbx->sndsem);
	} else {
		hard_restore_flags(flags);
	}
	return msg_size;
}


int rt_mbx_send_if(MBX *mbx, void *msg, int msg_size)
{
	unsigned long flags;

	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_SEND_IF, mbx, msg_size, 0);

	hard_save_flags_and_cli(flags);
	if (mbx->sndsem.count && msg_size <= mbx->frbs) {
		mbx->sndsem.count = 0;
		if (mbx->sndsem.type > 0) {
			(mbx->sndsem.owndby = mbx->owndby = rt_current)->owndres += 2;
		}
		hard_restore_flags(flags);
		mbxput(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
		rt_sem_signal(&mbx->sndsem);
		return 0;
	}
	hard_restore_flags(flags);
	return msg_size;
}


int rt_mbx_send_until(MBX *mbx, void *msg, int msg_size, RTIME time)
{
	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_SEND_UNTIL, mbx, msg_size, time);

	if (rt_sem_wait_until(&mbx->sndsem, time) > 1) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait_until(mbx, &mbx->frbs, time)) {
			rt_sem_signal(&(mbx->sndsem));
			return msg_size;
		}
		msg_size = mbxput(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
	}
	rt_sem_signal(&(mbx->sndsem));
	return 0;
}


int rt_mbx_send_timed(MBX *mbx, void *msg, int msg_size, RTIME delay)
{
	return rt_mbx_send_until(mbx, msg, msg_size, (oneshot_timer ? rdtsc(): rt_times.tick_time) + delay);
}


int rt_mbx_receive(MBX *mbx, void *msg, int msg_size)
{

	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_RECV, mbx, msg_size, 0);

	if (rt_sem_wait(&mbx->rcvsem) > 1) {
		return msg_size;
	}
	while (msg_size) {
		if (mbx_wait(mbx, &mbx->avbs)) {
			rt_sem_signal(&mbx->rcvsem);
			return msg_size;
		}
		msg_size = mbxget(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
	}
	rt_sem_signal(&mbx->rcvsem);
	return 0;
}


int rt_mbx_receive_wp(MBX *mbx, void *msg, int msg_size)
{
	unsigned long flags;

	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_RECV_WP, mbx, msg_size, 0);

	hard_save_flags_and_cli(flags);
	if (mbx->rcvsem.count && mbx->avbs) {
		mbx->rcvsem.count = 0;
		if (mbx->rcvsem.type > 0) {
			(mbx->rcvsem.owndby = mbx->owndby = rt_current)->owndres += 2;
		}
		hard_restore_flags(flags);
		msg_size = mbxget(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
		rt_sem_signal(&mbx->rcvsem);
	} else {
		hard_restore_flags(flags);
	}
	return msg_size;
}


int rt_mbx_receive_if(MBX *mbx, void *msg, int msg_size)
{
	unsigned long flags;

	CHK_MBX_MAGIC;

        TRACE_RTAI_MBX(TRACE_RTAI_EV_MBX_RECV_IF, mbx, msg_size, 0);

	hard_save_flags_and_cli(flags);
	if (mbx->rcvsem.count && msg_size <= mbx->avbs) {
		mbx->rcvsem.count = 0;
		if (mbx->rcvsem.type > 0) {
			(mbx->rcvsem.owndby = mbx->owndby = rt_current)->owndres += 2;
		}
		hard_restore_flags(flags);
		mbxget(mbx, (char **)(&msg), msg_size);
		mbx_signal(mbx);
		rt_sem_si