Add multi-reader, single-writer locks to kernel.

Any number of readers may be in the critical section at a time and writers
are mutually exclusive to all other threads. They are a better choice when
data is rarely modified but often read and multiple threads can safely
access it for reading.

Priority inheritance is fully implemented along with other changes to the
kernel to fully support it on multiowner objects.

This also cleans up priority code in the kernel and updates some associated
structures in existing objects to the cleaner form.

Currently doesn't add the mrsw_lock.[ch] files since they're not yet
needed by anything but the supporting improvements are still useful.

This includes a typed bitarray API (bitarray.h) which is pretty basic
for now.

Change-Id: Idbe43dcd9170358e06d48d00f1c69728ff45b0e3
Reviewed-on: http://gerrit.rockbox.org/801
Reviewed-by: Michael Sevakis <jethead71@rockbox.org>
Tested: Michael Sevakis <jethead71@rockbox.org>

1 files changed, 502 insertions, 459 deletions

diff --git a/firmware/kernel/thread.c b/firmware/kernel/thread.c
index 43ff584a68..0a47f97e93 100644
--- a/firmware/kernel/thread.c
+++ b/firmware/kernel/thread.c
@@ -246,13 +246,13 @@ static void thread_stkov(struct thread_entry *thread)
        cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
 #else
 #define LOCK_THREAD(thread) \
-    ({ })
+    ({ (void)(thread); })
 #define TRY_LOCK_THREAD(thread) \
-    ({ })
+    ({ (void)(thread); })
 #define UNLOCK_THREAD(thread) \
-    ({ })
+    ({ (void)(thread); })
 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
-    ({ })
+    ({ (void)(thread); })
 #endif
 
 /* RTR list */
@@ -279,6 +279,100 @@ static void thread_stkov(struct thread_entry *thread)
 #define rtr_move_entry_inl(core, from, to)
 #endif
 
+static inline void thread_store_context(struct thread_entry *thread)
+{
+#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
+    thread->__errno = errno;
+#endif
+    store_context(&thread->context);
+}
+
+static inline void thread_load_context(struct thread_entry *thread)
+{
+    load_context(&thread->context);
+#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
+    errno = thread->__errno;
+#endif
+}
+
+static inline unsigned int should_switch_tasks(void)
+{
+    unsigned int result = THREAD_OK;
+
+#ifdef HAVE_PRIORITY_SCHEDULING
+    struct thread_entry *current = cores[CURRENT_CORE].running;
+    if (current &&
+        priobit_ffs(&cores[IF_COP_CORE(current->core)].rtr.mask)
+            < current->priority)
+    {
+        /* There is a thread ready to run of higher priority on the same
+         * core as the current one; recommend a task switch. */
+        result |= THREAD_SWITCH;
+    }
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+    return result;
+}
+
+#ifdef HAVE_PRIORITY_SCHEDULING
+/*---------------------------------------------------------------------------
+ * Locks the thread registered as the owner of the block and makes sure it
+ * didn't change in the meantime
+ *---------------------------------------------------------------------------
+ */
+#if NUM_CORES == 1
+static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
+{
+    return bl->thread;
+}
+#else /* NUM_CORES > 1 */
+static struct thread_entry * lock_blocker_thread(struct blocker *bl)
+{
+    /* The blocker thread may change during the process of trying to
+       capture it */
+    while (1)
+    {
+        struct thread_entry *t = bl->thread;
+
+        /* TRY, or else deadlocks are possible */
+        if (!t)
+        {
+            struct blocker_splay *blsplay = (struct blocker_splay *)bl;
+            if (corelock_try_lock(&blsplay->cl))
+            {
+                if (!bl->thread)
+                    return NULL;    /* Still multi */
+
+                corelock_unlock(&blsplay->cl);
+            }
+        }
+        else
+        {
+            if (TRY_LOCK_THREAD(t))
+            {
+                if (bl->thread == t)
+                    return t;
+
+                UNLOCK_THREAD(t);
+            }
+        }
+    }
+}
+#endif /* NUM_CORES */
+
+static inline void unlock_blocker_thread(struct blocker *bl)
+{
+#if NUM_CORES > 1
+    struct thread_entry *blt = bl->thread;
+    if (blt)
+        UNLOCK_THREAD(blt);
+    else
+        corelock_unlock(&((struct blocker_splay *)bl)->cl);
+#endif /* NUM_CORES > 1*/
+    (void)bl;
+}
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
 /*---------------------------------------------------------------------------
  * Thread list structure - circular:
  *    +------------------------------+
@@ -420,7 +514,6 @@ static void remove_from_list_tmo(struct thread_entry *thread)
     }
 }
 
-
 #ifdef HAVE_PRIORITY_SCHEDULING
 /*---------------------------------------------------------------------------
  * Priority distribution structure (one category for each possible priority):
@@ -476,19 +569,9 @@ static void remove_from_list_tmo(struct thread_entry *thread)
 static inline unsigned int prio_add_entry(
     struct priority_distribution *pd, int priority)
 {
-    unsigned int count;
-    /* Enough size/instruction count difference for ARM makes it worth it to
-     * use different code (192 bytes for ARM). Only thing better is ASM. */
-#ifdef CPU_ARM
-    count = pd->hist[priority];
-    if (++count == 1)
-        pd->mask |= 1 << priority;
-    pd->hist[priority] = count;
-#else /* This one's better for Coldfire */
-    if ((count = ++pd->hist[priority]) == 1)
-        pd->mask |= 1 << priority;
-#endif
-
+    unsigned int count = ++pd->hist[priority];
+    if (count == 1)
+        priobit_set_bit(&pd->mask, priority);
     return count;
 }
 
