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, 120 insertions, 309 deletions

diff --git a/firmware/target/hosted/sdl/thread-sdl.c b/firmware/target/hosted/sdl/thread-sdl.c
index fda877e0f5..a76941f103 100644
--- a/firmware/target/hosted/sdl/thread-sdl.c
+++ b/firmware/target/hosted/sdl/thread-sdl.c
@@ -32,13 +32,13 @@
 #include "core_alloc.h"
 
 /* Define this as 1 to show informational messages that are not errors. */
-#define THREAD_SDL_DEBUGF_ENABLED 0
+#define THREAD_SDL_DEBUGF_ENABLED 1
 
 #if THREAD_SDL_DEBUGF_ENABLED
 #define THREAD_SDL_DEBUGF(...) DEBUGF(__VA_ARGS__)
-static char __name[32];
+static char __name[sizeof (((struct thread_debug_info *)0)->name)];
 #define THREAD_SDL_GET_NAME(thread) \
-    ({ thread_get_name(__name, ARRAYLEN(__name), thread); __name; })
+    ({ format_thread_name(__name, sizeof (__name), thread); __name; })
 #else
 #define THREAD_SDL_DEBUGF(...)
 #define THREAD_SDL_GET_NAME(thread)
@@ -47,9 +47,6 @@ static char __name[32];
 #define THREAD_PANICF(str...) \
     ({ fprintf(stderr, str); exit(-1); })
 
-/* Thread/core entries as in rockbox core */
-static struct core_entry cores[NUM_CORES];
-struct thread_entry threads[MAXTHREADS];
 /* Jump buffers for graceful exit - kernel threads don't stay neatly
  * in their start routines responding to messages so this is the only
  * way to get them back in there so they may exit */
@@ -74,7 +71,7 @@ void sim_thread_shutdown(void)
 
     /* Tell all threads jump back to their start routines, unlock and exit
        gracefully - we'll check each one in turn for it's status. Threads
-       _could_ terminate via remove_thread or multiple threads could exit
+       _could_ terminate via thread_exit or multiple threads could exit
        on each unlock but that is safe. */
 
     /* Do this before trying to acquire lock */
@@ -86,7 +83,7 @@ void sim_thread_shutdown(void)
     /* Signal all threads on delay or block */
     for (i = 0; i < MAXTHREADS; i++)
     {
-        struct thread_entry *thread = &threads[i];
+        struct thread_entry *thread = __thread_slot_entry(i);
         if (thread->context.s == NULL)
             continue;
         SDL_SemPost(thread->context.s);
@@ -95,7 +92,7 @@ void sim_thread_shutdown(void)
     /* Wait for all threads to finish and cleanup old ones. */
     for (i = 0; i < MAXTHREADS; i++)
     {
-        struct thread_entry *thread = &threads[i];
+        struct thread_entry *thread = __thread_slot_entry(i);
         SDL_Thread *t = thread->context.t;
 
         if (t != NULL)
@@ -111,11 +108,11 @@ void sim_thread_shutdown(void)
         }
         else
         {
-            /* Wait on any previous thread in this location-- could be one not quite
-             * finished exiting but has just unlocked the mutex. If it's NULL, the
-             * call returns immediately.
+            /* Wait on any previous thread in this location-- could be one not
+             * quite finished exiting but has just unlocked the mutex. If it's
+             * NULL, the call returns immediately.
              *
-             * See remove_thread below for more information. */
+             * See thread_exit below for more information. */
             SDL_WaitThread(thread->context.told, NULL);
         }
     }
@@ -126,103 +123,6 @@ void sim_thread_shutdown(void)
     threads_status = THREADS_EXIT_COMMAND_DONE;
 }
 
