Add support for priority inheritance on the low priority worker queue

include/nuttx/wqueue.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  * include/nuttx/wqueue.h
  *
- *   Copyright (C) 2009, 2011-2013 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2009, 2011-2014 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -486,6 +486,48 @@ int work_signal(int qid);
 
 #define work_available(work) ((work)->worker == NULL)
 
+/****************************************************************************
+ * Name: lpwork_boostpriority
+ *
+ * Description:
+ *   Called by the work queue client to assure that the priority of the low-
+ *   priority worker thread is at least at the requested level, reqprio. This
+ *   function would normally be called just before calling work_queue().
+ *
+ * Parameters:
+ *   reqprio - Requested minimum worker thread priority
+ *
+ * Return Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_SCHED_LPWORK) && defined(CONFIG_PRIORITY_INHERITANCE)
+void lpwork_boostpriority(uint8_t reqprio);
+#endif
+
+/****************************************************************************
+ * Name: lpwork_restorepriority
+ *
+ * Description:
+ *   This function is called to restore the priority after it was previously
+ *   boosted.  This is often done by client logic on the worker thread when
+ *   the scheduled work completes.  It will check if we need to drop the
+ *   priority of the worker thread.
+ *
+ * Parameters:
+ *   reqprio - Previously requested minimum worker thread priority to be
+ *     "unboosted"
+ *
+ * Return Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_SCHED_LPWORK) && defined(CONFIG_PRIORITY_INHERITANCE)
+void lpwork_restorepriority(uint8_t reqprio);
+#endif
+
 #undef EXTERN
 #ifdef __cplusplus
 }

libc/Kconfig

@@ -452,9 +452,9 @@ if SCHED_LPWORK
 
 config SCHED_LPWORKPRIORITY
 	int "Low priority worker thread priority"
-	default 100
+	default 50
 	---help---
-		The execution priority of the lopwer priority worker thread.  Default: 192
+		The execution priority of the lopwer priority worker thread.  Default: 50
 
 config SCHED_LPWORKPERIOD
 	int "Low priority worker thread period"

sched/Makefile

@@ -35,6 +35,9 @@
 
 -include $(TOPDIR)/Make.defs
 
+ASRCS =
+CSRCS =
+
 VPATH =
 DEPPATH = --dep-path .
 
@@ -53,13 +56,7 @@ include mqueue/Make.defs
 include clock/Make.defs
 include timer/Make.defs
 include environ/Make.defs
-
+include wqueue/Make.defs
-ASRCS =
-
-CSRCS  = $(INIT_SRCS) $(IRQ_SRCS) $(PG_SRCS) $(GRP_SRCS) $(SCHED_SRCS)
-CSRCS += $(TSK_SRCS) $(ERRNO_SRCS) $(WDOG_SRCS) $(SEM_SRCS)
-CSRCS += $(SIGNAL_SRCS) $(PTHREAD_SRCS) $(MQUEUE_SRCS) $(CLOCK_SRCS)
-CSRCS += $(TIMER_SRCS) $(ENV_SRCS)
 
 AOBJS = $(ASRCS:.S=$(OBJEXT))
 COBJS = $(CSRCS:.c=$(OBJEXT))

sched/wqueue/Make.defs

@@ -0,0 +1,54 @@
+############################################################################
+# sched/wqueue/Make.defs
+#
+#   Copyright (C) 2014 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# Add work queue files
+
+ifeq ($(CONFIG_SCHED_WORKQUEUE),y)
+
+# Add low priority work queue files
+
+ifeq ($(CONFIG_SCHED_LPWORK),y)
+ifeq ($(CONFIG_PRIORITY_INHERITANCE),y)
+
+CSRCS += work_inherit.c
+
+# Include wqueue build support
+
+DEPPATH += --dep-path wqueue
+VPATH += :wqueue
+
+endif # CONFIG_PRIORITY_INHERITANCE
+endif # CONFIG_SCHED_LPWORK
+endif # CONFIG_SCHED_WORKQUEUE

sched/wqueue/work_inherit.c

@@ -0,0 +1,325 @@
+/****************************************************************************
+ * sched/environ/env_putenv.c
+ *
+ *   Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sched.h>
+
+#include <nuttx/wqueue.h>
+
+#include "sched/sched.h"
+
+#if defined(CONFIG_SCHED_WORKQUEUE) && defined(CONFIG_SCHED_LPWORK) && \
+    defined(CONFIG_PRIORITY_INHERITANCE)
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: lpwork_boostpriority
+ *
+ * Description:
+ *   Called by the work queue client to assure that the priority of the low-
+ *   priority worker thread is at least at the requested level, reqprio. This
+ *   function would normally be called just before calling work_queue().
+ *
+ * Parameters:
+ *   reqprio - Requested minimum worker thread priority
+ *
+ * Return Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void lpwork_boostpriority(uint8_t reqprio)
+{
+  FAR struct tcb_s *wtcb;
+  irqstate_t flags;
+  pid_t wpid;
+
+  /* Get the process ID of the low priority worker thread from the low
+   * priority work queue.  Then get the TCB of the low priority worker
+   * thread from the process ID.
+   */
+
+  wpid = g_work[LPWORK].pid;
+  wtcb = sched_gettcb(wpid);
+
+  /* Prevent context switches until we get the priorities right */
+
+  flags = irqsave();
+  sched_lock();
+
+#if CONFIG_SEM_NNESTPRIO > 0
+  /* If the priority of the client thread that is greater than the base
+   * priority of the worker thread, then we may need to adjust the worker
+   * thread's priority now or later to that priority.
+   */
+
+  if (reqprio > wtcb->base_priority)
+    {
+      /* If the new priority is greater than the current, possibly already
+       * boosted priority of the worker thread, then we will have to raise
+       * the worker thread's priority now.
+       */
+
+      if (reqprio > wtcb->sched_priority)
+        {
+          /* If the current priority of worker thread has already been
+           * boosted, then add the boost priority to the list of restoration
+           * priorities.  When the higher priority waiter thread gets its
+           * count, then we need to revert the worker thread to this saved
+           * priority (not to its base priority).
+           */
+
+          if (wtcb->sched_priority > wtcb->base_priority)
+            {
+              /* Save the current, boosted priority of the worker thread. */
+
+              if (wtcb->npend_reprio < CONFIG_SEM_NNESTPRIO)
+                {
+                  wtcb->pend_reprios[wtcb->npend_reprio] = wtcb->sched_priority;
+                  wtcb->npend_reprio++;
+                }
+            }
+
+          /* Raise the priority of the worker.  This cannot cause a context
+           * switch because we have preemption disabled.  The worker thread
+           * may be marked "pending" and the switch may occur during
+           * sched_unblock() processing.
+           */
+
+          (void)sched_setpriority(wtcb, reqprio);
+        }
+      else
+        {
+          /* The new priority is above the base priority of the worker,
+           * but not as high as its current working priority.  Just put it
+           * in the list of pending restoration priorities so that when the
+           * higher priority thread gets its count, we can revert to this
+           * saved priority and not to the base priority.
+           */
+
+          wtcb->pend_reprios[wtcb->npend_reprio] = reqprio;
+          wtcb->npend_reprio++;
+        }
+    }
+#else
+  /* If the priority of the client thread that is less than of equal to the
+   * priority of the worker thread, then do nothing because the thread is
+   * already running at a sufficient priority.
+   */
+
+  if (reqprio > wtcb->sched_priority)
+    {
+      /* Raise the priority of the worker thread.  This cannot cause 
+       * context switch because we have preemption disabled.  The task
+       * will be marked "pending" and the switch will occur during
+       * sched_unlock() processing.
+       */
+
+      (void)sched_setpriority(wtcb, reqprio);
+    }
+#endif
+
+  sched_unlock();
+  irqrestore(flags);
+}
+
+/****************************************************************************
+ * Name: lpwork_restorepriority
+ *
+ * Description:
+ *   This function is called to restore the priority after it was previously
+ *   boosted.  This is often done by client logic on the worker thread when
+ *   the scheduled work completes.  It will check if we need to drop the
+ *   priority of the worker thread.
+ *
+ * Parameters:
+ *   reqprio - Previously requested minimum worker thread priority to be
+ *     "unboosted"
+ *
+ * Return Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void lpwork_restorepriority(uint8_t reqprio)
+{
+  FAR struct tcb_s *wtcb;
+  irqstate_t flags;
+  pid_t wpid;
+  uint8_t wpriority;
+  int index;
+  int selected;
+
+  /* Get the process ID of the low priority worker thread from the low
+   * priority work queue.  Then get the TCB of the low priority worker
+   * thread from the process ID.
+   */
+
+  wpid = g_work[LPWORK].pid;
+  wtcb = sched_gettcb(wpid);
+
+  /* Prevent context switches until we get the priorities right */
+
+  flags = irqsave();
+  sched_lock();
+
+  /* Was the priority of the worker thread boosted? If so, then drop its
+   * priority back to the correct level.  What is the correct level?
+   */
+
+  if (wtcb->sched_priority != wtcb->base_priority)
+    {
+#if CONFIG_SEM_NNESTPRIO > 0
+      /* Are there other, pending priority levels to revert to? */
+
+      if (wtcb->npend_reprio < 1)
+        {
+          /* No... the worker thread has only been boosted once.
+           * npend_reprio should be 0 and the boosted priority should be the
+           * priority of the client task (reqprio)
+           *
+           * That latter assumption may not be true if the client's priority
+           * was also boosted so that it no longer matches the wtcb's
+           * sched_priority.  Or if CONFIG_SEM_NNESTPRIO is too small (so
+           * that we do not have a proper record of the reprioritizations).
+           */
+
+          DEBUGASSERT(/* wtcb->sched_priority == reqprio && */
+                      wtcb->npend_reprio == 0);
+
+          /* Reset the worker's priority back to the base priority. */
+
+          sched_reprioritize(wtcb, wtcb->base_priority);
+        }
+
+      /* There are multiple pending priority levels. The worker thread's
+       * "boosted" priority could greater than or equal to "reqprio" (it could
+       * be greater if its priority we boosted because it also holds some
+       * semaphore).
+       */
+
+      else if (wtcb->sched_priority <= reqprio)
+        {
+          /* The worker thread has been boosted to the same priority as the
+           * waiter thread that just received the count.  We will simply
+           * reprioritize to the next highest pending priority.
+           */
+
+          /* Find the highest pending priority and remove it from the list */
+
+          for (index = 1, selected = 0; index < wtcb->npend_reprio; index++)
+            {
+              if (wtcb->pend_reprios[index] > wtcb->pend_reprios[selected])
+                {
+                  selected = index;
+                }
+            }
+
+          /* Remove the highest priority pending priority from the list */
+
+          wpriority = wtcb->pend_reprios[selected];
+          index = wtcb->npend_reprio - 1;
+          if (index > 0)
+            {
+              wtcb->pend_reprios[selected] = wtcb->pend_reprios[index];
+            }
+
+          wtcb->npend_reprio = index;
+
+          /* And apply that priority to the thread (while retaining the
+           * base_priority)
+           */
+
+          sched_setpriority(wtcb, wpriority);
+        }
+      else
+        {
+          /* The worker thread has been boosted to a higher priority than the
+           * waiter task.  The pending priority should be in the list (unless
+           * it was lost because of of list overflow or because the worker
+           * was reprioritized again unbeknownst to the priority inheritance
+           * logic).
+           *
+           * Search the list for the matching priority.
+           */
+
+          for (index = 0; index < wtcb->npend_reprio; index++)
+            {
+              /* Does this pending priority match the priority of the thread
+               * that just received the count?
+               */
+
+              if (wtcb->pend_reprios[index] == reqprio)
+                {
+                  /* Yes, remove it from the list */
+
+                  selected = wtcb->npend_reprio - 1;
+                  if (selected > 0)
+                    {
+                      wtcb->pend_reprios[index] = wtcb->pend_reprios[selected];
+                    }
+
+                   wtcb->npend_reprio = selected;
+                   break;
+                }
+            }
+        }
+#else
+      /* There is no alternative restore priorities, drop the priority
+       * of the worker thread all the way back to the threads "base"
+       * priority.
+       */
+
+      sched_reprioritize(wtcb, wtcb->base_priority);
+#endif
+    }
+
+  sched_unlock();
+  irqrestore(flags);
+}
+
+#endif /* CONFIG_SCHED_WORKQUEUE && CONFIG_SCHED_LPWORK && CONFIG_PRIORITY_INHERITANCE */