4 files changed, 175 insertions, 48 deletions

diff --git a/uisimulator/sdl/button.c b/uisimulator/sdl/button.c
index 62c2c33150..04475122b5 100644
--- a/uisimulator/sdl/button.c
+++ b/uisimulator/sdl/button.c
@@ -30,11 +30,6 @@
 
 static intptr_t button_data; /* data value from last message dequeued */
 
-/* Special thread-synced queue_post for button driver or any other preemptive sim thread */
-extern void queue_syncpost(struct event_queue *q, long id, intptr_t data);
-/* Special thread-synced queue_broadcast for button driver or any other preemptive sim thread */
-extern int queue_syncbroadcast(long id, intptr_t data);
-
 /* how long until repeat kicks in */
 #define REPEAT_START      6
 
@@ -115,9 +110,9 @@ void button_event(int key, bool pressed)
         {
             usb_connected = !usb_connected;
             if (usb_connected)
-                queue_syncpost(&button_queue, SYS_USB_CONNECTED, 0);
+                queue_post(&button_queue, SYS_USB_CONNECTED, 0);
             else
-                queue_syncpost(&button_queue, SYS_USB_DISCONNECTED, 0);
+                queue_post(&button_queue, SYS_USB_DISCONNECTED, 0);
         }
         return;
 
@@ -634,7 +629,7 @@ void button_event(int key, bool pressed)
     case SDLK_F5:
         if(pressed)
         {
-            queue_syncbroadcast(SYS_SCREENDUMP, 0);
+            queue_broadcast(SYS_SCREENDUMP, 0);
             return;
         }
         break;
@@ -655,17 +650,17 @@ void button_event(int key, bool pressed)
 #ifdef HAVE_REMOTE_LCD
         if(diff & BUTTON_REMOTE)
             if(!skip_remote_release)
-                queue_syncpost(&button_queue, BUTTON_REL | diff, 0);
+                queue_post(&button_queue, BUTTON_REL | diff, 0);
             else
                 skip_remote_release = false;
         else
 #endif
             if(!skip_release)
-                queue_syncpost(&button_queue, BUTTON_REL | diff, 0);
+                queue_post(&button_queue, BUTTON_REL | diff, 0);
             else
                 skip_release = false;
 #else
-        queue_syncpost(&button_queue, BUTTON_REL | diff, 0);
+        queue_post(&button_queue, BUTTON_REL | diff, 0);
 #endif
     }
 
