dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/nuttx-patches/00011-BACKPORT-priority-res...

457 lines
14 KiB
Diff
Raw Blame History

diff --git NuttX/nuttx/include/semaphore.h NuttX/nuttx/include/semaphore.h
index 8821b12..0056909 100644
--- NuttX/nuttx/include/semaphore.h
+++ NuttX/nuttx/include/semaphore.h
@@ -93,7 +93,7 @@ struct sem_s
# if CONFIG_SEM_PREALLOCHOLDERS > 0
FAR struct semholder_s *hhead; /* List of holders of semaphore counts */
# else
- struct semholder_s holder; /* Single holder */
+ struct semholder_s holder[2]; /* Slot for old and new holder */
# endif
#endif
};
@@ -104,12 +104,15 @@ typedef struct sem_s sem_t;
#ifdef CONFIG_PRIORITY_INHERITANCE
# if CONFIG_SEM_PREALLOCHOLDERS > 0
-# define SEM_INITIALIZER(c) {(c), 0, NULL} /* semcount, flags, hhead */
+# define SEM_INITIALIZER(c) \
+ {(c), 0, NULL} /* semcount, flags, hhead */
# else
-# define SEM_INITIALIZER(c) {(c), 0, SEMHOLDER_INITIALIZER} /* semcount, flags, holder */
+# define SEM_INITIALIZER(c) \
+ {(c), 0, {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}} /* semcount, flags, holder[2] */
# endif
#else
-# define SEM_INITIALIZER(c) {(c)} /* semcount */
+# define SEM_INITIALIZER(c) \
+ {(c)} /* semcount */
#endif
/****************************************************************************
diff --git NuttX/nuttx/libc/semaphore/sem_init.c NuttX/nuttx/libc/semaphore/sem_init.c
index f4037d3..ec7de27 100644
--- NuttX/nuttx/libc/semaphore/sem_init.c
+++ NuttX/nuttx/libc/semaphore/sem_init.c
@@ -83,17 +83,19 @@ int sem_init(FAR sem_t *sem, int pshared, unsigned int value)
{
/* Initialize the seamphore count */
- sem->semcount = (int16_t)value;
+ sem->semcount = (int16_t)value;
/* Initialize to support priority inheritance */
#ifdef CONFIG_PRIORITY_INHERITANCE
- sem->flags = 0;
+ sem->flags = 0;
# if CONFIG_SEM_PREALLOCHOLDERS > 0
- sem->hhead = NULL;
+ sem->hhead = NULL;
# else
- sem->holder.htcb = NULL;
- sem->holder.counts = 0;
+ sem->holder[0].htcb = NULL;
+ sem->holder[0].counts = 0;
+ sem->holder[1].htcb = NULL;
+ sem->holder[1].counts = 0;
# endif
#endif
return OK;
diff --git NuttX/nuttx/sched/semaphore/sem_holder.c NuttX/nuttx/sched/semaphore/sem_holder.c
index 6b9f05a..003b0ab 100644
--- NuttX/nuttx/sched/semaphore/sem_holder.c
+++ NuttX/nuttx/sched/semaphore/sem_holder.c
@@ -93,7 +93,7 @@ static inline FAR struct semholder_s *sem_allocholder(sem_t *sem)
#if CONFIG_SEM_PREALLOCHOLDERS > 0
pholder = g_freeholders;
- if (pholder)
+ if (pholder != NULL)
{
/* Remove the holder from the free list an put it into the semaphore's
* holder list
@@ -108,18 +108,24 @@ static inline FAR struct semholder_s *sem_allocholder(sem_t *sem)
pholder->counts = 0;
}
#else
- if (!sem->holder.htcb)
+ if (sem->holder[0].htcb == NULL)
{
- pholder = &sem->holder;
+ 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;
+ pholder = NULL;
}
+ DEBUGASSERT(pholder != NULL);
return pholder;
}
@@ -132,16 +138,29 @@ static FAR struct semholder_s *sem_findholder(sem_t *sem,
{
FAR struct semholder_s *pholder;
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
/* Try to find the holder in the list of holders associated with this
* semaphore
*/
-#if CONFIG_SEM_PREALLOCHOLDERS > 0
- for (pholder = sem->hhead; pholder; pholder = pholder->flink)
+ for (pholder = sem->hhead; pholder != NULL; pholder = pholder->flink)
+ {
+ if (pholder->htcb == htcb)
+ {
+ /* Got it! */
+
+ return pholder;
+ }
+ }
#else
- pholder = &sem->holder;
-#endif
+ int i;
+ pholder = NULL;
+
+ /* We have two hard-allocated holder structures in sem_t */
+
+ for (i = 0; i < 2; i++)
{
+ pholder = &sem->holder[i];
if (pholder->htcb == htcb)
{
/* Got it! */
@@ -149,6 +168,7 @@ static FAR struct semholder_s *sem_findholder(sem_t *sem,
return pholder;
}
}
+#endif
/* The holder does not appear in the list */
@@ -194,11 +214,11 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
curr && curr != pholder;
prev = curr, curr = curr->flink);
- if (curr)
+ if (curr != NULL)
{
/* Remove the holder from the list */
- if (prev)
+ if (prev != NULL)
{
prev->flink = pholder->flink;
}
@@ -216,6 +236,24 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
}
/****************************************************************************
+ * Name: sem_findandfreeholder
+ ****************************************************************************/
+
+static inline void sem_findandfreeholder(sem_t *sem, FAR struct tcb_s *htcb)
+{
+ FAR struct semholder_s *pholder = sem_findholder(sem, htcb);
+
+ /* When no more counts are held, remove the holder from the list. The
+ * count was decremented in sem_releaseholder.
+ */
+
+ if (pholder != NULL && pholder->counts <= 0)
+ {
+ sem_freeholder(sem, pholder);
+ }
+}
+
+/****************************************************************************
* Name: sem_foreachholder
****************************************************************************/
@@ -223,31 +261,47 @@ static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler,
FAR void *arg)
{
FAR struct semholder_s *pholder;
-#if CONFIG_SEM_PREALLOCHOLDERS > 0
- FAR struct semholder_s *next;
-#endif
int ret = 0;
#if CONFIG_SEM_PREALLOCHOLDERS > 0
+ FAR struct semholder_s *next;
+
for (pholder = sem->hhead; pholder && ret == 0; pholder = next)
-#else
- pholder = &sem->holder;
-#endif
{
-#if CONFIG_SEM_PREALLOCHOLDERS > 0
/* In case this holder gets deleted */
next = pholder->flink;
-#endif
- /* The initial "built-in" container may hold a NULL holder */
- if (pholder->htcb)
+ /* Check if there is a handler... there should always be one
+ * in this configuration.
+ */
+
+ if (pholder->htcb != NULL)
{
/* Call the handler */
ret = handler(pholder, sem, arg);
}
}
+#else
+ int i;
+
+ /* We have two hard-allocated holder structures in sem_t */
+
+ for (i = 0; i < 2; i++)
+ {
+ pholder = &sem->holder[i];
+
+ /* The hard-allocated containers may hold a NULL holder */
+
+ if (pholder->htcb != NULL)
+ {
+ /* Call the handler */
+
+ ret = handler(pholder, sem, arg);
+ }
+ }
+#endif
return ret;
}
@@ -284,11 +338,11 @@ static int sem_boostholderprio(FAR struct semholder_s *pholder,
if (!sched_verifytcb(htcb))
{
serr("ERROR: TCB 0x%08x is a stale handle, counts lost\n", htcb);
+ DEBUGASSERT(sched_verifytcb(htcb));
sem_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.
@@ -322,6 +376,7 @@ static int sem_boostholderprio(FAR struct semholder_s *pholder,
else
{
serr("ERROR: CONFIG_SEM_NNESTPRIO exceeded\n");
+ DEBUGASSERT(htcb->npend_reprio < CONFIG_SEM_NNESTPRIO);
}
}
@@ -342,8 +397,16 @@ static int sem_boostholderprio(FAR struct semholder_s *pholder,
* saved priority and not to the base priority.
*/
- htcb->pend_reprios[htcb->npend_reprio] = rtcb->sched_priority;
- htcb->npend_reprio++;
+ 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);
+ }
}
}
@@ -415,10 +478,10 @@ static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem,
* Name: sem_restoreholderprio
****************************************************************************/
-static int sem_restoreholderprio(FAR struct semholder_s *pholder,
+static int sem_restoreholderprio(FAR struct tcb_s *htcb,
FAR sem_t *sem, FAR void *arg)
{
- FAR struct tcb_s *htcb = (FAR struct tcb_s *)pholder->htcb;
+ FAR struct semholder_s *pholder = 0;
#if CONFIG_SEM_NNESTPRIO > 0
FAR struct tcb_s *stcb = (FAR struct tcb_s *)arg;
int rpriority;
@@ -435,7 +498,12 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder,
if (!sched_verifytcb(htcb))
{
serr("ERROR: TCB 0x%08x is a stale handle, counts lost\n", htcb);
- sem_freeholder(sem, pholder);
+ DEBUGASSERT(sched_verifytcb(htcb));
+ pholder = sem_findholder(sem, htcb);
+ if (pholder != NULL)
+ {
+ sem_freeholder(sem, pholder);
+ }
}
/* Was the priority of the holder thread boosted? If so, then drop its
@@ -555,6 +623,20 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder,
}
/****************************************************************************
+ * Name: sem_restoreholderprioall
+ *
+ * Description:
+ * Reprioritize all holders
+ *
+ ****************************************************************************/
+
+static int sem_restoreholderprioall(FAR struct semholder_s *pholder,
+ FAR sem_t *sem, FAR void *arg)
+{
+ return sem_restoreholderprio(pholder->htcb, sem, arg);
+}
+
+/****************************************************************************
* Name: sem_restoreholderprioA
*
* Description:
@@ -568,7 +650,7 @@ static int sem_restoreholderprioA(FAR struct semholder_s *pholder,
FAR struct tcb_s *rtcb = this_task();
if (pholder->htcb != rtcb)
{
- return sem_restoreholderprio(pholder, sem, arg);
+ return sem_restoreholderprio(pholder->htcb, sem, arg);
}
return 0;
@@ -586,9 +668,22 @@ static int sem_restoreholderprioB(FAR struct semholder_s *pholder,
FAR sem_t *sem, FAR void *arg)
{
FAR struct tcb_s *rtcb = this_task();
+
if (pholder->htcb == rtcb)
{
- (void)sem_restoreholderprio(pholder, sem, arg);
+
+ /* 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 insure we have space. Release the holder
+ * if all counts have been given up. before reprioritizing
+ * causes a context switch.
+ */
+
+ sem_findandfreeholder(sem, rtcb);
+#endif
+ (void)sem_restoreholderprio(rtcb, sem, arg);
return 1;
}
@@ -637,11 +732,11 @@ static inline void sem_restorebaseprio_irq(FAR struct tcb_s *stcb,
* next highest pending priority.
*/
- if (stcb)
+ if (stcb != NULL)
{
/* Drop the priority of all holder threads */
- (void)sem_foreachholder(sem, sem_restoreholderprio, stcb);
+ (void)sem_foreachholder(sem, sem_restoreholderprioall, stcb);
}
/* If there are no tasks waiting for available counts, then all holders
@@ -692,7 +787,6 @@ static inline void sem_restorebaseprio_task(FAR struct tcb_s *stcb,
FAR sem_t *sem)
{
FAR struct tcb_s *rtcb = this_task();
- FAR struct semholder_s *pholder;
/* Perform the following actions only if a new thread was given a count.
* The thread that received the count should be the highest priority
@@ -701,7 +795,7 @@ static inline void sem_restorebaseprio_task(FAR struct tcb_s *stcb,
* next highest pending priority.
*/
- if (stcb)
+ if (stcb != NULL)
{
/* The currently executed thread should be the lower priority
* thread that just posted the count and caused this action.
@@ -735,18 +829,8 @@ static inline void sem_restorebaseprio_task(FAR struct tcb_s *stcb,
* counts, then we need to remove it from the list of holders.
*/
- pholder = sem_findholder(sem, rtcb);
- if (pholder)
- {
- /* When no more counts are held, remove the holder from the list. The
- * count was decremented in sem_releaseholder.
- */
+ sem_findandfreeholder(sem, rtcb);
- if (pholder->counts <= 0)
- {
- sem_freeholder(sem, pholder);
- }
- }
}
/****************************************************************************
@@ -817,18 +901,21 @@ void sem_destroyholder(FAR sem_t *sem)
*/
#if CONFIG_SEM_PREALLOCHOLDERS > 0
- if (sem->hhead)
+ if (sem->hhead != NULL)
{
serr("ERROR: Semaphore destroyed with holders\n");
+ DEBUGASSERT(sem->hhead == NULL);
(void)sem_foreachholder(sem, sem_recoverholders, NULL);
}
#else
- if (sem->holder.htcb)
+ if (sem->holder[0].htcb != NULL || sem->holder[1].htcb != NULL)
{
+ DEBUGASSERT(sem->holder[0].htcb == NULL || sem->holder[1].htcb == NULL);
serr("ERROR: Semaphore destroyed with holder\n");
}
- sem->holder.htcb = NULL;
+ sem->holder[0].htcb = NULL;
+ sem->holder[1].htcb = NULL;
#endif
}
@@ -952,7 +1039,7 @@ void sem_releaseholder(FAR sem_t *sem)
/* Find the container for this holder */
pholder = sem_findholder(sem, rtcb);
- if (pholder && pholder->counts > 0)
+ if (pholder != NULL && pholder->counts > 0)
{
/* Decrement the counts on this holder -- the holder will be freed
* later in sem_restorebaseprio.
@@ -1048,7 +1135,7 @@ void sem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem)
/* Adjust the priority of every holder as necessary */
- (void)sem_foreachholder(sem, sem_restoreholderprio, stcb);
+ (void)sem_foreachholder(sem, sem_restoreholderprioall, stcb);
}
#endif
View Git Blame Copy Permalink