Extend, improve, and partially fix priority inheritance logic

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1590 42af7a65-404d-4744-a932-0658087f49c3

8 files changed, 575 insertions, 103 deletions

diff --git a/nuttx/sched/Makefile b/nuttx/sched/Makefile
index 89da69f14..b846163d8 100644
--- a/nuttx/sched/Makefile
+++ b/nuttx/sched/Makefile
@@ -1,7 +1,7 @@
 ############################################################################
 # sched/Makefile
 #
-#   Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
+#   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
 #   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
 #
 # Redistribution and use in source and binary forms, with or without
@@ -42,28 +42,35 @@ MISC_SRCS	= os_start.c get_errno_ptr.c \
 		  sched_setupstreams.c sched_getfiles.c sched_getsockets.c sched_getstreams.c \
 		  sched_setupidlefiles.c sched_setuptaskfiles.c sched_setuppthreadfiles.c \
 		  sched_releasefiles.c
+
 TSK_SRCS	= task_create.c task_init.c task_setup.c task_activate.c \
 		  task_start.c task_delete.c task_deletecurrent.c task_restart.c \
 		  exit.c abort.c atexit.c getpid.c  \
 		  sched_addreadytorun.c sched_removereadytorun.c sched_addprioritized.c \
 		  sched_mergepending.c sched_addblocked.c sched_removeblocked.c \
 		  sched_free.c sched_gettcb.c sched_releasetcb.c
+
 SCHED_SRCS	= sched_setparam.c sched_settcbprio.c sched_getparam.c \
 		  sched_setscheduler.c sched_getscheduler.c \
 		  sched_yield.c sched_rrgetinterval.c sched_foreach.c \
 		  sched_getprioritymax.c sched_getprioritymin.c \
 		  sched_lock.c sched_unlock.c sched_lockcount.c
+
 ENV_SRCS	= env_getenvironptr.c env_dup.c env_share.c env_release.c \
 		  env_findvar.c env_removevar.c \
 		  env_clearenv.c env_getenv.c env_putenv.c env_setenv.c env_unsetenv.c
+
 WDOG_SRCS	= wd_initialize.c wd_create.c wd_start.c wd_cancel.c wd_delete.c \
 		  wd_gettime.c
+
 TIME_SRCS	= sched_processtimer.c
+
 ifneq ($(CONFIG_DISABLE_SIGNALS),y)
 TIME_SRCS	+= sleep.c usleep.c
 endif
 CLOCK_SRCS	= clock_initialize.c mktime.c gmtime_r.c clock_settime.c clock_gettime.c \
 		  clock_getres.c clock_time2ticks.c clock_abstime2ticks.c clock_ticks2time.c
+
 SIGNAL_SRCS	= sig_initialize.c \
 		  sig_action.c sig_procmask.c sig_pending.c sig_suspend.c \
 		  sig_kill.c sig_queue.c sig_waitinfo.c sig_timedwait.c \
@@ -73,6 +80,7 @@ SIGNAL_SRCS	= sig_initialize.c \
                   sig_unmaskpendingsignal.c sig_removependingsignal.c \
 		  sig_releasependingsignal.c sig_lowest.c sig_mqnotempty.c \
 		  sig_cleanup.c sig_received.c sig_deliver.c
+
 MQUEUE_SRCS	= mq_open.c mq_close.c mq_unlink.c \
 		  mq_send.c mq_timedsend.c mq_sndinternal.c \
 		  mq_receive.c mq_timedreceive.c mq_rcvinternal.c \
@@ -81,6 +89,7 @@ MQUEUE_SRCS	= mq_open.c mq_close.c mq_unlink.c \
 ifneq ($(CONFIG_DISABLE_SIGNALS),y)
 MQUEUE_SRCS	+= mq_notify.c
 endif
+
 PTHREAD_SRCS	= pthread_attrinit.c pthread_attrdestroy.c \
 		  pthread_attrsetschedpolicy.c pthread_attrgetschedpolicy.c \
 		  pthread_attrsetinheritsched.c pthread_attrgetinheritsched.c \
@@ -108,14 +117,20 @@ endif
 ifeq ($(CONFIG_MUTEX_TYPES),y)
 PTHREAD_SRCS	+= pthread_mutexattrsettype.c pthread_mutexattrgettype.c
 endif
