Integrate nxmutex support fully into nxsem

This puts the mutex support fully inside nxsem, allowing
locking the mutex and setting the holder with single atomic
operation.

This enables fast mutex locking from userspace, avoiding taking
critical_sections, which may be heavy in SMP and cleanup
of nxmutex library in the future.

Signed-off-by: Jukka Laitinen <[email protected]>

Author: jlaitine

include/nuttx/mutex.h

@@ -36,10 +36,12 @@
  * Pre-processor Definitions
  ****************************************************************************/
 
-#define NXMUTEX_NO_HOLDER      ((pid_t)-1)
-#define NXMUTEX_INITIALIZER    {NXSEM_INITIALIZER(1, SEM_TYPE_MUTEX | \
-                                SEM_PRIO_INHERIT), NXMUTEX_NO_HOLDER}
-#define NXRMUTEX_INITIALIZER   {NXMUTEX_INITIALIZER, 0}
+#define NXMUTEX_NO_HOLDER      NXSEM_NO_MHOLDER
+#define NXMUTEX_INITIALIZER  {                                             \
+  NXSEM_INITIALIZER(NXMUTEX_NO_HOLDER, SEM_TYPE_MUTEX | SEM_PRIO_INHERIT), \
+  NXMUTEX_NO_HOLDER}
+
+#define NXRMUTEX_INITIALIZER {NXMUTEX_INITIALIZER, 0}
 
 /****************************************************************************
  * Public Type Definitions

include/nuttx/semaphore.h

@@ -45,23 +45,42 @@
 /* semcount, flags, waitlist, hhead */
 
 #    define NXSEM_INITIALIZER(c, f) \
-       {(c), (f), SEM_WAITLIST_INITIALIZER, NULL}
+       {{c}, (f), SEM_WAITLIST_INITIALIZER, NULL}
 #  else
 /* semcount, flags, waitlist, holder[2] */
 
 #    define NXSEM_INITIALIZER(c, f) \
-       {(c), (f), SEM_WAITLIST_INITIALIZER, SEMHOLDER_INITIALIZER}
+       {{(c)}, (f), SEM_WAITLIST_INITIALIZER, SEMHOLDER_INITIALIZER}
 #  endif
 #else /* CONFIG_PRIORITY_INHERITANCE */
 /* semcount, flags, waitlist */
 
 #  define NXSEM_INITIALIZER(c, f) \
-     {(c), (f), SEM_WAITLIST_INITIALIZER}
+     {{(c)}, (f), SEM_WAITLIST_INITIALIZER}
 #endif /* CONFIG_PRIORITY_INHERITANCE */
 
-/* Macro to retrieve sem count */
+/* Macros to retrieve sem count and to check if nxsem is mutex */
 
-#define NXSEM_COUNT(s) ((FAR atomic_t *)&(s)->semcount)
+#define NXSEM_COUNT(s)        ((FAR atomic_t *)&(s)->val.semcount)
+#define NXSEM_IS_MUTEX(s)     (((s)->flags & SEM_TYPE_MUTEX) != 0)
+
+/* Mutex related helper macros */
+
+#define NXSEM_MBLOCKS_BIT    (((uint32_t)1) << 31)
+#define NXSEM_NO_MHOLDER     ((uint32_t)0x7ffffffe)
+#define NXSEM_MRESET         ((uint32_t)0x7fffffff)
+
+/* Macro to retrieve mutex's atomic holder's ptr */
+
+#define NXSEM_MHOLDER(s)     ((FAR atomic_t *)&(s)->val.mholder)
+
+/* Check if holder value (TID) is not NO_HOLDER or RESET */
+
+#define NXSEM_MACQUIRED(h)   (!(((h) & NXSEM_NO_MHOLDER) == NXSEM_NO_MHOLDER))
+
+/* Check if mutex is acquired and blocks some other task */
+
+#define NXSEM_MBLOCKS(h)     (((h) & NXSEM_MBLOCKS_BIT) != 0)
 
 /****************************************************************************
  * Public Type Definitions
@@ -128,7 +147,7 @@ extern "C"
  *
  ****************************************************************************/
 
