sem.c

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

		sem->count--;
		rt_current->state |= RT_SCHED_SEMAPHORE;
		rem_ready_current(rt_current);
		enqueue_blocked(rt_current, &sem->queue, sem->qtype);
		RT_SCHEDULE_MAP_BOTH(schedmap);
		if (rt_current->blocked_on || sem->magic != RT_SEM_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return SEM_ERR;
		} else { 
			count = sem->count;
		}
	} else {
		sem->count--;
	}
	if (sem->type > 0) {
		(sem->owndby = rt_current)->owndres++;
	}
	rt_global_restore_flags(flags);
	return count;
}


/**
 * @anchor rt_sem_wait_if
 * @brief Take a semaphore, only if the calling task is not blocked.
 *
 * rt_sem_wait_if is a version of the semaphore wait operation is
 * similar to @ref rt_sem_wait() but it is never blocks the caller. If
 * the semaphore is not free, rt_sem_wait_if returns immediately and
 * the semaphore value remains unchanged.
 *
 * @param sem points to the structure used in the call to @ref
 * rt_sem_init().
 *
 * @return the number of events already signaled upon success.
 * A special value as described below in case of a failure:
 * - @b 0xFFFF: @e sem does not refer to a valid semaphore.
 *
 * @note In principle 0xFFFF could theoretically be a usable
 *	 semaphores events count so it could be returned also under
 *	 normal circumstances. It is unlikely you are going to count
 *	 up to such number  of events, in any case avoid counting up
 *	 to 0xFFFF.
 */
int rt_sem_wait_if(SEM *sem)
{
	int count;
	unsigned long flags;

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

	flags = rt_global_save_flags_and_cli();
	if ((count = sem->count) <= 0) {
		if (sem->type > 0 && sem->owndby == RT_CURRENT) {
			sem->type++;
			rt_global_restore_flags(flags);
			return 0;
		}
	} else {
		sem->count--;
		if (sem->type > 0) {
			(sem->owndby = RT_CURRENT)->owndres++;
		}
	}
	rt_global_restore_flags(flags);
	return count;
}


/**
 * @anchor rt_sem_wait_until
 * @brief Wait a semaphore with timeout.
 *
 * rt_sem_wait_until, like @ref rt_sem_wait_timed() is a timed version
 * of the standard semaphore wait call. The semaphore value is
 * decremented and tested. If it is still non-negative these functions
 * return immediately. Otherwise the caller task is blocked and queued
 * up. Queuing may happen in priority order or on FIFO base. This is
 * determined by the compile time option @e SEM_PRIORD. In this case
 * the function returns if:
 *	- The caller task is in the first place of the waiting queue
 *	  and an other task issues a @ref rt_sem_signal call();
 *	- a timeout occurs;
 *	- an error occurs (e.g. the semaphore is destroyed);
 *
 * In case of a timeout, the semaphore value is incremented before 
 * return.  
 *
 * @param sem points to the structure used in the call to @ref
 *	  rt_sem_init().
 *
 * @param time is an absolute value to the current time.
 *
 * @return the number of events already signaled upon success.
 * Aa special value" as described below in case of a failure:
 * - @b 0xFFFF: @e sem does not refer to a valid semaphore.
 * 
 * @note In principle 0xFFFF could theoretically be a usable
 *	 semaphores events count so it could be returned also under
 *	 normal circumstances. It is unlikely you are going to count
 *	 up to such number of events, in any case avoid counting up to
 *	 0xFFFF.
 */
int rt_sem_wait_until(SEM *sem, RTIME time)
{
	DECLARE_RT_CURRENT;
	int count;
	unsigned long flags;

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

	flags = rt_global_save_flags_and_cli();
	ASSIGN_RT_CURRENT;
	if ((count = sem->count) <= 0) {
		rt_current->blocked_on = &sem->queue;
		if ((rt_current->resume_time = time) > rt_time_h) {
			unsigned long schedmap;
			if (sem->type > 0) {
				if (sem->owndby == rt_current) {
					sem->type++;
					rt_global_restore_flags(flags);
					return 0;
				}
				schedmap = pass_prio(sem->owndby, rt_current);
			} else {
				schedmap = 0;
			}	
			sem->count--;
			rt_current->state |= (RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED);
			rem_ready_current(rt_current);
			enqueue_blocked(rt_current, &sem->queue, sem->qtype);
			enq_timed_task(rt_current);
			RT_SCHEDULE_MAP_BOTH(schedmap);
		} 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;
			rt_global_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;
				}
				rt_global_restore_flags(flags);
				return SEM_TIMOUT;
			} else {
				count = sem->count;
			}
		}
	} else {
		sem->count--;
	}
	if (sem->type > 0) {
		(sem->owndby = rt_current)->owndres++;
	}
	rt_global_restore_flags(flags);
	return count;
}