+
 SEM_SRCS	= sem_initialize.c sem_init.c sem_destroy.c\
 		  sem_open.c sem_close.c sem_unlink.c \
 		  sem_wait.c sem_trywait.c sem_post.c sem_getvalue.c \
                   sem_waitirq.c sem_findnamed.c
+ifeq ($(CONFIG_PRIORITY_INHERITANCE),y)
+SEM_SRCS	+= sem_holder.c
+endif
+
 ifneq ($(CONFIG_DISABLE_POSIX_TIMERS),y)
 TIMER_SRCS	= timer_initialize.c timer_create.c timer_delete.c timer_getoverrun.c \
 		  timer_gettime.c timer_settime.c timer_release.c
 endif
+
 IRQ_SRCS	= irq_initialize.c irq_attach.c irq_dispatch.c irq_unexpectedisr.c
 
 CSRCS		= $(MISC_SRCS) $(TSK_SRCS) $(SCHED_SRCS) $(WDOG_SRCS) $(TIME_SRCS) \
diff --git a/nuttx/sched/sem_destroy.c b/nuttx/sched/sem_destroy.c
index 663522303..3a96fcef9 100644
--- a/nuttx/sched/sem_destroy.c
+++ b/nuttx/sched/sem_destroy.c
@@ -42,10 +42,6 @@
 #include "sem_internal.h"
 
 /****************************************************************************
- * Compilation Switches
- ****************************************************************************/
-
-/****************************************************************************
  * Definitions
  ****************************************************************************/
 
@@ -115,9 +111,10 @@ int sem_destroy (FAR sem_t *sem)
         {
           sem->semcount = 1;
         }
-#ifdef CONFIG_PRIORITY_INHERITANCE
-      sem->holder = NULL;
-#endif
+
+      /* Release holders of the semaphore */
+
+      sem_destroyholder(sem);
       ret = OK;
     }
 