@@ -717,7 +712,7 @@ void button_event(int key, bool pressed)
                 {
                     if (queue_empty(&button_queue))
                     {
-                        queue_syncpost(&button_queue, BUTTON_REPEAT | btn, 0);
+                        queue_post(&button_queue, BUTTON_REPEAT | btn, 0);
 #ifdef HAVE_BACKLIGHT
 #ifdef HAVE_REMOTE_LCD
                             if(btn & BUTTON_REMOTE)
@@ -739,18 +734,18 @@ void button_event(int key, bool pressed)
 #ifdef HAVE_REMOTE_LCD
                         if (btn & BUTTON_REMOTE) {
                             if (!remote_filter_first_keypress || is_remote_backlight_on())
-                                queue_syncpost(&button_queue, btn, 0);
+                                queue_post(&button_queue, btn, 0);
                             else
                                 skip_remote_release = true;
                         }
                         else
 #endif                                    
                             if (!filter_first_keypress || is_backlight_on())
-                                queue_syncpost(&button_queue, btn, 0);
+                                queue_post(&button_queue, btn, 0);
                             else
                                 skip_release = true;
 #else /* no backlight, nothing to skip */
-                        queue_syncpost(&button_queue, btn, 0);
+                        queue_post(&button_queue, btn, 0);
 #endif
                     post = false;
                 }    
diff --git a/uisimulator/sdl/kernel.c b/uisimulator/sdl/kernel.c
index 6a8c9e4538..4e0a508f74 100644
--- a/uisimulator/sdl/kernel.c
+++ b/uisimulator/sdl/kernel.c
@@ -18,13 +18,93 @@
  ****************************************************************************/
 
 #include <stdlib.h>
+#include <SDL.h>
+#include <SDL_thread.h>
 #include "memory.h"
+#include "system-sdl.h"
 #include "uisdl.h"
 #include "kernel.h"
 #include "thread-sdl.h"
 #include "thread.h"
 #include "debug.h"
 
+/* Prevent "irq handler" from thread concurrent access as well as current
+ * access on multiple handlers */ 
+static SDL_cond *sim_thread_cond;
+/* Protect sim irq object when it is being changed */
+static SDL_mutex *sim_irq_mtx;
+static int interrupt_level = HIGHEST_IRQ_LEVEL;
+static int status_reg = 0;
+
+extern struct core_entry cores[NUM_CORES];
+
+/* Nescessary logic:
+ * 1) All threads must pass unblocked
+ * 2) Current handler must always pass unblocked
+ * 3) Threads must be excluded when irq routine is running
+ * 4) No more than one handler routine should execute at a time
+ */
+int set_irq_level(int level)
+{
+    SDL_LockMutex(sim_irq_mtx);
+
+    int oldlevel = interrupt_level;
+
+    if (status_reg == 0 && level == 0 && oldlevel != 0)
+    {
+        /* Not in a handler and "interrupts" are being reenabled */
+        SDL_CondSignal(sim_thread_cond);
+    }
+
+    interrupt_level = level; /* save new level */
+
+    SDL_UnlockMutex(sim_irq_mtx);
+    return oldlevel;
+}
+
+void sim_enter_irq_handler(void)
+{
+    SDL_LockMutex(sim_irq_mtx);
+    if(interrupt_level != 0)
+    {
+        /* "Interrupts" are disabled. Wait for reenable */
+        SDL_CondWait(sim_thread_cond, sim_irq_mtx);
+    }
+    status_reg = 1;
+}
+
+void sim_exit_irq_handler(void)
+{
+    status_reg = 0;
+    SDL_UnlockMutex(sim_irq_mtx);
+}
+
+bool sim_kernel_init(void)
+{
+    sim_irq_mtx = SDL_CreateMutex();
+    if (sim_irq_mtx == NULL)
+    {
+        fprintf(stderr, "Cannot create sim_handler_mtx\n");
+        return false;
+    }
+
+    /* Create with a count of 0 to have interrupts disabled by default */
+    sim_thread_cond = SDL_CreateCond();
+    if (sim_thread_cond == NULL)
+    {
+        fprintf(stderr, "Cannot create sim_thread_cond\n");
+        return false;
+    }
+
+    return true;
+}
+
+void sim_kernel_shutdown(void)
+{
+    SDL_DestroyMutex(sim_irq_mtx);
+    SDL_DestroyCond(sim_thread_cond);
+}
+
 volatile long current_tick = 0;
 static void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
 
@@ -85,17 +165,21 @@ static void queue_release_all_senders(struct event_queue *q)
 void queue_enable_queue_send(struct event_queue *q,
                              struct queue_sender_list *send)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
     q->send = NULL;
     if(send)
     {
         q->send = send;
         memset(send, 0, sizeof(*send));
     }
+    set_irq_level(oldlevel);
 }
 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 
 void queue_init(struct event_queue *q, bool register_queue)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     q->read   = 0;
     q->write  = 0;
     thread_queue_init(&q->queue);
@@ -113,6 +197,8 @@ void queue_init(struct event_queue *q, bool register_queue)
         /* Add it to the all_queues array */
         all_queues[num_queues++] = q;
     }
+
+    set_irq_level(oldlevel);
 }
 
 void queue_delete(struct event_queue *q)
@@ -120,6 +206,8 @@ void queue_delete(struct event_queue *q)
     int i;
     bool found = false;
 
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     /* Find the queue to be deleted */
     for(i = 0;i < num_queues;i++)
     {
@@ -153,11 +241,14 @@ void queue_delete(struct event_queue *q)
 
     q->read = 0;
     q->write = 0;
+
+    set_irq_level(oldlevel);
 }
 
 void queue_wait(struct event_queue *q, struct queue_event *ev)
 {
     unsigned int rd;
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     if (q->send && q->send->curr_sender)
@@ -171,7 +262,9 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
     {
         do
         {
+            cores[CURRENT_CORE].irq_level = oldlevel;
             block_thread(&q->queue);
+            oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
         }
         while (q->read == q->write);
     }
@@ -186,10 +279,14 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
         queue_fetch_sender(q->send, rd);
     }
 #endif
+
+    set_irq_level(oldlevel);
 }
 
 void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     if (q->send && q->send->curr_sender)
     {
@@ -200,7 +297,9 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 
     if (q->read == q->write && ticks > 0)
     {
+        cores[CURRENT_CORE].irq_level = oldlevel;
         block_thread_w_tmo(&q->queue, ticks);
+        oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
     }
 
     if(q->read != q->write)
@@ -220,10 +319,14 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
     {
         ev->id = SYS_TIMEOUT;
     }
+
+    set_irq_level(oldlevel);
 }
 
 void queue_post(struct event_queue *q, long id, intptr_t data)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     unsigned int wr = q->write++ & QUEUE_LENGTH_MASK;
 
     q->events[wr].id   = id;
