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, 32 insertions, 27 deletions

diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c
index 1505038fbc..5e9e46798f 100644
--- a/firmware/kernel/semaphore.c
+++ b/firmware/kernel/semaphore.c
@@ -24,6 +24,7 @@
 /****************************************************************************
  * Simple semaphore functions ;)
  ****************************************************************************/
+
 /* Initialize the semaphore object.
  * max = maximum up count the semaphore may assume (max >= 1)
  * start = initial count of semaphore (0 <= count <= max) */
@@ -31,7 +32,7 @@ void semaphore_init(struct semaphore *s, int max, int start)
 {
     KERNEL_ASSERT(max > 0 && start >= 0 && start <= max,
                   "semaphore_init->inv arg\n");
-    s->queue = NULL;
+    wait_queue_init(&s->queue);
     s->max = max;
     s->count = start;
     corelock_init(&s->cl);
@@ -42,44 +43,49 @@ void semaphore_init(struct semaphore *s, int max, int start)
  * safely be used in an ISR. */
 int semaphore_wait(struct semaphore *s, int timeout)
 {
-    int ret;
-    int oldlevel;
-    int count;
+    int ret = OBJ_WAIT_TIMEDOUT;
 
-    oldlevel = disable_irq_save();
+    int oldlevel = disable_irq_save();
     corelock_lock(&s->cl);
 
-    count = s->count;
-
+    int count = s->count;
     if(LIKELY(count > 0))
     {
         /* count is not zero; down it */
         s->count = count - 1;
         ret = OBJ_WAIT_SUCCEEDED;
     }
-    else if(timeout == 0)
-    {
-        /* just polling it */
-        ret = OBJ_WAIT_TIMEDOUT;
-    }
-    else
+    else if(timeout != 0)
     {
         /* too many waits - block until count is upped... */
-        struct thread_entry * current = thread_self_entry();
-        IF_COP( current->obj_cl = &s->cl; )
-        current->bqp = &s->queue;
-        /* return value will be OBJ_WAIT_SUCCEEDED after wait if wake was
-         * explicit in semaphore_release */
-        current->retval = OBJ_WAIT_TIMEDOUT;
-
-        block_thread(current, timeout);
+        struct thread_entry *current = __running_self_entry();
+
+        block_thread(current, timeout, &s->queue, NULL);
         corelock_unlock(&s->cl);
 
         /* ...and turn control over to next thread */
         switch_thread();
 
-        return current->retval;
+        /* if explicit wake indicated; do no more */
+        if(LIKELY(!wait_queue_ptr(current)))
+            return OBJ_WAIT_SUCCEEDED;
+
+        disable_irq();
+        corelock_lock(&s->cl);
+
+        /* see if anyone got us after the expired wait */
+        if(wait_queue_try_remove(current))
+        {
+            count = s->count;
+            if(count > 0)
+            {
+                /* down it lately */
+                s->count = count - 1;
+                ret = OBJ_WAIT_SUCCEEDED;
+            }
+        }
     }
+    /* else just polling it */
 
     corelock_unlock(&s->cl);
     restore_irq(oldlevel);
@@ -93,18 +99,17 @@ int semaphore_wait(struct semaphore *s, int timeout)
 void semaphore_release(struct semaphore *s)
 {
     unsigned int result = THREAD_NONE;
-    int oldlevel;
 
-    oldlevel = disable_irq_save();
+    int oldlevel = disable_irq_save();
     corelock_lock(&s->cl);
 
-    if(LIKELY(s->queue != NULL))
+    struct thread_entry *thread = WQ_THREAD_FIRST(&s->queue);
+    if(LIKELY(thread != NULL))
     {
         /* a thread was queued - wake it up and keep count at 0 */
         KERNEL_ASSERT(s->count == 0,
             "semaphore_release->threads queued but count=%d!\n", s->count);
-        s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */
-        result = wakeup_thread(&s->queue, WAKEUP_DEFAULT);
+        result = wakeup_thread(thread, WAKEUP_DEFAULT);
     }
     else
     {