diff --git a/nuttx/sched/sem_holder.c b/nuttx/sched/sem_holder.c
new file mode 100644
index 000000000..f4ee07569
--- /dev/null
+++ b/nuttx/sched/sem_holder.c
@@ -0,0 +1,476 @@
+/****************************************************************************
+ * sched/sem_holder.c
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <sys/types.h>
+#include <semaphore.h>
+#include <sched.h>
+#include <debug.h>
+#include <nuttx/arch.h>
+
+#include "os_internal.h"
+#include "sem_internal.h"
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+#ifndef CONFIG_SEM_PREALLOCHOLDERS
+#  define CONFIG_SEM_PREALLOCHOLDERS (4*CONFIG_MAX_TASKS)
+#endif
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/* Preallocated holder structures */
+
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+static struct semholder_s g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS];
+static FAR struct semholder_s *g_freeholders;
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Function:  sem_allocholder
+ ****************************************************************************/
+
+static inline FAR struct semholder_s *sem_allocholder(sem_t *sem)
+{
+  FAR struct semholder_s *pholder;
+
+  /* Check if the "built-in" holder is being used.  We have this built-in
+   * holder to optimize for the simplest case where semaphores are only
+   * used to implement mutexes.
+   */
+
+  if (!sem->hlist.holder)
+    {
+      pholder = &sem->hlist;
+    }
+  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 */
+
+          g_freeholders    = pholder->flink;
+          pholder->flink   = sem->hlist.flink;
+          sem->hlist.flink = pholder;
+
+          /* Make sure the the initial count is zero */
+
+          pholder->counts  = 0;
+        }
+      else
+#else
+      pholder = NULL;
+#endif
+      sdbg("Insufficient pre-allocated holders\n");
+    }
+  return pholder;
+}
+
+/****************************************************************************
+ * Function:  sem_findholder
+ ****************************************************************************/
+
+static FAR struct semholder_s *sem_findholder(sem_t *sem, FAR _TCB *htcb)
+{
+  FAR struct semholder_s *pholder;
+
+  /* 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)
+#endif
+    {
+      if (pholder->holder == htcb)
+        {
+          /* Got it! */
+
+          return pholder;
+        }
+    }
+
+  /* The holder does not appear in the list */
+
+  return NULL;
+}
+
+/****************************************************************************
+ * Function:  sem_findorallocateholder
+ ****************************************************************************/
+
+static inline FAR struct semholder_s *sem_findorallocateholder(sem_t *sem, FAR _TCB *htcb)
+{
+  FAR struct semholder_s *pholder = sem_findholder(sem, htcb);
+  if (!pholder)
+    {
+      pholder = sem_allocholder(sem);
+    }
+  return pholder;
+}
+
+/****************************************************************************
+ * Function:  sem_freeholder
+ ****************************************************************************/
+
+static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
+{
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  FAR struct semholder_s *curr;
+  FAR struct semholder_s *prev;
+#endif
+
+  /* Release the holder and counts */
+
+  pholder->holder = 0;
+  pholder->counts = 0;
+
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  /* If this is the holder inside the semaphore, then do nothing more */
+
+  if (pholder != &sem->hlist)
+    {
+      /* Otherwise, search the list for the matching holder */
+
+      for (prev = &sem->hlist, curr = sem->hlist.flink;
+           curr && curr != pholder;
+           prev = curr, curr = curr->flink);
+
+      if (curr)
+        {
+          /* Remove the holder from the list */
+
+          prev->flink = pholder->flink;
+
+          /* And put it in the free list */
+
+          pholder->flink = g_freeholders;
+          g_freeholders  = pholder;
+        }
+    }
+#endif
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Function:  sem_initholders
+ *
+ * Description:
+ *   Called from sem_initialize() to set up semaphore holder information.
+ *
+ * Parameters:
+ *   None
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_initholders(void)
+{
+#ifdef CONFIG_SEM_PREALLOCHOLDERS
+ int i;
+
+  /* Put all of the pre-allocated holder structures into  free list */
+
+  g_freeholders = g_holderalloc;
+  for (i = 0; i < (CONFIG_SEM_PREALLOCHOLDERS-1); i++)
+    {
+      g_holderalloc[i].flink = &g_holderalloc[i+1];
+    }
+  g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS-1].flink = NULL;
+#endif
+}
+
+/****************************************************************************
+ * Function:  sem_destroyholder
+ *
+ * Description:
+ *   Called from sem_destroy() to handle any holders of a semaphore when
+ *   it is destroyed.
+ *
+ * Parameters:
+ *   sem - A reference to the semaphore being destroyed
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_destroyholder(sem_t *sem)
+{
+#if 0
+  FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
+
+  /* It is an error is a semaphore is destroyed while there are any holders
+   * (except perhaps the thread releas 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.
+   */
+
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  DEBUGASSERT((!sem->hlist.holder || sem->hlist.holder == rtcb) && !sem->hlist.flink);
+#else
+  DEBUGASSERT(!sem->hlist.holder || sem->hlist.holder == rtcb);
+#endif
+#endif
+
+  sem->hlist.holder = NULL;
+}
+
+/****************************************************************************
+ * Function:  sem_addholder
+ *
+ * Description:
+ *   Called from sem_wait() when the calling thread obtains the semaphore
+ *
+ * Parameters:
+ *   sem - A reference to the incremented semaphore
+ *
+ * Return Value:
+ *   0 (OK) or -1 (ERROR) if unsuccessful
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_addholder(sem_t *sem)
+{
+  FAR _TCB               *rtcb = (FAR _TCB*)g_readytorun.head;
+  FAR struct semholder_s *pholder;
+
+  /* Find or allocate a container for this new holder */
+
+  pholder = sem_findorallocateholder(sem, rtcb);
+  if (pholder)
+    {
+      /* Then set the holder and increment the number of counts held by this holder */
+
+      pholder->holder = rtcb;
+      pholder->counts++;
+    }
+}
+
+/****************************************************************************
+ * Function:  void sem_boostpriority(sem_t *sem)
+ *
+ * Description:
+ *   
+ *
+ * Parameters:
+ *   None
+ *
+ * Return Value:
+ *   0 (OK) or -1 (ERROR) if unsuccessful
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_boostpriority(sem_t *sem)
+{
+  FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
+  FAR _TCB *htcb;
+  struct semholder_s *pholder;
+
+  /* Traverse the list of holders */
+
+  pholder = &sem->hlist;
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  for (; pholder; pholder = pholder->flink)
+#endif
+    {
+      /* As an artifact, there will be holders that have no counts.  This
+       * because they have posted the semaphore and there count was decrement
+       * by sem_releaseholder(), but they are still being retained in the
+       * list to be harvested by sem_restorebaseprio.
+       */
+
+      if (pholder->counts > 0)
+        {
+          htcb = pholder->holder;
+          if (htcb && htcb->sched_priority < rtcb->sched_priority)
+            {
+              /* Raise the priority of the holder of the semaphore.  This
+               * cannot cause a context switch because we have preemption
+               * disabled.  The task will be marked "pending" and the switch
+               * will occur during up_block_task() processing.
+               *
+               * NOTE that we have to restore base_priority because
+               * sched_setparam() should set both.
+               */
+
+              int base_priority = htcb->base_priority;
+              (void)sched_settcbprio(htcb, rtcb->sched_priority);
+              htcb->base_priority = base_priority;
+            }
+        }
+    }
+}
+
+/****************************************************************************
+ * Function:  sem_releaseholder
+ *
+ * Description:
+ *   Called from sem_post() after a thread releases one count on the
+ *   semaphore.
+ *
+ * Parameters:
+ *   sem - A reference to the semaphore being posted
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_releaseholder(sem_t *sem)
+{
+  FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
+  FAR struct semholder_s *pholder;
+
+  /* Find the container for this holder */
+
+  pholder = sem_findholder(sem, rtcb);
+  if (pholder && pholder->counts > 0)
+    {
+      /* Decrement the counts on this holder */
+
+      pholder->counts--;
+    }
+}
+
+/****************************************************************************
+ * Function:  sem_restorebaseprio
+ *
+ * Description:
+ *   Check if we need to drop the priority of any threads holding the
+ *   semaphore.  The priority could have been boosted while they held the
+ *   semaphore.
+ *
+ * Parameters:
+ *   sem - A reference to the semaphore being posted
+ *
+ * Return Value:
+ *   0 (OK) or -1 (ERROR) if unsuccessful
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_restorebaseprio(sem_t *sem)
+{
+  FAR _TCB  *rtcb = (FAR _TCB*)g_readytorun.head;
+  FAR _TCB *htcb;
+  struct semholder_s *pholder;
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  struct semholder_s *next;
+#endif
+
+  /* Check if the semaphore is still available */
+
+  if (sem->semcount > 0)
+    {
+      /* Traverse the list of holders */
+
+      pholder = &sem->hlist;
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+      for (; pholder; pholder = next)
+#endif
+        {
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+          next = pholder->flink; /* In case pholder gets deleted */
+#endif
+          /* The initial "built-in" container may hold a NULL holder */
+
+          htcb = pholder->holder;
+          if (htcb)
+            {
+              /* Was the priority of this thread boosted? NOTE:  There is
+               * a logical flaw here:  If the thread holds multiple semaphore
+               * and has been boosted multiple times, then there is no mechanism
+               * to know the correct priority to restore and we may error in
+               * prematurely lowering the priority.
+               */
+
+              if (htcb->sched_priority != htcb->base_priority)
+                {
+                  up_reprioritize_rtr(rtcb, htcb->base_priority);
+                }
+
+              /* When no more counts are held, remove the holder from the list */
+
+              if (pholder->counts <= 0)
+                {
+	          sem_freeholder(sem, pholder);
+                }
+            }
+        }
+    }
+}
+
+#endif /* CONFIG_PRIORITY_INHERITANCE */
diff --git a/nuttx/sched/sem_init.c b/nuttx/sched/sem_init.c
index b9280d22e..05485a600 100644
--- a/nuttx/sched/sem_init.c
+++ b/nuttx/sched/sem_init.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * sched/sem_init.c
  *