@@ -243,20 +346,15 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
 #endif
 
     wakeup_thread(&q->queue);
-}
 
-/* Special thread-synced queue_post for button driver or any other preemptive sim thread */
-void queue_syncpost(struct event_queue *q, long id, intptr_t data)
-{
-    thread_sdl_lock();
-    /* No rockbox threads can be running here */
-    queue_post(q, id, data);
-    thread_sdl_unlock();
+    set_irq_level(oldlevel);
 }
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 {
+    int oldlevel = set_irq_level(oldlevel);
+
     unsigned int wr = q->write++ & QUEUE_LENGTH_MASK;
 
     q->events[wr].id   = id;
@@ -274,11 +372,14 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 
         wakeup_thread(&q->queue);
 
+        cores[CURRENT_CORE].irq_level = oldlevel;
         block_thread_no_listlock(spp);
         return thread_get_current()->retval;
     }
 
     /* Function as queue_post if sending is not enabled */
+    wakeup_thread(&q->queue);
+    set_irq_level(oldlevel);
     return 0;
 }
 
@@ -307,6 +408,8 @@ bool queue_empty(const struct event_queue* q)
 
 void queue_clear(struct event_queue* q)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     /* fixme: This is potentially unsafe in case we do interrupt-like processing */
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     /* Release all thread waiting in the queue for a reply -
@@ -315,10 +418,14 @@ void queue_clear(struct event_queue* q)
 #endif
     q->read = 0;
     q->write = 0;
+
+    set_irq_level(oldlevel);
 }
 
 void queue_remove_from_head(struct event_queue *q, long id)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     while(q->read != q->write)
     {
         unsigned int rd = q->read & QUEUE_LENGTH_MASK;
@@ -342,6 +449,8 @@ void queue_remove_from_head(struct event_queue *q, long id)
 #endif
         q->read++;
     }
+
+    set_irq_level(oldlevel);
 }
 
 int queue_count(const struct event_queue *q)
@@ -351,25 +460,16 @@ int queue_count(const struct event_queue *q)
 
 int queue_broadcast(long id, intptr_t data)
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
     int i;
     
     for(i = 0;i < num_queues;i++)
     {
         queue_post(all_queues[i], id, data);
     }
-   
-    return num_queues;
-}
 
-/* Special thread-synced queue_broadcast for button driver or any other preemptive sim thread */
-int queue_syncbroadcast(long id, intptr_t data)
-{
-    int i;
-    thread_sdl_lock();
-    /* No rockbox threads can be running here */
-    i = queue_broadcast(id, data);
-    thread_sdl_unlock();
-    return i;
+    set_irq_level(oldlevel);   
+    return num_queues;
 }
 
 void yield(void)
@@ -398,6 +498,7 @@ void sim_tick_tasks(void)
 
 int tick_add_task(void (*f)(void))
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
     int i;
 
     /* Add a task if there is room */
@@ -406,6 +507,7 @@ int tick_add_task(void (*f)(void))
         if(tick_funcs[i] == NULL)
         {
             tick_funcs[i] = f;
+            set_irq_level(oldlevel);
             return 0;
         }
     }
@@ -416,6 +518,7 @@ int tick_add_task(void (*f)(void))
 
 int tick_remove_task(void (*f)(void))
 {
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
     int i;
 
     /* Remove a task if it is there */
@@ -424,10 +527,12 @@ int tick_remove_task(void (*f)(void))
         if(tick_funcs[i] == f)
         {
             tick_funcs[i] = NULL;
+            set_irq_level(oldlevel);
             return 0;
         }
     }
-    
+
+    set_irq_level(oldlevel);
     return -1;
 }
 
diff --git a/uisimulator/sdl/thread-sdl.c b/uisimulator/sdl/thread-sdl.c
index 6a3c4af9eb..a07ac29738 100644
--- a/uisimulator/sdl/thread-sdl.c
+++ b/uisimulator/sdl/thread-sdl.c
@@ -24,6 +24,7 @@
 #include <stdlib.h>
 #include <memory.h>
 #include <setjmp.h>
+#include "system-sdl.h"
 #include "thread-sdl.h"
 #include "kernel.h"
 #include "thread.h"
@@ -45,7 +46,8 @@ static char __name[32];
 #define THREAD_PANICF(str...) \
     ({ fprintf(stderr, str); exit(-1); })
 
