sem: change sem wait to atomic operation

Add sem_wait fast operations, use atomic to ensure
atomicity of semcount operations, and do not depend
on critical section.

Test with robot:
before modify:
nxmutex_lock cost: 78 ns
nxmutex_unlock cost: 82 ns

after modify:
nxmutex_lock cost: 28 ns
nxmutex_unlock cost: 14 ns

Signed-off-by: zhangyuan29 <zhangyuan29@xiaomi.com>

include/limits.h

@@ -194,7 +194,7 @@
 /* Required for POSIX semaphores */
 
 #define _POSIX_SEM_NSEMS_MAX  INT_MAX
-#define _POSIX_SEM_VALUE_MAX  0x7fff
+#define _POSIX_SEM_VALUE_MAX  INT_MAX
 
 /* Numerical limits.  These values may be increased from the POSIX minimum
  * values above or made indeterminate

include/semaphore.h

@@ -73,7 +73,7 @@ struct semholder_s
   FAR struct semholder_s *tlink;  /* List of task held semaphores          */
   FAR struct sem_s *sem;          /* Ths corresponding semaphore           */
   FAR struct tcb_s *htcb;         /* Ths corresponding TCB                 */
-  int16_t counts;                 /* Number of counts owned by this holder */
+  int32_t counts;                 /* Number of counts owned by this holder */
 };
 
 #if CONFIG_SEM_PREALLOCHOLDERS > 0
@@ -104,7 +104,7 @@ struct semholder_s
 
 struct sem_s
 {
-  volatile int16_t semcount;     /* >0 -> Num counts available */
+  volatile int32_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

libs/libc/semaphore/sem_init.c

@@ -70,7 +70,7 @@ int nxsem_init(FAR sem_t *sem, int pshared, unsigned int value)
 
   /* Initialize the semaphore count */
 
-  sem->semcount = (int16_t)value;
+  sem->semcount = (int32_t)value;
 
   /* Initialize semaphore wait list */
 

sched/semaphore/sem_destroy.c

@@ -60,6 +60,8 @@
 
 int nxsem_destroy(FAR sem_t *sem)
 {
+  int32_t old;
+
   DEBUGASSERT(sem != NULL);
 
   /* There is really no particular action that we need
@@ -72,10 +74,15 @@ int nxsem_destroy(FAR sem_t *sem)
    * leave the count unchanged but still return OK.
    */
 
-  if (sem->semcount >= 0)
+  old = atomic_read(NXSEM_COUNT(sem));
+  do
     {
-      sem->semcount = 1;
+      if (old < 0)
+        {
+          break;
+        }
     }
+  while (!atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &old, 1));
 
   /* Release holders of the semaphore */
 

sched/semaphore/sem_holder.c

@@ -880,7 +880,7 @@ void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem)
 {
   /* Check our assumptions */
 
-  DEBUGASSERT(sem->semcount <= 0);
+  DEBUGASSERT(atomic_read(NXSEM_COUNT(sem)) <= 0);
 
   /* Adjust the priority of every holder as necessary */
 
@@ -978,7 +978,7 @@ void nxsem_release_all(FAR struct tcb_s *htcb)
        * that was taken by sem_wait() or sem_post().
        */
 
-      sem->semcount++;
+      atomic_fetch_add(NXSEM_COUNT(sem), 1);
     }
 }
 

sched/semaphore/sem_post.c

@@ -37,11 +37,11 @@
 #include "semaphore/semaphore.h"
 
 /****************************************************************************
- * Public Functions
+ * Private Functions
  ****************************************************************************/
 
 /****************************************************************************
- * Name: nxsem_post
+ * Name: nxsem_post_slow
  *
  * Description:
  *   When a kernel thread has finished with a semaphore, it will call
@@ -69,17 +69,15 @@
  *
  ****************************************************************************/
 
-int nxsem_post(FAR sem_t *sem)
+static int nxsem_post_slow(FAR sem_t *sem)
 {
   FAR struct tcb_s *stcb = NULL;
   irqstate_t flags;
-  int16_t sem_count;
+  int32_t sem_count;
 #if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT)
   uint8_t proto;
 #endif
 
-  DEBUGASSERT(sem != NULL);
-
   /* The following operations must be performed with interrupts
    * disabled because sem_post() may be called from an interrupt
    * handler.
@@ -87,15 +85,19 @@ int nxsem_post(FAR sem_t *sem)
 
   flags = enter_critical_section();
 
-  sem_count = sem->semcount;
-
   /* Check the maximum allowable value */
 
-  if (sem_count >= SEM_VALUE_MAX)
+  sem_count = atomic_read(NXSEM_COUNT(sem));
+  do
     {
-      leave_critical_section(flags);
+      if (sem_count >= SEM_VALUE_MAX)
-      return -EOVERFLOW;
+        {
+          leave_critical_section(flags);
+          return -EOVERFLOW;
+        }
     }
+  while (!atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &sem_count,
+                                     sem_count + 1));
 
   /* Perform the semaphore unlock operation, releasing this task as a
    * holder then also incrementing the count on the semaphore.
@@ -115,8 +117,6 @@ int nxsem_post(FAR sem_t *sem)
    */
 
   nxsem_release_holder(sem);