@@ -499,18 +582,9 @@ static inline unsigned int prio_add_entry(
 static inline unsigned int prio_subtract_entry(
     struct priority_distribution *pd, int priority)
 {
-    unsigned int count;
-
-#ifdef CPU_ARM
-    count = pd->hist[priority];
-    if (--count == 0)
-        pd->mask &= ~(1 << priority);
-    pd->hist[priority] = count;
-#else
-    if ((count = --pd->hist[priority]) == 0)
-        pd->mask &= ~(1 << priority);
-#endif
-
+    unsigned int count = --pd->hist[priority];
+    if (count == 0)
+        priobit_clear_bit(&pd->mask, priority);
     return count;
 }
 
@@ -521,31 +595,38 @@ static inline unsigned int prio_subtract_entry(
 static inline void prio_move_entry(
     struct priority_distribution *pd, int from, int to)
 {
-    uint32_t mask = pd->mask;
+    if (--pd->hist[from] == 0)
+        priobit_clear_bit(&pd->mask, from);
+
+    if (++pd->hist[to] == 1)
+        priobit_set_bit(&pd->mask, to);
+}
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+/*---------------------------------------------------------------------------
+ * Move a thread back to a running state on its core.
+ *---------------------------------------------------------------------------
+ */
+static void core_schedule_wakeup(struct thread_entry *thread)
+{
+    const unsigned int core = IF_COP_CORE(thread->core);
 
-#ifdef CPU_ARM
-    unsigned int count;
+    RTR_LOCK(core);
 
-    count = pd->hist[from];
-    if (--count == 0)
-        mask &= ~(1 << from);
-    pd->hist[from] = count;
+    thread->state = STATE_RUNNING;
 
-    count = pd->hist[to];
-    if (++count == 1)
-        mask |= 1 << to;
-    pd->hist[to] = count;
-#else
-    if (--pd->hist[from] == 0)
-        mask &= ~(1 << from);
+    add_to_list_l(&cores[core].running, thread);
+    rtr_add_entry(core, thread->priority);
 
-    if (++pd->hist[to] == 1)
-        mask |= 1 << to;
-#endif
+    RTR_UNLOCK(core);
 
-    pd->mask = mask;
+#if NUM_CORES > 1
+    if (core != CURRENT_CORE)
+        core_wake(core);
+#endif
 }
 
+#ifdef HAVE_PRIORITY_SCHEDULING
 /*---------------------------------------------------------------------------
  * Change the priority and rtr entry for a running thread
  *---------------------------------------------------------------------------
@@ -605,191 +686,211 @@ static int find_highest_priority_in_list_l(
  * those are prevented, right? :-)
  *---------------------------------------------------------------------------
  */
-static struct thread_entry *
-    blocker_inherit_priority(struct thread_entry *current)
+static void inherit_priority(
+    struct blocker * const blocker0, struct blocker *bl,
+    struct thread_entry *blt, int newblpr)
 {
-    const int priority = current->priority;
-    struct blocker *bl = current->blocker;
-    struct thread_entry * const tstart = current;
-    struct thread_entry *bl_t = bl->thread;
-
-    /* Blocker cannot change since the object protection is held */
-    LOCK_THREAD(bl_t);
+    int oldblpr = bl->priority;
 
-    for (;;)
+    while (1)
     {
-        struct thread_entry *next;
-        int bl_pr = bl->priority;
+        if (blt == NULL)
+        {
+            /* Multiple owners */
+            struct blocker_splay *blsplay = (struct blocker_splay *)bl;
+            
+            /* Recurse down the all the branches of this; it's the only way.
+               We might meet the same queue several times if more than one of
+               these threads is waiting the same queue. That isn't a problem
+               for us since we early-terminate, just notable. */
+            FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum)
+            {
+                bl->priority = oldblpr; /* To see the change each time */
+                blt = &threads[slotnum];
+                LOCK_THREAD(blt);
+                inherit_priority(blocker0, bl, blt, newblpr);
+            }
 
-        if (priority >= bl_pr)
-            break; /* Object priority already high enough */
+            corelock_unlock(&blsplay->cl);
+            return;
+        }
 
-        bl->priority = priority;
+        bl->priority = newblpr;
 
-        /* Add this one */
-        prio_add_entry(&bl_t->pdist, priority);
+        /* Update blocker thread inheritance record */
+        if (newblpr < PRIORITY_IDLE)
+            prio_add_entry(&blt->pdist, newblpr);
 
-        if (bl_pr < PRIORITY_IDLE)
-        {
-            /* Not first waiter - subtract old one */
-            prio_subtract_entry(&bl_t->pdist, bl_pr);
-        }
+        if (oldblpr < PRIORITY_IDLE)
+            prio_subtract_entry(&blt->pdist, oldblpr);
 
-        if (priority >= bl_t->priority)
-            break; /* Thread priority high enough */
+        int oldpr = blt->priority;
+        int newpr = priobit_ffs(&blt->pdist.mask);
+        if (newpr == oldpr)
+            break; /* No blocker thread priority change */
 
-        if (bl_t->state == STATE_RUNNING)
+        if (blt->state == STATE_RUNNING)
         {
-            /* Blocking thread is a running thread therefore there are no
-             * further blockers. Change the "run queue" on which it
-             * resides. */
-            set_running_thread_priority(bl_t, priority);
-            break;
+            set_running_thread_priority(blt, newpr);
+            break; /* Running: last in chain */
         }
 
-        bl_t->priority = priority;
+        /* Blocker is blocked */
+        blt->priority = newpr;
 
-        /* If blocking thread has a blocker, apply transitive inheritance */
-        bl = bl_t->blocker;
+        bl = blt->blocker;
+        if (LIKELY(bl == NULL))
+            break; /* Block doesn't support PIP */
 
-        if (bl == NULL)
-            break; /* End of chain or object doesn't support inheritance */
+        if (UNLIKELY(bl == blocker0))
+            break; /* Full circle - deadlock! */
 
-        next = bl->thread;
+        /* Blocker becomes current thread and the process repeats */
+        struct thread_entry **bqp = blt->bqp;
+        struct thread_entry *t = blt;
+        blt = lock_blocker_thread(bl);
 
-        if (UNLIKELY(next == tstart))
-            break; /* Full-circle - deadlock! */
+        UNLOCK_THREAD(t);
 
-        UNLOCK_THREAD(current);
+        /* Adjust this wait queue */
+        oldblpr = bl->priority;
+        if (newpr <= oldblpr)
+            newblpr = newpr;
+        else if (oldpr <= oldblpr)
+            newblpr = find_highest_priority_in_list_l(*bqp);
 
-#if NUM_CORES > 1
-        for (;;)
-        {
-            LOCK_THREAD(next);
+        if (newblpr == oldblpr)
+            break; /* Queue priority not changing */
+    }
 
-            /* Blocker could change - retest condition */
-            if (LIKELY(bl->thread == next))
-                break;
+    UNLOCK_THREAD(blt);
+}
 
-            UNLOCK_THREAD(next);
-            next = bl->thread;
-        }
-#endif
-        current = bl_t;
-        bl_t = next;
+/*---------------------------------------------------------------------------
+ * Quick-disinherit of priority elevation. 'thread' must be a running thread.
+ *---------------------------------------------------------------------------
+ */
+static void priority_disinherit_internal(struct thread_entry *thread,
+                                         int blpr)
+{
+    if (blpr < PRIORITY_IDLE &&
+        prio_subtract_entry(&thread->pdist, blpr) == 0 &&
+        blpr <= thread->priority)
+    {
+        int priority = priobit_ffs(&thread->pdist.mask);
+        if (priority != thread->priority)
+            set_running_thread_priority(thread, priority);
     }
+}
 
-    UNLOCK_THREAD(bl_t);
-
-    return current;
+void priority_disinherit(struct thread_entry *thread, struct blocker *bl)
+{
+    LOCK_THREAD(thread);
+    priority_disinherit_internal(thread, bl->priority);
+    UNLOCK_THREAD(thread);
 }
 
 /*---------------------------------------------------------------------------
- * Readjust priorities when waking a thread blocked waiting for another
- * in essence "releasing" the thread's effect on the object owner. Can be
- * performed from any context.
+ * Transfer ownership from a single owner to a multi-owner splay from a wait
+ * queue
  *---------------------------------------------------------------------------
  */
-struct thread_entry *
-    wakeup_priority_protocol_release(struct thread_entry *thread)
+static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread)
 {
-    const int priority = thread->priority;
-    struct blocker *bl = thread->blocker;
-    struct thread_entry * const tstart = thread;
-    struct thread_entry *bl_t = bl->thread;
+    /* All threads will have the same blocker and queue; only we are changing
+       it now */
+    struct thread_entry **bqp = thread->bqp;
+    struct blocker_splay *blsplay = (struct blocker_splay *)thread->blocker;
+    struct thread_entry *blt = blsplay->blocker.thread;
+
+    /* The first thread is already locked and is assumed tagged "multi" */
+    int count = 1;
+    struct thread_entry *temp_queue = NULL;
+
+    /* 'thread' is locked on entry */
+    while (1)
+    {
+        LOCK_THREAD(blt);
 
-    /* Blocker cannot change since object will be locked */
-    LOCK_THREAD(bl_t);
+        remove_from_list_l(bqp, thread);
+        thread->blocker = NULL;
 
-    thread->blocker = NULL; /* Thread not blocked */
+        struct thread_entry *tnext = *bqp;
+        if (tnext == NULL || tnext->retval == 0)
+            break;
 
-    for (;;)
-    {
-        struct thread_entry *next;
-        int bl_pr = bl->priority;
+        add_to_list_l(&temp_queue, thread);
 
-        if (priority > bl_pr)
-            break; /* Object priority higher */
+        UNLOCK_THREAD(thread);
+        UNLOCK_THREAD(blt);
 
-        next = *thread->bqp;
+        count++;
+        thread = tnext;
 
-        if (next == NULL)
-        {
-            /* No more threads in queue */
-            prio_subtract_entry(&bl_t->pdist, bl_pr);
-            bl->priority = PRIORITY_IDLE;
-        }
-        else
-        {
-            /* Check list for highest remaining priority */
-            int queue_pr = find_highest_priority_in_list_l(next);
+        LOCK_THREAD(thread);
+    }
 
-            if (queue_pr == bl_pr)
-                break; /* Object priority not changing */
+    int blpr = blsplay->blocker.priority;
+    priority_disinherit_internal(blt, blpr);
 
-            /* Change queue priority */
-            prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
-            bl->priority = queue_pr;
-        }
+    /* Locking order reverses here since the threads are no longer on the
+       queue side */
+    if (count > 1)
+    {
+        add_to_list_l(&temp_queue, thread);
+        UNLOCK_THREAD(thread);
+        corelock_lock(&blsplay->cl);
+
+        blpr = find_highest_priority_in_list_l(*bqp);
+        blsplay->blocker.thread = NULL;
 
-        if (bl_pr > bl_t->priority)
-            break; /* thread priority is higher */
+        thread = temp_queue;
+        LOCK_THREAD(thread);
+    }
+    else
+    {
+        /* Becomes a simple, direct transfer */
+        if (thread->priority <= blpr)
+            blpr = find_highest_priority_in_list_l(*bqp);
+        blsplay->blocker.thread = thread;
+    }
 
-        bl_pr = find_first_set_bit(bl_t->pdist.mask);
+    blsplay->blocker.priority = blpr;
 
-        if (bl_pr == bl_t->priority)
-            break; /* Thread priority not changing */
+    while (1)
+    {
+        unsigned int slotnum = THREAD_ID_SLOT(thread->id);
+        threadbit_set_bit(&blsplay->mask, slotnum);
 
-        if (bl_t->state == STATE_RUNNING)
+        if (blpr < PRIORITY_IDLE)
         {
-            /* No further blockers */
-            set_running_thread_priority(bl_t, bl_pr);
-            break;
+            prio_add_entry(&thread->pdist, blpr);
+            if (blpr < thread->priority)
+                thread->priority = blpr;
         }
 
-        bl_t->priority = bl_pr;
-
-        /* If blocking thread has a blocker, apply transitive inheritance */
-        bl = bl_t->blocker;
+        if (count > 1)
+            remove_from_list_l(&temp_queue, thread);
 
-        if (bl == NULL)
-            break; /* End of chain or object doesn't support inheritance */
-
-        next = bl->thread;
-
-        if (UNLIKELY(next == tstart))
-            break; /* Full-circle - deadlock! */
+        core_schedule_wakeup(thread);
 
         UNLOCK_THREAD(thread);
 
-#if NUM_CORES > 1
-        for (;;)
-        {
-            LOCK_THREAD(next);
-
-            /* Blocker could change - retest condition */
-            if (LIKELY(bl->thread == next))
-                break;
+        thread = temp_queue;
+        if (thread == NULL)
+            break;
 
-            UNLOCK_THREAD(next);
-            next = bl->thread;
-        }
-#endif
-        thread = bl_t;
-        bl_t = next;
+        LOCK_THREAD(thread);
     }
 
-    UNLOCK_THREAD(bl_t);
+    UNLOCK_THREAD(blt);
 
-#if NUM_CORES > 1
-    if (UNLIKELY(thread != tstart))
+    if (count > 1)
     {
-        /* Relock original if it changed */
-        LOCK_THREAD(tstart);
+        corelock_unlock(&blsplay->cl);
     }
-#endif
 
-    return cores[CURRENT_CORE].running;
+    blt->retval = count;
 }
 
 /*---------------------------------------------------------------------------
@@ -801,67 +902,95 @@ struct thread_entry *
  * it is the running thread is made.
  *---------------------------------------------------------------------------
  */
-struct thread_entry *
-    wakeup_priority_protocol_transfer(struct thread_entry *thread)
+static void wakeup_thread_transfer(struct thread_entry *thread)
 {
-    /* Waking thread inherits priority boost from object owner */
+    /* Waking thread inherits priority boost from object owner (blt) */
     struct blocker *bl = thread->blocker;
-    struct thread_entry *bl_t = bl->thread;
-    struct thread_entry *next;
-    int bl_pr;
+    struct thread_entry *blt = bl->thread;
 
-    THREAD_ASSERT(cores[CURRENT_CORE].running == bl_t,
+    THREAD_ASSERT(cores[CURRENT_CORE].running == blt,
                   "UPPT->wrong thread", cores[CURRENT_CORE].running);
 
-    LOCK_THREAD(bl_t);
+    LOCK_THREAD(blt);
+
+    struct thread_entry **bqp = thread->bqp;
+    remove_from_list_l(bqp, thread);
+    thread->blocker = NULL;
 
-    bl_pr = bl->priority;
+    int blpr = bl->priority;
 
     /* Remove the object's boost from the owning thread */
-    if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 &&
-        bl_pr <= bl_t->priority)
+    if (prio_subtract_entry(&blt->pdist, blpr) == 0 && blpr <= blt->priority)
     {
         /* No more threads at this priority are waiting and the old level is
          * at least the thread level */
-        int priority = find_first_set_bit(bl_t->pdist.mask);
-
-        if (priority != bl_t->priority)
-        {
-            /* Adjust this thread's priority */
-            set_running_thread_priority(bl_t, priority);
-        }
+        int priority = priobit_ffs(&blt->pdist.mask);
+        if (priority != blt->priority)
+            set_running_thread_priority(blt, priority);
     }
 
-    next = *thread->bqp;
+    struct thread_entry *tnext = *bqp;
 
-    if (LIKELY(next == NULL))
+    if (LIKELY(tnext == NULL))
     {
         /* Expected shortcut - no more waiters */
-        bl_pr = PRIORITY_IDLE;
+        blpr = PRIORITY_IDLE;
     }
     else
     {
-        if (thread->priority <= bl_pr)
-        {
-            /* Need to scan threads remaining in queue */
-            bl_pr = find_highest_priority_in_list_l(next);
-        }
+        /* If lowering, we need to scan threads remaining in queue */
+        int priority = thread->priority;
+        if (priority <= blpr)
+            blpr = find_highest_priority_in_list_l(tnext);
 
-        if (prio_add_entry(&thread->pdist, bl_pr) == 1 &&
-            bl_pr < thread->priority)
-        {
-            /* Thread priority must be raised */
-            thread->priority = bl_pr;
-        }
+        if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < priority)
+            thread->priority = blpr; /* Raise new owner */
     }
 
-    bl->thread = thread;    /* This thread pwns */
-    bl->priority = bl_pr;   /* Save highest blocked priority */
-    thread->blocker = NULL; /* Thread not blocked */
+    core_schedule_wakeup(thread);
+    UNLOCK_THREAD(thread);
+
+    bl->thread   = thread;  /* This thread pwns */
+    bl->priority = blpr;    /* Save highest blocked priority */
+    UNLOCK_THREAD(blt);
+}
+
+/*---------------------------------------------------------------------------
+ * Readjust priorities when waking a thread blocked waiting for another
+ * in essence "releasing" the thread's effect on the object owner. Can be
+ * performed from any context.
+ *---------------------------------------------------------------------------
+ */
+static void wakeup_thread_release(struct thread_entry *thread)
+{
+    struct blocker *bl = thread->blocker;
+    struct thread_entry *blt = lock_blocker_thread(bl);
+    struct thread_entry **bqp = thread->bqp;
+    remove_from_list_l(bqp, thread);
+    thread->blocker = NULL;
+
+    /* Off to see the wizard... */
+    core_schedule_wakeup(thread);
+
+    if (thread->priority > bl->priority)
+    {
+        /* Queue priority won't change */
+        UNLOCK_THREAD(thread);
+        unlock_blocker_thread(bl);
+        return;
+    }
+
+    UNLOCK_THREAD(thread);
 
-    UNLOCK_THREAD(bl_t);
+    int newblpr = find_highest_priority_in_list_l(*bqp);
+    if (newblpr == bl->priority)
+    {
+        /* Blocker priority won't change */
+        unlock_blocker_thread(bl);
+        return;
+    }
 
-    return bl_t;
+    inherit_priority(bl, bl, blt, newblpr);
 }
 
 /*---------------------------------------------------------------------------
@@ -877,9 +1006,8 @@ static void __attribute__((noinline)) check_for_obj_waiters(
 {
     /* Only one bit in the mask should be set with a frequency on 1 which
      * represents the thread's own base priority */