-static void new_thread_id(unsigned int slot_num,
-                          struct thread_entry *thread)
-{
-    unsigned int version =
-        (thread->id + (1u << THREAD_ID_VERSION_SHIFT))
-            & THREAD_ID_VERSION_MASK;
-
-    if (version == 0)
-        version = 1u << THREAD_ID_VERSION_SHIFT;
-
-    thread->id = version | (slot_num & THREAD_ID_SLOT_MASK);
-}
-
-static struct thread_entry * find_empty_thread_slot(void)
-{
-    struct thread_entry *thread = NULL;
-    int n;
-
-    for (n = 0; n < MAXTHREADS; n++)
-    {
-        int state = threads[n].state;
-
-        if (state == STATE_KILLED)
-        {
-            thread = &threads[n];
-            break;
-        }
-    }
-
-    return thread;
-}
-
-
-/* Initialize SDL threading */
-void init_threads(void)
-{
-    static uintptr_t main_stack[] = { DEADBEEF, 0 };
-    struct thread_entry *thread;
-    int n;
-
-    memset(cores, 0, sizeof(cores));
-    memset(threads, 0, sizeof(threads));
-
-    m = SDL_CreateMutex();
-
-    if (SDL_LockMutex(m) == -1)
-    {
-        fprintf(stderr, "Couldn't lock mutex\n");
-        return;
-    }
-
-    /* Initialize all IDs */
-    for (n = 0; n < MAXTHREADS; n++)
-        threads[n].id = THREAD_ID_INIT(n);
-
-    /* Slot 0 is reserved for the main thread - initialize it here and
-       then create the SDL thread - it is possible to have a quick, early
-       shutdown try to access the structure. */
-    thread = &threads[0];
-    thread->stack = main_stack;
-    thread->stack_size = sizeof (main_stack);
-    thread->name = "main";
-    thread->state = STATE_RUNNING;
-    thread->context.s = SDL_CreateSemaphore(0);
-    thread->context.t = NULL; /* NULL for the implicit main thread */
-    cores[CURRENT_CORE].running = thread;
- 
-    if (thread->context.s == NULL)
-    {
-        fprintf(stderr, "Failed to create main semaphore\n");
-        return;
-    }
-
-    /* Tell all threads jump back to their start routines, unlock and exit
-       gracefully - we'll check each one in turn for it's status. Threads
-       _could_ terminate via remove_thread or multiple threads could exit
-       on each unlock but that is safe. */
-
-    /* Setup jump for exit */
-    if (setjmp(thread_jmpbufs[0]) == 0)
-    {
-        THREAD_SDL_DEBUGF("Main thread: %p\n", thread);
-        return;
-    }
-
-    SDL_UnlockMutex(m);
-
-    /* Set to 'COMMAND_DONE' when other rockbox threads have exited. */
-    while (threads_status < THREADS_EXIT_COMMAND_DONE)
-        SDL_Delay(10);
-
-    SDL_DestroyMutex(m);
-
-    /* We're the main thead - perform exit - doesn't return. */
-    sim_do_exit();
-}
-
 void sim_thread_exception_wait(void)
 {
     while (1)
@@ -237,7 +137,7 @@ void sim_thread_exception_wait(void)
 void sim_thread_lock(void *me)
 {
     SDL_LockMutex(m);
-    cores[CURRENT_CORE].running = (struct thread_entry *)me;
+    __running_self_entry() = (struct thread_entry *)me;
 
     if (threads_status != THREADS_RUN)
         thread_exit();
@@ -245,70 +145,14 @@ void sim_thread_lock(void *me)
 
 void * sim_thread_unlock(void)
 {
-    struct thread_entry *current = cores[CURRENT_CORE].running;
+    struct thread_entry *current = __running_self_entry();
     SDL_UnlockMutex(m);
     return current;
 }
 
-struct thread_entry * thread_id_entry(unsigned int thread_id)
-{
-    return &threads[thread_id & THREAD_ID_SLOT_MASK];
-}
-
-static void add_to_list_l(struct thread_entry **list,
-                          struct thread_entry *thread)
-{
-    if (*list == NULL)
-    {
-        /* Insert into unoccupied list */
-        thread->l.next = thread;
-        thread->l.prev = thread;
-        *list = thread;
-    }
-    else
-    {
-        /* Insert last */
-        thread->l.next = *list;
-        thread->l.prev = (*list)->l.prev;
-        thread->l.prev->l.next = thread;
-        (*list)->l.prev = thread;
-    }
-}
-
-static void remove_from_list_l(struct thread_entry **list,
-                               struct thread_entry *thread)
-{
-    if (thread == thread->l.next)
-    {
-        /* The only item */
-        *list = NULL;
-        return;
-    }
-
-    if (thread == *list)
-    {
-        /* List becomes next item */
-        *list = thread->l.next;
-    }
-
-    /* Fix links to jump over the removed entry. */
-    thread->l.prev->l.next = thread->l.next;
-    thread->l.next->l.prev = thread->l.prev;
-}
-
-unsigned int thread_self(void)
-{
-    return cores[CURRENT_CORE].running->id;
-}
-
-struct thread_entry* thread_self_entry(void)
-{
-    return cores[CURRENT_CORE].running;
-}
-
 void switch_thread(void)
 {
-    struct thread_entry *current = cores[CURRENT_CORE].running;
+    struct thread_entry *current = __running_self_entry();
 
     enable_irq();
 
@@ -346,17 +190,7 @@ void switch_thread(void)
 
         oldlevel = disable_irq_save();
 
-        if (current->state == STATE_BLOCKED_W_TMO)
-        {
-            /* Timed out */
-            remove_from_list_l(current->bqp, current);
-
-#ifdef HAVE_WAKEUP_EXT_CB
-            if (current->wakeup_ext_cb != NULL)
-                current->wakeup_ext_cb(current);
-#endif
-            current->state = STATE_RUNNING;
-        }
+        current->state = STATE_RUNNING;
 
         if (result == SDL_MUTEX_TIMEDOUT)
         {
@@ -384,7 +218,7 @@ void switch_thread(void)
 #ifdef DEBUG
     core_check_valid();
 #endif
-    cores[CURRENT_CORE].running = current;
+    __running_self_entry() = current;
 
     if (threads_status != THREADS_RUN)
         thread_exit();
@@ -392,7 +226,7 @@ void switch_thread(void)
 
 void sleep_thread(int ticks)
 {
-    struct thread_entry *current = cores[CURRENT_CORE].running;
+    struct thread_entry *current = __running_self_entry();
     int rem;
 
     current->state = STATE_SLEEPING;
@@ -404,7 +238,7 @@ void sleep_thread(int ticks)
     current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem;
 }
 
-void block_thread(struct thread_entry *current, int ticks)
+void block_thread_(struct thread_entry *current, int ticks)
 {
     if (ticks < 0)
         current->state = STATE_BLOCKED;
@@ -414,24 +248,19 @@ void block_thread(struct thread_entry *current, int ticks)
         current->tmo_tick = (1000/HZ)*ticks;
     }
 
-    add_to_list_l(current->bqp, current);
+    wait_queue_register(current);
 }
 
-unsigned int wakeup_thread_(struct thread_entry **list)
+unsigned int wakeup_thread_(struct thread_entry *thread)
 {
-    struct thread_entry *thread = *list;
-
-    if (thread != NULL)
+    switch (thread->state)
     {
-        switch (thread->state)
-        {
-        case STATE_BLOCKED:
-        case STATE_BLOCKED_W_TMO:
-            remove_from_list_l(list, thread);
-            thread->state = STATE_RUNNING;
-            SDL_SemPost(thread->context.s);
-            return THREAD_OK;
-        }
+    case STATE_BLOCKED:
+    case STATE_BLOCKED_W_TMO:
+        wait_queue_remove(thread);
+        thread->state = STATE_RUNNING;
+        SDL_SemPost(thread->context.s);
+        return THREAD_OK;
     }
 
     return THREAD_NONE;
@@ -439,7 +268,7 @@ unsigned int wakeup_thread_(struct thread_entry **list)
 
 void thread_thaw(unsigned int thread_id)
 {
-    struct thread_entry *thread = thread_id_entry(thread_id);
+    struct thread_entry *thread = __thread_id_entry(thread_id);
 
     if (thread->id == thread_id && thread->state == STATE_FROZEN)
     {
@@ -450,15 +279,14 @@ void thread_thaw(unsigned int thread_id)
 
 int runthread(void *data)
 {
-    struct thread_entry *current;
-    jmp_buf *current_jmpbuf;
-
     /* Cannot access thread variables before locking the mutex as the
        data structures may not be filled-in yet. */
     SDL_LockMutex(m);
-    cores[CURRENT_CORE].running = (struct thread_entry *)data;
-    current = cores[CURRENT_CORE].running;
-    current_jmpbuf = &thread_jmpbufs[current - threads];
+
+    struct thread_entry *current = (struct thread_entry *)data;
+    __running_self_entry() = current;
+
+    jmp_buf *current_jmpbuf = &thread_jmpbufs[THREAD_ID_SLOT(current->id)];
 
     /* Setup jump for exit */
     if (setjmp(*current_jmpbuf) == 0)
@@ -469,14 +297,15 @@ int runthread(void *data)
             SDL_UnlockMutex(m);
             SDL_SemWait(current->context.s);
             SDL_LockMutex(m);
-            cores[CURRENT_CORE].running = current;
+            __running_self_entry() = current;
         }
 
         if (threads_status == THREADS_RUN)
         {
             current->context.start();
             THREAD_SDL_DEBUGF("Thread Done: %d (%s)\n",
-                              current - threads, THREAD_SDL_GET_NAME(current));
+                              THREAD_ID_SLOT(current->id),
+                              THREAD_SDL_GET_NAME(current));
             /* Thread routine returned - suicide */
         }
 
@@ -495,27 +324,23 @@ unsigned int create_thread(void (*function)(void),
                            void* stack, size_t stack_size,
                            unsigned flags, const char *name)
 {
-    struct thread_entry *thread;
-    SDL_Thread* t;
-    SDL_sem *s;
-
     THREAD_SDL_DEBUGF("Creating thread: (%s)\n", name ? name : "");
 
-    thread = find_empty_thread_slot();
+    struct thread_entry *thread = thread_alloc();
     if (thread == NULL)
     {
         DEBUGF("Failed to find thread slot\n");
         return 0;
     }
 
-    s = SDL_CreateSemaphore(0);
+    SDL_sem *s = SDL_CreateSemaphore(0);
     if (s == NULL)
     {
         DEBUGF("Failed to create semaphore\n");
         return 0;
     }
 
-    t = SDL_CreateThread(runthread, thread);
+    SDL_Thread *t = SDL_CreateThread(runthread, thread);
     if (t == NULL)
     {
         DEBUGF("Failed to create SDL thread\n");
@@ -523,12 +348,6 @@ unsigned int create_thread(void (*function)(void),
         return 0;
     }
 
-    unsigned int stack_words = stack_size / sizeof (uintptr_t);
-    for (unsigned int i = stack_words; i-- > 0;)
-        ((uintptr_t *)stack)[i] = DEADBEEF;
-
-    thread->stack = stack;
-    thread->stack_size = stack_size;
     thread->name = name;
     thread->state = (flags & CREATE_THREAD_FROZEN) ?
         STATE_FROZEN : STATE_RUNNING;
@@ -536,27 +355,22 @@ unsigned int create_thread(void (*function)(void),
     thread->context.t = t;
     thread->context.s = s;
 
-    THREAD_SDL_DEBUGF("New Thread: %d (%s)\n",
-                      thread - threads, THREAD_SDL_GET_NAME(thread));
+    THREAD_SDL_DEBUGF("New Thread: %lu (%s)\n",
+                      (unsigned long)thread->id,
+                      THREAD_SDL_GET_NAME(thread));
 
     return thread->id;
+    (void)stack; (void)stack_size;
 }
 
-static void remove_thread(unsigned int thread_id)
+void thread_exit(void)
 {
-    struct thread_entry *current = cores[CURRENT_CORE].running;
-    struct thread_entry *thread = thread_id_entry(thread_id);
-
-    SDL_Thread *t;
-    SDL_sem *s;
-
-    if (thread->id != thread_id)
-        return;
+    struct thread_entry *current = __running_self_entry();
 
     int oldlevel = disable_irq_save();
 
-    t = thread->context.t;
-    s = thread->context.s;
+    SDL_Thread *t = current->context.t;
+    SDL_sem *s = current->context.s;
 
     /* Wait the last thread here and keep this one or SDL will leak it since
      * it doesn't free its own library allocations unless a wait is performed.
@@ -566,59 +380,27 @@ static void remove_thread(unsigned int thread_id)
      *
      * However, see more below about SDL_KillThread.
      */
-    SDL_WaitThread(thread->context.told, NULL);
+    SDL_WaitThread(current->context.told, NULL);
 
-    thread->context.t = NULL;
-    thread->context.s = NULL;
-    thread->context.told = t;
+    current->context.t = NULL;
+    current->context.s = NULL;
+    current->context.told = t;
 
-    if (thread != current)
-    {
-        switch (thread->state)
-        {
-        case STATE_BLOCKED:
-        case STATE_BLOCKED_W_TMO:
-            /* Remove thread from object it's waiting on */
-            remove_from_list_l(thread->bqp, thread);
-
-#ifdef HAVE_WAKEUP_EXT_CB
-            if (thread->wakeup_ext_cb != NULL)
-                thread->wakeup_ext_cb(thread);
-#endif
-            break;
-        }
-
-        SDL_SemPost(s);
-    }
-
-    THREAD_SDL_DEBUGF("Removing thread: %d (%s)\n",
-        thread - threads, THREAD_SDL_GET_NAME(thread));
-
-    new_thread_id(thread->id, thread);
-    thread->state = STATE_KILLED;
-    thread_queue_wake(&thread->queue);
+    unsigned int id = current->id;
+    new_thread_id(current);
+    current->state = STATE_KILLED;
+    wait_queue_wake(&current->queue);
 
     SDL_DestroySemaphore(s);
 
-    if (thread == current)
-    {
-        /* Do a graceful exit - perform the longjmp back into the thread
-           function to return */
-        restore_irq(oldlevel);
-        longjmp(thread_jmpbufs[current - threads], 1);
-    }
-
-    /* SDL_KillThread frees the old pointer too because it uses SDL_WaitThread
-     * to wait for the host to remove it. */
-    thread->context.told = NULL;
-    SDL_KillThread(t);
+    /* Do a graceful exit - perform the longjmp back into the thread
+       function to return */
     restore_irq(oldlevel);
-}
 
-void thread_exit(void)
-{
-    unsigned int id = thread_self();
-    remove_thread(id);
+    thread_free(current);
+
+    longjmp(thread_jmpbufs[THREAD_ID_SLOT(id)], 1);
+
     /* This should never and must never be reached - if it is, the
      * state is corrupted */
     THREAD_PANICF("thread_exit->K:*R (ID: %d)", id);
@@ -627,44 +409,73 @@ void thread_exit(void)
 
 void thread_wait(unsigned int thread_id)
 {
-    struct thread_entry *current = cores[CURRENT_CORE].running;
-    struct thread_entry *thread = thread_id_entry(thread_id);
+    struct thread_entry *current = __running_self_entry();
+    struct thread_entry *thread = __thread_id_entry(thread_id);
 
     if (thread->id == thread_id && thread->state != STATE_KILLED)
     {
-        current->bqp = &thread->queue;
-        block_thread(current, TIMEOUT_BLOCK);
+        block_thread(current, TIMEOUT_BLOCK, &thread->queue);
         switch_thread();
     }
 }
 
-/*---------------------------------------------------------------------------
- * Suspends a thread's execution for at least the specified number of ticks.
- *
- * May result in CPU core entering wait-for-interrupt mode if no other thread
- * may be scheduled.
- *
- * NOTE: sleep(0) sleeps until the end of the current tick
- *       sleep(n) that doesn't result in rescheduling:
- *                      n <= ticks suspended < n + 1
- *       n to n+1 is a lower bound. Other factors may affect the actual time
- *       a thread is suspended before it runs again.
- *---------------------------------------------------------------------------
- */
-unsigned sleep(unsigned ticks)
+/* Initialize SDL threading */
+void init_threads(void)
 {
-    disable_irq();
-    sleep_thread(ticks);
-    switch_thread();
-    return 0;
-}
+    m = SDL_CreateMutex();
 
-/*---------------------------------------------------------------------------
- * Elects another thread to run or, if no other thread may be made ready to
- * run, immediately returns control back to the calling thread.
- *---------------------------------------------------------------------------
- */
-void yield(void)
-{
-    switch_thread();
+    if (SDL_LockMutex(m) == -1)
+    {
+        fprintf(stderr, "Couldn't lock mutex\n");
+        return;
+    }
+
+    thread_alloc_init();
+
+    struct thread_entry *thread = thread_alloc();
+    if (thread == NULL)
+    {
+        fprintf(stderr, "Main thread alloc failed\n");
+        return;
+    }
+
+    /* Slot 0 is reserved for the main thread - initialize it here and
+       then create the SDL thread - it is possible to have a quick, early
+       shutdown try to access the structure. */
+    thread->name = __main_thread_name;
+    thread->state = STATE_RUNNING;
+    thread->context.s = SDL_CreateSemaphore(0);
+    thread->context.t = NULL; /* NULL for the implicit main thread */
+    __running_self_entry() = thread;
+ 
+    if (thread->context.s == NULL)
+    {
+        fprintf(stderr, "Failed to create main semaphore\n");
+        return;
+    }
+
+    /* Tell all threads jump back to their start routines, unlock and exit
+       gracefully - we'll check each one in turn for it's status. Threads
+       _could_ terminate via thread_exit or multiple threads could exit
+       on each unlock but that is safe. */
+
+    /* Setup jump for exit */
+    if (setjmp(thread_jmpbufs[THREAD_ID_SLOT(thread->id)]) == 0)
+    {
+        THREAD_SDL_DEBUGF("Main Thread: %lu (%s)\n",
+                          (unsigned long)thread->id,
+                          THREAD_SDL_GET_NAME(thread));
+        return;
+    }
+
+    SDL_UnlockMutex(m);
+
+    /* Set to 'COMMAND_DONE' when other rockbox threads have exited. */
+    while (threads_status < THREADS_EXIT_COMMAND_DONE)
+        SDL_Delay(10);
+
+    SDL_DestroyMutex(m);
+
+    /* We're the main thead - perform exit - doesn't return. */
+    sim_do_exit();
 }