semaphore: export priority ceiling interfaces in semaphore

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

include/nuttx/semaphore.h

@@ -680,6 +680,47 @@ int nxsem_clockwait_uninterruptible(FAR sem_t *sem, clockid_t clockid,
 
 int nxsem_tickwait_uninterruptible(FAR sem_t *sem, uint32_t delay);
 
+/****************************************************************************
+ * Name: nxsem_getprioceiling
+ *
+ * Description:
+ *   This function attempts to get the priority ceiling of a semaphore.
+ *
+ * Input Parameters:
+ *   sem          - A pointer to the semaphore whose attributes are to be
+ *                  modified
+ *   prioceiling  - Location to return the semaphore's priority ceiling
+ *
+ * Return 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
+ *
+ ****************************************************************************/
+
+int nxsem_getprioceiling(FAR const sem_t *sem, FAR int *prioceiling);
+
+/****************************************************************************
+ * Name: nxsem_setprioceiling
+ *
+ * Description:
+ *   Set the priority ceiling of a semaphore.
+ *
+ * Input Parameters:
+ *   mutex       - The mutex in which to set the mutex priority ceiling.
+ *   prioceiling - The mutex priority ceiling value to set.
+ *   old_ceiling - Location to return the mutex ceiling priority set before.
+ *
+ * Return 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
+ *
+ ****************************************************************************/
+
+int nxsem_setprioceiling(FAR sem_t *sem, int prioceiling,
+                         FAR int *old_ceiling);
+
 #undef EXTERN
 #ifdef __cplusplus
 }

include/semaphore.h

@@ -116,6 +116,10 @@ struct sem_s
   struct semholder_s holder;     /* Slot for old and new holder */
 #  endif
 #endif
+#ifdef CONFIG_PRIORITY_PROTECT
+  uint8_t ceiling;               /* The priority ceiling owned by mutex  */
+  uint8_t saved;                 /* The saved priority of thread before boost */
+#endif
 };
 
 typedef struct sem_s sem_t;

libs/libc/semaphore/CMakeLists.txt

@@ -34,4 +34,8 @@ if(CONFIG_FS_NAMED_SEMAPHORES)
   list(APPEND SRCS sem_open.c sem_close.c sem_unlink.c)
 endif()
 
+if(CONFIG_PRIORITY_PROTECT)
+  list(APPEND CSRCS sem_getprioceiling.c sem_setprioceiling.c)
+endif()
+
 target_sources(c PRIVATE ${SRCS})

libs/libc/semaphore/Make.defs

@@ -28,6 +28,10 @@ ifeq ($(CONFIG_FS_NAMED_SEMAPHORES),y)
 CSRCS += sem_open.c sem_close.c sem_unlink.c
 endif
 
+ifeq ($(CONFIG_PRIORITY_PROTECT),y)
+CSRCS += sem_getprioceiling.c sem_setprioceiling.c
+endif
+
 # Add the semaphore directory to the build
 
 DEPPATH += --dep-path semaphore

libs/libc/semaphore/sem_getprioceiling.c

@@ -0,0 +1,65 @@
+/****************************************************************************
+ * libs/libc/semaphore/sem_getprioceiling.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <assert.h>
+#include <errno.h>
+
+#include <nuttx/semaphore.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_getprioceiling
+ *
+ * Description:
+ *   This function attempts to get the priority ceiling of a semaphore.
+ *
+ * Input Parameters:
+ *   sem          - A pointer to the semaphore whose attributes are to be
+ *                  modified
+ *   prioceiling  - Location to return the semaphore's priority ceiling
+ *
+ * Return 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
+ *
+ ****************************************************************************/
+
+int nxsem_getprioceiling(FAR const sem_t *sem, FAR int *prioceiling)
+{
+  DEBUGASSERT(sem != NULL);
+
+  if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT)
+    {
+      *prioceiling = sem->ceiling;
+      return OK;
+    }
+
+  return -EINVAL;
+}

libs/libc/semaphore/sem_setprioceiling.c

@@ -0,0 +1,74 @@
+/****************************************************************************
+ * libs/libc/semaphore/sem_setprioceiling.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <assert.h>
+#include <errno.h>
+#include <sched.h>
+
+#include <nuttx/semaphore.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_setprioceiling
+ *
+ * Description:
+ *   Set the priority ceiling of a semaphore.
+ *
+ * Input Parameters:
+ *   mutex       - The mutex in which to set the mutex priority ceiling.
+ *   prioceiling - The mutex priority ceiling value to set.
+ *   old_ceiling - Location to return the mutex ceiling priority set before.
+ *
+ * Return 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
+ *
+ ****************************************************************************/
+
+int nxsem_setprioceiling(FAR sem_t *sem, int prioceiling,
+                         FAR int *old_ceiling)
+{
+  DEBUGASSERT(sem != NULL);
+
+  if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT &&
+      prioceiling >= sched_get_priority_min(SCHED_FIFO) &&
+      prioceiling <= sched_get_priority_max(SCHED_FIFO))
+    {
+      if (old_ceiling != NULL)
+        {
+          *old_ceiling = sem->ceiling;
+        }
+
+      sem->ceiling = prioceiling;
+      return OK;
+    }
+
+  return -EINVAL;
+}

