712 lines
14 KiB
C
712 lines
14 KiB
C
/* semaphore kernel services */
|
|
|
|
/*
|
|
* Copyright (c) 1997-2010, 2012-2015 Wind River Systems, Inc.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met:
|
|
*
|
|
* 1) Redistributions of source code must retain the above copyright notice,
|
|
* this list of conditions and the following disclaimer.
|
|
*
|
|
* 2) Redistributions in binary form must reproduce the above copyright notice,
|
|
* this list of conditions and the following disclaimer in the documentation
|
|
* and/or other materials provided with the distribution.
|
|
*
|
|
* 3) Neither the name of Wind River Systems nor the names of its contributors
|
|
* may be used to endorse or promote products derived from this software without
|
|
* specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <microkernel.h>
|
|
#include <toolchain.h>
|
|
#include <sections.h>
|
|
|
|
#include <micro_private.h>
|
|
|
|
/**
|
|
*
|
|
* @brief Common code for signaling a semaphore
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
static void signal_semaphore(int n, struct sem_struct *S)
|
|
{
|
|
struct k_args *A, *X, *Y;
|
|
|
|
#ifdef CONFIG_OBJECT_MONITOR
|
|
S->Count += n;
|
|
#endif
|
|
|
|
S->Level += n;
|
|
A = S->Waiters;
|
|
Y = NULL;
|
|
while (A && S->Level) {
|
|
X = A->Forw;
|
|
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Comm == WAITSREQ || A->Comm == WAITSTMO)
|
|
#else
|
|
if (A->Comm == WAITSREQ)
|
|
#endif
|
|
{
|
|
S->Level--;
|
|
if (Y) {
|
|
Y->Forw = X;
|
|
} else {
|
|
S->Waiters = X;
|
|
}
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer) {
|
|
_k_timeout_cancel(A);
|
|
A->Comm = WAITSRPL;
|
|
} else {
|
|
#endif
|
|
A->Time.rcode = RC_OK;
|
|
_k_state_bit_reset(A->Ctxt.proc, TF_SEMA);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
}
|
|
#endif
|
|
} else if (A->Comm == WAITMREQ) {
|
|
S->Level--;
|
|
A->Comm = WAITMRDY;
|
|
GETARGS(Y);
|
|
*Y = *A;
|
|
SENDARGS(Y);
|
|
Y = A;
|
|
} else {
|
|
Y = A;
|
|
}
|
|
A = X;
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Finish handling incomplete waits on semaphores
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait(struct k_args *R)
|
|
{
|
|
struct k_args *A = R->Ctxt.args;
|
|
|
|
FREEARGS(R);
|
|
if (--(A->Args.s1.nsem) == 0) {
|
|
_k_state_bit_reset(A->Ctxt.proc, TF_LIST);
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle cancellation of a semaphore involved in a
|
|
* semaphore group wait request
|
|
*
|
|
* This routine only applies to semaphore group wait requests. It is invoked
|
|
* for each semaphore in the semaphore group that "lost" the semaphore group
|
|
* wait request.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait_cancel(struct k_args *A)
|
|
{
|
|
struct sem_struct *S = _k_sem_list + OBJ_INDEX(A->Args.s1.sema);
|
|
struct k_args *X = S->Waiters;
|
|
struct k_args *Y = NULL;
|
|
|
|
while (X && (X->Prio <= A->Prio)) {
|
|
if (X->Ctxt.args == A->Ctxt.args) {
|
|
if (Y) {
|
|
Y->Forw = X->Forw;
|
|
} else {
|
|
S->Waiters = X->Forw;
|
|
}
|
|
if (X->Comm == WAITMREQ || X->Comm == WAITMRDY) {
|
|
if (X->Comm == WAITMRDY) {
|
|
/* obtain struct k_args of waiting task */
|
|
|
|
struct k_args *waitTaskArgs = X->Ctxt.args;
|
|
|
|
/*
|
|
* Determine if the wait cancellation request is being
|
|
* processed after the state of the 'Waiters' packet state
|
|
* has been updated to WAITMRDY, but before the WAITMRDY
|
|
* packet has been processed. This will occur if a WAITMTMO
|
|
* timer expiry occurs between the update of the packet state
|
|
* and the processing of the WAITMRDY packet.
|
|
*/
|
|
if (unlikely(waitTaskArgs->Args.s1.sema ==
|
|
ENDLIST)) {
|
|
waitTaskArgs->Args.s1.sema = A->Args.s1.sema;
|
|
} else {
|
|
signal_semaphore(1, S);
|
|
}
|
|
}
|
|
|
|
_k_sem_group_wait(X);
|
|
} else {
|
|
FREEARGS(X); /* ERROR */
|
|
}
|
|
FREEARGS(A);
|
|
return;
|
|
} else {
|
|
Y = X;
|
|
X = X->Forw;
|
|
}
|
|
}
|
|
A->Forw = X;
|
|
if (Y) {
|
|
Y->Forw = A;
|
|
} else {
|
|
S->Waiters = A;
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle acceptance of the ready semaphore request
|
|
*
|
|
* This routine only applies to semaphore group wait requests. It handles
|
|
* the request for the one semaphore in the group that "wins" the semaphore
|
|
* group wait request.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait_accept(struct k_args *A)
|
|
{
|
|
struct sem_struct *S = _k_sem_list + OBJ_INDEX(A->Args.s1.sema);
|
|
struct k_args *X = S->Waiters;
|
|
struct k_args *Y = NULL;
|
|
|
|
while (X && (X->Prio <= A->Prio)) {
|
|
if (X->Ctxt.args == A->Ctxt.args) {
|
|
if (Y) {
|
|
Y->Forw = X->Forw;
|
|
} else {
|
|
S->Waiters = X->Forw;
|
|
}
|
|
if (X->Comm == WAITMRDY) {
|
|
_k_sem_group_wait(X);
|
|
} else {
|
|
FREEARGS(X); /* ERROR */
|
|
}
|
|
FREEARGS(A);
|
|
return;
|
|
} else {
|
|
Y = X;
|
|
X = X->Forw;
|
|
}
|
|
}
|
|
/* ERROR */
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore group timeout request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait_timeout(struct k_args *A)
|
|
{
|
|
ksem_t *L;
|
|
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer) {
|
|
FREETIMER(A->Time.timer);
|
|
}
|
|
#endif
|
|
|
|
L = A->Args.s1.list;
|
|
while (*L != ENDLIST) {
|
|
struct k_args *R;
|
|
|
|
GETARGS(R);
|
|
R->Prio = A->Prio;
|
|
R->Comm =
|
|
(K_COMM)((*L == A->Args.s1.sema) ? WAITMACC : WAITMCAN);
|
|
R->Ctxt.args = A;
|
|
R->Args.s1.sema = *L++;
|
|
SENDARGS(R);
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore ready request
|
|
*
|
|
* This routine only applies to semaphore group wait requests. It identifies
|
|
* the one semaphore in the group that "won" the semaphore group wait request
|
|
* before triggering the semaphore group timeout handler.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_ready(struct k_args *R)
|
|
{
|
|
struct k_args *A = R->Ctxt.args;
|
|
|
|
if (A->Args.s1.sema == ENDLIST) {
|
|
A->Args.s1.sema = R->Args.s1.sema;
|
|
A->Comm = WAITMTMO;
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer) {
|
|
_k_timeout_cancel(A);
|
|
} else
|
|
#endif
|
|
_k_sem_group_wait_timeout(A);
|
|
}
|
|
FREEARGS(R);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Reply to a semaphore wait request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_wait_reply(struct k_args *A)
|
|
{
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer) {
|
|
FREETIMER(A->Time.timer);
|
|
}
|
|
if (A->Comm == WAITSTMO) {
|
|
REMOVE_ELM(A);
|
|
A->Time.rcode = RC_TIME;
|
|
} else
|
|
#endif
|
|
A->Time.rcode = RC_OK;
|
|
_k_state_bit_reset(A->Ctxt.proc, TF_SEMA);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle internal wait request on a semaphore involved in a
|
|
* semaphore group wait request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait_request(struct k_args *A)
|
|
{
|
|
struct sem_struct *S = _k_sem_list + OBJ_INDEX(A->Args.s1.sema);
|
|
struct k_args *X = S->Waiters;
|
|
struct k_args *Y = NULL;
|
|
|
|
while (X && (X->Prio <= A->Prio)) {
|
|
if (X->Ctxt.args == A->Ctxt.args) {
|
|
if (Y) {
|
|
Y->Forw = X->Forw;
|
|
} else {
|
|
S->Waiters = X->Forw;
|
|
}
|
|
if (X->Comm == WAITMCAN) {
|
|
_k_sem_group_wait(X);
|
|
} else {
|
|
FREEARGS(X); /* ERROR */
|
|
}
|
|
FREEARGS(A);
|
|
return;
|
|
} else {
|
|
Y = X;
|
|
X = X->Forw;
|
|
}
|
|
}
|
|
A->Forw = X;
|
|
if (Y) {
|
|
Y->Forw = A;
|
|
} else {
|
|
S->Waiters = A;
|
|
}
|
|
signal_semaphore(0, S);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore group wait request
|
|
*
|
|
* This routine splits the single semaphore group wait request into several
|
|
* internal wait requests--one for each semaphore in the group.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_wait_any(struct k_args *A)
|
|
{
|
|
ksem_t *L;
|
|
|
|
L = A->Args.s1.list;
|
|
A->Args.s1.sema = ENDLIST;
|
|
A->Args.s1.nsem = 0;
|
|
|
|
if (*L == ENDLIST) {
|
|
return;
|
|
}
|
|
|
|
while (*L != ENDLIST) {
|
|
struct k_args *R;
|
|
|
|
GETARGS(R);
|
|
R->Prio = _k_current_task->Prio;
|
|
R->Comm = WAITMREQ;
|
|
R->Ctxt.args = A;
|
|
R->Args.s1.sema = *L++;
|
|
SENDARGS(R);
|
|
(A->Args.s1.nsem)++;
|
|
}
|
|
|
|
A->Ctxt.proc = _k_current_task;
|
|
_k_state_bit_set(_k_current_task, TF_LIST);
|
|
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.ticks != TICKS_NONE) {
|
|
if (A->Time.ticks == TICKS_UNLIMITED) {
|
|
A->Time.timer = NULL;
|
|
} else {
|
|
A->Comm = WAITMTMO;
|
|
_k_timeout_alloc(A);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore test and wait request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_wait_request(struct k_args *A)
|
|
{
|
|
struct sem_struct *S;
|
|
uint32_t Sid;
|
|
|
|
Sid = A->Args.s1.sema;
|
|
S = _k_sem_list + OBJ_INDEX(Sid);
|
|
|
|
if (S->Level) {
|
|
S->Level--;
|
|
A->Time.rcode = RC_OK;
|
|
} else if (A->Time.ticks != TICKS_NONE) {
|
|
A->Ctxt.proc = _k_current_task;
|
|
A->Prio = _k_current_task->Prio;
|
|
_k_state_bit_set(_k_current_task, TF_SEMA);
|
|
INSERT_ELM(S->Waiters, A);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.ticks == TICKS_UNLIMITED) {
|
|
A->Time.timer = NULL;
|
|
} else {
|
|
A->Comm = WAITSTMO;
|
|
_k_timeout_alloc(A);
|
|
}
|
|
#endif
|
|
return;
|
|
} else {
|
|
A->Time.rcode = RC_FAIL;
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Test a semaphore
|
|
*
|
|
* This routine tests a semaphore to see if it has been signaled. If the signal
|
|
* count is greater than zero, it is decremented.
|
|
*
|
|
* @param sema Semaphore to test.
|
|
* @param time Maximum number of ticks to wait.
|
|
*
|
|
* @return RC_OK, RC_FAIL, RC_TIME on success, failure, timeout respectively
|
|
*/
|
|
|
|
int _task_sem_take(ksem_t sema, int32_t time)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = WAITSREQ;
|
|
A.Time.ticks = time;
|
|
A.Args.s1.sema = sema;
|
|
KERNEL_ENTRY(&A);
|
|
return A.Time.rcode;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Test multiple semaphores
|
|
*
|
|
* This routine tests a group of semaphores. A semaphore group is an array of
|
|
* semaphore names terminated by the predefined constant ENDLIST.
|
|
*
|
|
* It returns the ID of the first semaphore in the group whose signal count is
|
|
* greater than zero, and decrements the signal count.
|
|
*
|
|
* @param group Group of semaphores to test.
|
|
* @param time Maximum number of ticks to wait.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
ksem_t _task_sem_group_take(ksemg_t group, int32_t time)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = WAITMANY;
|
|
A.Prio = _k_current_task->Prio;
|
|
A.Time.ticks = time;
|
|
A.Args.s1.list = group;
|
|
KERNEL_ENTRY(&A);
|
|
return A.Args.s1.sema;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore signal request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_signal(struct k_args *A)
|
|
{
|
|
uint32_t Sid = A->Args.s1.sema;
|
|
|
|
signal_semaphore(1, _k_sem_list + OBJ_INDEX(Sid));
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle signal semaphore group request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_signal(struct k_args *A)
|
|
{
|
|
ksem_t *L = A->Args.s1.list;
|
|
|
|
while ((A->Args.s1.sema = *L++) != ENDLIST) {
|
|
_k_sem_signal(A);
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Signal a semaphore
|
|
*
|
|
* This routine signals the specified semaphore.
|
|
*
|
|
* @param sema Semaphore to signal.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void task_sem_give(ksem_t sema)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = SIGNALS;
|
|
A.Args.s1.sema = sema;
|
|
KERNEL_ENTRY(&A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Signal a group of semaphores
|
|
*
|
|
* This routine signals a group of semaphores. A semaphore group is an array of
|
|
* semaphore names terminated by the predefined constant ENDLIST.
|
|
*
|
|
* If the semaphore list of waiting tasks is empty, the signal count is
|
|
* incremented, otherwise the highest priority waiting task is released.
|
|
*
|
|
* Using task_sem_group_give() is faster than using multiple single signals,
|
|
* and ensures all signals take place before other tasks run.
|
|
*
|
|
* @param group Group of semaphores to signal.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void task_sem_group_give(ksemg_t group)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = SIGNALM;
|
|
A.Args.s1.list = group;
|
|
KERNEL_ENTRY(&A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Signal a semaphore from a fiber
|
|
*
|
|
* This routine (to only be called from a fiber) signals a semaphore. It
|
|
* requires a statically allocated command packet (from a command packet set)
|
|
* that is implicitly released once the command packet has been processed.
|
|
* To signal a semaphore from a task, task_sem_give() should be used instead.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
FUNC_ALIAS(isr_sem_give, fiber_sem_give, void);
|
|
|
|
/**
|
|
*
|
|
* @brief Signal a semaphore from an ISR
|
|
*
|
|
* This routine (to only be called from an ISR) signals a semaphore. It
|
|
* requires a statically allocated command packet (from a command packet set)
|
|
* that is implicitly released once the command packet has been processed.
|
|
* To signal a semaphore from a task, task_sem_give() should be used instead.
|
|
*
|
|
* @param sema Semaphore to signal.
|
|
* @param pSet Pointer to command packet set.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void isr_sem_give(ksem_t sema, struct cmd_pkt_set *pSet)
|
|
{
|
|
struct k_args *pCommand; /* ptr to command packet */
|
|
|
|
/*
|
|
* The cmdPkt_t data structure was designed to work seamlessly with the
|
|
* struct k_args data structure and it is thus safe (and expected) to typecast
|
|
* the return value of _cmd_pkt_get() to "struct k_args *".
|
|
*/
|
|
|
|
pCommand = (struct k_args *)_cmd_pkt_get(pSet);
|
|
pCommand->Comm = SIGNALS;
|
|
pCommand->Args.s1.sema = sema;
|
|
|
|
nano_isr_stack_push(&_k_command_stack, (uint32_t)pCommand);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore reset request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_reset(struct k_args *A)
|
|
{
|
|
uint32_t Sid = A->Args.s1.sema;
|
|
|
|
_k_sem_list[OBJ_INDEX(Sid)].Level = 0;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore group reset request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_group_reset(struct k_args *A)
|
|
{
|
|
ksem_t *L = A->Args.s1.list;
|
|
|
|
while ((A->Args.s1.sema = *L++) != ENDLIST) {
|
|
_k_sem_reset(A);
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Reset semaphore count to zero
|
|
*
|
|
* This routine resets the signal count of the specified semaphore to zero.
|
|
*
|
|
* @param sema Semaphore to reset.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void task_sem_reset(ksem_t sema)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = RESETS;
|
|
A.Args.s1.sema = sema;
|
|
KERNEL_ENTRY(&A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Reset a group of semaphores
|
|
*
|
|
* This routine resets the signal count for a group of semaphores. A semaphore
|
|
* group is an array of semaphore names terminated by the predefined constant
|
|
* ENDLIST.
|
|
*
|
|
* @param group Group of semaphores to reset.
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void task_sem_group_reset(ksemg_t group)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = RESETM;
|
|
A.Args.s1.list = group;
|
|
KERNEL_ENTRY(&A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Handle semaphore inquiry request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
|
|
void _k_sem_inquiry(struct k_args *A)
|
|
{
|
|
struct sem_struct *S;
|
|
uint32_t Sid;
|
|
|
|
Sid = A->Args.s1.sema;
|
|
S = _k_sem_list + OBJ_INDEX(Sid);
|
|
A->Time.rcode = S->Level;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Read the semaphore signal count
|
|
*
|
|
* This routine reads the signal count of the specified semaphore.
|
|
*
|
|
* @param sema Semaphore to query.
|
|
*
|
|
* @return signal count
|
|
*/
|
|
|
|
int task_sem_count_get(ksem_t sema)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = INQSEMA;
|
|
A.Args.s1.sema = sema;
|
|
KERNEL_ENTRY(&A);
|
|
return A.Time.rcode;
|
|
}
|