-/* Thread entries as in core */
+/* Thread/core entries as in rockbox core */
+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
@@ -133,6 +135,7 @@ bool thread_sdl_init(void *param)
     running->name = "main";
     running->state = STATE_RUNNING;
     running->context.c = SDL_CreateCond();
+    cores[CURRENT_CORE].irq_level = STAY_IRQ_LEVEL;
 
     if (running->context.c == NULL)
     {
@@ -154,16 +157,6 @@ 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;
@@ -220,6 +213,17 @@ static void remove_from_list_l(struct thread_entry **list,
     thread->l.next->l.prev = thread->l.prev;
 }
 
+static void run_blocking_ops(void)
+{
+    int level = cores[CURRENT_CORE].irq_level;
+
+    if (level != STAY_IRQ_LEVEL)
+    {
+        cores[CURRENT_CORE].irq_level = STAY_IRQ_LEVEL;
+        set_irq_level(level);
+    }
+}
+
 struct thread_entry *thread_get_current(void)
 {
     return running;
@@ -373,6 +377,8 @@ void _block_thread(struct thread_queue *tq)
     thread->bqp = tq;
     add_to_list_l(&tq->queue, thread);
 
+    run_blocking_ops();
+
     SDL_CondWait(thread->context.c, m);
     running = thread;
 
@@ -388,6 +394,8 @@ void block_thread_w_tmo(struct thread_queue *tq, int ticks)
     thread->bqp = tq;
     add_to_list_l(&tq->queue, thread);
 
+    run_blocking_ops();
+
     SDL_CondWaitTimeout(thread->context.c, m, (1000/HZ) * ticks);
     running = thread;
 
@@ -451,6 +459,8 @@ void remove_thread(struct thread_entry *thread)
     SDL_Thread *t;
     SDL_cond *c;
 
+    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     if (thread == NULL)
     {
         thread = current;
@@ -486,10 +496,12 @@ void remove_thread(struct thread_entry *thread)
     {
         /* Do a graceful exit - perform the longjmp back into the thread
            function to return */
+        set_irq_level(oldlevel);
         longjmp(thread_jmpbufs[current - threads], 1);
     }
 
     SDL_KillThread(t);
+    set_irq_level(oldlevel);
 }
 
 void thread_wait(struct thread_entry *thread)
diff --git a/uisimulator/sdl/uisdl.c b/uisimulator/sdl/uisdl.c
index 9dcdbc7e26..d9c64abce2 100644
--- a/uisimulator/sdl/uisdl.c
+++ b/uisimulator/sdl/uisdl.c
@@ -22,6 +22,7 @@
 #include <setjmp.h>
 #include "autoconf.h"
 #include "button.h"
+#include "system-sdl.h"
 #include "thread.h"
 #include "kernel.h"
 #include "uisdl.h"
@@ -75,7 +76,11 @@ Uint32 tick_timer(Uint32 interval, void *param)
     if (new_tick != current_tick) {
         long i;
         for (i = new_tick - current_tick; i > 0; i--)
+        {
+            sim_enter_irq_handler();
             sim_tick_tasks();
+            sim_exit_irq_handler();
+        }
         current_tick = new_tick;
     }
     
@@ -92,10 +97,14 @@ void gui_message_loop(void)
         switch(event.type)
         {
             case SDL_KEYDOWN:
+                sim_enter_irq_handler();
                 button_event(event.key.keysym.sym, true);
+                sim_exit_irq_handler();
                 break;
             case SDL_KEYUP:
+                sim_enter_irq_handler();
                 button_event(event.key.keysym.sym, false);
+                sim_exit_irq_handler();
                 break;
             case SDL_QUIT:
                 done = true;
@@ -170,11 +179,12 @@ bool gui_startup(void)
 
 bool gui_shutdown(void)
 {
-    SDL_RemoveTimer(tick_timer_id);
     /* Order here is relevent to prevent deadlocks and use of destroyed
        sync primitives by kernel threads */
     thread_sdl_shutdown();
+    SDL_RemoveTimer(tick_timer_id);
     sim_io_shutdown();
+    sim_kernel_shutdown();
     return true;
 }
 
@@ -228,6 +238,11 @@ int main(int argc, char *argv[])
         background = false;
     }
 
+    if (!sim_kernel_init()) {
+        fprintf(stderr, "sim_kernel_init failed\n");
+        return -1;
+    }
+
     if (!sim_io_init()) {
         fprintf(stderr, "sim_io_init failed\n");
         return -1;