Base scheduler queues off linked lists and do cleanup/consolidation

Abstracts threading from itself a bit, changes the way its queues are
handled and does type hiding for that as well.

Do alot here due to already required major brain surgery.

Threads may now be on a run queue and a wait queue simultaneously so
that the expired timer only has to wake the thread but not remove it
from the wait queue which simplifies the implicit wake handling.

List formats change for wait queues-- doubly-linked, not circular.
Timeout queue is now singly-linked. The run queue is still circular
as before.

Adds a better thread slot allocator that may keep the slot marked as
used regardless of the thread state. Assists in dumping special tasks
that switch_thread was tasked to perform (blocking tasks).

Deletes alot of code yet surprisingly, gets larger than expected.
Well, I'm not not minding that for the time being-- omlettes and break
a few eggs and all that.

Change-Id: I0834d7bb16b2aecb2f63b58886eeda6ae4f29d59

1 files changed, 36 insertions, 59 deletions

diff --git a/firmware/target/arm/pp/thread-pp.c b/firmware/target/arm/pp/thread-pp.c
index 184d243e8d..0af8caa43a 100644
--- a/firmware/target/arm/pp/thread-pp.c
+++ b/firmware/target/arm/pp/thread-pp.c
@@ -82,46 +82,22 @@ static void INIT_ATTR core_thread_init(unsigned int core)
  * to use a stack from an unloaded module until another thread runs on it.
  *---------------------------------------------------------------------------
  */
-static inline void NORETURN_ATTR __attribute__((always_inline))
-    thread_final_exit(struct thread_entry *current)
+static void __attribute__((naked, noinline, noreturn))
+    thread_exit_finalize(unsigned int core, struct thread_entry *current)
 {
     asm volatile (
-        "cmp    %1, #0               \n" /* CPU? */
+        "ldr    r2, =idle_stacks     \n" /* switch to idle stack  */
+        "ldr    sp, [r2, r0, lsl #2] \n"
+        "add    sp, sp, %0*4         \n"
+        "cmp    r0, #0               \n" /* CPU? */
+        "mov    r4, r1               \n"
         "blne   commit_dcache        \n"
-        "mov    r0, %0               \n" /* copy thread parameter */
-        "mov    sp, %2               \n" /* switch to idle stack  */
-        "bl     thread_final_exit_do \n" /* finish removal        */
-        : : "r"(current),
-            "r"(current->core),
-            "r"(&idle_stacks[current->core][IDLE_STACK_WORDS])
-        : "r0", "r1", "r2", "r3", "ip", "lr"); /* Because of flush call,
-                                                  force inputs out
-                                                  of scratch regs */
-    while (1);
-}
+        "mov    r0, r4               \n"
+        "b      thread_exit_final    \n"
+        : : "i"(IDLE_STACK_WORDS));
 
-/*---------------------------------------------------------------------------
- * Perform core switch steps that need to take place inside switch_thread.
- *
- * These steps must take place while before changing the processor and after
- * having entered switch_thread since switch_thread may not do a normal return
- * because the stack being used for anything the compiler saved will not belong
- * to the thread's destination core and it may have been recycled for other
- * purposes by the time a normal context load has taken place. switch_thread
- * will also clobber anything stashed in the thread's context or stored in the
- * nonvolatile registers if it is saved there before the call since the
- * compiler's order of operations cannot be known for certain.
- */
-static void core_switch_blk_op(unsigned int core, struct thread_entry *thread)
-{
-    /* Flush our data to ram */
-    commit_dcache();
-    /* Stash thread in r4 slot */
-    thread->context.r[0] = (uint32_t)thread;
-    /* Stash restart address in r5 slot */
-    thread->context.r[1] = thread->context.start;
-    /* Save sp in context.sp while still running on old core */
-    thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1];
+    while (1);
+    (void)core; (void)current;
 }
 
 /*---------------------------------------------------------------------------
@@ -136,31 +112,32 @@ static void core_switch_blk_op(unsigned int core, struct thread_entry *thread)
 /*---------------------------------------------------------------------------
  * This actually performs the core switch.
  */
-static void __attribute__((naked))
-    switch_thread_core(unsigned int core, struct thread_entry *thread)
+static void __attribute__((naked, noinline))
+    switch_thread_core(unsigned int old_core, struct thread_entry *thread)
 {
-    /* Pure asm for this because compiler behavior isn't sufficiently predictable.
-     * Stack access also isn't permitted until restoring the original stack and
-     * context. */
     asm volatile (
-        "stmfd  sp!, { r4-r11, lr }      \n" /* Stack all non-volatile context on current core */
-        "ldr    r2, =idle_stacks         \n" /* r2 = &idle_stacks[core][IDLE_STACK_WORDS] */
-        "ldr    r2, [r2, r0, lsl #2]     \n"
-        "add    r2, r2, %0*4             \n"
-        "stmfd  r2!, { sp }              \n" /* save original stack pointer on idle stack */
-        "mov    sp, r2                   \n" /* switch stacks */
-        "adr    r2, 1f                   \n" /* r2 = new core restart address */
-        "str    r2, [r1, #40]            \n" /* thread->context.start = r2 */
-        "ldr    pc, =switch_thread       \n" /* r0 = thread after call - see load_context */
-    "1:                                  \n"
-        "ldr    sp, [r0, #32]            \n" /* Reload original sp from context structure */
-        "mov    r1, #0                   \n" /* Clear start address */
-        "str    r1, [r0, #40]            \n"
-        "bl     commit_discard_idcache   \n" /* Invalidate new core's cache */
-        "ldmfd  sp!, { r4-r11, pc }      \n" /* Restore non-volatile context to new core and return */
-        : : "i"(IDLE_STACK_WORDS)
-    );
-    (void)core; (void)thread;
+        "stmfd  sp!, { r4-r5, lr }     \n" /* can't use the first two ctx fields */
+        "add    r2, r1, #8             \n"
+        "stmia  r2, { r6-r11, sp }     \n" /* save remaining context */
+        "adr    r2, .new_core_restart  \n" /* save context ptr + restart address */
+        "str    r2, [r1, #40]          \n" /* make 'start' non-null */
+        "stmia  r1, { r1-r2 }          \n"
+        "ldr    r2, =idle_stacks       \n" /* switch to idle stack on old core */
+        "ldr    sp, [r2, r0, lsl #2]   \n"
+        "add    sp, sp, %0*4           \n"
+        "stmfd  sp!, { r0-r1 }         \n"
+        "bl     commit_dcache          \n" /* write back everything */
+        "ldmfd  sp!, { r0-r1 }         \n"
+        "b      switch_core_final      \n"
+    ".new_core_restart:                \n"
+        "mov    r1, #0                 \n" /* mark as started */
+        "str    r1, [r0, #40]          \n"
+        "add    r0, r0, #8             \n"
+        "ldmia  r0, { r6-r11, sp }     \n" /* restore non-volatiles and stack */
+        "bl     commit_discard_idcache \n" /* invalidate new core's cache */
+        "ldmfd  sp!, { r4-r5, pc }     \n" /* restore remaining context */
+        : : "i"(IDLE_STACK_WORDS));
+    (void)old_core; (void)thread;
 }
 
 /** PP-model-specific dual-core code **/