- *   Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -43,10 +43,6 @@
 #include "sem_internal.h"
 
 /****************************************************************************
- * Compilation Switches
- ****************************************************************************/
-
-/****************************************************************************
  * Definitions
  ****************************************************************************/
 
@@ -101,7 +97,12 @@ int sem_init (FAR sem_t *sem, int pshared, unsigned int value)
 
   if (sem && value <= SEM_VALUE_MAX)
     {
-      sem->semcount = (sint16)value;
+      sem->semcount     = (sint16)value;
+#ifdef CONFIG_PRIORITY_INHERITANCE
+      sem->hlist.flink  = NULL;
+      sem->hlist.holder = NULL;
+      sem->hlist.counts = 0;
+#endif
       ret = OK;
     }
 
diff --git a/nuttx/sched/sem_initialize.c b/nuttx/sched/sem_initialize.c
index ed5e75599..f61d71f23 100644
--- a/nuttx/sched/sem_initialize.c
+++ b/nuttx/sched/sem_initialize.c
@@ -1,7 +1,7 @@
-/************************************************************
- * sem_initialize.c
+/****************************************************************************
+ * schec/sem_initialize.c
  *
- *   Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name Gregory Nutt nor the names of its contributors may be
+ * 3. Neither the name NuttX nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -31,49 +31,46 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Included Files
- ************************************************************/
+ ****************************************************************************/
 
 #include <sys/types.h>
 #include <queue.h>
