Fix some list handling associated with priority inheritance

git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5053 7fd9a85b-ad96-42d3-883c-3090e2eb8679

8 files changed, 111 insertions, 77 deletions

diff --git a/apps/examples/ostest/prioinherit.c b/apps/examples/ostest/prioinherit.c
index 993c9e14a2..87849abcfd 100644
--- a/apps/examples/ostest/prioinherit.c
+++ b/apps/examples/ostest/prioinherit.c
@@ -58,12 +58,22 @@
 #ifndef CONFIG_SEM_PREALLOCHOLDERS
 #  define CONFIG_SEM_PREALLOCHOLDERS 0
 #endif
-#define NLOWPRI_THREADS  (CONFIG_SEM_PREALLOCHOLDERS+1)
+
+#if CONFIG_SEM_PREALLOCHOLDERS > 3
+#  define NLOWPRI_THREADS 3
+#else
+#  define NLOWPRI_THREADS (CONFIG_SEM_PREALLOCHOLDERS+1)
+#endif
 
 #ifndef CONFIG_SEM_NNESTPRIO
 #  define CONFIG_SEM_NNESTPRIO 0
 #endif
-#define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1)
+
+#if CONFIG_SEM_NNESTPRIO > 3
+#  define NHIGHPRI_THREADS 3
+#else
+#  define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1)
+#endif
 
 /****************************************************************************
  * Private Data
diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog
index 7a6e3f6eb0..4135bd19ae 100644
--- a/nuttx/ChangeLog
+++ b/nuttx/ChangeLog
@@ -3178,3 +3178,8 @@
 	  functional (although more testing is necesary).
 
 6.22 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
+
+	* include/semaphore.h, sched/sem_holders.c, and lib/semaphore/sem_init.c:
+	  Fix some strange (and probably wrong) list handling when
+	  CONFIG_PRIORITY_INHERITANCE and CONFIG_SEM_PREALLOCHOLDERS are defined.
+	  This list handling was probably causing errors reported by Mike Smith
diff --git a/nuttx/configs/stm3220g-eval/README.txt b/nuttx/configs/stm3220g-eval/README.txt
index da29b15922..64a86c4dd7 100644
--- a/nuttx/configs/stm3220g-eval/README.txt
+++ b/nuttx/configs/stm3220g-eval/README.txt
@@ -860,7 +860,7 @@ Where <subdir> is one of the following:
 
     8. USB OTG FS Device or Host Support
  
-       CONFIG_USBDEV          - Enable USB device support
+       CONFIG_USBDEV          - Enable USB device support, OR
        CONFIG_USBHOST         - Enable USB host support
        CONFIG_STM32_OTGFS     - Enable the STM32 USB OTG FS block
        CONFIG_STM32_SYSCFG    - Needed
diff --git a/nuttx/configs/stm3240g-eval/README.txt b/nuttx/configs/stm3240g-eval/README.txt
index 908fb93c85..949db18913 100755
--- a/nuttx/configs/stm3240g-eval/README.txt
+++ b/nuttx/configs/stm3240g-eval/README.txt
@@ -1023,7 +1023,7 @@ Where <subdir> is one of the following:
 
     8. USB OTG FS Device or Host Support
  
-       CONFIG_USBDEV          - Enable USB device support
+       CONFIG_USBDEV          - Enable USB device support, OR
        CONFIG_USBHOST         - Enable USB host support
        CONFIG_STM32_OTGFS     - Enable the STM32 USB OTG FS block
        CONFIG_STM32_SYSCFG    - Needed
diff --git a/nuttx/include/semaphore.h b/nuttx/include/semaphore.h
index 85214b9c20..257a5826f3 100644
--- a/nuttx/include/semaphore.h
+++ b/nuttx/include/semaphore.h
@@ -60,16 +60,16 @@ extern "C" {
  * Public Type Declarations
  ****************************************************************************/
 
