refactor ready list management and make lower priority numbers higher priority

cxx/examples/mutex.cpp

@@ -71,4 +71,4 @@ static void timeout(void)
 RT_TASK(increment_lock, RT_STACK_MIN, 1);
 RT_TASK(increment_trylock, RT_STACK_MIN, 1);
 RT_TASK(increment_timedlock, RT_STACK_MIN, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

cxx/examples/notify.cpp

@@ -35,5 +35,5 @@ static void waiter(void)
     rt::trap();
 }
 
-RT_TASK(notifier, RT_STACK_MIN, 1);
+RT_TASK(notifier, RT_STACK_MIN, 0);
-RT_TASK(waiter, RT_STACK_MIN, 1);
+RT_TASK(waiter, RT_STACK_MIN, 0);

cxx/examples/once.cpp

@@ -35,5 +35,5 @@ static void twicer(void)
 
 #define STACK_SIZE (RT_STACK_MIN * 2)
 
-RT_TASK(oncer, STACK_SIZE, 1);
+RT_TASK(oncer, STACK_SIZE, 0);
-RT_TASK(twicer, STACK_SIZE, 1);
+RT_TASK(twicer, STACK_SIZE, 0);

cxx/examples/queue.cpp

@@ -51,4 +51,4 @@ static void timeout(void)
 RT_TASK(popper, RT_STACK_MIN, 1);
 RT_TASK_ARG(pusher, 0, RT_STACK_MIN, 1);
 RT_TASK_ARG(pusher, TASK_INC, RT_STACK_MIN, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

cxx/examples/recursive.cpp

@@ -25,4 +25,4 @@ static void locker(void)
     rt::trap();
 }
 
-RT_TASK(locker, RT_STACK_MIN, 1);
+RT_TASK(locker, RT_STACK_MIN, 0);

cxx/examples/rwlock.cpp

@@ -44,4 +44,4 @@ static void timeout(void)
 RT_TASK(reader, STACK_SIZE, 1);
 RT_TASK(reader, STACK_SIZE, 1);
 RT_TASK(writer, STACK_SIZE, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

cxx/examples/sem.cpp

@@ -33,5 +33,5 @@ static void waiter(void)
     rt::trap();
 }
 
-RT_TASK(poster, RT_STACK_MIN, 1);
+RT_TASK(poster, RT_STACK_MIN, 0);
-RT_TASK(waiter, RT_STACK_MIN, 1);
+RT_TASK(waiter, RT_STACK_MIN, 0);

cxx/examples/sleep.cpp

@@ -26,5 +26,5 @@ static void sleep_periodic(uintptr_t period)
     }
 }
 
-RT_TASK_ARG(sleep_periodic, 5, RT_STACK_MIN, 2);
+RT_TASK_ARG(sleep_periodic, 5, RT_STACK_MIN, 0);
 RT_TASK_ARG(sleep_periodic, 10, RT_STACK_MIN, 1);

cxx/examples/water/water.cpp

@@ -64,7 +64,7 @@ static void hydrogen_loop(void)
 
 #define STACK_SIZE (RT_STACK_MIN * 2)
 
-RT_TASK(timeout, RT_STACK_MIN, 3);
+RT_TASK(timeout, RT_STACK_MIN, 0);
-RT_TASK(hydrogen_loop, STACK_SIZE, 1);
+RT_TASK(hydrogen_loop, STACK_SIZE, 2);
-RT_TASK(hydrogen_loop, STACK_SIZE, 1);
+RT_TASK(hydrogen_loop, STACK_SIZE, 2);
-RT_TASK(oxygen_loop, STACK_SIZE, 2);
+RT_TASK(oxygen_loop, STACK_SIZE, 1);

examples/cycle/mutex.c

@@ -35,4 +35,4 @@ static void task1(void)
 }
 
 RT_TASK(task0, RT_STACK_MIN, 1);
-RT_TASK(task1, RT_STACK_MIN, 2);
+RT_TASK(task1, RT_STACK_MIN, 0);

examples/cycle/notify.c

@@ -31,4 +31,4 @@ static void waiter(void)
 }
 
 RT_TASK(poster, RT_STACK_MIN, 1);
-RT_TASK(waiter, RT_STACK_MIN, 2);
+RT_TASK(waiter, RT_STACK_MIN, 0);

examples/cycle/queue.c

@@ -32,5 +32,5 @@ static void popper(void)
     rt_trap();
 }
 
-RT_TASK(popper, RT_STACK_MIN, 2);
+RT_TASK(popper, RT_STACK_MIN, 0);
 RT_TASK(pusher, RT_STACK_MIN, 1);