-int nxsem_init(FAR sem_t *sem, int pshared, unsigned int value);
+int nxsem_init(FAR sem_t *sem, int pshared, int32_t value);
 
 /****************************************************************************
  * Name: nxsem_destroy

include/pthread.h

@@ -348,8 +348,9 @@ typedef struct pthread_mutex_s pthread_mutex_t;
 #define PTHREAD_MUTEX_DEFAULT_PRIO_FLAGS (PTHREAD_MUTEX_DEFAULT_PRIO_INHERIT | \
                                           PTHREAD_MUTEX_DEFAULT_PRIO_PROTECT)
 
-#define PTHREAD_NXMUTEX_INITIALIZER { \
-    NXSEM_INITIALIZER(1, SEM_TYPE_MUTEX | PTHREAD_MUTEX_DEFAULT_PRIO_FLAGS), \
+#define PTHREAD_NXMUTEX_INITIALIZER  {                                    \
+    NXSEM_INITIALIZER(NXMUTEX_NO_HOLDER,                                  \
+                      SEM_TYPE_MUTEX | PTHREAD_MUTEX_DEFAULT_PRIO_FLAGS), \
     NXMUTEX_NO_HOLDER}
 #define PTHREAD_NXRMUTEX_INITIALIZER {PTHREAD_NXMUTEX_INITIALIZER, 0}
 

include/semaphore.h

@@ -104,8 +104,16 @@ struct semholder_s
 
 struct sem_s
 {
-  volatile int32_t semcount;     /* >0 -> Num counts available */
-                                 /* <0 -> Num tasks waiting for semaphore */
+  union
+    {
+      volatile int32_t semcount;     /* >0 -> Num counts available */
+                                     /* <0 -> Num tasks waiting for semaphore */
+      volatile uint32_t mholder;     /* == NXMUTEX_NO_HOLDER -> mutex has no holder */
+                                     /* == NXMUTEX_RESET -> mutex has been reset */
+                                     /* Otherwise: */
+                                     /*   bits[30:0]: TID of the current holder */
+                                     /*   bit [31]: Mutex is blocking some task */
+  } val;
 
   /* If priority inheritance is enabled, then we have to keep track of which
    * tasks hold references to the semaphore.
@@ -137,18 +145,18 @@ typedef struct sem_s sem_t;
 /* semcount, flags, waitlist, hhead */
 
 #    define SEM_INITIALIZER(c) \
-       {(c), 0, SEM_WAITLIST_INITIALIZER, NULL}
+       {{(c)}, 0, SEM_WAITLIST_INITIALIZER, NULL}
 #  else
 /* semcount, flags, waitlist, holder[2] */
 
 #    define SEM_INITIALIZER(c) \
-       {(c), 0, SEM_WAITLIST_INITIALIZER, SEMHOLDER_INITIALIZER}
+       {{(c)}, 0, SEM_WAITLIST_INITIALIZER, SEMHOLDER_INITIALIZER}
 #  endif
 #else
 /* semcount, flags, waitlist */
 
 #  define SEM_INITIALIZER(c) \
-     {(c), 0, SEM_WAITLIST_INITIALIZER}
+     {{(c)}, 0, SEM_WAITLIST_INITIALIZER}
 #endif
 
 #define SEM_WAITLIST(sem)       (&((sem)->waitlist))

libs/libc/misc/lib_mutex.c

@@ -35,7 +35,7 @@
  * Pre-processor Definitions
  ****************************************************************************/
 