-/* This structure contains the holder of a semaphore */
+/* This structure contains information about the holder of a semaphore */
 
 #ifdef CONFIG_PRIORITY_INHERITANCE
 struct semholder_s
 {
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-  struct semholder_s *flink;    /* Implements singly linked list */
+  struct semholder_s *flink;     /* Implements singly linked list */
 #endif
-  void   *holder;               /* Holder TCB (actual type is _TCB) */
-  int16_t counts;               /* Number of counts owned by this holder */
+  void *htcb;                    /* Holder TCB (actual type is _TCB) */
+  int16_t counts;                /* Number of counts owned by this holder */
 };
 
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
@@ -83,12 +83,21 @@ struct semholder_s
 
 struct sem_s
 {
-  int16_t semcount;             /* >0 -> Num counts available */
-                                /* <0 -> Num tasks waiting for semaphore */
+  int16_t semcount;              /* >0 -> Num counts available */
+                                 /* <0 -> Num tasks waiting for semaphore */
+  /* If priority inheritance is enabled, then we have to keep track of which
+   * tasks hold references to the semaphore.
+   */
+
 #ifdef CONFIG_PRIORITY_INHERITANCE
-  struct semholder_s hlist;     /* List of holders of semaphore counts */
+# if CONFIG_SEM_PREALLOCHOLDERS > 0
+  FAR struct semholder_s *hhead; /* List of holders of semaphore counts */
+# else
+  struct semholder_s holder;     /* Single holder */
+# endif
 #endif
 };
+
 typedef struct sem_s sem_t;
 
 /* Initializers */
diff --git a/nuttx/lib/semaphore/sem_init.c b/nuttx/lib/semaphore/sem_init.c
index ea1c6e84ea..bc14415f97 100644
--- a/nuttx/lib/semaphore/sem_init.c
+++ b/nuttx/lib/semaphore/sem_init.c
@@ -103,16 +103,17 @@ 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
 #  if CONFIG_SEM_PREALLOCHOLDERS > 0
-      sem->hlist.flink  = NULL;
+      sem->hhead         = NULL;
+#  else
+      sem->holder.htcb   = NULL;
+      sem->holder.counts = 0;
 #  endif
-      sem->hlist.holder = NULL;
-      sem->hlist.counts = 0;
 #endif
       return OK;
     }
diff --git a/nuttx/sched/os_start.c b/nuttx/sched/os_start.c
index 6cd508b8ca..c0b16236dd 100644
--- a/nuttx/sched/os_start.c
+++ b/nuttx/sched/os_start.c
@@ -289,6 +289,18 @@ void os_start(void)
   g_idletcb.flags = TCB_FLAG_TTYPE_KERNEL;
   up_initial_state(&g_idletcb);
 
+  /* Initialize the semaphore facility(if in link).  This has to be done
+   * very early because many subsystems depend upon fully functional
+   * semaphores.
+   */
+
+#ifdef CONFIG_HAVE_WEAKFUNCTIONS
+  if (sem_initialize != NULL)
+#endif
+    {
+      sem_initialize();
+    }
+
   /* Initialize the memory manager */
 
 #ifndef CONFIG_HEAP_BASE
@@ -351,15 +363,6 @@ void os_start(void)
     }
 #endif
 
-  /* Initialize the semaphore facility. (if in link) */
-
-#ifdef CONFIG_HAVE_WEAKFUNCTIONS
-  if (sem_initialize != NULL)
-#endif
-    {
-      sem_initialize();
-    }
-
   /* Initialize the named message queue facility (if in link) */
 
 #ifndef CONFIG_DISABLE_MQUEUE
diff --git a/nuttx/sched/sem_holder.c b/nuttx/sched/sem_holder.c
index 6003c563dc..34f88185a9 100644
--- a/nuttx/sched/sem_holder.c
+++ b/nuttx/sched/sem_holder.c
@@ -99,32 +99,31 @@ static inline FAR struct semholder_s *sem_allocholder(sem_t *sem)
    * used to implement mutexes.
    */
 