-    uint32_t mask = thread->pdist.mask;
-    if ((mask & (mask - 1)) != 0 ||
-        thread->pdist.hist[find_first_set_bit(mask)] > 1)
+    if (priobit_popcount(&thread->pdist.mask) != 1 ||
+        thread->pdist.hist[priobit_ffs(&thread->pdist.mask)] > 1)
     {
         unsigned char name[32];
         thread_get_name(name, 32, thread);
@@ -889,26 +1017,72 @@ static void __attribute__((noinline)) check_for_obj_waiters(
 #endif /* HAVE_PRIORITY_SCHEDULING */
 
 /*---------------------------------------------------------------------------
- * Move a thread back to a running state on its core.
+ * Explicitly wakeup a thread on a blocking queue. Only effects threads of
+ * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
+ *
+ * This code should be considered a critical section by the caller meaning
+ * that the object's corelock should be held.
+ *
+ * INTERNAL: Intended for use by kernel objects and not for programs.
  *---------------------------------------------------------------------------
  */
-static void core_schedule_wakeup(struct thread_entry *thread)
+unsigned int wakeup_thread_(struct thread_entry **list
+                            IF_PRIO(, enum wakeup_thread_protocol proto))
 {
-    const unsigned int core = IF_COP_CORE(thread->core);
+    struct thread_entry *thread = *list;
 
-    RTR_LOCK(core);
+    /* Check if there is a blocked thread at all. */
+    if (*list == NULL)
+        return THREAD_NONE;
 
-    thread->state = STATE_RUNNING;
+    LOCK_THREAD(thread);
 
-    add_to_list_l(&cores[core].running, thread);
-    rtr_add_entry(core, thread->priority);
+    /* Determine thread's current state. */
+    switch (thread->state)
+    {
+    case STATE_BLOCKED:
+    case STATE_BLOCKED_W_TMO:
+#ifdef HAVE_PRIORITY_SCHEDULING
+        /* Threads with PIP blockers cannot specify "WAKEUP_DEFAULT" */
+        if (thread->blocker != NULL)
+        {
+            static void (* const funcs[])(struct thread_entry *thread)
+                ICONST_ATTR =
+            {
+                [WAKEUP_DEFAULT]        = NULL,
+                [WAKEUP_TRANSFER]       = wakeup_thread_transfer,
+                [WAKEUP_RELEASE]        = wakeup_thread_release,
+                [WAKEUP_TRANSFER_MULTI] = wakeup_thread_queue_multi_transfer,
+            };
+
+            /* Call the specified unblocking PIP (does the rest) */
+            funcs[proto](thread);
+        }
+        else
+#endif /* HAVE_PRIORITY_SCHEDULING */
+        {
+            /* No PIP - just boost the thread by aging */
+#ifdef HAVE_PRIORITY_SCHEDULING
+            IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
+                thread->skip_count = thread->priority;
+#endif /* HAVE_PRIORITY_SCHEDULING */
+            remove_from_list_l(list, thread);
+            core_schedule_wakeup(thread);
+            UNLOCK_THREAD(thread);
+        }
 
-    RTR_UNLOCK(core);
+        return should_switch_tasks();
 
-#if NUM_CORES > 1
-    if (core != CURRENT_CORE)
-        core_wake(core);
+    /* Nothing to do. State is not blocked. */
+    default:
+#if THREAD_EXTRA_CHECKS
+        THREAD_PANICF("wakeup_thread->block invalid", thread);
+    case STATE_RUNNING:
+    case STATE_KILLED:
 #endif
+        UNLOCK_THREAD(thread);
+        return THREAD_NONE;
+    }
 }
 
 /*---------------------------------------------------------------------------
@@ -990,8 +1164,6 @@ void check_tmo_threads(void)
                 }
 #endif /* NUM_CORES */
 
-                remove_from_list_l(curr->bqp, curr);
-
 #ifdef HAVE_WAKEUP_EXT_CB
                 if (curr->wakeup_ext_cb != NULL)
                     curr->wakeup_ext_cb(curr);
@@ -999,8 +1171,11 @@ void check_tmo_threads(void)
 
 #ifdef HAVE_PRIORITY_SCHEDULING
                 if (curr->blocker != NULL)
-                    wakeup_priority_protocol_release(curr);
+                    wakeup_thread_release(curr);
+                else
 #endif
+                    remove_from_list_l(curr->bqp, curr);
+
                 corelock_unlock(ocl);
             }
             /* else state == STATE_SLEEPING */
@@ -1161,8 +1336,7 @@ void switch_thread(void)
     /* Begin task switching by saving our current context so that we can
      * restore the state of the current thread later to the point prior
      * to this call. */
-    store_context(&thread->context);
-
+    thread_store_context(thread);
 #ifdef DEBUG
     /* Check core_ctx buflib integrity */
     core_check_valid();
@@ -1212,8 +1386,7 @@ void switch_thread(void)
             /* Select the new task based on priorities and the last time a
              * process got CPU time relative to the highest priority runnable
              * task. */
-            struct priority_distribution *pd = &cores[core].rtr;
-            int max = find_first_set_bit(pd->mask);
+            int max = priobit_ffs(&cores[core].rtr.mask);
 
             if (block == NULL)
             {
@@ -1269,7 +1442,7 @@ void switch_thread(void)
     }
 
     /* And finally give control to the next thread. */
-    load_context(&thread->context);
+    thread_load_context(thread);
 
 #ifdef RB_PROFILE
     profile_thread_started(thread->id & THREAD_ID_SLOT_MASK);
@@ -1291,140 +1464,59 @@ void sleep_thread(int ticks)
     LOCK_THREAD(current);
 
     /* Set our timeout, remove from run list and join timeout list. */
-    current->tmo_tick = current_tick + ticks + 1;
+    current->tmo_tick = current_tick + MAX(ticks, 0) + 1;
     block_thread_on_l(current, STATE_SLEEPING);
 
     UNLOCK_THREAD(current);
 }
 
 /*---------------------------------------------------------------------------
- * Indefinitely block a thread on a blocking queue for explicit wakeup.
+ * Block a thread on a blocking queue for explicit wakeup. If timeout is
+ * negative, the block is infinite.
  *
  * INTERNAL: Intended for use by kernel objects and not for programs.
  *---------------------------------------------------------------------------
  */
