From 8f716a386b56e4c865e2cde71ef8d2912648eee2 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Thu, 29 Dec 2016 10:40:58 -0600
Subject: [PATCH] SMP: Use irq_cpu_locked() in sched_removereadytorun() and
 sched_setpriority()

---
 sched/sched/sched_removereadytorun.c | 21 +++++---
 sched/sched/sched_setpriority.c      | 72 +++++++++++++++++++++++++++-
 2 files changed, 85 insertions(+), 8 deletions(-)

diff --git a/sched/sched/sched_removereadytorun.c b/sched/sched/sched_removereadytorun.c
index 5b6b663d97..be80d3ad0f 100644
--- a/sched/sched/sched_removereadytorun.c
+++ b/sched/sched/sched_removereadytorun.c
@@ -165,7 +165,7 @@ bool sched_removereadytorun(FAR struct tcb_s *rtcb)
   if (rtcb->blink == NULL && TLIST_ISRUNNABLE(rtcb->task_state))
     {
       FAR struct tcb_s *nxttcb;
-      FAR struct tcb_s *rtrtcb;
+      FAR struct tcb_s *rtrtcb = NULL;
       int me;
 
       /* There must always be at least one task in the list (the IDLE task)
@@ -198,14 +198,21 @@ bool sched_removereadytorun(FAR struct tcb_s *rtcb)
        * g_readytorun list.  We can only select a task from that list if
        * the affinity mask includes the current CPU.
        *
-       * REVISIT: What should we do, if anything, if pre-emption is locked
-       * by the another CPU?  Should just used nxttcb?  Should we select
-       * from the pending task list instead of the g_readytorun list?
+       * If pre-emption is locked or another CPU is in a critical section,
+       * then use the 'nxttcb' which will probably be the IDLE thread.
+       * REVISIT: What if it is not the IDLE thread?
        */
 
-      for (rtrtcb = (FAR struct tcb_s *)g_readytorun.head;
-           rtrtcb != NULL && !CPU_ISSET(cpu, &rtrtcb->affinity);
-           rtrtcb = (FAR struct tcb_s *)rtrtcb->flink);
+      if (!spin_islocked(&g_cpu_schedlock) && !irq_cpu_locked(me))
+        {
+          /* Search for the highest priority task that can run on this
+           * CPU.
+           */
+
+          for (rtrtcb = (FAR struct tcb_s *)g_readytorun.head;
+               rtrtcb != NULL && !CPU_ISSET(cpu, &rtrtcb->affinity);
+               rtrtcb = (FAR struct tcb_s *)rtrtcb->flink);
+        }
 
       /* Did we find a task in the g_readytorun list?  Which task should
        * we use?  We decide strictly by the priority of the two tasks:
diff --git a/sched/sched/sched_setpriority.c b/sched/sched/sched_setpriority.c
index 9e2c9384aa..2dfd1c1417 100644
--- a/sched/sched/sched_setpriority.c
+++ b/sched/sched/sched_setpriority.c
@@ -52,6 +52,64 @@
  * Private Functions
  ****************************************************************************/
 
+/****************************************************************************
+ * Name: sched_nexttcb
+ *
+ * Description:
+ *   Get the next highest priority ready-to-run task.
+ *
+ * Inputs:
+ *   tcb - the TCB of task to reprioritize.
+ *
+ * Return Value:
+ *   TCB of the next highest priority ready-to-run task.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+static FAR struct tcb_s *sched_nexttcb(FAR struct tcb_s *tcb)
+{
+  FAR struct tcb_s *nxttcb = (FAR struct tcb_s *)tcb->flink;
+  FAR struct tcb_s *rtrtcb;
+  int cpu = this_cpu();
+
+  /* Which task should run next?  It will be either the next tcb in the
+   * assigned task list (nxttcb) or a TCB in the g_readytorun list.  We can
+   * only select a task from that list if the affinity mask includes the
+   * current CPU.
+   *
+   * If pre-emption is locked or another CPU is in a critical section,
+   * then use the 'nxttcb' which will probably be the IDLE thread.
+   */
+
+  if (!spin_islocked(&g_cpu_schedlock) && !irq_cpu_locked(cpu))
+    {
+      /* Search for the highest priority task that can run on this CPU. */
+
+      for (rtrtcb = (FAR struct tcb_s *)g_readytorun.head;
+           rtrtcb != NULL && !CPU_ISSET(cpu, &rtrtcb->affinity);
+           rtrtcb = (FAR struct tcb_s *)rtrtcb->flink);
+
+      /* Return the TCB from the readyt-to-run list if it is the next
+       * highest priority task.
+       */
+
+      if (rtrtcb != NULL &&
+          rtrtcb->sched_priority >= nxttcb->sched_priority)
+        {
+          return rtrtcb;
+        }
+    }
+
+  /* Otherwise, return the next TCB in the g_assignedtasks[] list...
+   * probably the TCB of the IDLE thread.
+   * REVISIT:  What if it is not the IDLE thread?
+   */
+
+  return nxttcb;
+}
+#endif
+
 /****************************************************************************
  * Name:  sched_running_setpriority
  *
@@ -77,12 +135,24 @@
 static inline void sched_running_setpriority(FAR struct tcb_s *tcb,
                                              int sched_priority)
 {
+  FAR struct tcb_s *nxttcb;
+
+  /* Get the TCB of the next highest priority, ready to run task */
+
+#ifdef CONFIG_SMP
+  nxttcb = sched_nexttcb(tcb);
+#else
+  nxttcb = (FAR struct tcb_s *)tcb->flink;
+#endif
+
+  DEBUGASSERT(nxttcb != NULL);
+
   /* A context switch will occur if the new priority of the running
    * task becomes less than OR EQUAL TO the next highest priority
    * ready to run task.
    */
 
-  if (sched_priority <= tcb->flink->sched_priority)
+  if (sched_priority <= nxttcb->sched_priority)
     {
       /* A context switch will occur. */