-  if (!sem->hlist.holder)
-    {
-      pholder          = &sem->hlist;
-      pholder->counts  = 0;
-    }
-  else
-    {
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-      pholder = g_freeholders;
-      if (pholder)
-        {
-          /* Remove the holder from the free list an put it into the semaphore's holder list */
+  pholder = g_freeholders;
+  if (pholder)
+    {
+      /* Remove the holder from the free list an put it into the semaphore's holder list */
 
-          g_freeholders    = pholder->flink;
-          pholder->flink   = sem->hlist.flink;
-          sem->hlist.flink = pholder;
+      g_freeholders    = pholder->flink;
+      pholder->flink   = sem->hhead;
+      sem->hhead       = pholder;
 
-          /* Make sure the initial count is zero */
+      /* Make sure the initial count is zero */
 
-          pholder->counts  = 0;
-        }
-      else
+      pholder->counts  = 0;
+    }
 #else
-      pholder = NULL;
+  if (!sem->holder.htcb)
+    {
+      pholder          = &sem->holder;
+      pholder->counts  = 0;
+    }
 #endif
+  else
+    {
       sdbg("Insufficient pre-allocated holders\n");
+      pholder = NULL;
     }
 
   return pholder;
@@ -140,12 +139,13 @@ static FAR struct semholder_s *sem_findholder(sem_t *sem, FAR _TCB *htcb)
 
   /* Try to find the holder in the list of holders associated with this semaphore */
 
-  pholder = &sem->hlist;
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-  for (; pholder; pholder = pholder->flink)
+  for (pholder = sem->hhead; pholder; pholder = pholder->flink)
+#else
+  pholder = &sem->holder;
 #endif
     {
-      if (pholder->holder == htcb)
+      if (pholder->htcb == htcb)
         {
           /* Got it! */
 
@@ -186,31 +186,33 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
 
   /* Release the holder and counts */
 
-  pholder->holder = 0;
+  pholder->htcb   = NULL;
   pholder->counts = 0;
 
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-  /* If this is the holder inside the semaphore, then do nothing more */
+  /* Search the list for the matching holder */
 
-  if (pholder != &sem->hlist)
-    {
-      /* Otherwise, search the list for the matching holder */
+  for (prev = NULL, curr = sem->hhead;
+       curr && curr != pholder;
+       prev = curr, curr = curr->flink);
 
-      for (prev = &sem->hlist, curr = sem->hlist.flink;
-           curr && curr != pholder;
-           prev = curr, curr = curr->flink);
+  if (curr)
+    {
+      /* Remove the holder from the list */
 
-      if (curr)
+      if (prev)
         {
-          /* Remove the holder from the list */
-
           prev->flink = pholder->flink;
+        }
+      else
+        {
+          sem->hhead = pholder->flink;
+        }
 
-          /* 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
 }
@@ -221,14 +223,16 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
 
 static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler, FAR void *arg)
 {
-  FAR struct semholder_s *pholder = &sem->hlist;
+  FAR struct semholder_s *pholder;
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
   FAR struct semholder_s *next;
 #endif
   int ret = 0;
 
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-  for (; pholder && ret == 0; pholder = next)
+  for (pholder = sem->hhead; pholder && ret == 0; pholder = next)
+#else
+  pholder = &sem->holder;
 #endif
     {
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
@@ -238,7 +242,7 @@ static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler, FAR void *
 #endif
       /* The initial "built-in" container may hold a NULL holder */
 
-      if (pholder->holder)
+      if (pholder->htcb)
         {
           /* Call the handler */
 
@@ -268,7 +272,7 @@ static int sem_recoverholders(FAR struct semholder_s *pholder, FAR sem_t *sem, F
 static int sem_boostholderprio(FAR struct semholder_s *pholder,
                                FAR sem_t *sem, FAR void *arg)
 {
-  FAR _TCB *htcb = (FAR _TCB *)pholder->holder;
+  FAR _TCB *htcb = (FAR _TCB *)pholder->htcb;
   FAR _TCB *rtcb = (FAR _TCB*)arg;
 
   /* Make sure that the thread is still active.  If it exited without releasing
@@ -371,7 +375,7 @@ static int sem_boostholderprio(FAR struct semholder_s *pholder,
 static int sem_verifyholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
 {
 #if 0 // Need to revisit this, but these assumptions seem to be untrue -- OR there is a bug???
-  FAR _TCB *htcb = (FAR _TCB *)pholder->holder;
+  FAR _TCB *htcb = (FAR _TCB *)pholder->htcb;
 
   /* Called after a semaphore has been released (incremented), the semaphore
    * could is non-negative, and there is no thread waiting for the count.
@@ -396,9 +400,9 @@ static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR v
 {
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
   dbg("  %08x: %08x %08x %04x\n",
-      pholder, pholder->flink, pholder->holder, pholder->counts);
+      pholder, pholder->flink, pholder->htcb, pholder->counts);
 #else
-  dbg("  %08x: %08x %04x\n", pholder, pholder->holder, pholder->counts);
+  dbg("  %08x: %08x %04x\n", pholder, pholder->htcb, pholder->counts);
 #endif
   return 0;
 }
@@ -410,7 +414,7 @@ static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR v
 
 static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
 {
-  FAR _TCB *htcb = (FAR _TCB *)pholder->holder;
+  FAR _TCB *htcb = (FAR _TCB *)pholder->htcb;
 #if CONFIG_SEM_NNESTPRIO > 0
   FAR _TCB *stcb = (FAR _TCB *)arg;
   int rpriority;
@@ -510,6 +514,7 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem
                     {
                       htcb->pend_reprios[i] = htcb->pend_reprios[j];
                     }
+
                    htcb->npend_reprio = j;
                    break;
                 }
@@ -534,7 +539,7 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem
 static int sem_restoreholderprioA(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
 {
   FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
-  if (pholder->holder != rtcb)
+  if (pholder->htcb != rtcb)
     {
       return sem_restoreholderprio(pholder, sem, arg);
     }
@@ -545,7 +550,7 @@ static int sem_restoreholderprioA(FAR struct semholder_s *pholder, FAR sem_t *se
 static int sem_restoreholderprioB(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
 {
   FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
-  if (pholder->holder == rtcb)
+  if (pholder->htcb == rtcb)
     {
       (void)sem_restoreholderprio(pholder, sem, arg);
       return 1;
@@ -586,6 +591,7 @@ void sem_initholders(void)
     {
       g_holderalloc[i].flink = &g_holderalloc[i+1];
     }
+
   g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS-1].flink = NULL;
 #endif
 }
@@ -609,8 +615,8 @@ void sem_initholders(void)
 
 void sem_destroyholder(FAR sem_t *sem)
 {
-  /* It is an error is a semaphore is destroyed while there are any holders
-   * (except perhaps the thread releas the semaphore itself).  Hmmm.. but
+  /* It is an error if a semaphore is destroyed while there are any holders
+   * (except perhaps the thread release the semaphore itself).  Hmmm.. but
    * we actually have to assume that the caller knows what it is doing because
    * could have killed another thread that is the actual holder of the semaphore.
    * We cannot make any assumptions about the state of the semaphore or the
@@ -621,18 +627,18 @@ void sem_destroyholder(FAR sem_t *sem)
    */
 
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
-  if (sem->hlist.holder || sem->hlist.flink)
+  if (sem->hhead)
     {
       sdbg("Semaphore destroyed with holders\n");
       (void)sem_foreachholder(sem, sem_recoverholders, NULL);
     }
 #else
-  if (sem->hlist.holder)
+  if (sem->holder.htcb)
     {
       sdbg("Semaphore destroyed with holder\n");
     }
 
-  sem->hlist.holder = NULL;
+  sem->holder.htcb = NULL;
 #endif
 }
 
@@ -664,7 +670,7 @@ void sem_addholder(FAR sem_t *sem)
     {
       /* Then set the holder and increment the number of counts held by this holder */
 
-      pholder->holder = rtcb;
+      pholder->htcb = rtcb;
       pholder->counts++;
     }
 }