-  sem_count++;
-  sem->semcount = sem_count;
 
 #if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT)
   /* Don't let any unblocked tasks run until we complete any priority
@@ -138,7 +138,7 @@ int nxsem_post(FAR sem_t *sem)
    * there must be some task waiting for the semaphore.
    */
 
-  if (sem_count <= 0)
+  if (sem_count < 0)
     {
       /* Check if there are any tasks in the waiting for semaphore
        * task list that are waiting for this semaphore.  This is a
@@ -217,3 +217,58 @@ int nxsem_post(FAR sem_t *sem)
 
   return OK;
 }
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_post
+ *
+ * Description:
+ *   When a kernel thread has finished with a semaphore, it will call
+ *   nxsem_post().  This function unlocks the semaphore referenced by sem
+ *   by performing the semaphore unlock operation on that semaphore.
+ *
+ *   If the semaphore value resulting from this operation is positive, then
+ *   no tasks were blocked waiting for the semaphore to become unlocked; the
+ *   semaphore is simply incremented.
+ *
+ *   If the value of the semaphore resulting from this operation is zero,
+ *   then one of the tasks blocked waiting for the semaphore shall be
+ *   allowed to return successfully from its call to nxsem_wait().
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor
+ *
+ * Returned Value:
+ *   This is an internal OS interface and should not be used by applications.
+ *   It follows the NuttX internal error return policy:  Zero (OK) is
+ *   returned on success.  A negated errno value is returned on failure.
+ *
+ * Assumptions:
+ *   This function may be called from an interrupt handler.
+ *
+ ****************************************************************************/
+
+int nxsem_post(FAR sem_t *sem)
+{
+  DEBUGASSERT(sem != NULL);
+
+  /* If this is a mutex, we can try to unlock the mutex in fast mode,
+   * else try to get it in slow mode.
+   */
+
+#if !defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_PRIORITY_PROTECT)
+  if (sem->flags & SEM_TYPE_MUTEX)
+    {
+      int32_t old = 0;
+      if (atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &old, 1))
+        {
+          return OK;
+        }
+    }
+#endif
+
+  return nxsem_post_slow(sem);
+}

sched/semaphore/sem_recover.c

@@ -85,7 +85,7 @@ void nxsem_recover(FAR struct tcb_s *tcb)
   if (tcb->task_state == TSTATE_WAIT_SEM)
     {
       FAR sem_t *sem = tcb->waitobj;
-      DEBUGASSERT(sem != NULL && sem->semcount < 0);
+      DEBUGASSERT(sem != NULL && atomic_read(NXSEM_COUNT(sem)) < 0);
 
       /* Restore the correct priority of all threads that hold references
        * to this semaphore.
@@ -99,7 +99,7 @@ void nxsem_recover(FAR struct tcb_s *tcb)
        * place.
        */
 
-      sem->semcount++;
+      atomic_fetch_add(NXSEM_COUNT(sem), 1);
     }
 
   /* Release all semphore holders for the task */

sched/semaphore/sem_reset.c

