sched: semaphore wait list optimize

include/nuttx/semaphore.h

@@ -40,15 +40,21 @@
 
 #ifdef CONFIG_PRIORITY_INHERITANCE
 # if CONFIG_SEM_PREALLOCHOLDERS > 0
+/* semcount, waitlist, flags, hhead */
+
 #  define NXSEM_INITIALIZER(c, f) \
-    {(c), (f), NULL}                    /* semcount, flags, hhead */
+    {(c), SEM_WAITLIST_INITIALIZER, (f), NULL}
 # else
+/* semcount, waitlist, flags, holder[2] */
+
 #  define NXSEM_INITIALIZER(c, f) \
-    {(c), (f), {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}}  /* semcount, flags, holder[2] */
+    {(c), SEM_WAITLIST_INITIALIZER, (f), {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}}
 # endif
 #else /* CONFIG_PRIORITY_INHERITANCE */
+/* semcount, waitlist */
+
 #  define NXSEM_INITIALIZER(c, f) \
-    {(c)}                               /* semcount, flags */
+    {(c), SEM_WAITLIST_INITIALIZER}
 #endif /* CONFIG_PRIORITY_INHERITANCE */
 
 /* Most internal nxsem_* interfaces are not available in the user space in

include/semaphore.h

@@ -30,6 +30,7 @@
 #include <stdint.h>
 #include <limits.h>
 #include <time.h>
+#include <nuttx/queue.h>
 
 /****************************************************************************
  * Pre-processor Definitions
@@ -93,6 +94,8 @@ struct semholder_s
 #endif
 #endif /* CONFIG_PRIORITY_INHERITANCE */
 
+#define SEM_WAITLIST_INITIALIZER {NULL, NULL}
+
 /* This is the generic semaphore structure. */
 
 struct sem_s
@@ -100,6 +103,8 @@ struct sem_s
   volatile int16_t semcount;     /* >0 -> Num counts available */
                                  /* <0 -> Num tasks waiting for semaphore */
 
+  dq_queue_t waitlist;
+
   /* If priority inheritance is enabled, then we have to keep track of which
    * tasks hold references to the semaphore.
    */
@@ -120,17 +125,25 @@ typedef struct sem_s sem_t;
 
 #ifdef CONFIG_PRIORITY_INHERITANCE
 # if CONFIG_SEM_PREALLOCHOLDERS > 0
+/* semcount, waitlist, flags, hhead */
+
 #  define SEM_INITIALIZER(c) \
-    {(c), 0, NULL}               /* semcount, flags, hhead */
+    {(c), SEM_WAITLIST_INITIALIZER, 0, NULL}
 # else
+/* semcount, waitlist, flags, holder[2] */
+
 #  define SEM_INITIALIZER(c) \
-    {(c), 0, {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}} /* semcount, flags, holder[2] */
+    {(c), SEM_WAITLIST_INITIALIZER, 0, {SEMHOLDER_INITIALIZER, SEMHOLDER_INITIALIZER}}
 # endif
 #else
+/* semcount, waitlist */
+
 #  define SEM_INITIALIZER(c) \
-    {(c)}                        /* semcount */
+    {(c), SEM_WAITLIST_INITIALIZER}
 #endif
 
+# define SEM_WAITLIST(sem)        (&((sem)->waitlist))
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/

libs/libc/semaphore/sem_init.c

@@ -73,6 +73,10 @@ int nxsem_init(FAR sem_t *sem, int pshared, unsigned int value)
 
       sem->semcount         = (int16_t)value;
 
+      /* Initialize semaphore wait list */
+
+      dq_init(&sem->waitlist);
+
       /* Initialize to support priority inheritance */
 
 #ifdef CONFIG_PRIORITY_INHERITANCE

sched/init/nx_start.c

@@ -134,10 +134,6 @@ FAR struct tcb_s *g_running_tasks[CONFIG_SMP_NCPUS];
 
 dq_queue_t g_pendingtasks;
 
-/* This is the list of all tasks that are blocked waiting for a semaphore */
-
-dq_queue_t g_waitingforsemaphore;
-
 /* This is the list of all tasks that are blocked waiting for a signal */
 
 dq_queue_t g_waitingforsignal;