/**
 * @anchor rt_sem_wait_timed
 * @brief Wait a semaphore with timeout.
 *
 * rt_sem_wait_timed, like @ref rt_sem_wait_until(), is a timed version
 * of the standard semaphore wait call. The semaphore value is
 * decremented and tested. If it is still non-negative these functions
 * return immediately. Otherwise the caller task is blocked and queued
 * up. Queuing may happen in priority order or on FIFO base. This is
 * determined by the compile time option @e SEM_PRIORD. In this case
 * the function returns if:
 *	- The caller task is in the first place of the waiting queue
 *	  and an other task issues a @ref rt_sem_signal() call;
 *	- a timeout occurs;
 *	- an error occurs (e.g. the semaphore is destroyed);
 *
 * In case of a timeout, the semaphore value is incremented before 
 * return.  
 *
 * @param sem points to the structure used in the call to @ref
 *	  rt_sem_init().
 *
 * @param delay is an absolute value to the current time.
 *
 * @return the number of events already signaled upon success.
 * A special value as described below in case of a failure:
 * - @b 0xFFFF: @e sem does not refer to a valid semaphore.
 * 
 * @note In principle 0xFFFF could theoretically be a usable
 *	 semaphores events count so it could be returned also under
 *	 normal circumstances. It is unlikely you are going to count
 *	 up to such number of events, in any case avoid counting up to
 *	 0xFFFF.
 */
int rt_sem_wait_timed(SEM *sem, RTIME delay)
{
	return rt_sem_wait_until(sem, get_time() + delay);
}


/* ++++++++++++++++++++++++++ BARRIER SUPPORT +++++++++++++++++++++++++++++++ */

/**
 * @anchor rt_sem_wait_barrier
 * @brief Wait on a semaphore barrier.
 *
 * rt_sem_wait_barrier is a gang waiting in that a task issuing such
 * a request will be blocked till a number of tasks equal to the semaphore
 * count set at rt_sem_init is reached.
 *
 * @returns 0 always.
 */
int rt_sem_wait_barrier(SEM *sem)
{
	unsigned long flags;

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

	flags = rt_global_save_flags_and_cli();
	if (!sem->owndby) {
		sem->owndby = (void *)(sem->count < 1 ? 1 : sem->count);
		sem->count = sem->type = 0;
	}
	if ((1 - sem->count) < (int)sem->owndby) {
		rt_sem_wait(sem);
	} else {
		rt_sem_broadcast(sem);
	}
	rt_global_restore_flags(flags);
	return 0;
}

/* +++++++++++++++++++++++++ COND VARIABLES SUPPORT +++++++++++++++++++++++++ */

/**
 * @anchor rt_cond_signal
 * @brief Wait for a signal to a conditional variable.
 *
 * rt_cond_signal resumes one of the tasks that are waiting on the condition 
 * semaphore cnd. Nothing happens if no task is waiting on cnd, while it
 * resumed the first queued task blocked on cnd, according to the queueing
 * method set at rt_cond_init.
 *
 * @param cnd points to the structure used in the call to @ref
 *	  rt_cond_init().
 *
 * @it returns 0 always.
 *
 */
int rt_cond_signal(CND *cnd)
{
	unsigned long flags;
	RT_TASK *task;

	if (cnd->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}
	flags = rt_global_save_flags_and_cli();
	if ((task = (cnd->queue.next)->task)) {
		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());
		}
	}
	rt_global_restore_flags(flags);
	return 0;
}

static inline void rt_cndmtx_signal(SEM *mtx, RT_TASK *rt_current)
{
	RT_TASK *task;

	if (mtx->type <= 1) {
		if (++mtx->count > 1) {
			mtx->count = 1;
		}
		if ((task = (mtx->queue.next)->task)) {
			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);
			}
		}
		if (mtx->type > 0) {
			mtx->owndby = 0;
			if (rt_current->owndres & SEMHLF) {
				--rt_current->owndres;
			}
			if (!rt_current->owndres) {
				renq_current(rt_current, rt_current->base_priority);
			} else if (!(rt_current->owndres & SEMHLF)) {
				int priority;
				renq_current(rt_current, rt_current->base_priority > (priority = ((rt_current->msg_queue.next)->task)->priority) ? priority : rt_current->base_priority);
			}
			if (rt_current->suspdepth > 0) {
				rt_current->state |= RT_SCHED_SUSPENDED;
				rem_ready_current(rt_current);
			} else if (rt_current->suspdepth < 0) {
				rt_task_delete(rt_current);
			}
		}
	} else {
		task = 0;
		mtx->type--;
	}
 	if (task) {
		 RT_SCHEDULE_BOTH(task, hard_cpu_id());
	} else {
		rt_schedule();
	}
}

