More priority inheritance logic

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

8 files changed, 387 insertions, 82 deletions

diff --git a/nuttx/sched/Makefile b/nuttx/sched/Makefile
index 51554323d..b16ab0b94 100644
--- a/nuttx/sched/Makefile
+++ b/nuttx/sched/Makefile
@@ -48,7 +48,7 @@ TSK_SRCS	= task_create.c task_init.c task_setup.c task_activate.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_free.c sched_gettcb.c sched_verifytcb.c sched_releasetcb.c
 
 SCHED_SRCS	= sched_setparam.c sched_setpriority.c sched_getparam.c \
 		  sched_setscheduler.c sched_getscheduler.c \
@@ -56,7 +56,7 @@ SCHED_SRCS	= sched_setparam.c sched_setpriority.c sched_getparam.c \
 		  sched_getprioritymax.c sched_getprioritymin.c \
 		  sched_lock.c sched_unlock.c sched_lockcount.c
 ifeq ($(CONFIG_PRIORITY_INHERITANCE),y)
-SEM_SRCS	+= sched_reprioritize.c 
+SCHED_SRCS	+= sched_reprioritize.c 
 endif
 
 ENV_SRCS	= env_getenvironptr.c env_dup.c env_share.c env_release.c \
diff --git a/nuttx/sched/os_internal.h b/nuttx/sched/os_internal.h
index 56a4e8ec6..b07818345 100644
--- a/nuttx/sched/os_internal.h
+++ b/nuttx/sched/os_internal.h
@@ -259,6 +259,7 @@ extern int     sched_reprioritize(FAR _TCB *tcb, int sched_priority);
 #  define sched_reprioritize(tcb,sched_priority) sched_setpriority(tcb,sched_priority)
 #endif
 extern FAR _TCB *sched_gettcb(pid_t pid);
+extern boolean sched_verifytcb(FAR _TCB *tcb);
 
 #if CONFIG_NFILE_DESCRIPTORS > 0 || CONFIG_NSOCKET_DESCRIPTORS > 0
 extern int     sched_setupidlefiles(FAR _TCB *tcb);
diff --git a/nuttx/sched/sched_gettcb.c b/nuttx/sched/sched_gettcb.c
index 7f65f9342..a44e310e6 100644
--- a/nuttx/sched/sched_gettcb.c
+++ b/nuttx/sched/sched_gettcb.c
@@ -1,7 +1,7 @@
-/************************************************************
- * sched_gettcb.c
+/****************************************************************************
+ * sched/ched_gettcb.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,41 +31,41 @@
  * 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 <sched.h>
 #include "os_internal.h"
 
-/************************************************************
+/****************************************************************************
  * Definitions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Private Type Declarations
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Global Variables
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Private Variables
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Private Function Prototypes
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Public Functions
- ************************************************************/
+ ****************************************************************************/
 