@@ -237,8 +233,8 @@ const struct tasklist_s g_tasklisttable[NUM_TASK_STATES] =
     0
   },
   {                                              /* TSTATE_WAIT_SEM */
-    &g_waitingforsemaphore,
+    (FAR void *)offsetof(sem_t, waitlist),
-    TLIST_ATTR_PRIORITIZED
+    TLIST_ATTR_PRIORITIZED | TLIST_ATTR_OFFSET
   },
   {                                              /* TSTATE_WAIT_SIG */
     &g_waitingforsignal,
@@ -429,9 +425,9 @@ void nx_start(void)
        */
 
 #ifdef CONFIG_SMP
-      tasklist = TLIST_HEAD(TSTATE_TASK_RUNNING, i);
+      tasklist = TLIST_HEAD(&g_idletcb[i].cmn, i);
 #else
-      tasklist = TLIST_HEAD(TSTATE_TASK_RUNNING);
+      tasklist = TLIST_HEAD(&g_idletcb[i].cmn);
 #endif
       dq_addfirst((FAR dq_entry_t *)&g_idletcb[i], tasklist);
 

sched/sched/sched.h

@@ -68,22 +68,25 @@
 #define TLIST_ATTR_PRIORITIZED   (1 << 0) /* Bit 0: List is prioritized */
 #define TLIST_ATTR_INDEXED       (1 << 1) /* Bit 1: List is indexed by CPU */
 #define TLIST_ATTR_RUNNABLE      (1 << 2) /* Bit 2: List includes running tasks */
+#define TLIST_ATTR_OFFSET        (1 << 3) /* Bit 3: Pointer of task list is offset */
 
 #define __TLIST_ATTR(s)          g_tasklisttable[s].attr
 #define TLIST_ISPRIORITIZED(s)   ((__TLIST_ATTR(s) & TLIST_ATTR_PRIORITIZED) != 0)
 #define TLIST_ISINDEXED(s)       ((__TLIST_ATTR(s) & TLIST_ATTR_INDEXED) != 0)
 #define TLIST_ISRUNNABLE(s)      ((__TLIST_ATTR(s) & TLIST_ATTR_RUNNABLE) != 0)
+#define TLIST_ISOFFSET(s)        ((__TLIST_ATTR(s) & TLIST_ATTR_OFFSET) != 0)
 
-#define __TLIST_HEAD(s)          g_tasklisttable[s].list
+#define __TLIST_HEAD(t) \
-#define __TLIST_HEADINDEXED(s,c) (&(__TLIST_HEAD(s))[c])
+  (TLIST_ISOFFSET((t)->task_state) ? (FAR dq_queue_t *)((FAR uint8_t *)((t)->waitobj) + \
+  (uintptr_t)g_tasklisttable[(t)->task_state].list) : g_tasklisttable[(t)->task_state].list)
 
 #ifdef CONFIG_SMP
-#  define TLIST_HEAD(s,c) \
+#  define TLIST_HEAD(t,c) \
-  ((TLIST_ISINDEXED(s)) ? __TLIST_HEADINDEXED(s,c) : __TLIST_HEAD(s))
+    ((TLIST_ISINDEXED((t)->task_state)) ? (&(__TLIST_HEAD(t))[c]) : __TLIST_HEAD(t))
-#  define TLIST_BLOCKED(s)       __TLIST_HEAD(s)
+#  define TLIST_BLOCKED(t)       __TLIST_HEAD(t)
 #else
-#  define TLIST_HEAD(s)          __TLIST_HEAD(s)
+#  define TLIST_HEAD(t)          __TLIST_HEAD(t)
-#  define TLIST_BLOCKED(s)       __TLIST_HEAD(s)
+#  define TLIST_BLOCKED(t)       __TLIST_HEAD(t)
 #endif
 
 /****************************************************************************
@@ -171,10 +174,6 @@ extern FAR struct tcb_s *g_running_tasks[CONFIG_SMP_NCPUS];
 
 extern dq_queue_t g_pendingtasks;
 
-/* This is the list of all tasks that are blocked waiting for a semaphore */
-
-extern dq_queue_t g_waitingforsemaphore;
-
 /* This is the list of all tasks that are blocked waiting for a signal */
 
 extern dq_queue_t g_waitingforsignal;

sched/sched/sched_addblocked.c

@@ -63,9 +63,13 @@ void nxsched_add_blocked(FAR struct tcb_s *btcb, tstate_t task_state)
   DEBUGASSERT(task_state >= FIRST_BLOCKED_STATE &&
               task_state <= LAST_BLOCKED_STATE);
 
+  /* Make sure the TCB's state corresponds to the list */
+
+  btcb->task_state = task_state;
+
   /* Add the TCB to the blocked task list associated with this state. */
 
-  tasklist = TLIST_BLOCKED(task_state);
+  tasklist = TLIST_BLOCKED(btcb);
 
   /* Determine if the task is to be added to a prioritized task list. */
 
@@ -81,8 +85,4 @@ void nxsched_add_blocked(FAR struct tcb_s *btcb, tstate_t task_state)
 
       dq_addlast((FAR dq_entry_t *)btcb, tasklist);
     }
