sem: add mutex protect in sem_trywait

Signed-off-by: zhangyuan29 <[email protected]>

sched/semaphore/CMakeLists.txt

@@ -36,4 +36,8 @@ if(CONFIG_PRIORITY_INHERITANCE)
   list(APPEND CSRCS sem_initialize.c sem_holder.c sem_setprotocol.c)
 endif()
 
+if(CONFIG_PRIORITY_PROTECT)
+  list(APPEND CSRCS sem_protect.c)
+endif()
+
 target_sources(sched PRIVATE ${CSRCS})

sched/semaphore/Make.defs

@@ -28,6 +28,10 @@ ifeq ($(CONFIG_PRIORITY_INHERITANCE),y)
 CSRCS += sem_initialize.c sem_holder.c sem_setprotocol.c
 endif
 
+ifeq ($(CONFIG_PRIORITY_PROTECT),y)
+CSRCS += sem_protect.c
+endif
+
 # Include semaphore build support
 
 DEPPATH += --dep-path semaphore

sched/semaphore/sem_post.c

@@ -200,16 +200,9 @@ int nxsem_post(FAR sem_t *sem)
           nxsem_restore_baseprio(stcb, sem);
 #endif
         }
-
-      if (proto == SEM_PRIO_PROTECT)
+      else if (proto == SEM_PRIO_PROTECT)
         {
-#ifdef CONFIG_PRIORITY_PROTECT
-          if (sem->saved > 0)
-            {
-              nxsched_set_priority(this_task(), sem->saved);
-              sem->saved = 0;
-            }
-#endif
+          nxsem_protect_post(sem);
         }
 
       sched_unlock();

sched/semaphore/sem_protect.c

@@ -0,0 +1,116 @@
+/****************************************************************************
+ * sched/semaphore/sem_protect.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 <stdbool.h>
+#include <errno.h>
+
+#include "sched/sched.h"
+#include "semaphore/semaphore.h"
+
+#ifdef CONFIG_PRIORITY_PROTECT
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_protect_wait
+ *
+ * Description:
+ *   This function attempts to lock the protected semaphore, set the
+ *   holder tcb priority to ceiling priority.
+ *
+ *   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
+ *
+ ****************************************************************************/
+
+int nxsem_protect_wait(FAR sem_t *sem)
+{
+  if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT)
+    {
+      FAR struct tcb_s *rtcb = this_task();
+
+      if (rtcb->sched_priority > sem->ceiling)
+        {
+          return -EINVAL;
+        }
+
+      sem->saved = rtcb->sched_priority;
+      rtcb->sched_priority = sem->ceiling;
+    }
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: nxsem_protect_post
+ *
+ * Description:
+ *   This function attempts to unlock the protected semaphore, restore the
+ *   holder tcb priority.
+ *
+ *   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:
+ *
+ ****************************************************************************/
+
+void nxsem_protect_post(FAR sem_t *sem)
+{
+  if (sem->saved > 0)
+    {
+      nxsched_set_priority(this_task(), sem->saved);
+      sem->saved = 0;
+    }
+}
+
+#endif /* CONFIG_PRIORITY_PROTECT */

sched/semaphore/sem_trywait.c

@@ -88,6 +88,13 @@ int nxsem_trywait(FAR sem_t *sem)
     {
       /* It is, let the task take the semaphore */
 
+      ret = nxsem_protect_wait(sem);
+      if (ret < 0)
+        {
+          leave_critical_section(flags);
+          return ret;
+        }
+
       sem->semcount--;
       nxsem_add_holder(sem);
       rtcb->waitobj = NULL;

sched/semaphore/sem_wait.c

@@ -94,21 +94,12 @@ 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)
+      ret = nxsem_protect_wait(sem);
+      if (ret < 0)
         {
-          if (rtcb->sched_priority <= sem->ceiling)
-            {
-              sem->saved = rtcb->sched_priority;
-              rtcb->sched_priority = sem->ceiling;
-            }
-          else
-            {
-              leave_critical_section_nonirq(flags);
-              return -EINVAL;
-            }
+          leave_critical_section_nonirq(flags);
+          return ret;
         }
-#endif
 
       sem->semcount--;
       nxsem_add_holder(sem);

sched/semaphore/semaphore.h

@@ -61,7 +61,7 @@ void nxsem_wait_irq(FAR struct tcb_s *wtcb, int errcode);
 
 void nxsem_timeout(wdparm_t pid);
 
-/* Recover semaphore resources with a task or thread is destroyed  */
+/* Recover semaphore resources with a task or thread is destroyed */
 
 void nxsem_recover(FAR struct tcb_s *tcb);
 
@@ -91,6 +91,16 @@ void nxsem_release_all(FAR struct tcb_s *stcb);
 #  define nxsem_release_all(stcb)
 #endif
 
+/* Special logic needed only by priority protect */
+
+#ifdef CONFIG_PRIORITY_PROTECT
+int nxsem_protect_wait(FAR sem_t *sem);
+void nxsem_protect_post(FAR sem_t *sem);
+#else
+#  define nxsem_protect_wait(sem)  0
+#  define nxsem_protect_post(sem)
+#endif
+
 #undef EXTERN
 #ifdef __cplusplus
 }