-#define NXMUTEX_RESET          ((pid_t)-2)
+#define NXMUTEX_RESET          NXSEM_MRESET
 
 /****************************************************************************
  * Private Functions
@@ -113,7 +113,7 @@ static void nxmutex_add_backtrace(FAR mutex_t *mutex)
 
 int nxmutex_init(FAR mutex_t *mutex)
 {
-  int ret = nxsem_init(&mutex->sem, 0, 1);
+  int ret = nxsem_init(&mutex->sem, 0, NXMUTEX_NO_HOLDER);
 
   if (ret < 0)
     {
@@ -197,7 +197,7 @@ bool nxmutex_is_hold(FAR mutex_t *mutex)
 
 int nxmutex_get_holder(FAR mutex_t *mutex)
 {
-  return mutex->holder;
+  return mutex->holder < NXMUTEX_NO_HOLDER ? mutex->holder : -1;
 }
 
 /****************************************************************************

libs/libc/semaphore/sem_getvalue.c

@@ -50,6 +50,10 @@
  *   be zero or a negative number whose absolute value represents the number
  *   of tasks waiting for the semaphore.
  *
+ *   For a mutex, this sets svalue to 1 for a non-acquired mutex, 0 for an
+ *   acquired mutex and -1 for an acquired mutex which is blocking some
+ *   other task
+ *
  * Input Parameters:
  *   sem - Semaphore descriptor
  *   sval - Buffer by which the value is returned
@@ -65,7 +69,23 @@ int nxsem_get_value(FAR sem_t *sem, FAR int *sval)
 {
   if (sem != NULL && sval != NULL)
     {
-      *sval = atomic_read(NXSEM_COUNT(sem));
+      if (NXSEM_IS_MUTEX(sem))
+        {
+          uint32_t mholder = atomic_read(NXSEM_MHOLDER(sem));
+          if (NXSEM_MACQUIRED(mholder))
+            {
+              *sval = NXSEM_MBLOCKS(mholder) ? -1 : 0;
+            }
+          else
+            {
+              *sval = 1;
+            }
+        }
+      else
+        {
+          *sval = atomic_read(NXSEM_COUNT(sem));
+        }
+
       return OK;
     }
 

libs/libc/semaphore/sem_init.c

@@ -62,15 +62,15 @@
  *
  ****************************************************************************/
 
-int nxsem_init(FAR sem_t *sem, int pshared, unsigned int value)
+int nxsem_init(FAR sem_t *sem, int pshared, int32_t value)
 {
   UNUSED(pshared);
 
   DEBUGASSERT(sem != NULL && value <= SEM_VALUE_MAX);
 
-  /* Initialize the semaphore count */
+  /* Initialize the semaphore count or mutex holder */
 
-  sem->semcount = (int32_t)value;
+  sem->val.semcount = value;
 
   /* Initialize semaphore wait list */
 

libs/libc/semaphore/sem_post.c

@@ -29,6 +29,7 @@
 #include <errno.h>
 #include <assert.h>
 
+#include <nuttx/sched.h>
 #include <nuttx/semaphore.h>
 #include <nuttx/atomic.h>
 
@@ -133,8 +134,9 @@ int nxsem_post(FAR sem_t *sem)
 #  endif
       )
     {
-      int32_t old = 0;
-      if (atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &old, 1))
+      int32_t old = _SCHED_GETTID();
+      if (atomic_try_cmpxchg_release(NXSEM_MHOLDER(sem), &old,
+                                     NXSEM_NO_MHOLDER))
         {
           return OK;
         }

libs/libc/semaphore/sem_trywait.c

@@ -30,6 +30,7 @@
 #include <assert.h>
 #include <sched.h>
 
+#include <nuttx/sched.h>
 #include <nuttx/init.h>
 #include <nuttx/semaphore.h>
 #include <nuttx/atomic.h>
@@ -127,8 +128,9 @@ int nxsem_trywait(FAR sem_t *sem)
 #endif
       )
     {
-      int32_t old = 1;
-      return atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0) ?
+      int32_t old = NXSEM_NO_MHOLDER;
+      return atomic_try_cmpxchg_acquire(NXSEM_MHOLDER(sem), &old,
+                                        _SCHED_GETTID()) ?
         OK : -EAGAIN;
     }
 

libs/libc/semaphore/sem_wait.c

@@ -30,6 +30,7 @@
 #include <assert.h>
 #include <sched.h>
 