-void block_thread(struct thread_entry *current)
+void block_thread(struct thread_entry *current, int timeout)
 {
-    /* Set the state to blocked and take us off of the run queue until we
-     * are explicitly woken */
     LOCK_THREAD(current);
 
-    /* Set the list for explicit wakeup */
-    block_thread_on_l(current, STATE_BLOCKED);
-
+    struct blocker *bl = NULL;
 #ifdef HAVE_PRIORITY_SCHEDULING
-    if (current->blocker != NULL)
+    bl = current->blocker;
+    struct thread_entry *blt = bl ? lock_blocker_thread(bl) : NULL;
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+    if (LIKELY(timeout < 0))
     {
-        /* Object supports PIP */
-        current = blocker_inherit_priority(current);
+        /* Block until explicitly woken */
+        block_thread_on_l(current, STATE_BLOCKED);
     }
-#endif
-
-    UNLOCK_THREAD(current);
-}
-
-/*---------------------------------------------------------------------------
- * Block a thread on a blocking queue for a specified time interval or until
- * explicitly woken - whichever happens first.
- *
- * INTERNAL: Intended for use by kernel objects and not for programs.
- *---------------------------------------------------------------------------
- */
-void block_thread_w_tmo(struct thread_entry *current, int timeout)
-{
-    /* Get the entry for the current running thread. */
-    LOCK_THREAD(current);
-
-    /* Set the state to blocked with the specified timeout */
-    current->tmo_tick = current_tick + timeout;
-
-    /* Set the list for explicit wakeup */
-    block_thread_on_l(current, STATE_BLOCKED_W_TMO);
-
-#ifdef HAVE_PRIORITY_SCHEDULING
-    if (current->blocker != NULL)
+    else
     {
-        /* Object supports PIP */
-        current = blocker_inherit_priority(current);
+        /* Set the state to blocked with the specified timeout */
+        current->tmo_tick = current_tick + timeout;
+        block_thread_on_l(current, STATE_BLOCKED_W_TMO);
     }
-#endif
 
-    UNLOCK_THREAD(current);
-}
-
-/*---------------------------------------------------------------------------
- * Explicitly wakeup a thread on a blocking queue. Only effects threads of
- * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
- *
- * This code should be considered a critical section by the caller meaning
- * that the object's corelock should be held.
- *
- * INTERNAL: Intended for use by kernel objects and not for programs.
- *---------------------------------------------------------------------------
- */
-unsigned int wakeup_thread(struct thread_entry **list)
-{
-    struct thread_entry *thread = *list;
-    unsigned int result = THREAD_NONE;
-
-    /* Check if there is a blocked thread at all. */
-    if (thread == NULL)
-        return result;
-
-    LOCK_THREAD(thread);
-
-    /* Determine thread's current state. */
-    switch (thread->state)
+    if (bl == NULL)
     {
-    case STATE_BLOCKED:
-    case STATE_BLOCKED_W_TMO:
-        remove_from_list_l(list, thread);
-
-        result = THREAD_OK;
+        UNLOCK_THREAD(current);
+        return;
+    }
 
 #ifdef HAVE_PRIORITY_SCHEDULING
-        struct thread_entry *current;
-        struct blocker *bl = thread->blocker;
-
-        if (bl == NULL)
-        {
-            /* No inheritance - just boost the thread by aging */
-            IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
-                thread->skip_count = thread->priority;
-            current = cores[CURRENT_CORE].running;
-        }
-        else
-        {
-            /* Call the specified unblocking PIP */
-            current = bl->wakeup_protocol(thread);
-        }
-
-        if (current != NULL &&
-            find_first_set_bit(cores[IF_COP_CORE(current->core)].rtr.mask)
-                < current->priority)
-        {
-            /* There is a thread ready to run of higher or same priority on
-             * the same core as the current one; recommend a task switch.
-             * Knowing if this is an interrupt call would be helpful here. */
-            result |= THREAD_SWITCH;
-        }
-#endif /* HAVE_PRIORITY_SCHEDULING */
-
-        core_schedule_wakeup(thread);
-        break;
+    int newblpr = current->priority;
+    UNLOCK_THREAD(current);
 
-    /* Nothing to do. State is not blocked. */
-#if THREAD_EXTRA_CHECKS
-    default:
-        THREAD_PANICF("wakeup_thread->block invalid", thread);
-    case STATE_RUNNING:
-    case STATE_KILLED:
-        break;
-#endif
+    if (newblpr >= bl->priority)
+    {
+        unlock_blocker_thread(bl);
+        return; /* Queue priority won't change */
     }
 
-    UNLOCK_THREAD(thread);
-    return result;
+    inherit_priority(bl, bl, blt, newblpr);
+#endif /* HAVE_PRIORITY_SCHEDULING */
 }
 
 /*---------------------------------------------------------------------------
@@ -1435,25 +1527,31 @@ unsigned int wakeup_thread(struct thread_entry **list)
  * INTERNAL: Intended for use by kernel objects and not for programs.
  *---------------------------------------------------------------------------
  */