examples/cycle/sem.c

@@ -31,4 +31,4 @@ static void waiter(void)
 }
 
 RT_TASK(poster, RT_STACK_MIN, 1);
-RT_TASK(waiter, RT_STACK_MIN, 2);
+RT_TASK(waiter, RT_STACK_MIN, 0);

examples/cycle/sleep.c

@@ -27,5 +27,5 @@ static void task1(void)
     rt_trap();
 }
 
-RT_TASK(sleep, RT_STACK_MIN, 2);
+RT_TASK(sleep, RT_STACK_MIN, 0);
 RT_TASK(task1, RT_STACK_MIN, 1);

examples/cycle/yield.c

@@ -29,5 +29,5 @@ static void task1(void)
 
 /* NOTE: Tasks of equal priority will initially be executed in the order they
  * are created. */
-RT_TASK(task0, RT_STACK_MIN, 1);
+RT_TASK(task0, RT_STACK_MIN, 0);
-RT_TASK(task1, RT_STACK_MIN, 1);
+RT_TASK(task1, RT_STACK_MIN, 0);

examples/donate.c

@@ -70,7 +70,7 @@ static void donator(void)
     sequence(9);
 }
 
-RT_TASK(locker0, RT_STACK_MIN, 1);
+RT_TASK(locker0, RT_STACK_MIN, 3);
 RT_TASK(locker1, RT_STACK_MIN, 2);
-RT_TASK(spinner, RT_STACK_MIN, 3);
+RT_TASK(spinner, RT_STACK_MIN, 1);
-RT_TASK(donator, RT_STACK_MIN, 4);
+RT_TASK(donator, RT_STACK_MIN, 0);

examples/empty.c

@@ -7,4 +7,4 @@ static void empty(void)
     rt_trap();
 }
 
-RT_TASK(empty, RT_STACK_MIN, 1);
+RT_TASK(empty, RT_STACK_MIN, 0);

examples/fair.c

@@ -49,4 +49,4 @@ RT_TASK_ARG(task, 0, RT_STACK_MIN * 2, 1);
 RT_TASK_ARG(task, 1, RT_STACK_MIN * 2, 1);
 RT_TASK_ARG(task, 2, RT_STACK_MIN * 2, 1);
 RT_TASK_ARG(task, 3, RT_STACK_MIN * 2, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

examples/float.c

@@ -29,4 +29,4 @@ static void timeout(void)
 RT_TASK_ARG(f, 1, RT_STACK_FP_MIN, 1);
 RT_TASK_ARG(f, 2, RT_STACK_FP_MIN, 1);
 
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

examples/mutex.c

@@ -72,4 +72,4 @@ static void timeout(void)
 RT_TASK(increment_lock, RT_STACK_MIN, 1);
 RT_TASK(increment_trylock, RT_STACK_MIN, 1);
 RT_TASK(increment_timedlock, RT_STACK_MIN, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

examples/notify.c

@@ -35,5 +35,5 @@ static void waiter(void)
     rt_trap();
 }
 
-RT_TASK(notifier, RT_STACK_MIN, 1);
+RT_TASK(notifier, RT_STACK_MIN, 0);
-RT_TASK(waiter, RT_STACK_MIN, 1);
+RT_TASK(waiter, RT_STACK_MIN, 0);

examples/once.c

@@ -44,5 +44,5 @@ static void oncer_reset(void)
 
 #define STACK_SIZE (RT_STACK_MIN * 2)
 
-RT_TASK(oncer, STACK_SIZE, 1);
+RT_TASK(oncer, STACK_SIZE, 0);
-RT_TASK(oncer_reset, STACK_SIZE, 1);
+RT_TASK(oncer_reset, STACK_SIZE, 0);

examples/queue.c

@@ -51,4 +51,4 @@ static void timeout(void)
 RT_TASK(popper, RT_STACK_MIN, 1);
 RT_TASK_ARG(pusher, 0, RT_STACK_MIN, 1);
 RT_TASK_ARG(pusher, TASK_INC, RT_STACK_MIN, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

examples/recursive.c

@@ -27,4 +27,4 @@ static void locker(void)
     rt_trap();
 }
 
-RT_TASK(locker, RT_STACK_MIN, 1);
+RT_TASK(locker, RT_STACK_MIN, 0);

examples/rwlock.c

@@ -40,7 +40,7 @@ static void timeout(void)
 
 #define STACK_SIZE (RT_STACK_MIN * 2)
 
-RT_TASK(reader, STACK_SIZE, 1);
+RT_TASK(reader, STACK_SIZE, 2);
-RT_TASK(reader, STACK_SIZE, 1);
+RT_TASK(reader, STACK_SIZE, 2);
-RT_TASK(writer, STACK_SIZE, 2);
+RT_TASK(writer, STACK_SIZE, 1);
-RT_TASK(timeout, RT_STACK_MIN, 2);
+RT_TASK(timeout, RT_STACK_MIN, 0);

examples/sem.c

@@ -32,5 +32,5 @@ static void waiter(void)
     rt_trap();
 }
 
