Enable mutex functionality in nxsem

include/nuttx/mutex.h

@@ -37,8 +37,10 @@
  ****************************************************************************/
 
 #define NXMUTEX_NO_HOLDER      ((pid_t)-1)
-#define NXMUTEX_INITIALIZER    {NXSEM_INITIALIZER(1, SEM_TYPE_MUTEX | \
-                                SEM_PRIO_INHERIT), NXMUTEX_NO_HOLDER}
+#define NXMUTEX_INITIALIZER    {                                             \
+  NXSEM_INITIALIZER(NXSEM_NO_MHOLDER, SEM_TYPE_MUTEX | SEM_PRIO_INHERIT),  \
+  NXMUTEX_NO_HOLDER}
+
 #define NXRMUTEX_INITIALIZER   {NXMUTEX_INITIALIZER, 0}
 
 /****************************************************************************

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(NXSEM_NO_MHOLDER,                                   \
+                      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;     /* == NXSEM_NO_MHOLDER -> mutex has no holder */
+                                     /* == NXSEM_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

@@ -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, NXSEM_NO_MHOLDER);
 
   if (ret < 0)
     {

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>
 
@@ -127,14 +128,15 @@ int nxsem_post(FAR sem_t *sem)
 
 #ifndef CONFIG_LIBC_ARCH_ATOMIC
 
-  if ((sem->flags & SEM_TYPE_MUTEX)
-#  if defined(CONFIG_PRIORITY_PROTECT) || defined(CONFIG_PRIORITY_INHERITANCE)
-      && (sem->flags & SEM_PRIO_MASK) == SEM_PRIO_NONE
+  if (NXSEM_IS_MUTEX(sem)
+#  if defined(CONFIG_PRIORITY_PROTECT)
+      && (sem->flags & SEM_PRIO_MASK) != SEM_PRIO_PROTECT
 #  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>
@@ -122,14 +123,15 @@ int nxsem_trywait(FAR sem_t *sem)
 
 #ifndef CONFIG_LIBC_ARCH_ATOMIC
 
-  if ((sem->flags & SEM_TYPE_MUTEX)
-#if defined(CONFIG_PRIORITY_PROTECT) || defined(CONFIG_PRIORITY_INHERITANCE)
-      && (sem->flags & SEM_PRIO_MASK) == SEM_PRIO_NONE
+  if (NXSEM_IS_MUTEX(sem)
+#if defined(CONFIG_PRIORITY_PROTECT)
+      && (sem->flags & SEM_PRIO_MASK) != SEM_PRIO_PROTECT
 #endif
       )
     {
-      int32_t old = 1;
-      return atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0) ?
+      const int32_t tid = _SCHED_GETTID();
+      int32_t old = NXSEM_NO_MHOLDER;
+      return atomic_try_cmpxchg_acquire(NXSEM_MHOLDER(sem), &old, tid) ?
         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>
@@ -150,14 +151,15 @@ int nxsem_wait(FAR sem_t *sem)
 
 #ifndef CONFIG_LIBC_ARCH_ATOMIC
 
-  if ((sem->flags & SEM_TYPE_MUTEX)
-#  if defined(CONFIG_PRIORITY_PROTECT) || defined(CONFIG_PRIORITY_INHERITANCE)
-      && (sem->flags & SEM_PRIO_MASK) == SEM_PRIO_NONE
+  if (NXSEM_IS_MUTEX(sem)
+#  if defined(CONFIG_PRIORITY_PROTECT)
+      && (sem->flags & SEM_PRIO_MASK) != SEM_PRIO_PROTECT
 #  endif
       )
     {
-      int32_t old = 1;
-      if (atomic_try_cmpxchg_acquire(NXSEM_COUNT(sem), &old, 0))
+      const int32_t tid = _SCHED_GETTID();
+      int32_t old = NXSEM_NO_MHOLDER;
+      if (atomic_try_cmpxchg_acquire(NXSEM_MHOLDER(sem), &old, tid))
         {
           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;
+    }
+
+  old = atomic_read(plock);
   do
     {
-      if (old < 0)
+      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));
+  DEBUGASSERT((!NXSEM_IS_MUTEX(sem) ||
+               NXSEM_MACQUIRED(atomic_read(NXSEM_MHOLDER(sem)))));
 
   /* Adjust the priority of every holder as necessary */
 
@@ -970,11 +972,14 @@ 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);
+          atomic_fetch_add(NXSEM_COUNT(sem), 1);
+        }
     }
 }