rtai_sched.c

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

				rt_schedule();
				hard_restore_flags(flags);
				return 0;
			}
		}
	}
	if (sem->type > 0) {
		int sched;
		sem->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);
	return 0;
}


int rt_sem_broadcast(SEM *sem)
{
	unsigned long flags;
	RT_TASK *task;
	int sched;
	QUEUE *q;

	if (sem->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}

	sched = 0;
	q = &(sem->queue);
	hard_save_flags_and_cli(flags);
	while ((q = q->next) != &(sem->queue)) {
		dequeue_blocked(task = q->task);
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(SEMAPHORE | DELAYED)) == READY) {
			enq_ready_task(task);
			sched = 1;
		}
	}
	sem->count = 0;
	sem->queue.prev = sem->queue.next = &(sem->queue);
	if (sched) {
		rt_schedule();
	}
	hard_restore_flags(flags);
	return 0;
}


int rt_sem_wait(SEM *sem)
{
	unsigned long flags;
	int count;

	if (sem->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}

        TRACE_RTAI_SEM(TRACE_RTAI_EV_SEM_WAIT, sem, 0);

	hard_save_flags_and_cli(flags);
	if ((count = sem->count) <= 0) {
		if (sem->type > 0) {
			if (sem->owndby == rt_current) {
				sem->type++;
				hard_restore_flags(flags);
				return count;
			}
			pass_prio(sem->owndby, rt_current);
		}
		sem->count--;
		rt_current->state |= SEMAPHORE;
		rem_ready_current();
		enqueue_blocked(rt_current, &sem->queue, sem->qtype);
		rt_schedule();
		if (rt_current->blocked_on || sem->magic != RT_SEM_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			hard_restore_flags(flags);
			return SEM_ERR;
		} else { 
			count = sem->count;
		}
	} else {
		sem->count--;
	}
	if (sem->type > 0) {
		(sem->owndby = rt_current)->owndres++;
	}
	hard_restore_flags(flags);
	return count;
}


int rt_sem_wait_if(SEM *sem)
{
	int count;
	unsigned long flags;

	if (sem->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}

        TRACE_RTAI_SEM(TRACE_RTAI_EV_SEM_WAIT_IF, sem, 0);

	hard_save_flags_and_cli(flags);
	if ((count = sem->count) > 0) {
		if (sem->type > 0) {
			if (sem->owndby == rt_current) {
				sem->type++;
				hard_restore_flags(flags);
				return 0;
			}
			(sem->owndby = rt_current)->owndres++;
		}
		sem->count--;
	}
	hard_restore_flags(flags);
	return count;
}


int rt_sem_wait_until(SEM *sem, RTIME time)
{
	int count;
	unsigned long flags;

	if (sem->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}

        TRACE_RTAI_SEM(TRACE_RTAI_EV_SEM_WAIT_UNTIL, sem, time);

	hard_save_flags_and_cli(flags);
	if ((count = sem->count) <= 0) {
		rt_current->blocked_on = &sem->queue;
		if ((rt_current->resume_time = time) > rt_time_h) {
			if (sem->type > 0) {
				if (sem->owndby == rt_current) {
					sem->type++;
					hard_restore_flags(flags);
					return 0;
				}
				pass_prio(sem->owndby, rt_current);
			}
			sem->count--;
			rt_current->state |= (SEMAPHORE | DELAYED);
			rem_ready_current();
			enqueue_blocked(rt_current, &sem->queue, sem->qtype);
			enq_timed_task(rt_current);
			rt_schedule();
		} else {
			sem->count--;
			rt_current->queue.prev = rt_current->queue.next = &rt_current->queue;
		}
		if (sem->magic != RT_SEM_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			hard_restore_flags(flags);
			return SEM_ERR;
		} else {
			if (rt_current->blocked_on) {
				dequeue_blocked(rt_current);
				if(++sem->count > 1 && sem->type) {
					sem->count = 1;
				}
				hard_restore_flags(flags);
				return SEM_TIMOUT;
			} else {
				count = sem->count;
			}
		}
	} else {
		sem->count--;
	}
	if (sem->type > 0) {
		(sem->owndby = rt_current)->owndres++;
	}
	hard_restore_flags(flags);
	return count;
}


int rt_sem_wait_timed(SEM *sem, RTIME delay)
{
	return rt_sem_wait_until(sem, (oneshot_timer ? rdtsc(): rt_times.tick_time) + delay);
}

/* ++++++++++++++++++++++++++++++ MESSAGES +++++++++++++++++++++++++++++++++ */