-RT_TASK(poster, RT_STACK_MIN, 1);
+RT_TASK(poster, RT_STACK_MIN, 0);
-RT_TASK(waiter, RT_STACK_MIN, 1);
+RT_TASK(waiter, RT_STACK_MIN, 0);

examples/simple.c

@@ -15,5 +15,5 @@ static void simple(uintptr_t arg)
     }
 }
 
-RT_TASK_ARG(simple, 0, RT_STACK_MIN, 1);
+RT_TASK_ARG(simple, 0, RT_STACK_MIN, 0);
-RT_TASK_ARG(simple, 1, RT_STACK_MIN, 1);
+RT_TASK_ARG(simple, 1, RT_STACK_MIN, 0);

examples/sleep.c

@@ -27,5 +27,5 @@ static void sleep_periodic(uintptr_t period)
     }
 }
 
-RT_TASK_ARG(sleep_periodic, 5, RT_STACK_MIN, 2);
+RT_TASK_ARG(sleep_periodic, 5, RT_STACK_MIN, 0);
 RT_TASK_ARG(sleep_periodic, 10, RT_STACK_MIN, 1);

examples/water/water.c

@@ -64,7 +64,7 @@ static void hydrogen_loop(void)
 
 #define STACK_SIZE (RT_STACK_MIN * 2)
 
-RT_TASK(timeout, RT_STACK_MIN, 3);
+RT_TASK(timeout, RT_STACK_MIN, 0);
-RT_TASK(hydrogen_loop, STACK_SIZE, 1);
+RT_TASK(hydrogen_loop, STACK_SIZE, 2);
-RT_TASK(hydrogen_loop, STACK_SIZE, 1);
+RT_TASK(hydrogen_loop, STACK_SIZE, 2);
-RT_TASK(oxygen_loop, STACK_SIZE, 2);
+RT_TASK(oxygen_loop, STACK_SIZE, 1);

include/rt/task.h

@@ -9,26 +9,21 @@
 
 #include <limits.h>
 
-#ifndef RT_TASK_READY_CLZ_ENABLE
+#ifndef RT_TASK_READY_CTZ_ENABLE
 
 #if ((defined(__arm__) || defined(__aarch64__)) &&                             \
      defined(__ARM_FEATURE_CLZ)) ||                                            \
     defined(__x86_64__)
-#define RT_TASK_READY_CLZ_ENABLE 1
+#define RT_TASK_READY_CTZ_ENABLE 1
 #else
-#define RT_TASK_READY_CLZ_ENABLE 0
+#define RT_TASK_READY_CTZ_ENABLE 0
 #endif
 
 #endif
 