/**
 * @anchor rt_cond_wait
 * @brief Wait for a signal to a conditional variable.
 *
 * rt_cond_wait atomically unlocks mtx (as for using rt_sem_signal)
 * and waits for the condition semaphore cnd to be signaled. The task 
 * execution is suspended until the condition semaphore is signalled. 
 * Mtx must be obtained by the calling task, before calling rt_cond_wait is
 * called. Before returning to the calling task rt_cond_wait reacquires 
 * mtx by calling rt_sem_wait.
 *
 * @param cnd points to the structure used in the call to @ref
 *	  rt_cond_init().
 *
 * @param mtx points to the structure used in the call to @ref
 *	  rt_sem_init().
 *
 * @return 0 on succes, SEM_ERR in case of error.
 *
 */
int rt_cond_wait(CND *cnd, SEM *mtx)
{
	RT_TASK *rt_current;
	unsigned long flags;
	int retval;

	if (cnd->magic != RT_SEM_MAGIC || mtx->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}
	retval = 0;
	flags = rt_global_save_flags_and_cli();
	rt_current = RT_CURRENT;
	rt_current->state |= RT_SCHED_SEMAPHORE;
	rem_ready_current(rt_current);
	enqueue_blocked(rt_current, &cnd->queue, cnd->qtype);
	rt_cndmtx_signal(mtx, rt_current);
	if (rt_current->blocked_on || cnd->magic != RT_SEM_MAGIC) {
		retval = SEM_ERR;
	}
	rt_global_restore_flags(flags);
	rt_sem_wait(mtx);
	return retval;
}

/**
 * @anchor rt_cond_wait_until
 * @brief Wait a semaphore with timeout.
 *
 * rt_cond_wait_until atomically unlocks mtx (as for using rt_sem_signal)
 * and waits for the condition semaphore cnd to be signalled. The task 
 * execution is suspended until the condition semaphore is either signaled
 * or a timeout expires. Mtx must be obtained by the calling task, before 
 * calling rt_cond_wait is called. Before returning to the calling task 
 * rt_cond_wait_until reacquires mtx by calling rt_sem_wait and returns a 
 * value to indicate if it has been signalled pr timedout.
 *
 * @param cnd points to the structure used in the call to @ref
 *	  rt_cnd_init().
 *
 * @param mtx points to the structure used in the call to @ref
 *	  rt_sem_init().
 *
 * @param time is an absolute value to the current time, in timer count unit.
 *
 * @returns 0 if it was signaled, SEM_TIMOUT if a timeout occured, SEM_ERR
 * if the task has been resumed because of any other action (likely cnd
 * was deleted).
 */
int rt_cond_wait_until(CND *cnd, SEM *mtx, RTIME time)
{
	DECLARE_RT_CURRENT;
	unsigned long flags;
	int retval;

	if (cnd->magic != RT_SEM_MAGIC && mtx->magic != RT_SEM_MAGIC) {
		return SEM_ERR;
	}
	retval = SEM_TIMOUT;
	flags = rt_global_save_flags_and_cli();
	ASSIGN_RT_CURRENT;
	if ((rt_current->resume_time = time) > rt_time_h) {
		rt_current->state |= (RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED);
		rem_ready_current(rt_current);
		enqueue_blocked(rt_current, &cnd->queue, cnd->qtype);
		enq_timed_task(rt_current);
		rt_cndmtx_signal(mtx, rt_current);
		if (cnd->magic != RT_SEM_MAGIC) {
			retval = SEM_ERR;
		} else {
			if (rt_current->blocked_on) {
				dequeue_blocked(rt_current);
			} else {
				retval = 0;
			}
		}
	} else {
		rt_global_restore_flags(flags);
		return retval;
	}
	rt_global_restore_flags(flags);
	rt_sem_wait(mtx);
	return retval;
}

/**
 * @anchor rt_cond_wait_timed
 * @brief Wait a semaphore with timeout.
 *
 * rt_cond_wait_timed atomically unlocks mtx (as for using rt_sem_signal)
 * and waits for the condition semaphore cnd to be signalled. The task 
 * execution is suspended until the condition semaphore is either signaled
 * or a timeout expires. Mtx must be obtained by the calling task, before 
 * calling rt_cond_wait is called. Before returning to the calling task 
 * rt_cond_wait_until reacquires mtx by calling rt_sem_wait and returns a 
 * value to indicate if it has been signalled pr timedout.
 *
 * @param cnd points to the structure used in the call to @ref
 *	  rt_cnd_init().
 *
 * @param mtx points to the structure used in the call to @ref
 *	  rt_sem_init().
 *
 * @param delay is a realtive time values with respect to the current time,
 * in timer count unit.
 *
 * @returns 0 if it was signaled, SEM_TIMOUT if a timeout occured, SEM_ERR
 * if the task has been resumed because of any other action (likely cnd
 * was deleted).
 */