diff --git a/include/nuttx/sched.h b/include/nuttx/sched.h index ecf1e1d87f5..01302dcc7ac 100644 --- a/include/nuttx/sched.h +++ b/include/nuttx/sched.h @@ -605,11 +605,11 @@ struct tcb_s #ifdef CONFIG_PRIORITY_INHERITANCE #if CONFIG_SEM_NNESTPRIO > 0 - uint8_t nsem_held; /* used to condition priority restore */ uint8_t npend_reprio; /* Number of nested reprioritizations */ 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 uint8_t task_state; /* Current state of the thread */ diff --git a/include/nuttx/semaphore.h b/include/nuttx/semaphore.h index 42bd308f658..32d93cf51a8 100644 --- a/include/nuttx/semaphore.h +++ b/include/nuttx/semaphore.h @@ -36,6 +36,21 @@ * Pre-processor Definitions ****************************************************************************/ +/* Initializers */ + +#ifdef CONFIG_PRIORITY_INHERITANCE +# if CONFIG_SEM_PREALLOCHOLDERS > 0 +# define NXSEM_INITIALIZER(c, f) \ + {(c), (f), NULL} /* semcount, flags, hhead */ +# else +# define NXSEM_INITIALIZER(c, f) \ + {(c), (f), {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}} /* semcount, flags, holder[2] */ +# endif +#else /* CONFIG_PRIORITY_INHERITANCE */ +# define NXSEM_INITIALIZER(c, f) \ + {(c)} /* semcount, flags */ +#endif /* CONFIG_PRIORITY_INHERITANCE */ + /* Most internal nxsem_* interfaces are not available in the user space in * PROTECTED and KERNEL builds. In that context, the application semaphore * interfaces must be used. The differences between the two sets of 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 11eee68252d..dd5aa7ca16d 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; } @@ -122,8 +129,8 @@ static inline FAR struct semholder_s *nxsem_allocholder(sem_t *sem) * ****************************************************************************/ -static FAR struct semholder_s *nxsem_findholder(sem_t *sem, - FAR struct tcb_s *htcb) +static FAR struct semholder_s * +nxsem_findholder(FAR sem_t *sem, FAR struct tcb_s *htcb) { FAR struct semholder_s *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 */ @@ -169,12 +175,12 @@ static FAR struct semholder_s *nxsem_findholder(sem_t *sem, ****************************************************************************/ static inline FAR struct semholder_s * -nxsem_findorallocateholder(sem_t *sem, FAR struct tcb_s *htcb) +nxsem_findorallocateholder(FAR 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,64 +190,70 @@ 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 */ - - for (prev = NULL, curr = sem->hhead; - curr && curr != pholder; - prev = curr, curr = curr->flink); + /* Remove the holder from the semaphore's list */ - if (curr != NULL) + for (curr = &sem->hhead; + *curr != NULL; + curr = &(*curr)->flink) { - /* Remove the holder from the list */ - - if (prev != NULL) - { - prev->flink = pholder->flink; - } - else + if (*curr == pholder) { - sem->hhead = pholder->flink; + *curr = pholder->flink; + break; } + } - /* And put it in the free list */ + /* And put it in the free list */ - pholder->flink = g_freeholders; - g_freeholders = pholder; - } + pholder->flink = g_freeholders; + g_freeholders = pholder; #endif } /**************************************************************************** - * Name: nxsem_findandfreeholder + * Name: nxsem_freecount0holder ****************************************************************************/ -static inline void nxsem_findandfreeholder(sem_t *sem, - FAR struct tcb_s *htcb) +static int nxsem_freecount0holder(FAR struct semholder_s *pholder, + FAR sem_t *sem, FAR void *arg) { - FAR struct semholder_s *pholder = nxsem_findholder(sem, htcb); - /* When no more counts are held, remove the holder from the list. The * count was decremented in nxsem_release_holder. */ - if (pholder != NULL && pholder->counts <= 0) + if (pholder->counts <= 0) { nxsem_freeholder(sem, pholder); + return 1; } + + return 0; } /**************************************************************************** @@ -263,23 +275,18 @@ static int nxsem_foreachholder(FAR sem_t *sem, holderhandler_t handler, next = pholder->flink; - /* Check if there is a handler... there should always be one - * in this configuration. - */ + DEBUGASSERT(pholder->htcb != NULL); - if (pholder->htcb != NULL) - { - /* Call the handler */ + /* Call the handler */ - ret = handler(pholder, sem, arg); - } + ret = handler(pholder, sem, arg); } #else int i; /* 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 +308,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,94 +325,12 @@ 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 the new priority is greater than the current, possibly already - * boosted priority of the holder thread, then we will have to raise - * the holder's priority now. - */ - - if (rtcb->sched_priority > htcb->sched_priority) - { - /* If the current priority of holder thread has already been - * boosted, then add the boost priority to the list of restoration - * priorities. When the higher priority waiter thread gets its - * count, then we need to revert the holder thread to this saved - * priority (not to its base priority). - */ - - if (htcb->sched_priority > htcb->base_priority) - { - /* Save the current, boosted priority of the holder thread. */ - - if (htcb->npend_reprio < CONFIG_SEM_NNESTPRIO) - { - htcb->pend_reprios[htcb->npend_reprio] = - htcb->sched_priority; - htcb->npend_reprio++; - } - else - { - serr("ERROR: CONFIG_SEM_NNESTPRIO exceeded\n"); - DEBUGASSERT(htcb->npend_reprio < CONFIG_SEM_NNESTPRIO); - } - } - - /* Raise the priority of the thread holding of the semaphore. - * This cannot cause a context switch because we have preemption - * disabled. The holder thread may be marked "pending" and the - * switch may occur during up_block_task() processing. - */ - - nxsched_set_priority(htcb, rtcb->sched_priority); - } - else - { - /* The new priority is above the base priority of the holder, - * but not as high as its current working priority. Just put it - * in the list of pending restoration priorities so that when the - * higher priority thread gets its count, we can revert to this - * saved priority and not to the base priority. - */ - - if (htcb->npend_reprio < CONFIG_SEM_NNESTPRIO) - { - htcb->pend_reprios[htcb->npend_reprio] = rtcb->sched_priority; - htcb->npend_reprio++; - } - else - { - serr("ERROR: CONFIG_SEM_NNESTPRIO exceeded\n"); - DEBUGASSERT(htcb->npend_reprio < CONFIG_SEM_NNESTPRIO); - } - } - } - -#else /* If the priority of the thread that is waiting for a count is less than * or equal to the priority of the thread holding a count, then do nothing * 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 @@ -417,7 +340,6 @@ static int nxsem_boostholderprio(FAR struct semholder_s *pholder, nxsched_set_priority(htcb, rtcb->sched_priority); } -#endif return 0; } @@ -442,11 +364,9 @@ static int nxsem_verifyholder(FAR struct semholder_s *pholder, * In this case, the priority of the holder should not be boosted. */ -#if CONFIG_SEM_NNESTPRIO > 0 - DEBUGASSERT(htcb->npend_reprio == 0); -#endif DEBUGASSERT(htcb->sched_priority == htcb->base_priority); #endif + return 0; } #endif @@ -460,11 +380,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 @@ -473,183 +395,57 @@ static int nxsem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, * Name: nxsem_restoreholderprio ****************************************************************************/ -static int nxsem_restoreholderprio(FAR struct tcb_s *htcb, +static int nxsem_restoreholderprio(FAR struct semholder_s *pholder, 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; + FAR struct tcb_s *htcb = pholder->htcb; + FAR struct tcb_s *wtcb; int rpriority; - int i; - 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. + /* Release the holder if all counts have been given up + * before reprioritizing causes a context switch. */ - pholder = nxsem_findholder(sem, htcb); - - if (!nxsched_verify_tcb(htcb)) + if (pholder->counts <= 0) { - swarn("WARNING: TCB %p is a stale handle, counts lost\n", htcb); - - if (pholder != NULL) - { - nxsem_freeholder(sem, pholder); - } + 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 - if (htcb->nsem_held == 0 && (pholder == NULL || pholder->counts == 0)) - { - /* if the holder does hold any other semaphores and the running - * thread is no longer a holder on this semaphre it can return to - * it's base priority . - * - * In the case of CONFIG_SEM_PREALLOCHOLDERS = 0 It is no longer a - * holder if it is not in the holders array. - * - * In the case of CONFIG_SEM_PREALLOCHOLDERS > 0 it is no longer - * a holder if it's count is 0. - */ - - htcb->npend_reprio = 0; - } - - /* Are there other, pending priority levels to revert to? */ - - if (htcb->npend_reprio < 1) - { - /* No... the holder thread has only been boosted once. - * OR it is no longer holding a count. - * - * npend_reprio should be 0 and the boosted priority should be the - * priority of the task that just got the semaphore - * (stcb->sched_priority) - * - * That latter assumption may not be true if the stcb's priority - * was also boosted so that it no longer matches the htcb's - * sched_priority. Or if CONFIG_SEM_NNESTPRIO is too small (so - * that we do not have a proper record of the reprioritizations). - */ - - DEBUGASSERT(/* htcb->sched_priority == stcb->sched_priority && */ - htcb->npend_reprio == 0); + rpriority = htcb->base_priority; - /* Reset the holder's priority back to the base priority. */ - - nxsched_reprioritize(htcb, htcb->base_priority); - } - - /* There are multiple pending priority levels. The holder thread's - * "boosted" priority could greater than or equal to - * "stcb->sched_priority" (it could be greater if its priority was - * boosted because it also holds another semaphore). + /* Try to find the highest priority across the tasks that are waiting + * for all semaphores that htcb holds */ - else if (htcb->sched_priority <= stcb->sched_priority) + for (wtcb = (FAR struct tcb_s *)g_waitingforsemaphore.head; + wtcb != NULL; wtcb = wtcb->flink) { - /* The holder thread has been boosted to the same priority as the - * waiter thread that just received the count. We will simply - * reprioritize to the next highest priority that we have in - * rpriority. - */ - - /* Find the highest pending priority and remove it from the list */ - - for (i = 1, j = 0; i < htcb->npend_reprio; i++) + if ((wtcb->waitsem->flags & PRIOINHERIT_FLAGS_DISABLE) == 0) { - if (htcb->pend_reprios[i] > htcb->pend_reprios[j]) + for (pholder = htcb->holdsem; pholder != NULL; + pholder = pholder->tlink) { - j = i; - } - } - - /* Remove the highest priority pending priority from the list */ - - rpriority = htcb->pend_reprios[j]; - i = htcb->npend_reprio - 1; - if (i > 0) - { - htcb->pend_reprios[j] = htcb->pend_reprios[i]; - } - - htcb->npend_reprio = i; - - /* And apply that priority to the thread (while retaining the - * base_priority) - */ - - nxsched_set_priority(htcb, rpriority); - } - else - { - /* The holder thread has been boosted to a higher priority than the - * waiter task. The pending priority should be in the list (unless - * it was lost because of list overflow or because the holder - * was reprioritized again unbeknownst to the priority inheritance - * logic). - * - * Search the list for the matching priority. - */ - - for (i = 0; i < htcb->npend_reprio; i++) - { - /* Does this pending priority match the priority of the thread - * that just received the count? - */ - - if (htcb->pend_reprios[i] == stcb->sched_priority) - { - /* Yes, remove it from the list */ - - j = htcb->npend_reprio - 1; - if (j > 0) + if (pholder->counts > 0 && pholder->sem == wtcb->waitsem && + wtcb->sched_priority > rpriority) { - htcb->pend_reprios[i] = htcb->pend_reprios[j]; + rpriority = wtcb->sched_priority; } - - htcb->npend_reprio = j; - break; } } } -#else - /* There is no alternative restore priorities, drop the priority - * of the holder thread all the way back to the threads "base" - * priority. - */ - nxsched_reprioritize(htcb, htcb->base_priority); -#endif + nxsched_set_priority(htcb, rpriority); } return 0; } -/**************************************************************************** - * Name: nxsem_restoreholderprioall - * - * Description: - * Reprioritize all holders - * - ****************************************************************************/ - -static int nxsem_restoreholderprioall(FAR struct semholder_s *pholder, - FAR sem_t *sem, FAR void *arg) -{ - return nxsem_restoreholderprio(pholder->htcb, sem, arg); -} - /**************************************************************************** * Name: nxsem_restoreholderprio_others * @@ -664,7 +460,7 @@ static int nxsem_restoreholderprio_others(FAR struct semholder_s *pholder, FAR struct tcb_s *rtcb = this_task(); if (pholder->htcb != rtcb) { - return nxsem_restoreholderprio(pholder->htcb, sem, arg); + return nxsem_restoreholderprio(pholder, sem, arg); } return 0; @@ -687,16 +483,7 @@ static int nxsem_restoreholderprio_self(FAR struct semholder_s *pholder, { /* The running task has given up a count on the semaphore */ -#if CONFIG_SEM_PREALLOCHOLDERS == 0 - /* In the case where there are only 2 holders. This step - * is necessary to ensure we have space. Release the holder - * if all counts have been given up before reprioritizing - * causes a context switch. - */ - - nxsem_findandfreeholder(sem, rtcb); -#endif - nxsem_restoreholderprio(rtcb, sem, arg); + nxsem_restoreholderprio(pholder, sem, arg); return 1; } @@ -738,30 +525,9 @@ static int nxsem_restoreholderprio_self(FAR struct semholder_s *pholder, static inline void nxsem_restore_baseprio_irq(FAR struct tcb_s *stcb, FAR sem_t *sem) { - /* Perform the following actions only if a new thread was given a count. - * The thread that received the count should be the highest priority - * of all threads waiting for a count from the semaphore. So in that - * case, the priority of all holder threads should be dropped to the - * next highest pending priority. - */ - - if (stcb != NULL) - { - /* Drop the priority of all holder threads */ - - nxsem_foreachholder(sem, nxsem_restoreholderprioall, stcb); - } + /* Drop the priority of all holder threads */ - /* If there are no tasks waiting for available counts, then all holders - * should be at their base priority. - */ - -#ifdef CONFIG_DEBUG_ASSERTIONS - else - { - nxsem_foreachholder(sem, nxsem_verifyholder, NULL); - } -#endif + nxsem_foreachholder(sem, nxsem_restoreholderprio, stcb); } /**************************************************************************** @@ -799,50 +565,20 @@ static inline void nxsem_restore_baseprio_irq(FAR struct tcb_s *stcb, static inline void nxsem_restore_baseprio_task(FAR struct tcb_s *stcb, FAR sem_t *sem) { - FAR struct tcb_s *rtcb = this_task(); - - /* Perform the following actions only if a new thread was given a count. - * The thread that received the count should be the highest priority - * of all threads waiting for a count from the semaphore. So in that - * case, the priority of all holder threads should be dropped to the - * next highest pending priority. + /* The currently executed thread should be the lower priority + * thread that just posted the count and caused this action. + * However, we cannot drop the priority of the currently running + * thread -- because that will cause it to be suspended. + * + * So, do this in two passes. First, reprioritizing all holders + * except for the running thread. */ - if (stcb != NULL) - { - /* The currently executed thread should be the lower priority - * thread that just posted the count and caused this action. - * However, we cannot drop the priority of the currently running - * thread -- because that will cause it to be suspended. - * - * So, do this in two passes. First, reprioritizing all holders - * except for the running thread. - */ - - nxsem_foreachholder(sem, nxsem_restoreholderprio_others, stcb); + nxsem_foreachholder(sem, nxsem_restoreholderprio_others, stcb); - /* Now, find an reprioritize only the ready to run task */ + /* Now, find an reprioritize only the ready to run task */ - nxsem_foreachholder(sem, nxsem_restoreholderprio_self, stcb); - } - - /* If there are no tasks waiting for available counts, then all holders - * should be at their base priority. - */ - -#ifdef CONFIG_DEBUG_ASSERTIONS - else - { - nxsem_foreachholder(sem, nxsem_verifyholder, NULL); - } -#endif - - /* In any case, the currently executing task should have an entry in the - * list. Its counts were previously decremented; if it now holds no - * counts, then we need to remove it from the list of holders. - */ - - nxsem_findandfreeholder(sem, rtcb); + nxsem_foreachholder(sem, nxsem_restoreholderprio_self, stcb); } /**************************************************************************** @@ -925,7 +661,6 @@ void nxsem_destroyholder(FAR sem_t *sem) */ DEBUGASSERT(sem->hhead->flink == NULL); - nxsem_foreachholder(sem, nxsem_recoverholders, NULL); } #else @@ -933,9 +668,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); } /**************************************************************************** @@ -971,23 +706,10 @@ void nxsem_add_holder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem) /* Find or allocate a container for this new holder */ pholder = nxsem_findorallocateholder(sem, htcb); - if (pholder != NULL) + if (pholder != NULL && pholder->counts < SEM_VALUE_MAX) { - /* 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; -#if CONFIG_SEM_NNESTPRIO > 0 - if (pholder->counts == 0) - { - /* This thread/task now holds a count on this semaphore - * so add it to the total held. - */ - - htcb->nsem_held++; - } -#endif pholder->counts++; } } @@ -1064,29 +786,60 @@ void nxsem_release_holder(FAR sem_t *sem) { FAR struct tcb_s *rtcb = this_task(); FAR struct semholder_s *pholder; + FAR struct semholder_s *candidate = NULL; + unsigned int total = 0; /* Find the container for this holder */ - pholder = nxsem_findholder(sem, rtcb); - if (pholder != NULL && pholder->counts > 0) +#if CONFIG_SEM_PREALLOCHOLDERS > 0 + for (pholder = sem->hhead; pholder; pholder = pholder->flink) +#else + int i; + + /* We have two hard-allocated holder structures in sem_t */ + + for (i = 0; i < 2; i++) +#endif { - /* Decrement the counts on this holder -- the holder will be freed - * later in nxsem_restore_baseprio. - */ +#if CONFIG_SEM_PREALLOCHOLDERS == 0 + pholder = &sem->holder[i]; + if (pholder->htcb == NULL) + { + continue; + } +#endif - pholder->counts--; -#if CONFIG_SEM_NNESTPRIO > 0 - if ((sem->flags & PRIOINHERIT_FLAGS_DISABLE) == 0 && - pholder->counts == 0) + DEBUGASSERT(pholder->counts > 0); + + if (pholder->htcb == rtcb) { - /* This thread/task holds no counts on this semaphore - * So remove it from the total held + /* Decrement the counts on this holder -- the holder will be freed + * later in nxsem_restore_baseprio. */ - rtcb->nsem_held--; + pholder->counts--; + return; } -#endif + + total++; + candidate = pholder; + } + + /* The current task is not a holder */ + + if (total == 1) + { + /* If the sempahore has only one holder, we can decrement the counts + * simply. + */ + + candidate->counts--; + return; } + + /* TODO: + * How do we choose the holder to decrement it's counts? + */ } /**************************************************************************** @@ -1131,21 +884,45 @@ void nxsem_restore_baseprio(FAR struct tcb_s *stcb, FAR sem_t *sem) (sem->semcount <= 0 && stcb != NULL)); #endif - /* Handler semaphore counts posed from an interrupt handler differently - * from interrupts posted from threads. The primary difference is that - * if the semaphore is posted from a thread, then the poster thread is - * a player in the priority inheritance scheme. The interrupt handler - * externally injects the new count without otherwise participating - * itself. + /* Perform the following actions only if a new thread was given a count. + * The thread that received the count should be the highest priority + * of all threads waiting for a count from the semaphore. So in that + * case, the priority of all holder threads should be dropped to the + * next highest pending priority. */ - if (up_interrupt_context()) + if (stcb != NULL) { - nxsem_restore_baseprio_irq(stcb, sem); + /* Handler semaphore counts posed from an interrupt handler differently + * from interrupts posted from threads. The primary difference is that + * if the semaphore is posted from a thread, then the poster thread is + * a player in the priority inheritance scheme. The interrupt handler + * externally injects the new count without otherwise participating + * itself. + */ + + if (up_interrupt_context()) + { + nxsem_restore_baseprio_irq(stcb, sem); + } + else + { + nxsem_restore_baseprio_task(stcb, sem); + } } else { - nxsem_restore_baseprio_task(stcb, sem); + /* Remove the holder from the list if it's counts is zero. */ + + nxsem_foreachholder(sem, nxsem_freecount0holder, NULL); + + /* If there are no tasks waiting for available counts, then all holders + * should be at their base priority. + */ + +#ifdef CONFIG_DEBUG_ASSERTIONS + nxsem_foreachholder(sem, nxsem_verifyholder, NULL); +#endif } } @@ -1176,7 +953,7 @@ void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem) /* Adjust the priority of every holder as necessary */ - nxsem_foreachholder(sem, nxsem_restoreholderprioall, stcb); + nxsem_foreachholder(sem, nxsem_restoreholderprio, stcb); } /**************************************************************************** @@ -1240,4 +1017,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