-/************************************************************
+/****************************************************************************
  * Name: sched_gettcb
  *
  * Description:
@@ -73,7 +73,7 @@
  *   the a pointer to the corresponding TCB (or NULL if there
  *   is no such task ID).
  *
- ************************************************************/
+ ****************************************************************************/
 
 FAR _TCB *sched_gettcb(pid_t pid)
 {
diff --git a/nuttx/sched/sched_verifytcb.c b/nuttx/sched/sched_verifytcb.c
new file mode 100644
index 000000000..21810940e
--- /dev/null
+++ b/nuttx/sched/sched_verifytcb.c
@@ -0,0 +1,83 @@
+/****************************************************************************
+ * sched/ched_verifytcb.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 <sched.h>
+#include "os_internal.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: sched_verifytcb
+ *
+ * Description:
+ *   Return TRUE if the tcb refers to an active task; FALSE if it is a stale
+ *   TCB handle.
+ *
+ ****************************************************************************/
+
+boolean sched_verifytcb(FAR _TCB *tcb)
+{
+  /* Return true if the PID hashes to this TCB. */
+
+  return tcb == g_pidhash[PIDHASH(tcb->pid)].tcb;
+}
+
diff --git a/nuttx/sched/sem_holder.c b/nuttx/sched/sem_holder.c
index b222512fd..d8694c79d 100644
--- a/nuttx/sched/sem_holder.c
+++ b/nuttx/sched/sem_holder.c
@@ -40,6 +40,7 @@
 #include <sys/types.h>
 #include <semaphore.h>
 #include <sched.h>
+#include <assert.h>
 #include <debug.h>
 #include <nuttx/arch.h>
 
@@ -62,6 +63,8 @@
  * Private Type Declarations
  ****************************************************************************/
 
+typedef int (*holderhandler_t)(struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg);
+
 /****************************************************************************
  * Global Variables
  ****************************************************************************/
@@ -208,6 +211,172 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
 }
 
 /****************************************************************************
+ * Name: sem_foreachholder
+ ****************************************************************************/
+
+static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler, FAR void *arg)
+{
+  struct semholder_s *pholder = &sem->hlist;
+  int ret = 0;
+
+#if CONFIG_SEM_PREALLOCHOLDERS > 0
+  for (; pholder && ret == 0; pholder = next)
+#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->holder)
+        {
+          /* Call the handler */
+
+          ret = handler(pholder, sem, arg);
+        }
+    }
+  return ret;
+}
+
+/****************************************************************************
+ * Name: sem_verifyholder
+ ****************************************************************************/
+
+#ifdef CONFIG_DEBUG
+static int sem_verifyholder(struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
+{
+  FAR _TCB *htcb = (FAR _TCB *)pholder->holder;
+
+#if CONFIG_SEM_NNESTPRIO > 0
+  DEBUGASSERT(htcb->npend_repri == 0);
+#endif
+  DEBUGASSERT(htcb->sched_priority == htcb->base_priority);
+  return 0;
+}
+#endif
+
+/****************************************************************************
+ * Name: sem_restoreholderprio
+ ****************************************************************************/
+
+static int sem_restoreholderprio(struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg)
+{
+  FAR _TCB *htcb = (FAR _TCB *)pholder->holder;
+#if CONFIG_SEM_NNESTPRIO > 0
+  FAR _TCB *stcb = (FAR _TCB *)arg;
+  int rpriority;
+  int i;
+  int j;
+#endif
+
+  /* Make sure that the 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 know that the program is doing.  Perhaps its
+   * plan is to kill a thread, then destroy the semaphore.
+   */
+
+  if (!sched_verifytcb(htcb))
+   {
+      sdbg("TCB 0x%08x is a stale handle, counts lost\n", htcb);
+      sem_freeholder(sem, pholder);
+   }
+
+  /* Was the priority of this thread boosted? If so, then drop its priority
+   * back to the correct level.
+   */
+
+  else if (htcb->sched_priority != htcb->base_priority)
+    {
+#if CONFIG_SEM_NNESTPRIO > 0
+      /* Are there other, pending priority levels to revert to? */
+
+      if (htcb->npend_reprio < 1)
+        {
+          /* No... the thread has only been boosted once */
+
+          DEBUGASSERT(hctb->sched_priority == stcb->sched_priority && npend_reprio == 0);
+          rpriority = htcb->base_priority;
+        }
+
+      /* There are multiple pending priority levels. The thread's "boosted"
+       * priority could greater than or equal to "stcb->sched_priority" (it could be
+       * greater if its priority we boosted becuase it also holds another semaphore.
+       */
+
+      else if (htcb->sched_priority <= stcb->sched_priority)
+        {
+          /* The thread has been boosted to the same priority as the task
+           * that just received the count.  We will simply reprioritized
+           * 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 (htcb->pend_reprios[i] > htcb->pend_reprios[j])
+                {
+                  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 */
+
+          sched_reprioritize(htcb, rpriority);
+        }                    
+      else
+        {
+          /* The thread has been boosted to a higher priority than the task.  The
+           * pending priority should be in he list (unless it was lost because of
+           * of list overflow).
+           *
+           * 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)
+                    {
+                      htcb->pend_reprios[i] = htcb->pend_reprios[j];
+                    }
+                   htcb->npend_reprio = j;
+                   break;
+                }
+            }
+        }
+#else
+      /* There is no alternative restore priorities, drop the priority
+       * all the way back to the threads "base" priority.
+       */
+
+      sched_reprioritize(htcb, htcb->base_priority);
+#endif
+    }
+  return 0;
+}
+
+/****************************************************************************
  * Public Functions
  ****************************************************************************/
 
@@ -260,7 +429,7 @@ void sem_initholders(void)
  *
  ****************************************************************************/
 
-void sem_destroyholder(sem_t *sem)
+void sem_destroyholder(FAR sem_t *sem)
 {
 #if 0
   FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
@@ -297,7 +466,7 @@ void sem_destroyholder(sem_t *sem)
  *
  ****************************************************************************/
 
-void sem_addholder(sem_t *sem)
+void sem_addholder(FAR sem_t *sem)
 {
   FAR _TCB               *rtcb = (FAR _TCB*)g_readytorun.head;
   FAR struct semholder_s *pholder;
@@ -330,7 +499,7 @@ void sem_addholder(sem_t *sem)
  *
  ****************************************************************************/
 
-void sem_boostpriority(sem_t *sem)
+void sem_boostpriority(FAR sem_t *sem)
 {
   FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
   FAR _TCB *htcb;
@@ -387,7 +556,7 @@ void sem_boostpriority(sem_t *sem)
  *
  ****************************************************************************/
 
-void sem_releaseholder(sem_t *sem)
+void sem_releaseholder(FAR sem_t *sem)
 {
   FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
   FAR struct semholder_s *pholder;
@@ -407,12 +576,24 @@ void sem_releaseholder(sem_t *sem)
  * 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.
+ *   This function is called after the current running task releases a
+ *   count on the semaphore.  It will check if we need to drop the priority
+ *   of any threads holding a count on the semaphore.  Their priority could
+ *   have been boosted while they held the count.
  *
  * Parameters:
- *   sem - A reference to the semaphore being posted
+ *   stcb - The TCB of the task that was just started (if any).  If the
+ *     post action caused a count to be given to another thread, then stcb
+ *     is the TCB that received the count.  Note, just because stcb received
+ *     the count, it does not mean that it it is higher priority than other threads.
+ *   sem - A reference to the semaphore being posted.
+ *     - If the semaphore count is <0 then there are still threads waiting
+ *       for a count.  stcb should be non-null and will be higher priority than
+ *       all of the other threads still waiting.
+ *     - If it is ==0 then stcb refers to the thread that got the last count; no
+ *       other threads are waiting.
+ *     - If it is >0 then there should be no threads waiting for counts and
+ *       stcb should be null.
  *
  * Return Value:
  *   0 (OK) or -1 (ERROR) if unsuccessful
@@ -421,58 +602,84 @@ void sem_releaseholder(sem_t *sem)
  *
  ****************************************************************************/
 
-void sem_restorebaseprio(sem_t *sem)
+void sem_restorebaseprio(FAR _TCB *stcb, FAR sem_t *sem)
 {
-  FAR _TCB  *rtcb = (FAR _TCB*)g_readytorun.head;
-  FAR _TCB *htcb;
+  FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
   struct semholder_s *pholder;
-#if CONFIG_SEM_PREALLOCHOLDERS > 0
-  struct semholder_s *next;
-#endif
 
-  /* Check if the semaphore is still available */
+  /* Check our assumptions */
+
+  DEBUGASSERT((sem->semcount > 0  && stcb == NULL) ||
+              (sem->semcount <= 0 && stcb != NULL));
+
+  /* Perfom the following actions only if a new thread was given a count. */
 
-  if (sem->semcount > 0)
+  if (stcb)
     {
-      /* Traverse the list of holders */
+      /* Adjust the priority of every holder as necessary */
 
-      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 */
+      (void)sem_foreachholder(sem, sem_restoreholderprio, stcb);
+    }
 
-          htcb = pholder->holder;
-          if (htcb)
-            {
-#if CONFIG_SEM_NNESTPRIO > 0
-#  error "Missing implementation"
-#else
-              /* 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 there are no tasks waiting for available counts, then all holders
+   * should be at their base priority.
+   */
 
-              if (htcb->sched_priority != htcb->base_priority)
-                {
-                  up_reprioritize_rtr(rtcb, htcb->base_priority);
-                }
+#ifdef CONFIG_DEBUG
+  else
+    {
+      (void)sem_foreachholder(sem, sem_verifyholder, NULL);
+    }
 #endif
-              /* When no more counts are held, remove the holder from the list */
 
-              if (pholder->counts <= 0)
-                {
-	          sem_freeholder(sem, pholder);
-                }
-            }
+  /* In any case, the currently execuing task should have an entry in the 
+   * list and we need to decrement the number of counts that it holds.  When it
+   * holds no further counts, it must be removed 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.
+       */
+
+      if (pholder->counts <= 0)
+        {
+          sem_freeholder(sem, pholder);
         }
     }
 }
 
+/****************************************************************************
+ * Function:  sem_canceled
+ *
+ * Description:
+ *   Called from sem_post() after a thread that was waiting for a semaphore
+ *   count was awakened because of a signal and the semaphore wait has been
+ *   canceld.
+ *
+ * Parameters:
+ *   sem - A reference to the semaphore no longer being waited for
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void sem_canceled(FAR sem_t *sem)
+{
+  FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head;
+
+  /* Check our assumptions */
+
+  DEBUGASSERT(sem->semcount <= 0);
+
+  /* Adjust the priority of every holder as necessary */
+
+  (void)sem_foreachholder(sem, sem_restoreholderprio, rtcb);
+}
+
 #endif /* CONFIG_PRIORITY_INHERITANCE */
diff --git a/nuttx/sched/sem_internal.h b/nuttx/sched/sem_internal.h
index 23c32df91..1fa1b9c81 100644
--- a/nuttx/sched/sem_internal.h
+++ b/nuttx/sched/sem_internal.h
@@ -92,18 +92,20 @@ 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);
+EXTERN void sem_destroyholder(FAR sem_t *sem);
+EXTERN void sem_addholder(FAR sem_t *sem);
+EXTERN void sem_boostpriority(FAR sem_t *sem);
+EXTERN void sem_releaseholder(FAR sem_t *sem);
+EXTERN void sem_restorebaseprio(FAR _TCB *stcb, FAR sem_t *sem);
+EXTERN void sem_canceled(FAR 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)
+#  define sem_restorebaseprio(stcb,sem)
+#  define sem_canceled(sem)
 #endif
 
 #undef EXTERN
diff --git a/nuttx/sched/sem_post.c b/nuttx/sched/sem_post.c
index 2a4700e74..3c5ccaf77 100644
--- a/nuttx/sched/sem_post.c
+++ b/nuttx/sched/sem_post.c
@@ -100,7 +100,7 @@
 
 int sem_post(FAR sem_t *sem)
 {
-  FAR _TCB  *stcb;
+  FAR _TCB  *stcb = NULL;
   STATUS     ret = ERROR;
   irqstate_t saved_state;
 
@@ -125,6 +125,13 @@ int sem_post(FAR sem_t *sem)
        * there must be some task waiting for the semaphore.
        */
 
+#ifdef CONFIG_PRIORITY_INHERITANCE
+      /* Don't let it run until we complete the priority restoration
+       * steps.
+       */
+
+      sched_lock();
+#endif
       if (sem->semcount <= 0)
         {
           /* Check if there are any tasks in the waiting for semaphore
@@ -154,7 +161,10 @@ int sem_post(FAR sem_t *sem)
        * held the semaphore.
        */
 
-      sem_restorebaseprio(sem);
+#ifdef CONFIG_PRIORITY_INHERITANCE
+      sem_restorebaseprio(stcb, sem);
+      sched_unlock();
+#endif
       ret = OK;
 
       /* Interrupts may now be enabled. */
diff --git a/nuttx/sched/sem_wait.c b/nuttx/sched/sem_wait.c
index e6630c771..94622ae37 100644
--- a/nuttx/sched/sem_wait.c
+++ b/nuttx/sched/sem_wait.c
@@ -172,9 +172,6 @@ int sem_wait(FAR sem_t *sem)
           errno = 0;
           up_block_task(rtcb, TSTATE_WAIT_SEM);
 
-#ifdef CONFIG_PRIORITY_INHERITANCE
-          sched_unlock();
-#endif
           /* When we resume at this point, either (1) the semaphore has been
            * assigned to this thread of execution, or (2) the semaphore wait
            * has been interrupted by a signal.  We can detect the latter case
@@ -190,8 +187,13 @@ int sem_wait(FAR sem_t *sem)
             }
           else
             {
+              sem_canceled(sem);	            
               sem->semcount++;
             }
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+          sched_unlock();
+#endif
         }
 
       /* Interrupts may now be enabled. */