-#define RT_TASK_PRIORITY_MIN 0U
+#define RT_TASK_PRIORITY_MIN UINT32_C(0)
-
+#define RT_TASK_PRIORITY_MAX ((sizeof(uint32_t) * CHAR_BIT) - 1)
-#if RT_TASK_READY_CLZ_ENABLE
+#define RT_TASK_PRIORITY_IDLE RT_TASK_PRIORITY_MAX
-#define RT_TASK_PRIORITY_MAX ((sizeof(unsigned) * CHAR_BIT) - 1)
-#else
-// It must be possible to add one to a priority without overflow.
-#define RT_TASK_PRIORITY_MAX (UINT_MAX - 1)
-#endif
 
 #ifdef __cplusplus
 extern "C" {
@@ -82,14 +77,14 @@ struct rt_task
     struct rt_list list;
     struct rt_list sleep_list;
     void *ctx;
-    struct rt_list *list_head;
+    uint32_t priority;
-    unsigned priority;
+    uint32_t base_priority;
-    unsigned base_priority;
     enum rt_task_state state;
     unsigned long wake_tick;
 #if RT_MPU_ENABLE
     struct rt_mpu_config mpu_config;
 #endif
+    struct rt_list *wait_list_head;
     struct rt_mutex *blocking_mutex;
     union
     {
@@ -133,8 +128,7 @@ void rt_task_ready(struct rt_task *task);
 /* Use a constructor with a priority based on __COUNTER__ so that tasks created
  * in the same file with equal priority are added to the ready list in the
  * order they appear in the file. Use % 60000 to ensure that the overall
- * constructor priority does not wrap around at 2^16 and is always greater than
+ * constructor priority does not wrap around at 2^16. */
- * the priority of the list initialization constructor. */
 
 #define RT_TASK_COMMON(fn, stack_size, priority, name, ctx_init, ...)          \
     RT_TASK_COMMON_(fn, stack_size, priority, name, ctx_init, __COUNTER__,     \
@@ -157,9 +151,9 @@ void rt_task_ready(struct rt_task *task);
         fn##_task.ctx = ctx_init;                                              \
         rt_task_ready(&fn##_task);                                             \
     }                                                                          \
-    rt_static_assert((intmax_t)(priority) <= (intmax_t)RT_TASK_PRIORITY_MAX,   \
+    rt_static_assert((priority) <= RT_TASK_PRIORITY_MAX,                       \
                      "the priority of task \"" name "\", " #priority           \
-                     ", is higher than the maximum allowed")
+                     ", exceeds the maximum value")
 
 /* Create a task that runs fn on a stack of at least stack_size, with the given
  * priority. Additional arguments are MPU regions that will be active while the

rust/examples/donate.rs

@@ -63,7 +63,7 @@ fn donator() {
     sequence(9);
 }
 
-rt::task!(locker0, rt::task::STACK_MIN, 1);
+rt::task!(locker0, rt::task::STACK_MIN, 3);
 rt::task!(locker1, rt::task::STACK_MIN, 2);
-rt::task!(spinner, rt::task::STACK_MIN, 3);
+rt::task!(spinner, rt::task::STACK_MIN, 1);
-rt::task!(donator, rt::task::STACK_MIN, 4);
+rt::task!(donator, rt::task::STACK_MIN, 0);

rust/examples/empty.rs

@@ -5,4 +5,4 @@ fn empty() {
     rt::trap();
 }
 
-rt::task!(empty, rt::task::STACK_MIN, 1);
+rt::task!(empty, rt::task::STACK_MIN, 0);

rust/examples/float.rs

@@ -26,4 +26,4 @@ fn timeout() {
 
 rt::task!(f(1), 2 * rt::task::STACK_MIN, 1);
 rt::task!(f(2), 2 * rt::task::STACK_MIN, 1);
-rt::task!(timeout, rt::task::STACK_MIN, 2);
+rt::task!(timeout, rt::task::STACK_MIN, 0);

rust/examples/mutex.rs

@@ -61,4 +61,4 @@ fn timeout() {
 rt::task!(increment_lock, rt::task::STACK_MIN, 1);
 rt::task!(increment_trylock, rt::task::STACK_MIN, 1);
 rt::task!(increment_timedlock, rt::task::STACK_MIN, 1);
-rt::task!(timeout, rt::task::STACK_MIN, 2);
+rt::task!(timeout, rt::task::STACK_MIN, 0);

rust/examples/notify.rs

@@ -28,5 +28,5 @@ fn waiter() {
     rt::trap();
 }
 
-rt::task!(notifier, rt::task::STACK_MIN, 1);
+rt::task!(notifier, rt::task::STACK_MIN, 0);
-rt::task!(waiter, rt::task::STACK_MIN, 1);
+rt::task!(waiter, rt::task::STACK_MIN, 0);

rust/examples/once.rs

@@ -30,5 +30,5 @@ fn oncer_trap() {
     rt::trap();
 }
 
-rt::task!(oncer, rt::task::STACK_MIN, 1);
+rt::task!(oncer, rt::task::STACK_MIN, 0);
-rt::task!(oncer_trap, rt::task::STACK_MIN, 1);
+rt::task!(oncer_trap, rt::task::STACK_MIN, 0);

rust/examples/queue.rs

@@ -45,4 +45,4 @@ fn timeout() {
 rt::task!(pusher(0), rt::task::STACK_MIN, 1);
 rt::task!(pusher(TASK_INC), rt::task::STACK_MIN, 1);
 rt::task!(popper, rt::task::STACK_MIN, 1);
-rt::task!(timeout, rt::task::STACK_MIN, 2);
+rt::task!(timeout, rt::task::STACK_MIN, 0);

rust/examples/rwlock.rs

@@ -35,4 +35,4 @@ fn timeout() {
 rt::task!(reader, rt::task::STACK_MIN, 1);
 rt::task!(reader, rt::task::STACK_MIN, 1);
 rt::task!(writer, rt::task::STACK_MIN, 1);
-rt::task!(timeout, rt::task::STACK_MIN, 2);
+rt::task!(timeout, rt::task::STACK_MIN, 0);

rust/examples/semaphore.rs

@@ -24,5 +24,5 @@ fn waiter() {
     rt::trap();
 }
 
-rt::task!(poster, rt::task::STACK_MIN, 1);
+rt::task!(poster, rt::task::STACK_MIN, 0);
-rt::task!(waiter, rt::task::STACK_MIN, 1);
+rt::task!(waiter, rt::task::STACK_MIN, 0);

rust/examples/simple.rs

@@ -11,5 +11,5 @@ fn simple(arg: usize) {
     }
 }
 
-rt::task!(simple(0), rt::task::STACK_MIN, 1);
+rt::task!(simple(0), rt::task::STACK_MIN, 0);
-rt::task!(simple(1), rt::task::STACK_MIN, 1);
+rt::task!(simple(1), rt::task::STACK_MIN, 0);

rust/examples/sleep.rs

@@ -22,5 +22,5 @@ fn sleep_periodic(period: rt::tick::Utick) {
     }
 }
 
-rt::task!(sleep_periodic(5), rt::task::STACK_MIN, 2);
+rt::task!(sleep_periodic(5), rt::task::STACK_MIN, 0);
 rt::task!(sleep_periodic(10), rt::task::STACK_MIN, 1);

rust/examples/water/water.rs

@@ -46,7 +46,7 @@ fn hydrogen_loop() {
     }
 }
 
-rt::task!(timeout, rt::task::STACK_MIN, 3);
+rt::task!(timeout, rt::task::STACK_MIN, 0);
-rt::task!(hydrogen_loop, rt::task::STACK_MIN, 1);
+rt::task!(hydrogen_loop, rt::task::STACK_MIN, 2);
-rt::task!(hydrogen_loop, rt::task::STACK_MIN, 1);
+rt::task!(hydrogen_loop, rt::task::STACK_MIN, 2);
-rt::task!(oxygen_loop, rt::task::STACK_MIN, 2);
+rt::task!(oxygen_loop, rt::task::STACK_MIN, 1);

rust/src/task.rs

@@ -62,11 +62,11 @@ impl Task {
                 list: list_init(&t.list),
                 sleep_list: list_init(&t.sleep_list),
                 ctx: null_mut(),
-                list_head: null_mut(),
                 priority,
                 base_priority: priority,
                 state: rt_task_state::RT_TASK_STATE_INIT,
                 wake_tick: 0,
+                wait_list_head: null_mut(),
                 blocking_mutex: null_mut(),
                 timeout_ptr: unsafe { zeroed() },
                 mutex_list: list_init(&t.mutex_list),

src/rt.c

@@ -29,80 +29,57 @@ static inline struct rt_mutex *mutex_from_list(const struct rt_list *l)
     return rt_container_of(l, struct rt_mutex, list);
 }
 
-static bool task_priority_greater_than(const struct rt_list *a,
+static bool task_priority_less_than(const struct rt_list *a,
-                                       const struct rt_list *b)
+                                    const struct rt_list *b)
 {
-    return task_from_list(a)->priority > task_from_list(b)->priority;
+    return task_from_list(a)->priority < task_from_list(b)->priority;
 }
 
 static void insert_by_priority(struct rt_list *list, struct rt_task *task)
 {
-    rt_list_insert_by(list, &task->list, task_priority_greater_than);
+    rt_list_insert_by(list, &task->list, task_priority_less_than);
-    task->list_head = list;
 }
 
-#if RT_TASK_READY_CLZ_ENABLE
 RT_MPU_PRIV_BSS(rt_ready_bits)
-static unsigned rt_ready_bits = 0;
+static uint32_t rt_ready_bits = 0;
 
 RT_MPU_PRIV_BSS(rt_ready_lists)
-static struct rt_list rt_ready_lists[RT_TASK_PRIORITY_MAX + 1];
+static struct rt_list *rt_ready_lists[RT_TASK_PRIORITY_MAX + 1];
 
-/* Ensure that the ready lists are initialized before any task constructors
+static uint32_t min_ready_priority(void)
- * execute, because they will insert tasks into the ready lists. */
-__attribute__((constructor(RT_TASK_CONSTRUCTOR_PRIORITY - 1), used)) static void
-rt_init_ready_lists(void)
 {
-    for (size_t i = 0; i <= RT_TASK_PRIORITY_MAX; ++i)
+#if RT_TASK_READY_CTZ_ENABLE
-    {
+    return (uint32_t)__builtin_ctz(rt_ready_bits);
-        rt_list_init(&rt_ready_lists[i]);
+#else  // !RT_TASK_READY_CTZ_ENABLE
-    }
+    static const unsigned char debruijn_ctz[32] = {
-}
+        0,  1,  28, 2,  29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4,  8,
-#else
+        31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6,  11, 5,  10, 9,
-RT_MPU_PRIV_DATA(rt_ready_list)
+    };
-static RT_LIST(rt_ready_list);
+    const uint32_t min_bit = rt_ready_bits & -rt_ready_bits;
-#endif
+    return debruijn_ctz[(min_bit * 0x077CB531U) >> 27];
-
+#endif // RT_TASK_READY_CTZ_ENABLE
-static bool ready_task_exists(void)
-{
-#if RT_TASK_READY_CLZ_ENABLE
-    return rt_ready_bits != 0;
-#else
-    return !rt_list_is_empty(&rt_ready_list);
-#endif
 }
 
-static struct rt_task *ready_task(void)
+static bool mutex_priority_less_than(const struct rt_list *a,
-{
+                                     const struct rt_list *b)
-#if RT_TASK_READY_CLZ_ENABLE
-    const unsigned max_ready_priority =
-        RT_TASK_PRIORITY_MAX - (unsigned)__builtin_clz(rt_ready_bits);
-    return task_from_list(rt_list_front(&rt_ready_lists[max_ready_priority]));
-#else
-    return task_from_list(rt_list_front(&rt_ready_list));
-#endif
-}
-
-static bool mutex_priority_greater_than(const struct rt_list *a,
-                                        const struct rt_list *b)
 {
     const struct rt_mutex *const ma = mutex_from_list(a);
     const struct rt_mutex *const mb = mutex_from_list(b);
     // Only mutexes that have waiters should be compared.
-    return task_from_list(rt_list_front(&ma->wait_list))->priority >
+    return task_from_list(rt_list_front(&ma->wait_list))->priority <
            task_from_list(rt_list_front(&mb->wait_list))->priority;
 }
 
 static void insert_mutex_by_priority(struct rt_list *list,
                                      struct rt_mutex *mutex)
 {
-    rt_list_insert_by(list, &mutex->list, mutex_priority_greater_than);
+    rt_list_insert_by(list, &mutex->list, mutex_priority_less_than);
 }
 
 RT_MPU_PRIV_BSS(rt_pending_syscalls)
 static rt_atomic(struct rt_syscall_record *) rt_pending_syscalls = NULL;
 
-RT_TASK(rt_idle, RT_STACK_MIN, RT_TASK_PRIORITY_MIN);
+RT_TASK(rt_idle, RT_STACK_MIN, RT_TASK_PRIORITY_IDLE);
 
 /* rt_active_task must be readable from user code.
  * Task structures themselves are privileged. */
@@ -126,26 +103,40 @@ const char *rt_task_name(void)
 void rt_task_ready(struct rt_task *task)
 {
     task->state = RT_TASK_STATE_READY;
-#if RT_TASK_READY_CLZ_ENABLE
+    struct rt_list *const list = rt_ready_lists[task->priority];
-    struct rt_list *list = &rt_ready_lists[task->priority];
+    if (list == NULL)
-    rt_list_push_back(list, &task->list);
+    {
-    rt_ready_bits |= 1U << task->priority;
+        rt_ready_lists[task->priority] = &task->list;
-    task->list_head = list;
+        rt_ready_bits |= 1U << task->priority;
-#else
+    }
-    insert_by_priority(&rt_ready_list, task);
+    else
-#endif
+    {
+        rt_list_push_back(list, &task->list);
+    }
 }
 
 static void task_unready(struct rt_task *task)
 {
-    rt_list_remove(&task->list);
+    if (rt_list_is_empty(&task->list))
-#if RT_TASK_READY_CLZ_ENABLE
-    if (rt_list_is_empty(task->list_head))
     {
+        rt_ready_lists[task->priority] = NULL;
         rt_ready_bits &= ~(1U << task->priority);
     }
-#endif
+    else
-    task->list_head = NULL;
+    {
+        if (rt_ready_lists[task->priority] == &task->list)
+        {
+            rt_ready_lists[task->priority] = task->list.next;
+        }
+        rt_list_remove(&task->list);
+    }
+}
+
+static void task_wait(struct rt_task *task, struct rt_list *list)
+{
+    task_unready(task);
+    insert_by_priority(list, task);
+    task->wait_list_head = list;
 }
 
 __attribute__((noreturn)) void rt_task_exit(void)
@@ -174,8 +165,8 @@ void *rt_start_context(void)
     rt_task_cycle_resume();
 
     // Initially all tasks are ready, including the idle task.
-    struct rt_task *const first_task = ready_task();
+    struct rt_task *const first_task =
-    task_unready(first_task);
+        task_from_list(rt_ready_lists[min_ready_priority()]);
     first_task->state = RT_TASK_STATE_RUNNING;
     rt_active_task = first_task;
 
@@ -191,70 +182,31 @@ void *rt_start_context(void)
 
 static void *sched(bool yield)
 {
-    if (!ready_task_exists())
+    struct rt_list **const list = &rt_ready_lists[min_ready_priority()];
+    if (yield)
     {
-        /* Note, this will only occur if the idle task is running and all other
+        task_from_list(*list)->state = RT_TASK_STATE_READY;
-         * tasks are blocked. If a different task is running, then the idle
+        *list = (*list)->next;
-         * task will be ready. This means that the active task's state doesn't
-         * need to be checked or adjusted here, because it will always be
-         * running. For active tasks other than idle, the state can be anything
-         * at this point. */
-        rt_logf("sched: no new tasks to run, continuing %s\n", rt_task_name());
-        return NULL;
     }
 
-    struct rt_task *const next_task = ready_task();
+    struct rt_task *const next_task = task_from_list(*list);
-
+    next_task->state = RT_TASK_STATE_RUNNING;
-    /* If the active task invoked a system call to suspend itself, its state
+    if (next_task == rt_active_task)
-     * will be something other than RUNNING here. */
-    const bool still_running = rt_active_task->state == RT_TASK_STATE_RUNNING;
-
-    /* If the active task is still running and has greater or equal priority to
-     * the next task, then continue executing the active task. If yielding,
-     * prefer the next task if they are equal priority. */
-    if (still_running &&
-        (rt_active_task->priority >= (next_task->priority + (unsigned)yield)))
     {
-        rt_logf("sched: %s is still highest priority (%u %s %u)\n",
+        // The same task should still run, so no context switch is required.
-                rt_task_name(), rt_active_task->priority, yield ? ">" : "≥",
-                next_task->priority);
         return NULL;
     }
 
-    /* The next task will be used, so remove it from the corresponding ready
-     * list and clear the ready bit for its priority if necessary. */
-    task_unready(next_task);
-
-    /* If a task made a system call to suspend itself but was then woken up by
-     * its own or another system call and is still the highest priority task,
-     * it should continue running, so don't context switch. */
-    if (rt_active_task == next_task)
-    {
-        rt_logf("sched: %s was suspended and reawakened\n", rt_task_name());
-        rt_active_task->state = RT_TASK_STATE_RUNNING;
-        return NULL;
-    }
-
-    /* If the active task is still running but we are switching to a new task,
-     * add the active task to the ready list and mark it as READY. */
-    if (still_running)
-    {
-        rt_logf("sched: %s is still runnable\n", rt_task_name());
-        rt_task_ready(rt_active_task);
-    }
-
     rt_context_prev = &rt_active_task->ctx;
-    next_task->state = RT_TASK_STATE_RUNNING;
     rt_active_task = next_task;
-
 #if RT_MPU_ENABLE
-    rt_mpu_config = &rt_active_task->mpu_config;
+    rt_mpu_config = &next_task->mpu_config;
 #endif
 
     rt_logf("sched: switching to %s with priority %u\n", rt_task_name(),
-            rt_active_task->priority);
+            next_task->priority);
 
-    return rt_active_task->ctx;
+    return next_task->ctx;
 }
 
 RT_MPU_PRIV_BSS(rt_woken_tick)
@@ -289,6 +241,7 @@ static void wake_sem_waiters(struct rt_sem *sem)
             task_from_list(rt_list_front(&sem->wait_list));
         rt_list_remove(&task->list);
         rt_list_remove(&task->sleep_list);
+        task->wait_list_head = NULL;
         rt_task_ready(task);
         --sem->num_waiters;
     }
@@ -312,6 +265,7 @@ static void wake_mutex_waiter(struct rt_mutex *mutex)
     {
         rt_list_remove(&task->list);
         rt_list_remove(&task->sleep_list);
+        task->wait_list_head = NULL;
         task->blocking_mutex = NULL;
         rt_task_ready(task);
     }
@@ -337,7 +291,7 @@ static void wake_mutex_waiter(struct rt_mutex *mutex)
 static bool task_donate(struct rt_task *task)
 {
     // Recalculate the task's priority starting from its base priority.
-    unsigned int priority = task->base_priority;
+    uint32_t priority = task->base_priority;
 
     /* If the task is holding any donating mutexes, donate the highest priority
      * among them to this task if necessary. */
@@ -345,9 +299,9 @@ static bool task_donate(struct rt_task *task)
     {
         struct rt_mutex *const next_mutex =
             mutex_from_list(rt_list_front(&task->mutex_list));
-        const unsigned int donated_priority =
+        const uint32_t donated_priority =
             task_from_list(rt_list_front(&next_mutex->wait_list))->priority;
-        if (priority < donated_priority)
+        if (priority > donated_priority)
         {
             priority = donated_priority;
         }
@@ -359,9 +313,10 @@ static bool task_donate(struct rt_task *task)
         return false;
     }
 
-    /* If the task's priority changed and it is in a wait list or a ready list,
+    /* If the task's priority changed and it is in a ready list, re-insert it
-     * re-insert it by its new priority. */
+     * by its new priority. */
-    if (task->state == RT_TASK_STATE_READY)
+    if ((task->state == RT_TASK_STATE_RUNNING) ||
+        (task->state == RT_TASK_STATE_READY))
     {
         task_unready(task);
         task->priority = priority;
@@ -369,13 +324,12 @@ static bool task_donate(struct rt_task *task)
     }
     else
     {
-        /* The task might be the active task (e.g., if its priority is being
-         * lowered after unlocking a mutex). */
         task->priority = priority;
-        if (task->list_head != NULL)
+        // If the task is in a wait list, re-insert it by its new priority.
+        if (task->wait_list_head != NULL)
         {
             rt_list_remove(&task->list);
-            insert_by_priority(task->list_head, task);
+            insert_by_priority(task->wait_list_head, task);
         }
     }
 
@@ -425,10 +379,11 @@ static void tick_syscall(void)
         }
 
         // Check if the task is blocked on a timed operation.
-        if (!rt_list_is_empty(&task->list))
+        if (task->wait_list_head != NULL)
         {
             // Unblock the task.
             rt_list_remove(&task->list);
+            task->wait_list_head = NULL;
             if (task->blocking_mutex != NULL)
             {
                 /* If the task was blocked on a mutex_timedlock, remove it from
@@ -537,6 +492,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
     {
         const unsigned long ticks = record->args.task_sleep.ticks;
         rt_active_task->state = RT_TASK_STATE_ASLEEP;
+        task_unready(rt_active_task);
         sleep_until(rt_active_task, rt_woken_tick + ticks);
         break;
     }
@@ -552,6 +508,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
         if (ticks_since_last_wake < period)
         {
             rt_active_task->state = RT_TASK_STATE_ASLEEP;
+            task_unready(rt_active_task);
             sleep_until(rt_active_task, last_wake_tick + period);
         }
         break;
@@ -560,7 +517,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
     {
         struct rt_sem *const sem = record->args.sem_wait.sem;
         rt_active_task->state = RT_TASK_STATE_BLOCKED;
-        insert_by_priority(&sem->wait_list, rt_active_task);
+        task_wait(rt_active_task, &sem->wait_list);
         ++sem->num_waiters;
         /* Evaluate semaphore wakes here as well in case a post occurred
          * before the wait syscall was handled. */
@@ -573,7 +530,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
         const unsigned long ticks = record->args.sem_timedwait.ticks;
         rt_active_task->state = RT_TASK_STATE_BLOCKED_TIMEOUT;
         rt_active_task->timeout_ptr.sem = &record->args.sem_timedwait.sem;
-        insert_by_priority(&sem->wait_list, rt_active_task);
+        task_wait(rt_active_task, &sem->wait_list);
         sleep_until(rt_active_task, rt_woken_tick + ticks);
         ++sem->num_waiters;
         wake_sem_waiters(sem);
@@ -597,7 +554,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
         struct rt_mutex *const mutex = record->args.mutex_lock.mutex;
         rt_active_task->state = RT_TASK_STATE_BLOCKED;
         rt_active_task->blocking_mutex = mutex;
-        insert_by_priority(&mutex->wait_list, rt_active_task);
+        task_wait(rt_active_task, &mutex->wait_list);
         /* When adding a new waiter, we must donate its priority to the
          * task holding the mutex, and transitively to any mutexes that
          * task is blocked on. */
@@ -618,7 +575,7 @@ static void rt_syscall_exec(struct rt_syscall_record *record)
         rt_active_task->state = RT_TASK_STATE_BLOCKED_TIMEOUT;
         rt_active_task->blocking_mutex = mutex;
         rt_active_task->timeout_ptr.mutex = &record->args.mutex_timedlock.mutex;
-        insert_by_priority(&mutex->wait_list, rt_active_task);
+        task_wait(rt_active_task, &mutex->wait_list);
         sleep_until(rt_active_task, rt_woken_tick + ticks);
         mutex_donate(mutex);
         wake_mutex_waiter(mutex);