libs/libc/semaphore/sem_setprotocol.c

@@ -83,8 +83,15 @@ int nxsem_set_protocol(FAR sem_t *sem, int protocol)
         break;
 
       case SEM_PRIO_INHERIT:
-      case SEM_PRIO_PROTECT:
         return -ENOTSUP;
+      case SEM_PRIO_PROTECT:
+#ifdef CONFIG_PRIORITY_PROTECT
+        break;
+#else
+        /* Not yet supported */
+
+        return -ENOTSUP;
+#endif
 
       default:
         return -EINVAL;

sched/Kconfig

@@ -1436,6 +1436,13 @@ config SEM_PREALLOCHOLDERS
 
 endif # PRIORITY_INHERITANCE
 
+config PRIORITY_PROTECT
+	bool "Enable priority protect"
+	default n
+	---help---
+		When a thread locks a mutex it inherits the priority ceiling of the
+		mutex, which is defined by the application as a mutex attribute.
+
 menu "RTOS hooks"
 
 config BOARD_EARLY_INITIALIZE

sched/semaphore/sem_post.c

@@ -72,8 +72,8 @@ int nxsem_post(FAR sem_t *sem)
   FAR struct tcb_s *stcb = NULL;
   irqstate_t flags;
   int16_t sem_count;
-#ifdef CONFIG_PRIORITY_INHERITANCE
-  uint8_t prioinherit;
+#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT)
+  uint8_t proto;
 #endif
 
   DEBUGASSERT(sem != NULL);
@@ -116,7 +116,7 @@ int nxsem_post(FAR sem_t *sem)
   sem_count++;
   sem->semcount = sem_count;
 
-#ifdef CONFIG_PRIORITY_INHERITANCE
+#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT)
   /* Don't let any unblocked tasks run until we complete any priority
    * restoration steps.  Interrupts are disabled, but we do not want
    * the head of the ready-to-run list to be modified yet.
@@ -125,8 +125,8 @@ int nxsem_post(FAR sem_t *sem)
    * will do nothing.
    */
 
-  prioinherit = sem->flags & SEM_PRIO_MASK;
-  if (prioinherit == SEM_PRIO_INHERIT)
+  proto = sem->flags & SEM_PRIO_MASK;
+  if (proto != SEM_PRIO_NONE)
     {
       sched_lock();
     }
@@ -191,10 +191,27 @@ int nxsem_post(FAR sem_t *sem)
    * held the semaphore.
    */
 
-#ifdef CONFIG_PRIORITY_INHERITANCE
-  if (prioinherit == SEM_PRIO_INHERIT)
+#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT)
+  if (proto != SEM_PRIO_NONE)
     {
-      nxsem_restore_baseprio(stcb, sem);
+      if (proto == SEM_PRIO_INHERIT)
+        {
+#ifdef CONFIG_PRIORITY_INHERITANCE
+          nxsem_restore_baseprio(stcb, sem);
+#endif
+        }
+
+      if (proto == SEM_PRIO_PROTECT)
+        {
+#ifdef CONFIG_PRIORITY_PROTECT
+          if (sem->saved > 0)
+            {
+              nxsched_set_priority(this_task(), sem->saved);
+              sem->saved = 0;
+            }
+#endif
+        }
+
       sched_unlock();
     }
 #endif

sched/semaphore/sem_setprotocol.c

@@ -90,10 +90,13 @@ int nxsem_set_protocol(FAR sem_t *sem, int protocol)
         break;
 
       case SEM_PRIO_PROTECT:
-
+#ifdef CONFIG_PRIORITY_PROTECT
+        break;
+#else
         /* Not yet supported */
 
         return -ENOTSUP;
+#endif
 
       default:
         return -EINVAL;

sched/semaphore/sem_wait.c

@@ -94,6 +94,22 @@ int nxsem_wait(FAR sem_t *sem)
     {
       /* It is, let the task take the semaphore. */
 
+#ifdef CONFIG_PRIORITY_PROTECT
+      if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT)
+        {
+          if (rtcb->sched_priority <= sem->ceiling)
+            {
+              sem->saved = rtcb->sched_priority;
+              rtcb->sched_priority = sem->ceiling;
+            }
+          else
+            {
+              leave_critical_section_nonirq(flags);
+              return -EINVAL;
+            }
+        }
+#endif
+
       sem->semcount--;
       nxsem_add_holder(sem);
       rtcb->waitobj = NULL;