+#include <nuttx/sched.h>
 #include <nuttx/init.h>
 #include <nuttx/cancelpt.h>
 #include <nuttx/semaphore.h>
@@ -155,8 +156,9 @@ int nxsem_wait(FAR sem_t *sem)
 #  endif
       )
     {
-      int32_t old = 1;
-      if (atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0))
+      int32_t old = NXSEM_NO_MHOLDER;
+      if (atomic_try_cmpxchg_acquire(NXSEM_MHOLDER(sem), &old,
+                                     _SCHED_GETTID()))
         {
           return OK;
         }

sched/pthread/pthread_mutexinit.c

@@ -66,7 +66,7 @@ int pthread_mutex_init(FAR pthread_mutex_t *mutex,
       return EINVAL;
     }
 
-  /* Initialize the mutex like a semaphore with initial count = 1 */
+  /* Initialize the semaphore of type mutex */
 
   status = mutex_init(&mutex->mutex);
   if (status < 0)

sched/semaphore/sem_destroy.c

@@ -61,6 +61,9 @@
 int nxsem_destroy(FAR sem_t *sem)
 {
   int32_t old;
+  int32_t new;
+  FAR atomic_t *plock;
+  bool mutex = NXSEM_IS_MUTEX(sem);
 
   DEBUGASSERT(sem != NULL);
 
@@ -74,15 +77,27 @@ int nxsem_destroy(FAR sem_t *sem)
    * leave the count unchanged but still return OK.
    */
 
-  old = atomic_read(NXSEM_COUNT(sem));
+  if (mutex)
+    {
+      plock = NXSEM_MHOLDER(sem);
+      new = NXSEM_NO_MHOLDER;
+    }
+  else
+    {
+      plock = NXSEM_COUNT(sem);
+      new = 1;
+    }
+
   do
     {
-      if (old < 0)
+      old = atomic_read(plock);
+      if ((mutex && NXSEM_MBLOCKS(old)) ||
+          (!mutex && old < 0))
         {
           break;
         }
     }
-  while (!atomic_try_cmpxchg_release(NXSEM_COUNT(sem), &old, 1));
+  while (!atomic_try_cmpxchg_release(plock, &old, new));
 
   /* Release holders of the semaphore */
 

sched/semaphore/sem_holder.c

@@ -876,7 +876,9 @@ void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem)
 {
   /* Check our assumptions */
 
-  DEBUGASSERT(atomic_read(NXSEM_COUNT(sem)) <= 0);
+  DEBUGASSERT((NXSEM_IS_MUTEX(sem) || atomic_read(NXSEM_COUNT(sem)) <= 0) &&
+              (!NXSEM_IS_MUTEX(sem) ||
+               NSEM_MACQUIRED(atomic_read(NXSEM_MHOLDER(sem)))));
 
   /* Adjust the priority of every holder as necessary */
 
@@ -970,11 +972,32 @@ void nxsem_release_all(FAR struct tcb_s *htcb)
 
       nxsem_freeholder(sem, pholder);
 
-      /* Increment the count on the semaphore, to releases the count
-       * that was taken by sem_wait() or sem_post().
-       */
+      if (!NXSEM_IS_MUTEX(sem))
+        {
+          /* Increment the count on the semaphore, to releases the count
+           * that was taken by sem_wait() or sem_post().
+           */
+
+          atomic_fetch_add(NXSEM_COUNT(sem), 1);
+        }
+      else
+        {
+          /* Lock the mutex for us by setting the blocking bit. We are in
+           * critical section
+           */
 
-      atomic_fetch_add(NXSEM_COUNT(sem), 1);
+          nxsem_get_mholder_acquire(sem);
+
+          /* Restore the mutex holder to the highest holding priority PID,
+           * and set the blocking bit according to how many items are still
+           * in the wait queue
+           */
+
+          FAR struct tcb_s *wtcb =
+            (FAR struct tcb_s *)dq_peek(SEM_WAITLIST(sem));
+
+          nxsem_set_mholder(wtcb, sem);
+        }
     }
 }