@@ -60,6 +60,7 @@
 int nxsem_reset(FAR sem_t *sem, int16_t count)
 {
   irqstate_t flags;
+  int32_t semcount;
 
   DEBUGASSERT(sem != NULL && count >= 0);
 
@@ -80,7 +81,7 @@ int nxsem_reset(FAR sem_t *sem, int16_t count)
    * out counts to any waiting threads.
    */
 
-  while (sem->semcount < 0 && count > 0)
+  while (atomic_read(NXSEM_COUNT(sem)) < 0 && count > 0)
     {
       /* Give out one counting, waking up one of the waiting threads
        * and, perhaps, kicking off a lot of priority inheritance
@@ -98,10 +99,15 @@ int nxsem_reset(FAR sem_t *sem, int16_t count)
    * value of sem->semcount is already correct in this case.
    */
 
-  if (sem->semcount >= 0)
+  semcount = atomic_read(NXSEM_COUNT(sem));
+  do
     {
-      sem->semcount = count;
+      if (semcount < 0)
+        {
+          break;
+        }
     }
+  while (!atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &semcount, count));
 
   /* Allow any pending context switches to occur now */
 

sched/semaphore/sem_trywait.c

@@ -38,6 +38,75 @@
 #include "sched/sched.h"
 #include "semaphore/semaphore.h"
 
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_trywait_slow
+ *
+ * Description:
+ *   This function locks the specified semaphore in slow mode.
+ *
+ * Input Parameters:
+ *   sem - the semaphore descriptor
+ *
+ * Returned Value:
+ *
+ *     EINVAL - Invalid attempt to get the semaphore
+ *     EAGAIN - The semaphore is not available.
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+static int nxsem_trywait_slow(FAR sem_t *sem)
+{
+  FAR struct tcb_s *rtcb;
+  irqstate_t flags;
+  int32_t semcount;
+  int ret;
+
+  /* The following operations must be performed with interrupts disabled
+   * because sem_post() may be called from an interrupt handler.
+   */
+
+  flags = enter_critical_section();
+  rtcb = this_task();
+
+  /* If the semaphore is available, give it to the requesting task */
+
+  semcount = atomic_read(NXSEM_COUNT(sem));
+  do
+    {
+      if (semcount <= 0)
+        {
+          leave_critical_section(flags);
+          return -EAGAIN;
+        }
+    }
+  while (!atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem),
+                                     &semcount, semcount - 1));
+
+  /* It is, let the task take the semaphore */
+
+  ret = nxsem_protect_wait(sem);
+  if (ret < 0)
+    {
+      atomic_fetch_add(NXSEM_COUNT(sem), 1);
+      leave_critical_section(flags);
+      return ret;
+    }
+
+  nxsem_add_holder(sem);
+  rtcb->waitobj = NULL;
+
+  /* Interrupts may now be enabled. */
+
+  leave_critical_section(flags);
+  return ret;
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -68,49 +137,28 @@
 
 int nxsem_trywait(FAR sem_t *sem)
 {
-  FAR struct tcb_s *rtcb = this_task();
-  irqstate_t flags;
-  int ret;
-
   /* This API should not be called from the idleloop */
 
   DEBUGASSERT(sem != NULL);
   DEBUGASSERT(!OSINIT_IDLELOOP() || !sched_idletask() ||
               up_interrupt_context());
 
-  /* The following operations must be performed with interrupts disabled
+  /* If this is a mutex, we can try to get the mutex in fast mode,
-   * because sem_post() may be called from an interrupt handler.
+   * else try to get it in slow mode.
    */
 
-  flags = enter_critical_section();
+#if !defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_PRIORITY_PROTECT)
-
+  if (sem->flags & SEM_TYPE_MUTEX)
-  /* If the semaphore is available, give it to the requesting task */
-
-  if (sem->semcount > 0)
     {
-      /* It is, let the task take the semaphore */
+      int32_t old = 1;
-
+      if (atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0))
-      ret = nxsem_protect_wait(sem);
-      if (ret < 0)
         {
-          leave_critical_section(flags);
+          return OK;
-          return ret;
         }
 
-      sem->semcount--;
+      return -EAGAIN;
-      nxsem_add_holder(sem);
-      rtcb->waitobj = NULL;
-      ret = OK;
     }
-  else
+#endif
-    {
-      /* Semaphore is not available */
 
-      ret = -EAGAIN;
+  return nxsem_trywait_slow(sem);
-    }
-
-  /* Interrupts may now be enabled. */
-
-  leave_critical_section(flags);
-  return ret;
 }

sched/semaphore/sem_wait.c

@@ -38,16 +38,15 @@
 #include "semaphore/semaphore.h"
 
 /****************************************************************************
- * Public Functions
+ * Private Functions
  ****************************************************************************/
 
 /****************************************************************************
- * Name: nxsem_wait
+ * Name: nxsem_wait_slow
  *
  * Description:
- *   This function attempts to lock the semaphore referenced by 'sem'.  If
+ *   This function attempts to lock the semaphore referenced by 'sem' in
- *   the semaphore value is (<=) zero, then the calling task will not return
+ *   slow mode.
- *   until it successfully acquires the lock.
  *
  *   This is an internal OS interface.  It is functionally equivalent to
  *   sem_wait except that:
@@ -69,17 +68,12 @@
  *
  ****************************************************************************/
 