/* ++++++++++++++++++++++++++++++++ SEND +++++++++++++++++++++++++++++++++++ */

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

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

        TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_SEND, task->tid, msg, 0);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
	      (!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		task->msg = msg;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
			rt_schedule();
		}
	} else {
		rt_current->msg = msg;
		rt_current->msg_queue.task = task;
		enqueue_blocked(rt_current, &task->msg_queue, 0);
		rt_current->state |= SEND;
		rem_ready_current();
		rt_schedule();
	}
	if (rt_current->msg_queue.task != rt_current) {
		rt_current->msg_queue.task = rt_current;
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


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

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

        TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_SEND_IF, task->tid, msg, 0);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
	      (!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		task->msg = msg;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
			rt_schedule();
		}
		if (rt_current->msg_queue.task != rt_current) {
			rt_current->msg_queue.task = rt_current;
			task = (RT_TASK *)0;
		}
	} else {
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


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

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

        TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_SEND_UNTIL, task->tid, msg, time);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
	      (!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		task->msg = msg;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
			rt_schedule();
		}
	} else {
		rt_current->msg_queue.task = task;
		if ((rt_current->resume_time = time) > rt_time_h) {
			rt_current->msg = msg;
			enqueue_blocked(rt_current, &task->msg_queue, 0);
			rt_current->state |= (SEND | DELAYED);
			rem_ready_current();
			enq_timed_task(rt_current);
			rt_schedule();
		} else {
			rt_current->queue.prev = rt_current->queue.next = &rt_current->queue;
		}
	}
	if (rt_current->msg_queue.task != rt_current) {
		dequeue_blocked(rt_current);
		rt_current->msg_queue.task = rt_current;
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


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

/* +++++++++++++++++++++++++++++++++ RPC +++++++++++++++++++++++++++++++++++ */

RT_TASK *rt_rpc(RT_TASK *task, unsigned int to_do, unsigned int *result)
{

	unsigned long flags;

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

        TRACE_RTAI_RPC(TRACE_RTAI_EV_RPC_MAKE, task->tid, to_do, 0);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
		(!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		rt_current->msg = task->msg = to_do;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
		}
		rt_current->state |= RETURN;
	} else {
		rt_current->msg = to_do;
		task->owndres += RPCINC;
		pass_prio(task, rt_current);
		enqueue_blocked(rt_current, &task->msg_queue, 0);
		rt_current->state |= RPC;
	}
	rem_ready_current();
	rt_current->msg_queue.task = task;
	rt_schedule();
	if (rt_current->msg_queue.task == rt_current) {
		*result = rt_current->msg;
	} else {
		rt_current->msg_queue.task = rt_current;
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


RT_TASK *rt_rpc_if(RT_TASK *task, unsigned int to_do, unsigned int *result)
{
	unsigned long flags;

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

        TRACE_RTAI_RPC(TRACE_RTAI_EV_RPC_MAKE_IF, task->tid, to_do, 0);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
	      (!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		rt_current->msg = task->msg = to_do;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
		}
		rt_current->state |= RETURN;
		rem_ready_current();
		rt_current->msg_queue.task = task;
		rt_schedule();
		if (rt_current->msg_queue.task == rt_current) {
			*result = rt_current->msg;
		} else {
			rt_current->msg_queue.task = rt_current;
			task = (RT_TASK *)0;
		}
	} else {
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


RT_TASK *rt_rpc_until(RT_TASK *task, unsigned int to_do, unsigned int *result, RTIME time)
{
	unsigned long flags;
	if (task->magic != RT_TASK_MAGIC) {
		return MSG_ERR;
	}

        TRACE_RTAI_RPC(TRACE_RTAI_EV_RPC_MAKE_UNTIL, task->tid, to_do, time);

	hard_save_flags_and_cli(flags);
	if ((task->state & RECEIVE) &&
	    (!task->msg_queue.task || task->msg_queue.task == rt_current)) {
		rt_current->msg = task->msg = to_do;
		task->msg_queue.task = rt_current;
		task->ret_queue.task = NOTHING;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RECEIVE | DELAYED)) == READY) {
			enq_ready_task(task);
		}
		rt_current->state |= (RETURN | DELAYED);
	} else {
		if ((rt_current->resume_time = time) <= rt_time_h) {
			hard_restore_flags(flags);
			return (RT_TASK *)0;
		}
		rt_current->msg = to_do;
		task->owndres += RPCINC;
		pass_prio(task, rt_current);
		enqueue_blocked(rt_current, &task->msg_queue, 0);
		rt_current->state |= (RPC | DELAYED);
	}
	rem_ready_current();
	rt_current->msg_queue.task = task;
	enq_timed_task(rt_current);
	rt_schedule();
	if (rt_current->msg_queue.task == rt_current) {
		*result = rt_current->msg;
	} else {
		dequeue_blocked(rt_current);
		rt_current->msg_queue.task = rt_current;
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}


RT_TASK *rt_rpc_timed(RT_TASK *task, unsigned int to_do, unsigned int *result, RTIME delay)
{
	return rt_rpc_until(task, to_do, result, (oneshot_timer ? rdtsc(): rt_times.tick_time) + delay);
}

/* ++++++++++++++++++++++++++++++ RPC_RETURN +++++++++++++++++++++++++++++++ */

int rt_isrpc(RT_TASK *task)
{
	return task->state & RETURN;
}


RT_TASK *rt_return(RT_TASK *task, unsigned int result)
{
	unsigned long flags;

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

        TRACE_RTAI_RPC(TRACE_RTAI_EV_RPC_RETURN, task->tid, result, 0);

	hard_save_flags_and_cli(flags);
	if ((task->state & RETURN) && task->msg_queue.task == rt_current) {
		int sched;
		dequeue_blocked(task);
		if (rt_current->owndres & RPCHLF) {
			rt_current->owndres -= RPCINC;
		}
		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;
		}
		task->msg = result;
		task->msg_queue.task = task;
		rem_timed_task(task);
		if (task->state != READY && (task->state &= ~(RETURN | DELAYED)) == READY) {
			enq_ready_task(task);
			rt_schedule();
		} else if (sched) {
			rt_schedule();
		}
	} else {
		task = (RT_TASK *)0;
	}
	hard_restore_flags(flags);
	return task;
}

/* +++++++++++++++++++++++++++++++ RECEIVE +++++++++++++++++++++++++++++++++ */

RT_TASK *rt_receive(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, 0, 0, 0);}
	else { TRACE_RTAI_MSG(TRACE_RTAI_EV_MSG_RECV, 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();