-#include "sem_internal.h"
 
-/************************************************************
- * Compilation Switches
- ************************************************************/
+#include "sem_internal.h"
 
-/************************************************************
+/****************************************************************************
  * Definitions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Private Type Declarations
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Global Variables
- ************************************************************/
+ ****************************************************************************/
 
 /* This is a list of dyanamically allocated named semaphores */
 
 dq_queue_t g_nsems;
 
-/************************************************************
+/****************************************************************************
  * Private Variables
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Private Functions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Public Functions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Function: sem_initialize
  *
  * Description:
@@ -89,11 +86,15 @@ dq_queue_t g_nsems;
  *
  * Assumptions:
  *
- ************************************************************/
+ ****************************************************************************/
 
 void sem_initialize(void)
 {
   /* Initialize the queue of named semaphores */
 
   dq_init(&g_nsems);
+
+  /* Initialize holder structures needed to support priority inheritiance */
+
+  sem_initholders();
 }
diff --git a/nuttx/sched/sem_internal.h b/nuttx/sched/sem_internal.h
index 9908d6e4c..23c32df91 100644
--- a/nuttx/sched/sem_internal.h
+++ b/nuttx/sched/sem_internal.h
@@ -1,7 +1,7 @@
-/************************************************************
- * sem_internal.h
+/****************************************************************************
+ * sched/sem_internal.h
  *
- *   Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name Gregory Nutt nor the names of its contributors may be
+ * 3. Neither the name NuttX nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -31,14 +31,14 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************/
+ ****************************************************************************/
 
 #ifndef __SEM_INTERNAL_H
 #define __SEM_INTERNAL_H
 
-/************************************************************
+/****************************************************************************
  * Included Files
- ************************************************************/
+ ****************************************************************************/
 
 #include <sys/types.h>
 #include <semaphore.h>
@@ -46,13 +46,13 @@
 #include <queue.h>
 #include <nuttx/compiler.h>
 
-/************************************************************
+/****************************************************************************
  * Definitions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Public Type Declarations
- ************************************************************/
+ ****************************************************************************/
 
 /* This is the named semaphore structure */
 
@@ -67,17 +67,17 @@ struct nsem_s
 };
 typedef struct nsem_s nsem_t;
 
-/************************************************************
+/****************************************************************************
  * Public Variables
- ************************************************************/
+ ****************************************************************************/
 
 /* This is a list of dyanamically allocated named semaphores */
 
 extern dq_queue_t g_nsems;
 
-/************************************************************
+/****************************************************************************
  * Public Function Prototypes
- ************************************************************/
+ ****************************************************************************/
 
 #ifdef __cplusplus
 #define EXTERN extern "C"
@@ -90,6 +90,22 @@ EXTERN void weak_function sem_initialize(void);
 EXTERN void               sem_waitirq(FAR _TCB *wtcb);
 EXTERN FAR nsem_t        *sem_findnamed(const char *name);
 
+#ifdef CONFIG_PRIORITY_INHERITANCE
+EXTERN void sem_initholders(void);
+EXTERN void sem_destroyholder(sem_t *sem);
+EXTERN void sem_addholder(sem_t *sem);
+EXTERN void sem_boostpriority(sem_t *sem);
+EXTERN void sem_releaseholder(sem_t *sem);
+EXTERN void sem_restorebaseprio(sem_t *sem);
+#else
+#  define sem_initholders()
+#  define sem_destroyholder(sem)
+#  define sem_addholder(sem)
+#  define sem_boostpriority(sem)
+#  define sem_releaseholder(sem)
+#  define sem_restorebaseprio(sem)
+#endif
+
 #undef EXTERN
 #ifdef __cplusplus
 }
diff --git a/nuttx/sched/sem_post.c b/nuttx/sched/sem_post.c
index ca2047598..2a4700e74 100644
--- a/nuttx/sched/sem_post.c
+++ b/nuttx/sched/sem_post.c
@@ -46,10 +46,6 @@
 #include "sem_internal.h"
 
 /****************************************************************************
- * Compilation Switches
- ****************************************************************************/
-
-/****************************************************************************
  * Definitions
  ****************************************************************************/
 
