3 files changed, 311 insertions, 78 deletions

diff --git a/uisimulator/sdl/button.c b/uisimulator/sdl/button.c
index a15faf72c0..e50bfea087 100644
--- a/uisimulator/sdl/button.c
+++ b/uisimulator/sdl/button.c
@@ -743,7 +743,7 @@ int button_queue_count( void )
 
 long button_get(bool block)
 {
-    struct event ev;
+    struct queue_event ev;
 
     if ( block || !queue_empty(&button_queue) ) {
         queue_wait(&button_queue, &ev);
@@ -755,7 +755,7 @@ long button_get(bool block)
 
 long button_get_w_tmo(int ticks)
 {
-    struct event ev;
+    struct queue_event ev;
     queue_wait_w_tmo(&button_queue, &ev, ticks);
     if (ev.id == SYS_TIMEOUT)
         ev.id = BUTTON_NONE;
diff --git a/uisimulator/sdl/kernel.c b/uisimulator/sdl/kernel.c
index e2aa0d31c4..8a54cd91fc 100644
--- a/uisimulator/sdl/kernel.c
+++ b/uisimulator/sdl/kernel.c
@@ -29,7 +29,7 @@ volatile long current_tick = 0;
 static void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
 
 /* This array holds all queues that are initiated. It is used for broadcast. */
-static struct event_queue *all_queues[32];
+static struct event_queue *all_queues[MAX_NUM_QUEUES];
 static int num_queues = 0;
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
@@ -53,7 +53,7 @@ static void queue_release_sender(struct thread_entry **sender,
                                  intptr_t retval)
 {
     (*sender)->retval = retval;
-    wakeup_thread(sender);
+    wakeup_thread_no_listlock(sender);
     if(*sender != NULL)
     {
         fprintf(stderr, "queue->send slot ovf: %p\n", *sender);
@@ -98,14 +98,14 @@ void queue_init(struct event_queue *q, bool register_queue)
 {
     q->read   = 0;
     q->write  = 0;
-    q->thread = NULL;
+    thread_queue_init(&q->queue);
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     q->send   = NULL; /* No message sending by default */
 #endif
 
     if(register_queue)
     {
-        if(num_queues >= 32)
+        if(num_queues >= MAX_NUM_QUEUES)
         {
             fprintf(stderr, "queue_init->out of queues");
             exit(-1);
@@ -142,7 +142,7 @@ void queue_delete(struct event_queue *q)
     }
 
     /* Release threads waiting on queue head */
-    wakeup_thread(&q->thread);
+    thread_queue_wake(&q->queue);
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     /* Release waiting threads and reply to any dequeued message
@@ -155,7 +155,7 @@ void queue_delete(struct event_queue *q)
     q->write = 0;
 }
 
-void queue_wait(struct event_queue *q, struct event *ev)
+void queue_wait(struct event_queue *q, struct queue_event *ev)
 {
     unsigned int rd;
 
@@ -169,7 +169,11 @@ void queue_wait(struct event_queue *q, struct event *ev)
 
     if (q->read == q->write)
     {
-        block_thread(&q->thread);
+        do
+        {
+            block_thread(&q->queue);
+        }
+        while (q->read == q->write);
     }
 
     rd = q->read++ & QUEUE_LENGTH_MASK;
@@ -184,7 +188,7 @@ void queue_wait(struct event_queue *q, struct event *ev)
 #endif
 }
 
-void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
+void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 {
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     if (q->send && q->send->curr_sender)
@@ -196,7 +200,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
 
     if (q->read == q->write && ticks > 0)
     {
-        block_thread_w_tmo(&q->thread, ticks);
+        block_thread_w_tmo(&q->queue, ticks);
     }
 
     if(q->read != q->write)
@@ -238,7 +242,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
     }
 #endif
 
-    wakeup_thread(&q->thread);
+    wakeup_thread(&q->queue);
 }
 
 /* Special thread-synced queue_post for button driver or any other preemptive sim thread */
@@ -268,9 +272,9 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
             queue_release_sender(spp, 0);
         }
 
-        wakeup_thread(&q->thread);
+        wakeup_thread(&q->queue);
 
-        block_thread(spp);
+        block_thread_no_listlock(spp);
         return thread_get_current()->retval;
     }
 
@@ -370,7 +374,7 @@ int queue_syncbroadcast(long id, intptr_t data)
 
 void yield(void)
 {
-    switch_thread(true, NULL);
+    switch_thread(NULL);
 }
 
 void sleep(int ticks)
@@ -431,39 +435,218 @@ int tick_remove_task(void (*f)(void))
    multitasking, but is better than nothing at all */
 void mutex_init(struct mutex *m)
 {
+    m->queue = NULL;
     m->thread = NULL;
+    m->count = 0;
     m->locked = 0;
 }
 
 void mutex_lock(struct mutex *m)
 {
-    if (test_and_set(&m->locked, 1))
+    struct thread_entry *const thread = thread_get_current();
+
+    if(thread == m->thread)
+    {
+        m->count++;
+        return;
+    }
+
+    if (!test_and_set(&m->locked, 1))
     {
-        block_thread(&m->thread);
+        m->thread = thread;
+        return;
     }
+
+    block_thread_no_listlock(&m->queue);
 }
 
 void mutex_unlock(struct mutex *m)
 {
-    if (m->thread != NULL)
+    /* unlocker not being the owner is an unlocking violation */
+    if(m->thread != thread_get_current())
     {
-        wakeup_thread(&m->thread);
+        fprintf(stderr, "spinlock_unlock->wrong thread");
+        exit(-1);
+    }    
+
+    if (m->count > 0)
+    {
+        /* this thread still owns lock */
+        m->count--;
+        return;
     }
-    else
+
+    m->thread = wakeup_thread_no_listlock(&m->queue);
+
+    if (m->thread == NULL)
     {
+        /* release lock */
         m->locked = 0;
     }
 }
 
-void spinlock_lock(struct mutex *l)
+void spinlock_init(struct spinlock *l)
+{
+    l->locked = 0;
+    l->thread = NULL;
+    l->count = 0;
+}
+
+void spinlock_lock(struct spinlock *l)
 {
+    struct thread_entry *const thread = thread_get_current();
+
+    if (l->thread == thread)
+    {
+        l->count++;
+        return;
+    }
+
     while(test_and_set(&l->locked, 1))
     {
-        switch_thread(true, NULL);
+        switch_thread(NULL);
     }
+
+    l->thread = thread;
 }
 
-void spinlock_unlock(struct mutex *l)
+void spinlock_unlock(struct spinlock *l)
 {
+    /* unlocker not being the owner is an unlocking violation */
+    if(l->thread != thread_get_current())
+    {
+        fprintf(stderr, "spinlock_unlock->wrong thread");
+        exit(-1);
+    }    
+
+    if (l->count > 0)
+    {
+        /* this thread still owns lock */
+        l->count--;
+        return;
+    }
+
+    /* clear owner */
+    l->thread = NULL;
     l->locked = 0;
 }
+
+void semaphore_init(struct semaphore *s, int max, int start)
+{
+    if(max <= 0 || start < 0 || start > max)
+    {
+        fprintf(stderr, "semaphore_init->inv arg");
+        exit(-1);
+    }
+    s->queue = NULL;
+    s->max = max;
+    s->count = start;
+}
+
+void semaphore_wait(struct semaphore *s)
+{
+    if(--s->count >= 0)
+        return;
+    block_thread_no_listlock(&s->queue);
+}
+
+void semaphore_release(struct semaphore *s)
+{
+    if(s->count < s->max)
+    {
+        if(++s->count <= 0)
+        {
+            if(s->queue == NULL)
+            {
+                /* there should be threads in this queue */
+                fprintf(stderr, "semaphore->wakeup");
+                exit(-1);
+            }
+            /* a thread was queued - wake it up */
+            wakeup_thread_no_listlock(&s->queue);
+        }
+    }
+}
+
+void event_init(struct event *e, unsigned int flags)
+{
+    e->queues[STATE_NONSIGNALED] = NULL;
+    e->queues[STATE_SIGNALED] = NULL;
+    e->state = flags & STATE_SIGNALED;
+    e->automatic = (flags & EVENT_AUTOMATIC) ? 1 : 0;
+}
+
+void event_wait(struct event *e, unsigned int for_state)
+{
+    unsigned int last_state = e->state;
+
+    if(e->automatic != 0)
+    {
+        /* wait for false always satisfied by definition
+           or if it just changed to false */
+        if(last_state == STATE_SIGNALED || for_state == STATE_NONSIGNALED)
+        {
+            /* automatic - unsignal */
+            e->state = STATE_NONSIGNALED;
+            return;
+        }
+        /* block until state matches */
+    }
+    else if(for_state == last_state)
+    {
+        /* the state being waited for is the current state */
+        return;
+    }
+
+    /* current state does not match wait-for state */
+    block_thread_no_listlock(&e->queues[for_state]);
+}
+
+void event_set_state(struct event *e, unsigned int state)
+{
+    unsigned int last_state = e->state;
+
+    if(last_state == state)
+    {
+        /* no change */
+        return;
+    }
+
+    if(state == STATE_SIGNALED)
+    {
+        if(e->automatic != 0)
+        {
+            struct thread_entry *thread;
+
+            if(e->queues[STATE_NONSIGNALED] != NULL)
+            {
+                /* no thread should have ever blocked for nonsignaled */
+                fprintf(stderr, "set_event_state->queue[NS]:S");
+                exit(-1);
+            }
+
+            /* pass to next thread and keep unsignaled - "pulse" */
+            thread = wakeup_thread_no_listlock(&e->queues[STATE_SIGNALED]);
+            e->state = thread != NULL ? STATE_NONSIGNALED : STATE_SIGNALED;
+        }
+        else
+        {
+            /* release all threads waiting for signaled */
+            thread_queue_wake_no_listlock(&e->queues[STATE_SIGNALED]);
+            e->state = STATE_SIGNALED;
+        }
+    }
+    else
+    {
+        /* release all threads waiting for unsignaled */
+        if(e->queues[STATE_NONSIGNALED] != NULL && e->automatic != 0)
+        {
+            /* no thread should have ever blocked */
+            fprintf(stderr, "set_event_state->queue[NS]:NS");
+            exit(-1);
+        }
+
+        thread_queue_wake_no_listlock(&e->queues[STATE_NONSIGNALED]);
+        e->state = STATE_NONSIGNALED;
+    }
+}
diff --git a/uisimulator/sdl/thread-sdl.c b/uisimulator/sdl/thread-sdl.c
index 0bd7d2534e..6a3c4af9eb 100644
--- a/uisimulator/sdl/thread-sdl.c
+++ b/uisimulator/sdl/thread-sdl.c
@@ -131,7 +131,7 @@ bool thread_sdl_init(void *param)
     running->stack = "       ";
     running->stack_size = 8;
     running->name = "main";
-    running->statearg = STATE_RUNNING;
+    running->state = STATE_RUNNING;
     running->context.c = SDL_CreateCond();
 
     if (running->context.c == NULL)
@@ -154,43 +154,55 @@ bool thread_sdl_init(void *param)
     return true;
 }
 
+void thread_sdl_lock(void)
+{
+    SDL_LockMutex(m);
+}
+
+void thread_sdl_unlock(void)
+{
+    SDL_UnlockMutex(m);
+}
+
 static int find_empty_thread_slot(void)
 {
     int n;
 
     for (n = 0; n < MAXTHREADS; n++)
     {
-        if (threads[n].name == NULL)
+        int state = threads[n].state;
+
+        if (state == STATE_KILLED)
             break;
     }
 
     return n;
 }
 
-static void add_to_list(struct thread_entry **list,
-                        struct thread_entry *thread)
+static void add_to_list_l(struct thread_entry **list,
+                          struct thread_entry *thread)
 {
     if (*list == NULL)
     {
         /* Insert into unoccupied list */
-        thread->next = thread;
-        thread->prev = thread;
+        thread->l.next = thread;
+        thread->l.prev = thread;
         *list = thread;
     }
     else
     {
         /* Insert last */
-        thread->next = *list;
-        thread->prev = (*list)->prev;
-        thread->prev->next = thread;
-        (*list)->prev = thread;
+        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(struct thread_entry **list,
-                             struct thread_entry *thread)
+static void remove_from_list_l(struct thread_entry **list,
+                               struct thread_entry *thread)
 {
-    if (thread == thread->next)
+    if (thread == thread->l.next)
     {
         /* The only item */
         *list = NULL;
@@ -200,12 +212,12 @@ static void remove_from_list(struct thread_entry **list,
     if (thread == *list)
     {
         /* List becomes next item */
-        *list = thread->next;
+        *list = thread->l.next;
     }
 
     /* Fix links to jump over the removed entry. */
-    thread->prev->next = thread->next;
-    thread->next->prev = thread->prev;
+    thread->l.prev->l.next = thread->l.next;
+    thread->l.next->l.prev = thread->l.prev;
 }
 
 struct thread_entry *thread_get_current(void)
@@ -213,17 +225,7 @@ struct thread_entry *thread_get_current(void)
     return running;
 }
 
-void thread_sdl_lock(void)
-{
-    SDL_LockMutex(m);
-}
-
-void thread_sdl_unlock(void)
-{
-    SDL_UnlockMutex(m);
-}
-
-void switch_thread(bool save_context, struct thread_entry **blocked_list)
+void switch_thread(struct thread_entry *old)
 {
     struct thread_entry *current = running;
 
@@ -235,7 +237,7 @@ void switch_thread(bool save_context, struct thread_entry **blocked_list)
     if (threads_exit)
         remove_thread(NULL);
 
-    (void)save_context; (void)blocked_list;
+    (void)old;
 }
 
 void sleep_thread(int ticks)
@@ -244,7 +246,7 @@ void sleep_thread(int ticks)
     int rem;
 
     current = running;
-    current->statearg = STATE_SLEEPING;
+    current->state = STATE_SLEEPING;
 
     rem = (SDL_GetTicks() - start_tick) % (1000/HZ);
     if (rem < 0)
@@ -267,7 +269,7 @@ void sleep_thread(int ticks)
 
     running = current;
 
-    current->statearg = STATE_RUNNING;
+    current->state = STATE_RUNNING;
 
     if (threads_exit)
         remove_thread(NULL);
@@ -289,10 +291,21 @@ int runthread(void *data)
     if (setjmp(*current_jmpbuf) == 0)
     {
         /* Run the thread routine */
-        current->context.start();
-        THREAD_SDL_DEBUGF("Thread Done: %d (%s)\n",
-                          current - threads, THREAD_SDL_GET_NAME(current));
-        /* Thread routine returned - suicide */
+        if (current->state == STATE_FROZEN)
+        {
+            SDL_CondWait(current->context.c, m);
+            running = current;
+
+        }
+
+        if (!threads_exit)
+        {
+            current->context.start();
+            THREAD_SDL_DEBUGF("Thread Done: %d (%s)\n",
+                              current - threads, THREAD_SDL_GET_NAME(current));
+            /* Thread routine returned - suicide */
+        }
+
         remove_thread(NULL);
     }
     else
@@ -306,7 +319,7 @@ int runthread(void *data)
 
 struct thread_entry* 
     create_thread(void (*function)(void), void* stack, int stack_size,
-                  const char *name)
+                  unsigned flags, const char *name)
 {
     /** Avoid compiler warnings */
     SDL_Thread* t;
@@ -340,7 +353,8 @@ struct thread_entry*
     threads[slot].stack = stack;
     threads[slot].stack_size = stack_size;
     threads[slot].name = name;
-    threads[slot].statearg = STATE_RUNNING;
+    threads[slot].state = (flags & CREATE_THREAD_FROZEN) ?
+        STATE_FROZEN : STATE_RUNNING;
     threads[slot].context.start = function;
     threads[slot].context.t = t;
     threads[slot].context.c = cond;
@@ -351,12 +365,13 @@ struct thread_entry*
     return &threads[slot];
 }
 
-void block_thread(struct thread_entry **list)
+void _block_thread(struct thread_queue *tq)
 {
     struct thread_entry *thread = running;
 
-    thread->statearg = STATE_BLOCKED;
-    add_to_list(list, thread);
+    thread->state = STATE_BLOCKED;
+    thread->bqp = tq;
+    add_to_list_l(&tq->queue, thread);
 
     SDL_CondWait(thread->context.c, m);
     running = thread;
@@ -365,44 +380,56 @@ void block_thread(struct thread_entry **list)
         remove_thread(NULL);
 }
 
-void block_thread_w_tmo(struct thread_entry **list, int ticks)
+void block_thread_w_tmo(struct thread_queue *tq, int ticks)
 {
     struct thread_entry *thread = running;
 
-    thread->statearg = STATE_BLOCKED_W_TMO;
-    add_to_list(list, thread);
+    thread->state = STATE_BLOCKED_W_TMO;
+    thread->bqp = tq;
+    add_to_list_l(&tq->queue, thread);
 
     SDL_CondWaitTimeout(thread->context.c, m, (1000/HZ) * ticks);
     running = thread;
 
-    if (thread->statearg == STATE_BLOCKED_W_TMO)
+    if (thread->state == STATE_BLOCKED_W_TMO)
     {
         /* Timed out */
-        remove_from_list(list, thread);
-        thread->statearg = STATE_RUNNING;
+        remove_from_list_l(&tq->queue, thread);
+        thread->state = STATE_RUNNING;
     }
 
     if (threads_exit)
         remove_thread(NULL);
 }
 
-void wakeup_thread(struct thread_entry **list)
+struct thread_entry * _wakeup_thread(struct thread_queue *tq)
 {
-    struct thread_entry *thread = *list;
+    struct thread_entry *thread = tq->queue;
 
     if (thread == NULL)
     {
-        return;
+        return NULL;
     }
 
-    switch (thread->statearg)
+    switch (thread->state)
     {
     case STATE_BLOCKED:
     case STATE_BLOCKED_W_TMO:
-        remove_from_list(list, thread);
-        thread->statearg = STATE_RUNNING;
+        remove_from_list_l(&tq->queue, thread);
+        thread->state = STATE_RUNNING;
+        SDL_CondSignal(thread->context.c);
+        return thread;
+    default:
+        return NULL;
+    }
+}
+
+void thread_thaw(struct thread_entry *thread)
+{
+    if (thread->state == STATE_FROZEN)
+    {
+        thread->state = STATE_RUNNING;
         SDL_CondSignal(thread->context.c);
-        break;
     }
 }
 
@@ -434,12 +461,24 @@ void remove_thread(struct thread_entry *thread)
     thread->context.t = NULL;
 
     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->queue, thread);
+            break;
+        }
+
         SDL_CondSignal(c);
+    }
 
     THREAD_SDL_DEBUGF("Removing thread: %d (%s)\n",
         thread - threads, THREAD_SDL_GET_NAME(thread));
 
-    thread->name = NULL;
+    thread_queue_wake_no_listlock(&thread->queue);
+    thread->state = STATE_KILLED;
 
     SDL_DestroyCond(c);
 
@@ -453,15 +492,26 @@ void remove_thread(struct thread_entry *thread)
     SDL_KillThread(t);
 }
 
+void thread_wait(struct thread_entry *thread)
+{
+    if (thread == NULL)
+        thread = running;
+
+    if (thread->state != STATE_KILLED)
+    {
+        block_thread_no_listlock(&thread->queue);
+    }
+}
+
 int thread_stack_usage(const struct thread_entry *thread)
 {
     return 50;
     (void)thread;
 }
 
-int thread_get_status(const struct thread_entry *thread)
+unsigned thread_get_status(const struct thread_entry *thread)
 {
-    return thread->statearg;
+    return thread->state;
 }
 
 /* Return name if one or ID if none */