-unsigned int thread_queue_wake(struct thread_entry **list)
+unsigned int thread_queue_wake(struct thread_entry **list,
+                               volatile int *count)
 {
+    int num = 0;
     unsigned result = THREAD_NONE;
 
     for (;;)
     {
-        unsigned int rc = wakeup_thread(list);
+        unsigned int rc = wakeup_thread(list, WAKEUP_DEFAULT);
 
         if (rc == THREAD_NONE)
             break; /* No more threads */
 
         result |= rc;
+        num++;
     }
 
+    if (count)
+        *count = num;
+
     return result;
 }
 
 /*---------------------------------------------------------------------------
- * Assign the thread slot a new ID. Version is 1-255.
+ * Assign the thread slot a new ID. Version is 0x00000100..0xffffff00.
  *---------------------------------------------------------------------------
  */
 static void new_thread_id(unsigned int slot_num,
@@ -1693,7 +1791,7 @@ void thread_wait(unsigned int thread_id)
         current->bqp = &thread->queue;
 
         disable_irq();
-        block_thread(current);
+        block_thread(current, TIMEOUT_BLOCK);
 
         corelock_unlock(&thread->waiter_cl);
 
@@ -1723,7 +1821,7 @@ static inline void thread_final_exit(struct thread_entry *current)
      * execution except the slot itself. */
 
     /* Signal this thread */
-    thread_queue_wake(&current->queue);
+    thread_queue_wake(&current->queue, NULL);
     corelock_unlock(&current->waiter_cl);
     switch_thread();
     /* This should never and must never be reached - if it is, the
@@ -1912,20 +2010,18 @@ IF_COP( retry_state: )
             }
         }
 #endif
-        remove_from_list_l(thread->bqp, thread);
-
 #ifdef HAVE_WAKEUP_EXT_CB
         if (thread->wakeup_ext_cb != NULL)
             thread->wakeup_ext_cb(thread);
 #endif
 
 #ifdef HAVE_PRIORITY_SCHEDULING
+        /* Remove thread's priority influence from its chain if needed */
         if (thread->blocker != NULL)
-        {
-            /* Remove thread's priority influence from its chain */
             wakeup_priority_protocol_release(thread);
-        }
+        else
 #endif
+            remove_from_list_l(thread->bqp, thread);
 
 #if NUM_CORES > 1
         if (ocl != NULL)
@@ -1970,130 +2066,77 @@ thread_killed: /* Thread was already killed */
  */
 int thread_set_priority(unsigned int thread_id, int priority)
 {
+    if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
+        return -1; /* Invalid priority argument */
+
     int old_base_priority = -1;
     struct thread_entry *thread = thread_id_entry(thread_id);
 
-    /* A little safety measure */
-    if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
-        return -1;
-
     /* Thread could be on any list and therefore on an interrupt accessible
        one - disable interrupts */
-    int oldlevel = disable_irq_save();
-
+    const int oldlevel = disable_irq_save();
     LOCK_THREAD(thread);
 
-    /* Make sure it's not killed */
-    if (thread->id == thread_id && thread->state != STATE_KILLED)
-    {
-        int old_priority = thread->priority;
-
-        old_base_priority = thread->base_priority;
-        thread->base_priority = priority;
-
-        prio_move_entry(&thread->pdist, old_base_priority, priority);
-        priority = find_first_set_bit(thread->pdist.mask);
-
-        if (old_priority == priority)
-        {
-            /* No priority change - do nothing */
-        }
-        else if (thread->state == STATE_RUNNING)
-        {
-            /* This thread is running - change location on the run
-             * queue. No transitive inheritance needed. */
-            set_running_thread_priority(thread, priority);
-        }
-        else
-        {
-            thread->priority = priority;
-
-            if (thread->blocker != NULL)
-            {
-                /* Bubble new priority down the chain */
-                struct blocker *bl = thread->blocker;   /* Blocker struct */
-                struct thread_entry *bl_t = bl->thread; /* Blocking thread */
-                struct thread_entry * const tstart = thread;   /* Initial thread */
-                const int highest = MIN(priority, old_priority); /* Higher of new or old */
-
-                for (;;)
-                {
-                    struct thread_entry *next; /* Next thread to check */
-                    int bl_pr;    /* Highest blocked thread */
-                    int queue_pr; /* New highest blocked thread */
-#if NUM_CORES > 1
-                    /* Owner can change but thread cannot be dislodged - thread
-                     * may not be the first in the queue which allows other
-                     * threads ahead in the list to be given ownership during the
-                     * operation. If thread is next then the waker will have to
-                     * wait for us and the owner of the object will remain fixed.
-                     * If we successfully grab the owner -- which at some point
-                     * is guaranteed -- then the queue remains fixed until we
-                     * pass by. */
-                    for (;;)
-                    {
-                        LOCK_THREAD(bl_t);
-
-                        /* Double-check the owner - retry if it changed */
-                        if (LIKELY(bl->thread == bl_t))
-                            break;
-
-                        UNLOCK_THREAD(bl_t);
-                        bl_t = bl->thread;
-                    }
-#endif
-                    bl_pr = bl->priority;
-
-                    if (highest > bl_pr)
-                        break; /* Object priority won't change */
+    if (thread->id != thread_id || thread->state == STATE_KILLED)
+        goto done; /* Invalid thread */
 
-                    /* This will include the thread being set */
-                    queue_pr = find_highest_priority_in_list_l(*thread->bqp);
+    old_base_priority = thread->base_priority;
+    if (priority == old_base_priority)
+        goto done; /* No base priority change */
 
-                    if (queue_pr == bl_pr)
-                        break; /* Object priority not changing */
+    thread->base_priority = priority;
 
-                    /* Update thread boost for this object */
-                    bl->priority = queue_pr;
-                    prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
-                    bl_pr = find_first_set_bit(bl_t->pdist.mask);
+    /* Adjust the thread's priority influence on itself */
+    prio_move_entry(&thread->pdist, old_base_priority, priority);
 
-                    if (bl_t->priority == bl_pr)
-                        break; /* Blocking thread priority not changing */
+    int old_priority = thread->priority;
+    int new_priority = priobit_ffs(&thread->pdist.mask);
 
-                    if (bl_t->state == STATE_RUNNING)
-                    {
-                        /* Thread not blocked - we're done */
-                        set_running_thread_priority(bl_t, bl_pr);
-                        break;
-                    }
+    if (old_priority == new_priority)
+        goto done; /* No running priority change */
 
-                    bl_t->priority = bl_pr;
-                    bl = bl_t->blocker; /* Blocking thread has a blocker? */
+    if (thread->state == STATE_RUNNING)
+    {
+        /* This thread is running - just change location on the run queue.
+           Also sets thread->priority. */
+        set_running_thread_priority(thread, new_priority);
+        goto done;
+    }
 
-                    if (bl == NULL)
-                        break; /* End of chain */
+    /* Thread is blocked */
+    struct blocker *bl = thread->blocker;
+    if (bl == NULL)
+    {
+        thread->priority = new_priority;
+        goto done; /* End of transitive blocks */
+    }
 
-                    next = bl->thread;
+    struct thread_entry *blt = lock_blocker_thread(bl);
+    struct thread_entry **bqp = thread->bqp;
 
-                    if (UNLIKELY(next == tstart))
-                        break; /* Full-circle */
+    thread->priority = new_priority;
 
-                    UNLOCK_THREAD(thread);
+    UNLOCK_THREAD(thread);
+    thread = NULL;
 
-                    thread = bl_t;
-                    bl_t = next;
-                } /* for (;;) */
+    int oldblpr = bl->priority;
+    int newblpr = oldblpr;
+    if (new_priority < oldblpr)
+        newblpr = new_priority;
+    else if (old_priority <= oldblpr)
+        newblpr = find_highest_priority_in_list_l(*bqp);
 
-                UNLOCK_THREAD(bl_t);
-            }
-        }
+    if (newblpr == oldblpr)
+    {
+        unlock_blocker_thread(bl);
+        goto done;
     }
 
-    UNLOCK_THREAD(thread);
-
+    inherit_priority(bl, bl, blt, newblpr);
+done:
+    if (thread)
+        UNLOCK_THREAD(thread);
     restore_irq(oldlevel);
-
     return old_base_priority;
 }