diff --git a/sched/semaphore/sem_holder.c b/sched/semaphore/sem_holder.c
index f56eb1bc8d..769444fd99 100644
--- a/sched/semaphore/sem_holder.c
+++ b/sched/semaphore/sem_holder.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * sched/semaphore/sem_holder.c
  *
- *   Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2009-2011, 2013, 2016 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -832,6 +832,43 @@ void sem_destroyholder(FAR sem_t *sem)
 #endif
 }
 
+/****************************************************************************
+ * Name: sem_addholder_tcb
+ *
+ * Description:
+ *   Called from sem_wait() when the calling thread obtains the semaphore;
+ *   Called from sem_post() when the waiting thread obtains the semaphore.
+ *
+ * Parameters:
+ *   htcb - TCB of the thread that just obtained the semaphore
+ *   sem  - A reference to the incremented semaphore
+ *
+ * Return Value:
+ *   0 (OK) or -1 (ERROR) if unsuccessful
+ *
+ * Assumptions:
+ *   Interrupts are disabled.
+ *
+ ****************************************************************************/
+
+void sem_addholder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem)
+{
+  FAR struct semholder_s *pholder;
+
+  /* Find or allocate a container for this new holder */
+
+  pholder = sem_findorallocateholder(sem, htcb);
+  if (pholder != NULL)
+    {
+      /* Then set the holder and increment the number of counts held by this
+       * holder
+       */
+
+      pholder->htcb = htcb;
+      pholder->counts++;
+    }
+}
+
 /****************************************************************************
  * Name: sem_addholder
  *
@@ -845,26 +882,13 @@ void sem_destroyholder(FAR sem_t *sem)
  *   0 (OK) or -1 (ERROR) if unsuccessful
  *
  * Assumptions:
+ *   Interrupts are disabled.
  *
  ****************************************************************************/
 
 void sem_addholder(FAR sem_t *sem)
 {
-  FAR struct tcb_s *rtcb = this_task();
-  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->htcb = rtcb;
-      pholder->counts++;
-    }
+  sem_addholder_tcb(this_task(), sem);
 }
 
 /****************************************************************************
diff --git a/sched/semaphore/sem_post.c b/sched/semaphore/sem_post.c
index c985ceb9c4..1c90449953 100644
--- a/sched/semaphore/sem_post.c
+++ b/sched/semaphore/sem_post.c
@@ -131,8 +131,14 @@ int sem_post(FAR sem_t *sem)
                (stcb && stcb->waitsem != sem);
                stcb = stcb->flink);
 
-          if (stcb)
+          if (stcb != NULL)
             {
+              /* The task will be the new holder of the semaphore when
+               * it is awakened.
+               */
+
+              sem_addholder_tcb(stcb, sem);
+
               /* It is, let the task take the semaphore */
 
               stcb->waitsem = NULL;
diff --git a/sched/semaphore/sem_wait.c b/sched/semaphore/sem_wait.c
index 49371e0ae6..9c19d406d7 100644
--- a/sched/semaphore/sem_wait.c
+++ b/sched/semaphore/sem_wait.c
@@ -173,9 +173,12 @@ int sem_wait(FAR sem_t *sem)
 
           if (get_errno() != EINTR && get_errno() != ETIMEDOUT)
             {
-              /* Not awakened by a signal or a timeout... We hold the semaphore */
+              /* Not awakened by a signal or a timeout...
+               *
+               * NOTE that in this case sem_addholder() was called by logic
+               * in sem_wait() fore this thread was restarted.
+               */
 
-              sem_addholder(sem);
               ret = OK;
             }
 
diff --git a/sched/semaphore/semaphore.h b/sched/semaphore/semaphore.h
index 85f4e6c876..cc7a929816 100644
--- a/sched/semaphore/semaphore.h
+++ b/sched/semaphore/semaphore.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * sched/semaphore/semaphore.h
  *
- *   Copyright (C) 2007, 2009-2015 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007, 2009-2016 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -89,6 +89,7 @@ void sem_recover(FAR struct tcb_s *tcb);
 void sem_initholders(void);
 void sem_destroyholder(FAR sem_t *sem);
 void sem_addholder(FAR sem_t *sem);
+void sem_addholder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem);
 void sem_boostpriority(FAR sem_t *sem);
 void sem_releaseholder(FAR sem_t *sem);
 void sem_restorebaseprio(FAR struct tcb_s *stcb, FAR sem_t *sem);
@@ -101,10 +102,11 @@ void sem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem);
 #  define sem_initholders()
 #  define sem_destroyholder(sem)
 #  define sem_addholder(sem)
+#  define sem_addholder_tcb(htcb,sem)
 #  define sem_boostpriority(sem)
 #  define sem_releaseholder(sem)
 #  define sem_restorebaseprio(stcb,sem)
-#  define sem_canceled(stcb, sem)
+#  define sem_canceled(stcb,sem)
 #endif
 
 #undef EXTERN