diff --git a/include/nuttx/sched.h b/include/nuttx/sched.h index e784a428ea5..9c15efaf603 100644 --- a/include/nuttx/sched.h +++ b/include/nuttx/sched.h @@ -610,6 +610,7 @@ struct tcb_s uint8_t pend_reprios[CONFIG_SEM_NNESTPRIO]; #endif uint8_t base_priority; /* "Normal" priority of the thread */ + FAR struct semholder_s *holdsem; /* List of held semaphores */ #endif #ifdef CONFIG_SMP diff --git a/include/nuttx/semaphore.h b/include/nuttx/semaphore.h index c9e488c3fdb..4f24f31761c 100644 --- a/include/nuttx/semaphore.h +++ b/include/nuttx/semaphore.h @@ -44,7 +44,7 @@ {(c), (f), NULL} /* semcount, flags, hhead */ # else # define NXSEM_INITIALIZER(c, f) \ - {(c), (f), {{NULL, 0}, {NULL, 0}}} /* semcount, flags, holder[2] */ + {(c), (f), {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}} /* semcount, flags, holder[2] */ # endif #else /* CONFIG_PRIORITY_INHERITANCE */ # define NXSEM_INITIALIZER(c, f) \ diff --git a/include/semaphore.h b/include/semaphore.h index 8c427be406b..6d377b1b874 100644 --- a/include/semaphore.h +++ b/include/semaphore.h @@ -58,19 +58,38 @@ #ifdef CONFIG_PRIORITY_INHERITANCE struct tcb_s; /* Forward reference */ +struct sem_s; + struct semholder_s { #if CONFIG_SEM_PREALLOCHOLDERS > 0 - struct semholder_s *flink; /* Implements singly linked list */ + FAR struct semholder_s *flink; /* List of semaphore's holder */ #endif - FAR struct tcb_s *htcb; /* Holder TCB */ - int16_t counts; /* Number of counts owned by this holder */ + FAR struct semholder_s *tlink; /* List of task held semaphores */ + FAR struct sem_s *sem; /* Ths corresponding semaphore */ + FAR struct tcb_s *htcb; /* Ths corresponding TCB */ + int16_t counts; /* Number of counts owned by this holder */ }; #if CONFIG_SEM_PREALLOCHOLDERS > 0 -# define SEMHOLDER_INITIALIZER {NULL, NULL, 0} +# define SEMHOLDER_INITIALIZER {NULL, NULL, NULL, NULL, 0} +# define INITIALIZE_SEMHOLDER(h) \ + do { \ + (h)->flink = NULL; \ + (h)->tlink = NULL; \ + (h)->sem = NULL; \ + (h)->htcb = NULL; \ + (h)->counts = 0; \ + } while (0) #else -# define SEMHOLDER_INITIALIZER {NULL, 0} +# define SEMHOLDER_INITIALIZER {NULL, NULL, NULL, 0} +# define INITIALIZE_SEMHOLDER(h) \ + do { \ + (h)->tlink = NULL; \ + (h)->sem = NULL; \ + (h)->htcb = NULL; \ + (h)->counts = 0; \ + } while (0) #endif #endif /* CONFIG_PRIORITY_INHERITANCE */ diff --git a/libs/libc/semaphore/sem_init.c b/libs/libc/semaphore/sem_init.c index fc8e0ed9bc9..6243e12333a 100644 --- a/libs/libc/semaphore/sem_init.c +++ b/libs/libc/semaphore/sem_init.c @@ -78,10 +78,8 @@ int nxsem_init(FAR sem_t *sem, int pshared, unsigned int value) # if CONFIG_SEM_PREALLOCHOLDERS > 0 sem->hhead = NULL; # else - sem->holder[0].htcb = NULL; - sem->holder[0].counts = 0; - sem->holder[1].htcb = NULL; - sem->holder[1].counts = 0; + INITIALIZE_SEMHOLDER(&sem->holder[0]); + INITIALIZE_SEMHOLDER(&sem->holder[1]); # endif #endif return OK; diff --git a/sched/semaphore/sem_holder.c b/sched/semaphore/sem_holder.c index 1ee4906dedb..6ed278518af 100644 --- a/sched/semaphore/sem_holder.c +++ b/sched/semaphore/sem_holder.c @@ -66,7 +66,8 @@ static FAR struct semholder_s *g_freeholders; * Name: nxsem_allocholder ****************************************************************************/ -static inline FAR struct semholder_s *nxsem_allocholder(sem_t *sem) +static inline FAR struct semholder_s * +nxsem_allocholder(FAR sem_t *sem, FAR struct tcb_s *htcb) { FAR struct semholder_s *pholder; @@ -79,37 +80,43 @@ static inline FAR struct semholder_s *nxsem_allocholder(sem_t *sem) pholder = g_freeholders; if (pholder != NULL) { - /* Remove the holder from the free list an put it into the semaphore's - * holder list + /* Remove the holder from the free list and + * put it into the semaphore's holder list */ g_freeholders = pholder->flink; pholder->flink = sem->hhead; sem->hhead = pholder; - - /* Make sure the initial count is zero */ - - pholder->counts = 0; } #else if (sem->holder[0].htcb == NULL) { pholder = &sem->holder[0]; - pholder->counts = 0; } else if (sem->holder[1].htcb == NULL) { pholder = &sem->holder[1]; - pholder->counts = 0; } #endif else { serr("ERROR: Insufficient pre-allocated holders\n"); pholder = NULL; + DEBUGASSERT(0); + } + + if (pholder != NULL) + { + pholder->sem = sem; + pholder->htcb = htcb; + pholder->counts = 0; + + /* Put it into the task's list */ + + pholder->tlink = htcb->holdsem; + htcb->holdsem = pholder; } - DEBUGASSERT(pholder != NULL); return pholder; } @@ -143,7 +150,6 @@ static FAR struct semholder_s *nxsem_findholder(sem_t *sem, } #else int i; - pholder = NULL; /* We have two hard-allocated holder structures in sem_t */ @@ -174,7 +180,7 @@ nxsem_findorallocateholder(sem_t *sem, FAR struct tcb_s *htcb) FAR struct semholder_s *pholder = nxsem_findholder(sem, htcb); if (!pholder) { - pholder = nxsem_allocholder(sem); + pholder = nxsem_allocholder(sem, htcb); } return pholder; @@ -184,44 +190,49 @@ nxsem_findorallocateholder(sem_t *sem, FAR struct tcb_s *htcb) * Name: nxsem_freeholder ****************************************************************************/ -static inline void nxsem_freeholder(sem_t *sem, +static inline void nxsem_freeholder(FAR sem_t *sem, FAR struct semholder_s *pholder) { -#if CONFIG_SEM_PREALLOCHOLDERS > 0 - FAR struct semholder_s *curr; - FAR struct semholder_s *prev; -#endif + FAR struct semholder_s * FAR *curr; + + /* Remove the holder from the task's list */ + + for (curr = &pholder->htcb->holdsem; + *curr != NULL; + curr = &(*curr)->tlink) + { + if (*curr == pholder) + { + *curr = pholder->tlink; + break; + } + } /* Release the holder and counts */ + pholder->tlink = NULL; + pholder->sem = NULL; pholder->htcb = NULL; pholder->counts = 0; #if CONFIG_SEM_PREALLOCHOLDERS > 0 - /* Search the list for the matching holder */ + /* Remove the holder from the semaphore's list */ - for (prev = NULL, curr = sem->hhead; - curr && curr != pholder; - prev = curr, curr = curr->flink); - - if (curr != NULL) + for (curr = &sem->hhead; + *curr != NULL; + curr = &(*curr)->flink) { - /* Remove the holder from the list */ - - if (prev != NULL) + if (*curr == pholder) { - prev->flink = pholder->flink; + *curr = pholder->flink; + break; } - else - { - sem->hhead = pholder->flink; - } - - /* And put it in the free list */ - - pholder->flink = g_freeholders; - g_freeholders = pholder; } + + /* And put it in the free list */ + + pholder->flink = g_freeholders; + g_freeholders = pholder; #endif } @@ -279,7 +290,7 @@ static int nxsem_foreachholder(FAR sem_t *sem, holderhandler_t handler, /* We have two hard-allocated holder structures in sem_t */ - for (i = 0; i < 2; i++) + for (i = 0; i < 2 && ret == 0; i++) { pholder = &sem->holder[i]; @@ -301,14 +312,12 @@ static int nxsem_foreachholder(FAR sem_t *sem, holderhandler_t handler, * Name: nxsem_recoverholders ****************************************************************************/ -#if CONFIG_SEM_PREALLOCHOLDERS > 0 static int nxsem_recoverholders(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { nxsem_freeholder(sem, pholder); return 0; } -#endif /**************************************************************************** * Name: nxsem_boostholderprio @@ -320,25 +329,13 @@ static int nxsem_boostholderprio(FAR struct semholder_s *pholder, FAR struct tcb_s *htcb = (FAR struct tcb_s *)pholder->htcb; FAR struct tcb_s *rtcb = (FAR struct tcb_s *)arg; - /* Make sure that the holder thread is still active. If it exited without - * releasing its counts, then that would be a bad thing. But we can take - * no real action because we don't know what the program is doing. - * Perhaps its plan is to kill a thread, then destroy the semaphore. - */ - - if (!nxsched_verify_tcb(htcb)) - { - swarn("WARNING: TCB %p is a stale handle, counts lost\n", htcb); - nxsem_freeholder(sem, pholder); - } - #if CONFIG_SEM_NNESTPRIO > 0 /* If the priority of the thread that is waiting for a count is greater * than the base priority of the thread holding a count, then we may need * to adjust the holder's priority now or later to that priority. */ - else if (rtcb->sched_priority > htcb->base_priority) + if (rtcb->sched_priority > htcb->base_priority) { /* If the new priority is greater than the current, possibly already * boosted priority of the holder thread, then we will have to raise @@ -407,7 +404,7 @@ static int nxsem_boostholderprio(FAR struct semholder_s *pholder, * because the thread is already running at a sufficient priority. */ - else if (rtcb->sched_priority > htcb->sched_priority) + if (rtcb->sched_priority > htcb->sched_priority) { /* Raise the priority of the holder of the semaphore. This * cannot cause a context switch because we have preemption @@ -447,6 +444,7 @@ static int nxsem_verifyholder(FAR struct semholder_s *pholder, #endif DEBUGASSERT(htcb->sched_priority == htcb->base_priority); #endif + return 0; } #endif @@ -460,11 +458,13 @@ static int nxsem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { #if CONFIG_SEM_PREALLOCHOLDERS > 0 - _info(" %08x: %08x %08x %04x\n", - pholder, pholder->flink, pholder->htcb, pholder->counts); + _info(" %08x: %08x %08x %08x %08x %04x\n", + pholder, pholder->flink, #else - _info(" %08x: %08x %04x\n", pholder, pholder->htcb, pholder->counts); + _info(" %08x: %08x %08x %08x %04x\n", + pholder, #endif + pholder->tlink, pholder->sem, pholder->htcb, pholder->counts); return 0; } #endif @@ -476,7 +476,6 @@ static int nxsem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, static int nxsem_restoreholderprio(FAR struct tcb_s *htcb, FAR sem_t *sem, FAR void *arg) { - FAR struct semholder_s *pholder = 0; #if CONFIG_SEM_NNESTPRIO > 0 FAR struct tcb_s *stcb = (FAR struct tcb_s *)arg; int rpriority; @@ -484,28 +483,11 @@ static int nxsem_restoreholderprio(FAR struct tcb_s *htcb, int j; #endif - /* Make sure that the holder thread is still active. If it exited without - * releasing its counts, then that would be a bad thing. But we can take - * no real action because we don't know what the program is doing. - * Perhaps its plan is to kill a thread, then destroy the semaphore. - */ - - if (!nxsched_verify_tcb(htcb)) - { - swarn("WARNING: TCB %p is a stale handle, counts lost\n", htcb); - - pholder = nxsem_findholder(sem, htcb); - if (pholder != NULL) - { - nxsem_freeholder(sem, pholder); - } - } - /* Was the priority of the holder thread boosted? If so, then drop its * priority back to the correct level. What is the correct level? */ - else if (htcb->sched_priority != htcb->base_priority) + if (htcb->sched_priority != htcb->base_priority) { #if CONFIG_SEM_NNESTPRIO > 0 /* Are there other, pending priority levels to revert to? */ @@ -906,7 +888,6 @@ void nxsem_destroyholder(FAR sem_t *sem) */ DEBUGASSERT(sem->hhead->flink == NULL); - nxsem_foreachholder(sem, nxsem_recoverholders, NULL); } #else @@ -914,9 +895,9 @@ void nxsem_destroyholder(FAR sem_t *sem) DEBUGASSERT(sem->holder[0].htcb == NULL || sem->holder[1].htcb == NULL); - sem->holder[0].htcb = NULL; - sem->holder[1].htcb = NULL; #endif + + nxsem_foreachholder(sem, nxsem_recoverholders, NULL); } /**************************************************************************** @@ -955,11 +936,8 @@ void nxsem_add_holder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem) pholder = nxsem_findorallocateholder(sem, htcb); if (pholder != NULL) { - /* Then set the holder and increment the number of counts held by - * this holder - */ + /* Increment the number of counts held by this holder */ - pholder->htcb = htcb; pholder->counts++; } } @@ -1201,4 +1179,32 @@ int nxsem_nfreeholders(void) } #endif +/**************************************************************************** + * Name: nxsem_release_all + * + * Description: + * Release all semaphore holders for the task. + * + * Input Parameters: + * htcb - TCB of the task + * + * Returned Value: + * None + * + * Assumptions: + * + ****************************************************************************/ + +void nxsem_release_all(FAR struct tcb_s *htcb) +{ + FAR struct semholder_s *pholder; + + while ((pholder = htcb->holdsem) != NULL) + { + FAR sem_t *sem = pholder->sem; + + nxsem_freeholder(sem, pholder); + } +} + #endif /* CONFIG_PRIORITY_INHERITANCE */ diff --git a/sched/semaphore/sem_recover.c b/sched/semaphore/sem_recover.c index 1f28f54b02b..40f297bd651 100644 --- a/sched/semaphore/sem_recover.c +++ b/sched/semaphore/sem_recover.c @@ -108,5 +108,9 @@ void nxsem_recover(FAR struct tcb_s *tcb) tcb->waitsem = NULL; } + /* Release all semphore holders for the task */ + + nxsem_release_all(tcb); + leave_critical_section(flags); } diff --git a/sched/semaphore/semaphore.h b/sched/semaphore/semaphore.h index 28995e1c4b1..71578176ab1 100644 --- a/sched/semaphore/semaphore.h +++ b/sched/semaphore/semaphore.h @@ -79,6 +79,7 @@ void nxsem_boost_priority(FAR sem_t *sem); void nxsem_release_holder(FAR sem_t *sem); void nxsem_restore_baseprio(FAR struct tcb_s *stcb, FAR sem_t *sem); void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem); +void nxsem_release_all(FAR struct tcb_s *stcb); #else # define nxsem_initialize_holders() # define nxsem_destroyholder(sem) @@ -88,6 +89,7 @@ void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem); # define nxsem_release_holder(sem) # define nxsem_restore_baseprio(stcb,sem) # define nxsem_canceled(stcb,sem) +# define nxsem_release_all(stcb) #endif #undef EXTERN