-int nxsem_wait(FAR sem_t *sem)
+static int nxsem_wait_slow(FAR sem_t *sem)
 {
   FAR struct tcb_s *rtcb = this_task();
   irqstate_t flags;
   int ret;
 
-  /* This API should not be called from interrupt handlers & idleloop */
-
-  DEBUGASSERT(sem != NULL && up_interrupt_context() == false);
-  DEBUGASSERT(!OSINIT_IDLELOOP() || !sched_idletask());
-
   /* The following operations must be performed with interrupts
    * disabled because nxsem_post() may be called from an interrupt
    * handler.
@@ -91,21 +85,20 @@ int nxsem_wait(FAR sem_t *sem)
 
   /* Check if the lock is available */
 
-  if (sem->semcount > 0)
+  if (atomic_fetch_sub(NXSEM_COUNT(sem), 1) > 0)
     {
       /* It is, let the task take the semaphore. */
 
       ret = nxsem_protect_wait(sem);
       if (ret < 0)
         {
+          atomic_fetch_add(NXSEM_COUNT(sem), 1);
           leave_critical_section(flags);
           return ret;
         }
 
-      sem->semcount--;
       nxsem_add_holder(sem);
       rtcb->waitobj = NULL;
-      ret = OK;
     }
 
   /* The semaphore is NOT available, We will have to block the
@@ -124,10 +117,6 @@ int nxsem_wait(FAR sem_t *sem)
 
       DEBUGASSERT(rtcb->waitobj == NULL);
 
-      /* Handle the POSIX semaphore (but don't set the owner yet) */
-
-      sem->semcount--;
-
       /* Save the waited on semaphore in the TCB */
 
       rtcb->waitobj = sem;
@@ -224,6 +213,63 @@ int nxsem_wait(FAR sem_t *sem)
   return ret;
 }
 
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_wait
+ *
+ * Description:
+ *   This function attempts to lock the semaphore referenced by 'sem'.  If
+ *   the semaphore value is (<=) zero, then the calling task will not return
+ *   until it successfully acquires the lock.
+ *
+ *   This is an internal OS interface.  It is functionally equivalent to
+ *   sem_wait except that:
+ *
+ *   - It is not a cancellation point, and
+ *   - It does not modify the errno value.
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor.
+ *
+ * Returned Value:
+ *   This is an internal OS interface and should not be used by applications.
+ *   It follows the NuttX internal error return policy:  Zero (OK) is
+ *   returned on success.  A negated errno value is returned on failure.
+ *   Possible returned errors:
+ *
+ *   - EINVAL:  Invalid attempt to get the semaphore
+ *   - EINTR:   The wait was interrupted by the receipt of a signal.
+ *
+ ****************************************************************************/
+
+int nxsem_wait(FAR sem_t *sem)
+{
+  /* This API should not be called from interrupt handlers & idleloop */
+
+  DEBUGASSERT(sem != NULL && up_interrupt_context() == false);
+  DEBUGASSERT(!OSINIT_IDLELOOP() || !sched_idletask());
+
+  /* If this is a mutex, we can try to get the mutex in fast mode,
+   * else try to get it in slow mode.
+   */
+
+#if !defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_PRIORITY_PROTECT)
+  if (sem->flags & SEM_TYPE_MUTEX)
+    {
+      int32_t old = 1;
+      if (atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0))
+        {
+          return OK;
+        }
+    }
+#endif
+
+  return nxsem_wait_slow(sem);
+}
+
 /****************************************************************************
  * Name: nxsem_wait_uninterruptible
  *

sched/semaphore/sem_waitirq.c

@@ -87,7 +87,7 @@ void nxsem_wait_irq(FAR struct tcb_s *wtcb, int errcode)
    * and already changed the task's state.
    */
 
-  DEBUGASSERT(sem != NULL && sem->semcount < 0);
+  DEBUGASSERT(sem != NULL && atomic_read(NXSEM_COUNT(sem)) < 0);
 
   /* Restore the correct priority of all threads that hold references
    * to this semaphore.
@@ -101,7 +101,7 @@ void nxsem_wait_irq(FAR struct tcb_s *wtcb, int errcode)
    * place.
    */
 
-  sem->semcount++;
+  atomic_fetch_add(NXSEM_COUNT(sem), 1);
 
   /* Remove task from waiting list */
 

sched/semaphore/semaphore.h

@@ -31,10 +31,17 @@
 #include <nuttx/compiler.h>
 #include <nuttx/semaphore.h>
 #include <nuttx/sched.h>
+#include <nuttx/atomic.h>
 
 #include <stdint.h>
 #include <stdbool.h>
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define NXSEM_COUNT(s) ((FAR atomic_t *)&(s)->semcount)
+
 /****************************************************************************
  * Public Function Prototypes
  ****************************************************************************/