-
-  /* Make sure the TCB's state corresponds to the list */
-
-  btcb->task_state = task_state;
 }

sched/sched/sched_removeblocked.c

@@ -66,7 +66,7 @@ void nxsched_remove_blocked(FAR struct tcb_s *btcb)
    * with this state
    */
 
-  dq_rem((FAR dq_entry_t *)btcb, TLIST_BLOCKED(task_state));
+  dq_rem((FAR dq_entry_t *)btcb, TLIST_BLOCKED(btcb));
 
   /* Indicate that the wait is over. */
 

sched/sched/sched_removereadytorun.c

@@ -130,7 +130,7 @@ bool nxsched_remove_readytorun(FAR struct tcb_s *rtcb)
    */
 
   cpu      = rtcb->cpu;
-  tasklist = TLIST_HEAD(rtcb->task_state, cpu);
+  tasklist = TLIST_HEAD(rtcb, cpu);
 
   /* Check if the TCB to be removed is at the head of a ready-to-run list.
    * For the case of SMP, there are two lists involved:  (1) the

sched/sched/sched_setpriority.c

@@ -278,7 +278,7 @@ static inline void nxsched_blocked_setpriority(FAR struct tcb_s *tcb,
 
   /* CASE 3a. The task resides in a prioritized list. */
 
-  tasklist = TLIST_BLOCKED(task_state);
+  tasklist = TLIST_BLOCKED(tcb);
   if (TLIST_ISPRIORITIZED(task_state))
     {
       /* Remove the TCB from the prioritized task list */

sched/semaphore/sem_post.c

@@ -139,9 +139,7 @@ int nxsem_post(FAR sem_t *sem)
            * that we want.
            */
 
-          for (stcb = (FAR struct tcb_s *)g_waitingforsemaphore.head;
+          stcb = (FAR struct tcb_s *)dq_peek(SEM_WAITLIST(sem));
-               (stcb && stcb->waitobj != sem);
-               stcb = stcb->flink);
 
           if (stcb != NULL)
             {

sched/task/task_restart.c

@@ -133,9 +133,9 @@ int nxtask_restart(pid_t pid)
    */
 
 #ifdef CONFIG_SMP
-  tasklist = TLIST_HEAD(tcb->cmn.task_state, tcb->cmn.cpu);
+  tasklist = TLIST_HEAD(&tcb->cmn, tcb->cmn.cpu);
 #else
-  tasklist = TLIST_HEAD(tcb->cmn.task_state);
+  tasklist = TLIST_HEAD(&tcb->cmn);
 #endif
 
   dq_rem((FAR dq_entry_t *)tcb, tasklist);

sched/task/task_terminate.c

@@ -131,13 +131,13 @@ int nxtask_terminate(pid_t pid, bool nonblocking)
 
   /* Get the task list associated with the thread's state and CPU */
 
-  tasklist = TLIST_HEAD(dtcb->task_state, cpu);
+  tasklist = TLIST_HEAD(dtcb, cpu);
 #else
   /* In the non-SMP case, we can be assured that the task to be terminated
    * is not running.  get the task list associated with the task state.
    */
 
-  tasklist = TLIST_HEAD(dtcb->task_state);
+  tasklist = TLIST_HEAD(dtcb);
 #endif
 
   /* Remove the task from the task list */