add sched_cpupri.c

sched/sched/sched.h

@@ -441,4 +441,14 @@ bool nxsched_verify_tcb(FAR struct tcb_s *tcb);
 struct tls_info_s; /* Forward declare */
 FAR struct tls_info_s *nxsched_get_tls(FAR struct tcb_s *tcb);
 
+/* cpupri operations */
+
+void pick_pull(int *cpu, int *pri);
+
+void pick_push(int *cpu, int *pri);
+
+void preempt_running_cpupri(int cpu, int pri);
+
+void replace_ready_cpupri(int cpu, int pri);
+
 #endif /* __SCHED_SCHED_SCHED_H */

sched/sched/sched_addreadytorun.c

@@ -89,7 +89,7 @@ bool nxsched_add_readytorun(FAR struct tcb_s *btcb,bool priority_smaller)
 
   /* Otherwise, add the new task to the ready-to-run task list */
 
-  else if (nxsched_add_prioritized(btcb, list_readytorun()),priority_smaller)
+  else if (nxsched_add_prioritized(btcb, list_readytorun(),priority_smaller))
     {
       /* The new btcb was added at the head of the ready-to-run list.  It
        * is now the new active task!

sched/sched/sched_cprpri.c

+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <assert.h>
+
+#include "sched/sched.h"
+
+struct cpupri_s {
+    int first_cpu;
+    int first_pri;
+    int vec_cpu_pri[CONFIG_SMP_NCPUS];
+};
+
+typedef struct cpupri_s cpupri_t;
+
+/* Record the first ready task priority for each cpu */
+cpupri_t ready_cpupri;
+
+/* Record the running task priority for each cpu */
+cpupri_t running_cpupri;
+
+/* Update the "first_cpu" and "first_pri" for a cpupri */
+void update_cpupri(cpupri_t* cpupri, bool max) {
+    int idx = 0;
+    int val;
+    if (max) {
+        val = 0;
+        for (size_t i = 0; i < CONFIG_SMP_NCPUS; i++)
+            if (cpupri->vec_cpu_pri[i] > val) {
+                val = cpupri->vec_cpu_pri[i];
+                idx = i;
+            }
+    }
+    else {
+        val = 255;
+        for (size_t i = 0; i < CONFIG_SMP_NCPUS; i++)
+            if (cpupri->vec_cpu_pri[i] < val) {
+                val = cpupri->vec_cpu_pri[i];
+                idx = i;
+            }
+    }
+    cpupri->first_cpu = idx;
+    cpupri->first_pri = val;
+}
+
+
+#define next_cpu(x) (x).first_cpu
+#define next_pri(x) (x).first_pri
+
+static inline void update_ready_cpupri() {
+    update_cpupri(&ready_cpupri, 1);
+}
+
+static inline void update_running_cpupri() {
+    update_cpupri(&running_cpupri, 0);
+}
+
+/* pick the next cpu and priority to pull */
+void pick_pull(int *cpu, int *pri) {
+    update_ready_cpupri();
+    *cpu = next_cpu(ready_cpupri);
+    *pri = next_pri(ready_cpupri);
+}
+
+/* pick the next cpu and priority to push */
+void pick_push(int *cpu, int *pri) {
+    update_running_cpupri();
+    *cpu = next_cpu(running_cpupri);
+    *pri = next_pri(running_cpupri);
+}
+
+/* "preempt" the "running cpu" and update priority record of that cpu */
+void preempt_running_cpupri(int cpu, int pri) {
+    int old_pri = running_cpupri.vec_cpu_pri[cpu];
+
+    /* new priority must higher */
+    DEBUGASSERT(pri > old_pri);
+
+    ready_cpupri.vec_cpu_pri[cpu] = old_pri;
+    running_cpupri.vec_cpu_pri[cpu] = pri;
+}
+
+void replace_ready_cpupri(int cpu, int pri) {
+    int old_pri = ready_cpupri.vec_cpu_pri[cpu];
+
+    /* new priority must lower or eq */
+    DEBUGASSERT(pri <= old_pri);
+
+    ready_cpupri.vec_cpu_pri[cpu] = pri;
+}
\ No newline at end of file