@@ -104,9 +100,6 @@
 
 int sem_post(FAR sem_t *sem)
 {
-#ifdef CONFIG_PRIORITY_INHERITANCE
-  FAR _TCB  *rtcb = (FAR _TCB*)g_readytorun.head;
-#endif
   FAR _TCB  *stcb;
   STATUS     ret = ERROR;
   irqstate_t saved_state;
@@ -125,9 +118,7 @@ int sem_post(FAR sem_t *sem)
       /* Perform the semaphore unlock operation. */
 
       ASSERT(sem->semcount < SEM_VALUE_MAX);
-#ifdef CONFIG_PRIORITY_INHERITANCE
-      sem->holder = NULL;
-#endif
+      sem_releaseholder(sem);
       sem->semcount++;
 
       /* If the result of of semaphore unlock is non-positive, then
@@ -158,17 +149,12 @@ int sem_post(FAR sem_t *sem)
             }
         }
 
-      /* Check if we need to drop our priority.  Our priority could have
-       * been boosted while we held the semaphore.
+      /* Check if we need to drop the priority of any threads holding
+       * this semaphore.  The priority could have been boosted while they
+       * held the semaphore.
        */
 
-#ifdef CONFIG_PRIORITY_INHERITANCE
-      if (rtcb->sched_priority != rtcb->base_priority)
-        {
-           up_reprioritize_rtr(rtcb, rtcb->base_priority);
-        }
-#endif
-
+      sem_restorebaseprio(sem);
       ret = OK;
 
       /* Interrupts may now be enabled. */
diff --git a/nuttx/sched/sem_wait.c b/nuttx/sched/sem_wait.c
index 62c549cee..e6630c771 100644
--- a/nuttx/sched/sem_wait.c
+++ b/nuttx/sched/sem_wait.c
@@ -42,14 +42,11 @@
 #include <errno.h>
 #include <assert.h>
 #include <nuttx/arch.h>
+
 #include "os_internal.h"
 #include "sem_internal.h"
 
 /****************************************************************************
- * Compilation Switches
- ****************************************************************************/
-
-/****************************************************************************
  * Definitions
  ****************************************************************************/
 
@@ -98,10 +95,7 @@
 int sem_wait(FAR sem_t *sem)
 {
   FAR _TCB  *rtcb = (FAR _TCB*)g_readytorun.head;
-#ifdef CONFIG_PRIORITY_INHERITANCE
-  FAR _TCB  *htcb;
-#endif
-  int        ret = ERROR;
+  int        ret  = ERROR;
   irqstate_t saved_state;
 
   /* This API should not be called from interrupt handlers */
@@ -128,9 +122,7 @@ int sem_wait(FAR sem_t *sem)
           /* It is, let the task take the semaphore. */
 
           sem->semcount--;
-#ifdef CONFIG_PRIORITY_INHERITANCE
-          sem->holder = rtcb;
-#endif
+          sem_addholder(sem);
           rtcb->waitsem = NULL;
           ret = OK;
         }
@@ -168,22 +160,12 @@ int sem_wait(FAR sem_t *sem)
            */
 
           sched_lock();
-          htcb = sem->holder;
-          if (htcb && htcb->sched_priority < rtcb->sched_priority)
-            {
-              /* Raise the priority of the holder of the semaphore.  This
-               * cannot cause a context switch because we have preemption
-               * disabled.  The task will be marked "pending" and the switch
-               * will occur during up_block_task() processing.
-               *
-               * NOTE that we have to restore base_priority because
-               * sched_setparam() should set both.
-               */
-
-              int base_priority = htcb->base_priority;
-              (void)sched_settcbprio(htcb, rtcb->sched_priority);
-              htcb->base_priority = base_priority;
-            }
+
+          /* Boost the priority of any threads holding a count on the
+           * semaphore.
+           */
+
+          sem_boostpriority(sem);
 #endif
           /* Add the TCB to the prioritized semaphore wait queue */
 
@@ -203,9 +185,7 @@ int sem_wait(FAR sem_t *sem)
             {
               /* We hold the semaphore */
 
-#ifdef CONFIG_PRIORITY_INHERITANCE
-              sem->holder = rtcb;
-#endif
+              sem_addholder(sem);
               ret = OK;
             }
           else