New scheduler, with priorities for swcodec platforms. Frequent task

switching should be more efficient and tasks are stored in linked
lists to eliminate unnecessary task switching to improve performance.
Audio should no longer skip on swcodec targets caused by too CPU
hungry UI thread or background threads.


git-svn-id: svn://svn.rockbox.org/rockbox/trunk@10958 a1c6a512-1295-4272-9138-f99709370657

24 files changed, 555 insertions, 208 deletions

diff --git a/firmware/backlight.c b/firmware/backlight.c
index bf88cbe..4e76072 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -618,12 +618,13 @@ static void backlight_tick(void)
 
 void backlight_init(void)
 {
-    queue_init(&backlight_queue);
+    queue_init(&backlight_queue, true);
 #ifdef X5_BACKLIGHT_SHUTDOWN
     backlight_thread_id =
 #endif
     create_thread(backlight_thread, backlight_stack,
-                  sizeof(backlight_stack), backlight_thread_name);
+                  sizeof(backlight_stack), backlight_thread_name 
+                  IF_PRIO(, PRIORITY_SYSTEM));
     tick_add_task(backlight_tick);
 #ifdef SIMULATOR
     /* do nothing */
diff --git a/firmware/buffer.c b/firmware/buffer.c
index 1eaff33..6af8eb9 100644
--- a/firmware/buffer.c
+++ b/firmware/buffer.c
@@ -40,7 +40,9 @@ void *buffer_alloc(size_t size)
     void *retval = audiobuf;
     
     audiobuf += size;
-    /* 32-bit aligned */;
+    /* 32-bit aligned */
     audiobuf = (void *)(((unsigned long)audiobuf + 3) & ~3);
+    
     return retval;
 }
+
diff --git a/firmware/common/dircache.c b/firmware/common/dircache.c
index 17033e1..0bdd065 100644
--- a/firmware/common/dircache.c
+++ b/firmware/common/dircache.c
@@ -690,9 +690,9 @@ void dircache_init(void)
         opendirs[i].secondary_entry.d_name = buffer_alloc(MAX_PATH);
     }
     
-    queue_init(&dircache_queue);
+    queue_init(&dircache_queue, true);
     create_thread(dircache_thread, dircache_stack,
-                sizeof(dircache_stack), dircache_thread_name);
+                sizeof(dircache_stack), dircache_thread_name IF_PRIO(, PRIORITY_BACKGROUND));
 }
 
 /**
diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 8593eeb..f570885 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -1932,11 +1932,12 @@ int ata_init(void)
         if (rc)
             return -60 + rc;
 
-        queue_init(&ata_queue);
+        queue_init(&ata_queue, true);
 
         last_disk_activity = current_tick;
         create_thread(ata_thread, ata_stack,
-                      sizeof(ata_stack), ata_thread_name);
+                      sizeof(ata_stack), ata_thread_name
+                      IF_PRIO(, PRIORITY_SYSTEM));
         initialized = true;
 
     }
diff --git a/firmware/drivers/ata_mmc.c b/firmware/drivers/ata_mmc.c
index b2e79c4..6303ca2 100644
--- a/firmware/drivers/ata_mmc.c
+++ b/firmware/drivers/ata_mmc.c
@@ -1182,9 +1182,9 @@ int ata_init(void)
         if (!last_mmc_status)
             mmc_status = MMC_UNTOUCHED;
 #ifdef HAVE_HOTSWAP
-        queue_init(&mmc_queue);
+        queue_init(&mmc_queue, true);
         create_thread(mmc_thread, mmc_stack,
-                      sizeof(mmc_stack), mmc_thread_name);
+                      sizeof(mmc_stack), mmc_thread_name IF_PRIO(, PRIORITY_SYSTEM));
 #endif
         tick_add_task(mmc_tick);
         initialized = true;
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index daad4f1..6536a34 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -674,7 +674,7 @@ void button_init(void)
     GPIOA_INT_CLR = GPIOA_INT_STAT;
     GPIOA_INT_EN  = 0xff;
 #endif /* CONFIG_KEYPAD */
-    queue_init(&button_queue);
+    queue_init(&button_queue, true);
     button_read();
     lastbtn = button_read();
     tick_add_task(button_tick);
diff --git a/firmware/drivers/fmradio_i2c.c b/firmware/drivers/fmradio_i2c.c
index 6f87e15..62761b3 100644
--- a/firmware/drivers/fmradio_i2c.c
+++ b/firmware/drivers/fmradio_i2c.c
@@ -317,8 +317,7 @@ static void fmradio_i2c_ack(int bit)
     
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the slave to release it */
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(0);
 
     DELAY;
     SCL_OUTPUT;
@@ -337,8 +336,7 @@ static int fmradio_i2c_getack(void)
     SDA_INPUT;   /* And set to input */
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the slave to release it */
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(0);
     
     if (SDA)
         /* ack failed */
diff --git a/firmware/drivers/i2c.c b/firmware/drivers/i2c.c
index 71cb9cf..2b439c2 100644
--- a/firmware/drivers/i2c.c
+++ b/firmware/drivers/i2c.c
@@ -145,8 +145,7 @@ void i2c_ack(int bit)
     
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the MAS to release it */
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
 
     DELAY;
     SCL_OUTPUT;
@@ -168,8 +167,7 @@ int i2c_getack(void)
     SDA_INPUT;   /* And set to input */
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the MAS to release it */
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
     
     if (SDA)
         /* ack failed */
diff --git a/firmware/drivers/lcd-16bit.c b/firmware/drivers/lcd-16bit.c
index e6ae28b..47c02ea 100644
--- a/firmware/drivers/lcd-16bit.c
+++ b/firmware/drivers/lcd-16bit.c
@@ -79,7 +79,7 @@ void lcd_init(void)
     /* Call device specific init */
     lcd_init_device();
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name, PRIORITY_SYSTEM);
 }
 
 /*** parameter handling ***/
diff --git a/firmware/drivers/lcd-2bit-horz.c b/firmware/drivers/lcd-2bit-horz.c
index 33f483d..dc49a37 100644
--- a/firmware/drivers/lcd-2bit-horz.c
+++ b/firmware/drivers/lcd-2bit-horz.c
@@ -76,7 +76,7 @@ void lcd_init(void)
     /* Call device specific init */
     lcd_init_device();
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 /*** parameter handling ***/
diff --git a/firmware/drivers/lcd-h100-remote.c b/firmware/drivers/lcd-h100-remote.c
index ed5816c..5db6d54 100644
--- a/firmware/drivers/lcd-h100-remote.c
+++ b/firmware/drivers/lcd-h100-remote.c
@@ -573,7 +573,7 @@ static void remote_tick(void)
 void lcd_remote_init(void)
 {
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 #else /* !SIMULATOR */
 
@@ -601,10 +601,11 @@ void lcd_remote_init(void)
 #endif
     lcd_remote_clear_display();
 
-    queue_clear(&remote_scroll_queue); /* no queue_init() -- private queue */
+    /* private queue */
+    queue_init(&remote_scroll_queue, false);
     tick_add_task(remote_tick);
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 /*** update functions ***/
diff --git a/firmware/drivers/lcd-h100.c b/firmware/drivers/lcd-h100.c
index 8407876..ada6f29 100644
--- a/firmware/drivers/lcd-h100.c
+++ b/firmware/drivers/lcd-h100.c
@@ -144,7 +144,7 @@ void lcd_set_flip(bool yesno)
 void lcd_init(void)
 {
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 #else
 
@@ -193,7 +193,7 @@ void lcd_init(void)
     lcd_write_command(LCD_CNTL_ON_OFF | 1); /* LCD ON */
 
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 /*** update functions ***/
diff --git a/firmware/drivers/lcd-player.c b/firmware/drivers/lcd-player.c
index 1601247..050258d 100644
--- a/firmware/drivers/lcd-player.c
+++ b/firmware/drivers/lcd-player.c
@@ -610,7 +610,7 @@ void lcd_init (void)
     lcd_set_contrast(lcd_default_contrast());
 
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 void lcd_jump_scroll (int mode) /* 0=off, 1=once, ..., JUMP_SCROLL_ALWAYS */
diff --git a/firmware/drivers/lcd-recorder.c b/firmware/drivers/lcd-recorder.c
index 1987d9a..e74cad7 100644
--- a/firmware/drivers/lcd-recorder.c
+++ b/firmware/drivers/lcd-recorder.c
@@ -232,7 +232,7 @@ void lcd_set_flip(bool yesno)
 void lcd_init(void)
 {
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 #else
 
@@ -278,7 +278,7 @@ void lcd_init(void)
     lcd_update();
 
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 /*** Update functions ***/
diff --git a/firmware/drivers/lcd-remote-2bit-vi.c b/firmware/drivers/lcd-remote-2bit-vi.c
index 48f8b8a..bae2824 100755
--- a/firmware/drivers/lcd-remote-2bit-vi.c
+++ b/firmware/drivers/lcd-remote-2bit-vi.c
@@ -1167,7 +1167,7 @@ static void scroll_thread(void)
 void lcd_remote_init(void)
 {
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 #else
 void lcd_remote_init(void)
@@ -1176,9 +1176,10 @@ void lcd_remote_init(void)
     lcd_remote_init_device();
     
     lcd_remote_clear_display();
-    queue_clear(&remote_scroll_queue); /* no queue_init() -- private queue */
+    /* private queue */
+    queue_init(&remote_scroll_queue, false);
     tick_add_task(remote_tick);
     create_thread(scroll_thread, scroll_stack,
-                  sizeof(scroll_stack), scroll_name);
+                  sizeof(scroll_stack), scroll_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 #endif
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 18adaee..1b756cc 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -216,8 +216,12 @@
 /* Enable the directory cache and tagcache in RAM if we have
  * plenty of RAM. Both features can be enabled independently. */
 #if (MEMORYSIZE > 8 || MEM > 8) && !defined(BOOTLOADER)
-#define HAVE_DIRCACHE 1
-#define HAVE_TC_RAMCACHE 1
+#define HAVE_DIRCACHE
+#define HAVE_TC_RAMCACHE
+#endif
+
+#if (CONFIG_CODEC == SWCODEC) && !defined(SIMULATOR) && !defined(BOOTLOADER)
+#define HAVE_PRIORITY_SCHEDULING
 #endif
 
 /* define for all cpus from coldfire family */
diff --git a/firmware/export/kernel.h b/firmware/export/kernel.h
index 26b1cbe..482516b 100644
--- a/firmware/export/kernel.h
+++ b/firmware/export/kernel.h
@@ -56,12 +56,14 @@ struct event
 struct event_queue
 {
     struct event events[QUEUE_LENGTH];
+    struct thread_entry *thread;
     unsigned int read;
     unsigned int write;
 };
 
 struct mutex
 {
+    struct thread_entry *thread;
     bool locked;
 };
 
@@ -85,7 +87,7 @@ extern void sleep(int ticks);
 int tick_add_task(void (*f)(void));
 int tick_remove_task(void (*f)(void));
 
-extern void queue_init(struct event_queue *q);
+extern void queue_init(struct event_queue *q, bool register_queue);
 extern void queue_delete(struct event_queue *q);
 extern void queue_wait(struct event_queue *q, struct event *ev);
 extern void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks);
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index e102997..7e053bc 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -21,8 +21,24 @@
 
 #include <stdbool.h>
 
+/* Priority scheduling (when enabled with HAVE_PRIORITY_SCHEDULING) works
+ * by giving high priority threads more CPU time than less priority threads
+ * when they need it.
+ * 
+ * If software playback codec pcm buffer is going down to critical, codec
+ * can change it own priority to REALTIME to override user interface and
+ * prevent playback skipping.
+ */
+#define PRIORITY_REALTIME        1
+#define PRIORITY_USER_INTERFACE  4 /* The main thread */
+#define PRIORITY_RECORDING       4 /* Recording thread */
+#define PRIORITY_PLAYBACK        4 /* or REALTIME when needed */
+#define PRIORITY_BUFFERING       4 /* Codec buffering thread */
+#define PRIORITY_SYSTEM          6 /* All other firmware threads */
+#define PRIORITY_BACKGROUND      8 /* Normal application threads */
+
 #if CONFIG_CODEC == SWCODEC
-#define MAXTHREADS	16
+#define MAXTHREADS	15
 #else
 #define MAXTHREADS  11
 #endif
@@ -32,7 +48,7 @@
 #ifndef SIMULATOR
 /* Need to keep structures inside the header file because debug_menu
  * needs them. */
-#ifdef CPU_COLDFIRE
+# ifdef CPU_COLDFIRE
 struct regs
 {
     unsigned int macsr;  /* EMAC status register */
@@ -41,7 +57,7 @@ struct regs
     void         *sp;    /* Stack pointer (a7) */
     void         *start; /* Thread start address, or NULL when started */
 };
-#elif CONFIG_CPU == SH7034
+# elif CONFIG_CPU == SH7034
 struct regs
 {
     unsigned int r[7];   /* Registers r8 thru r14 */
@@ -49,7 +65,7 @@ struct regs
     void         *pr;    /* Procedure register */
     void         *start; /* Thread start address, or NULL when started */
 };
-#elif defined(CPU_ARM)
+# elif defined(CPU_ARM)
 struct regs
 {
     unsigned int r[8];   /* Registers r4-r11 */
@@ -57,42 +73,72 @@ struct regs
     unsigned int lr;     /* r14 (lr) */
     void         *start; /* Thread start address, or NULL when started */
 };
-#elif CONFIG_CPU == TCC730
+# elif CONFIG_CPU == TCC730
 struct regs
 {
     void *sp;    /* Stack pointer (a15) */
     void *start; /* Thread start address */
     int started; /* 0 when not started */
 };
-#endif
+# endif
+
+#endif /* !SIMULATOR */
+
+#define STATE_RUNNING         0
+#define STATE_BLOCKED         1
+#define STATE_SLEEPING        2
+#define STATE_BLOCKED_W_TMO   3
+
+#define GET_STATE_ARG(state)  (state & 0x3FFFFFFF)
+#define GET_STATE(state)      ((state >> 30) & 3)
+#define SET_STATE(state,arg)  ((state << 30) | (arg))
 
 struct thread_entry {
+#ifndef SIMULATOR
     struct regs context;
+#endif
     const char *name;
     void *stack;
-    int stack_size;
+    unsigned long statearg;
+    unsigned short stack_size;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    unsigned short priority;
+    long last_run;
+#endif
+    struct thread_entry *next, *prev;
 };
 
 struct core_entry {
-    int num_threads;
-    volatile int num_sleepers;
-    int current_thread;
     struct thread_entry threads[MAXTHREADS];
+    struct thread_entry *running;
+    struct thread_entry *sleeping;
 };
+
+#ifdef HAVE_PRIORITY_SCHEDULING
+#define IF_PRIO(empty, type)  , type
+#else
+#define IF_PRIO(empty, type)
 #endif
 
-int create_thread(void (*function)(void), void* stack, int stack_size,
-                  const char *name);
-int create_thread_on_core(unsigned int core, void (*function)(void), void* stack, int stack_size,
-                  const char *name);
-void remove_thread(int threadnum);
-void remove_thread_on_core(unsigned int core, int threadnum);
-void switch_thread(void);
-void sleep_thread(void);
-void wake_up_thread(void);
+struct thread_entry*
+    create_thread(void (*function)(void), void* stack, int stack_size,
+                  const char *name IF_PRIO(, int priority));
+
+struct thread_entry*
+    create_thread_on_core(unsigned int core, void (*function)(void), 
+                          void* stack, int stack_size,
+                          const char *name
+                          IF_PRIO(, int priority));
+
+void remove_thread(struct thread_entry *thread);
+void switch_thread(bool save_context, struct thread_entry **blocked_list);
+void sleep_thread(int ticks);
+void block_thread(struct thread_entry **thread, int timeout);
+void wakeup_thread(struct thread_entry **thread);
+void thread_set_priority(struct thread_entry *thread, int priority);
 void init_threads(void);
-int thread_stack_usage(int threadnum);
-int thread_stack_usage_on_core(unsigned int core, int threadnum);
+int thread_stack_usage(const struct thread_entry *thread);
+int thread_get_status(const struct thread_entry *thread);
 #ifdef RB_PROFILE
 void profile_thread(void);
 #endif
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 889f950..4a6d615 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -35,7 +35,6 @@ static void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
 static struct event_queue *all_queues[32];
 static int num_queues;
 
-void sleep(int ticks) ICODE_ATTR;
 void queue_wait(struct event_queue *q, struct event *ev) ICODE_ATTR;
 
 /****************************************************************************
@@ -71,13 +70,7 @@ void sleep(int ticks)
     } while(counter > 0);
 
 #else
-    /* Always sleep at least 1 tick */
-    int timeout = current_tick + ticks + 1;
-
-    while (TIME_BEFORE( current_tick, timeout )) {
-        sleep_thread();
-    }
-    wake_up_thread();
+    sleep_thread(ticks);
 #endif
 }
 
@@ -86,21 +79,24 @@ void yield(void)
 #if (CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022) && defined(BOOTLOADER))
     /* Some targets don't like yielding in the bootloader */
 #else
-    switch_thread();
-    wake_up_thread();
+    switch_thread(true, NULL);
 #endif
 }
 
 /****************************************************************************
  * Queue handling stuff
  ****************************************************************************/
-void queue_init(struct event_queue *q)
+void queue_init(struct event_queue *q, bool register_queue)
 {
     q->read = 0;
     q->write = 0;
-
-    /* Add it to the all_queues array */
-    all_queues[num_queues++] = q;
+    q->thread = NULL;
+    
+    if (register_queue)
+    {
+        /* Add it to the all_queues array */
+        all_queues[num_queues++] = q;
+    }
 }
 
 void queue_delete(struct event_queue *q)
@@ -108,6 +104,8 @@ void queue_delete(struct event_queue *q)
     int i;
     bool found = false;
 
+    wakeup_thread(&q->thread);
+    
     /* Find the queue to be deleted */
     for(i = 0;i < num_queues;i++)
     {
@@ -132,26 +130,22 @@ void queue_delete(struct event_queue *q)
 
 void queue_wait(struct event_queue *q, struct event *ev)
 {
-    while(q->read == q->write)
+    if (q->read == q->write)
     {
-        sleep_thread();
+        block_thread(&q->thread, 0);
     }
-    wake_up_thread();
 
     *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 }
 
 void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
 {
-    unsigned int timeout = current_tick + ticks;
-
-    while(q->read == q->write && TIME_BEFORE( current_tick, timeout ))
+    if (q->read == q->write && ticks > 0)
     {
-        sleep_thread();
+        block_thread(&q->thread, ticks);
     }
-    wake_up_thread();
 
-    if(q->read != q->write)
+    if (q->read != q->write)
     {
         *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
     }
@@ -171,6 +165,9 @@ void queue_post(struct event_queue *q, long id, void *data)
 
     q->events[wr].id = id;
     q->events[wr].data = data;
+    
+    wakeup_thread(&q->thread);
+    
     set_irq_level(oldlevel);
 }
 
@@ -250,7 +247,6 @@ void IMIA0(void)
     }
 
     current_tick++;
-    wake_up_thread();
 
     TSR0 &= ~0x01;
 }
@@ -301,7 +297,6 @@ void TIMER0(void)
     }
 
     current_tick++;
-    wake_up_thread();
 
     TER0 = 0xff; /* Clear all events */
 }
@@ -330,7 +325,6 @@ void TIMER0(void)
     }
 
     current_tick++;
-    wake_up_thread();
 
     /* re-enable timer by clearing the counter */
     TACON |= 0x80;
@@ -382,7 +376,6 @@ void TIMER1(void)
     }
 
     current_tick++;
-    wake_up_thread();
 }
 #endif
 
@@ -415,7 +408,6 @@ void timer_handler(void)
     }
 
     current_tick++;
-    wake_up_thread();
 
     TIMERR0C = 1;
 }
@@ -513,22 +505,27 @@ int tick_remove_task(void (*f)(void))
 void mutex_init(struct mutex *m)
 {
     m->locked = false;
+    m->thread = NULL;
 }
 
 void mutex_lock(struct mutex *m)
 {
-    /* Wait until the lock is open... */
-    while(m->locked)
-        sleep_thread();
-    wake_up_thread();
-
+    if (m->locked)
+    {
+        /* Wait until the lock is open... */
+        block_thread(&m->thread, 0);
+    }
+    
     /* ...and lock it */
     m->locked = true;
 }
 
 void mutex_unlock(struct mutex *m)
 {
-    m->locked = false;
+    if (m->thread == NULL)
+        m->locked = false;
+    else
+        wakeup_thread(&m->thread);
 }
 
 #endif
diff --git a/firmware/mpeg.c b/firmware/mpeg.c
index df0cbba..61b0a22 100644
--- a/firmware/mpeg.c
+++ b/firmware/mpeg.c
@@ -761,7 +761,6 @@ void rec_tick(void)
             {
                 prerecord_timeout = current_tick + HZ;
                 queue_post(&mpeg_queue, MPEG_PRERECORDING_TICK, 0);
-                wake_up_thread();
             }
         }
         else
@@ -773,7 +772,6 @@ void rec_tick(void)
             {
                 saving_status = BUFFER_FULL;
                 queue_post(&mpeg_queue, MPEG_SAVE_DATA, 0);
-                wake_up_thread();
             }
         }
     }
@@ -894,8 +892,6 @@ static void transfer_end(unsigned char** ppbuf, int* psize)
             *psize = 0; /* no more transfer */
         }
     }
-
-    wake_up_thread();
 }
 
 static struct trackdata *add_track_to_tag_list(const char *filename)
@@ -2119,8 +2115,7 @@ void audio_init_playback(void)
     queue_post(&mpeg_queue, MPEG_INIT_PLAYBACK, NULL);
 
     while(!init_playback_done)
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
 }
 
 
@@ -2134,8 +2129,7 @@ void audio_init_recording(unsigned int buffer_offset)
     queue_post(&mpeg_queue, MPEG_INIT_RECORDING, NULL);
 
     while(!init_recording_done)
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
 }
 
 static void init_recording(void)
@@ -2886,10 +2880,10 @@ void audio_init(void)
 
 #ifndef SIMULATOR
     audiobuflen = audiobufend - audiobuf;
-    queue_init(&mpeg_queue);
+    queue_init(&mpeg_queue, true);
 #endif /* !SIMULATOR */
     create_thread(mpeg_thread, mpeg_stack,
-                  sizeof(mpeg_stack), mpeg_thread_name);
+                  sizeof(mpeg_stack), mpeg_thread_name IF_PRIO(, PRIORITY_SYSTEM));
 
     memset(trackdata, sizeof(trackdata), 0);
 
diff --git a/firmware/pcm_record.c b/firmware/pcm_record.c
index 68fc22b..bce6fb5 100644
--- a/firmware/pcm_record.c
+++ b/firmware/pcm_record.c
@@ -172,9 +172,9 @@ static void close_wave(void);
 /* Creates pcmrec_thread */
 void pcm_rec_init(void)
 {
-    queue_init(&pcmrec_queue);
+    queue_init(&pcmrec_queue, true);
     create_thread(pcmrec_thread, pcmrec_stack, sizeof(pcmrec_stack),
-        pcmrec_thread_name);
+                  pcmrec_thread_name IF_PRIO(, PRIORITY_RECORDING));
 }
 
 
@@ -196,8 +196,7 @@ void audio_init_recording(unsigned int buffer_offset)
     queue_post(&pcmrec_queue, PCMREC_INIT, 0);
     
     while(!init_done)
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
 }
 
 void audio_close_recording(void)
@@ -206,8 +205,7 @@ void audio_close_recording(void)
     queue_post(&pcmrec_queue, PCMREC_CLOSE, 0);
     
     while(!close_done)
-        sleep_thread();
-    wake_up_thread();
+        sleep_thread(1);
 
     audio_remove_encoder();
 }
@@ -421,8 +419,7 @@ void audio_record(const char *filename)
     queue_post(&pcmrec_queue, PCMREC_START, 0);
     
     while(!record_done)
-        sleep_thread();
-    wake_up_thread();    
+        sleep_thread(1);
 }
 
 
@@ -438,8 +435,7 @@ void audio_new_file(const char *filename)
     queue_post(&pcmrec_queue, PCMREC_NEW_FILE, 0);
     
     while(!new_file_done)
-        sleep_thread();
-    wake_up_thread();    
+        sleep_thread(1);
     
     logf("pcm_new_file done");
 }
@@ -459,8 +455,7 @@ void audio_stop_recording(void)
     queue_post(&pcmrec_queue, PCMREC_STOP, 0);
 
     while(!stop_done)
-        sleep_thread();
-    wake_up_thread();    
+        sleep_thread(1);
 
     logf("pcm_stop done");
 }
@@ -482,8 +477,7 @@ void audio_pause_recording(void)
     queue_post(&pcmrec_queue, PCMREC_PAUSE, 0);
 
     while(!pause_done)
-        sleep_thread();
-    wake_up_thread();    
+        sleep_thread(1);
 }
 
 void audio_resume_recording(void)
@@ -498,8 +492,7 @@ void audio_resume_recording(void)
     queue_post(&pcmrec_queue, PCMREC_RESUME, 0);
 
     while(!resume_done)
-        sleep_thread();
-    wake_up_thread();    
+        sleep_thread(1);
 }
 
 /* return peaks as int, so convert from short first
@@ -817,9 +810,8 @@ static void pcmrec_stop(void)
         /* wait for encoding finish */
         is_paused = true;
         while(!wav_queue_empty)
-            sleep_thread();
+            sleep_thread(1);
 
-        wake_up_thread();
         is_recording = false;
 
         /* Flush buffers to file */
diff --git a/firmware/powermgmt.c b/firmware/powermgmt.c
index 8f83296..f1e1aac 100644
--- a/firmware/powermgmt.c
+++ b/firmware/powermgmt.c
@@ -998,7 +998,7 @@ void powermgmt_init(void)
     memset(power_history, 0x00, sizeof(power_history));
 
     create_thread(power_thread, power_stack, sizeof(power_stack),
-                  power_thread_name);
+                  power_thread_name IF_PRIO(, PRIORITY_SYSTEM));
 }
 
 #endif /* SIMULATOR */
diff --git a/firmware/thread.c b/firmware/thread.c
index eb39c7a..e4dcbbc 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -23,12 +23,18 @@
 #include "system.h"
 #include "kernel.h"
 #include "cpu.h"
-
+#include "string.h"
 
 #define DEADBEEF ((unsigned int)0xdeadbeef)
 /* Cast to the the machine int type, whose size could be < 4. */
 
 struct core_entry cores[NUM_CORES] IBSS_ATTR;
+#ifdef HAVE_PRIORITY_SCHEDULING
+static unsigned short highest_priority IBSS_ATTR;
+#endif
+
+/* Define to enable additional checks for blocking violations etc. */
+// #define THREAD_EXTRA_CHECKS
 
 static const char main_thread_name[] = "main";
 
@@ -48,7 +54,16 @@ int *cop_stackend = stackend;
 #endif
 #endif
 
-void switch_thread(void) ICODE_ATTR;
+/* Conserve IRAM
+static void add_to_list(struct thread_entry **list,
+                        struct thread_entry *thread) ICODE_ATTR;
+static void remove_from_list(struct thread_entry **list,
+                             struct thread_entry *thread) ICODE_ATTR;
+*/
+
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
+    ICODE_ATTR;
+
 static inline void store_context(void* addr) __attribute__ ((always_inline));
 static inline void load_context(const void* addr) __attribute__ ((always_inline));
 
@@ -219,24 +234,109 @@ static inline void load_context(const void* addr)
 
 #endif
 
-/*---------------------------------------------------------------------------
- * Switch thread in round robin fashion.
- *---------------------------------------------------------------------------
- */
-void switch_thread(void)
+static void add_to_list(struct thread_entry **list,
+                        struct thread_entry *thread)
 {
-#ifdef RB_PROFILE
-    profile_thread_stopped(cores[CURRENT_CORE].current_thread);
-#endif
-    int current;
-    unsigned int *stackptr;
+    if (*list == NULL)
+    {
+        thread->next = thread;
+        thread->prev = thread;
+        *list = thread;
+    }
+    else
+    {
+        /* Insert last */
+        thread->next = *list;
+        thread->prev = (*list)->prev;
+        thread->prev->next = thread;
+        (*list)->prev = thread;
+        
+        /* Insert next
+         thread->next = (*list)->next;
+         thread->prev = *list;
+         thread->next->prev = thread;
+         (*list)->next = thread;
+         */
+    }
+}
 
-#ifdef SIMULATOR
-    /* Do nothing */
-#else
-    while (cores[CURRENT_CORE].num_sleepers == cores[CURRENT_CORE].num_threads)
+static void remove_from_list(struct thread_entry **list,
+                      struct thread_entry *thread)
+{
+    if (list != NULL)
+    {
+        if (thread == thread->next)
+        {
+            *list = NULL;
+            return;
+        }
+        
+        if (thread == *list)
+            *list = thread->next;
+    }
+    
+    /* Fix links to jump over the removed entry. */
+    thread->prev->next = thread->next;
+    thread->next->prev = thread->prev;
+}
+
+/* Compiler trick: Don't declare as static to prevent putting 
+ * function in IRAM. */
+void check_sleepers(void)
+{
+    struct thread_entry *current, *next;
+    
+    /* Check sleeping threads. */
+    current = cores[CURRENT_CORE].sleeping;
+    if (current == NULL)
+        return ;
+    
+    for (;;)
+    {
+        next = current->next;
+        
+        if ((unsigned)current_tick >= GET_STATE_ARG(current->statearg))
+        {
+            /* Sleep timeout has been reached so bring the thread
+             * back to life again. */
+            remove_from_list(&cores[CURRENT_CORE].sleeping, current);
+            add_to_list(&cores[CURRENT_CORE].running, current);
+            
+            /* If there is no more processes in the list, break the loop. */
+            if (cores[CURRENT_CORE].sleeping == NULL)
+                break;
+            
+            current = next;
+            continue;
+        }
+        
+        current = next;
+        
+        /* Break the loop once we have walked through the list of all
+         * sleeping processes. */
+        if (current == cores[CURRENT_CORE].sleeping)
+            break;
+    }
+}
+
+static inline void sleep_core(void)
+{
+    static long last_tick = 0;
+    
+    for (;;)
     {
-        /* Enter sleep mode, woken up on interrupt */
+        if (last_tick != current_tick)
+        {
+            check_sleepers();
+            last_tick = current_tick;
+        }
+        
+        /* We must sleep until there is at least one process in the list
+         * of running processes. */
+        if (cores[CURRENT_CORE].running != NULL)
+            break;
+
+        /* Enter sleep mode to reduce power usage, woken up on interrupt */
 #ifdef CPU_COLDFIRE
         asm volatile ("stop #0x2000");
 #elif CONFIG_CPU == SH7034
@@ -257,49 +357,232 @@ void switch_thread(void)
         CLKCON |= 2;
 #endif
     }
-#endif
-    current = cores[CURRENT_CORE].current_thread;
-    store_context(&cores[CURRENT_CORE].threads[current].context);
-
-#if CONFIG_CPU != TCC730
-    /* Check if the current thread stack is overflown */
-    stackptr = cores[CURRENT_CORE].threads[current].stack;
-    if(stackptr[0] != DEADBEEF)
-       panicf("Stkov %s", cores[CURRENT_CORE].threads[current].name);
+}
+
+#ifdef RB_PROFILE
+static int get_threadnum(struct thread_entry *thread)
+{
+    int i;
+    
+    for (i = 0; i < MAXTHREADS; i++)
+    {
+        if (&cores[CURRENT_CORE].threads[i] == thread)
+            return i;
+    }
+    
+    return -1;
+}
 #endif
 
-    if (++current >= cores[CURRENT_CORE].num_threads)
-        current = 0;
+/* Compiler trick: Don't declare as static to prevent putting 
+ * function in IRAM. */
+void change_thread_state(struct thread_entry **blocked_list)
+{
+    struct thread_entry *old;
+    
+    /* Remove the thread from the list of running threads. */
+    old = cores[CURRENT_CORE].running;
+    remove_from_list(&cores[CURRENT_CORE].running, old);
+    
+    /* And put the thread into a new list of inactive threads. */
+    if (GET_STATE(old->statearg) == STATE_BLOCKED)
+        add_to_list(blocked_list, old);
+    else
+        add_to_list(&cores[CURRENT_CORE].sleeping, old);
+    
+#ifdef HAVE_PRIORITY_SCHEDULING
+    /* Reset priorities */
+    if (old->priority == highest_priority)
+        highest_priority = 100;
+#endif
+}
 
-    cores[CURRENT_CORE].current_thread = current;
-    load_context(&cores[CURRENT_CORE].threads[current].context);
+/*---------------------------------------------------------------------------
+ * Switch thread in round robin fashion.
+ *---------------------------------------------------------------------------
+ */
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
+{
 #ifdef RB_PROFILE
-    profile_thread_started(cores[CURRENT_CORE].current_thread);
+    profile_thread_stopped(get_threadnum(cores[CURRENT_CORE].running));
+#endif
+    unsigned int *stackptr;
+    
+#ifdef SIMULATOR
+    /* Do nothing */
+#else
+    
+    /* Begin task switching by saving our current context so that we can
+     * restore the state of the current thread later to the point prior
+     * to this call. */
+    if (save_context)
+    {
+        store_context(&cores[CURRENT_CORE].running->context);
+
+# if CONFIG_CPU != TCC730
+        /* Check if the current thread stack is overflown */
+        stackptr = cores[CURRENT_CORE].running->stack;
+        if(stackptr[0] != DEADBEEF)
+        panicf("Stkov %s", cores[CURRENT_CORE].running->name);
+# endif
+        
+        /* Check if a thread state change has been requested. */
+        if (cores[CURRENT_CORE].running->statearg)
+        {
+            /* Change running thread state and switch to next thread. */
+            change_thread_state(blocked_list);
+        }
+        else
+        {
+            /* Switch to the next running thread. */
+            cores[CURRENT_CORE].running = cores[CURRENT_CORE].running->next;
+        }
+    }
+    
+    /* Go through the list of sleeping task to check if we need to wake up
+     * any of them due to timeout. Also puts core into sleep state until
+     * there is at least one running process again. */
+    sleep_core();
+    
+#ifdef HAVE_PRIORITY_SCHEDULING
+    /* Select the new task based on priorities and the last time a process
+     * got CPU time. */
+    for (;;)
+    {
+        int priority = cores[CURRENT_CORE].running->priority;
+        
+        if (priority < highest_priority)
+            highest_priority = priority;
+        
+        if (priority == highest_priority || (current_tick 
+            - cores[CURRENT_CORE].running->last_run > priority * 8))
+        {
+            break;
+        }
+        cores[CURRENT_CORE].running = cores[CURRENT_CORE].running->next;
+    }
+    
+    /* Reset the value of thread's last running time to the current time. */
+    cores[CURRENT_CORE].running->last_run = current_tick;
+#endif
+    
+#endif
+    /* And finally give control to the next thread. */
+    load_context(&cores[CURRENT_CORE].running->context);
+    
+#ifdef RB_PROFILE
+    profile_thread_started(get_threadnum(cores[CURRENT_CORE].running));
 #endif
 }
 
-void sleep_thread(void)
+void sleep_thread(int ticks)
 {
-    ++cores[CURRENT_CORE].num_sleepers;
-    switch_thread();
+    /* Set the thread's new state and timeout and finally force a task switch
+     * so that scheduler removes thread from the list of running processes
+     * and puts it in list of sleeping tasks. */
+    cores[CURRENT_CORE].running->statearg = 
+        SET_STATE(STATE_SLEEPING, current_tick + ticks + 1);
+    switch_thread(true, NULL);
+    
+    /* Clear all flags to indicate we are up and running again. */
+    cores[CURRENT_CORE].running->statearg = 0;
 }
 
-void wake_up_thread(void)
+void block_thread(struct thread_entry **list, int timeout)
 {
-    cores[CURRENT_CORE].num_sleepers = 0;
+    struct thread_entry *current;
+    
+    /* Get the entry for the current running thread. */
+    current = cores[CURRENT_CORE].running;
+    
+    /* At next task switch scheduler will immediately change the thread
+     * state (and we also force the task switch to happen). */
+    if (timeout)
+    {
+#ifdef THREAD_EXTRA_CHECKS
+        /* We can store only one thread to the "list" if thread is used
+         * in other list (such as core's list for sleeping tasks). */
+        if (*list)
+            panicf("Blocking violation T->*B");
+#endif
+        
+        current->statearg = 
+            SET_STATE(STATE_BLOCKED_W_TMO, current_tick + timeout);
+        *list = current;
+
+        /* Now force a task switch and block until we have been woken up
+         * by another thread or timeout is reached. */
+        switch_thread(true, NULL);
+        
+        /* If timeout is reached, we must set list back to NULL here. */
+        *list = NULL;
+    }
+    else
+    {
+#ifdef THREAD_EXTRA_CHECKS
+        /* We are not allowed to mix blocking types in one queue. */
+        if (*list && GET_STATE((*list)->statearg) == STATE_BLOCKED_W_TMO)
+            panicf("Blocking violation B->*T");
+#endif
+        
+        current->statearg = SET_STATE(STATE_BLOCKED, 0);
+        
+        /* Now force a task switch and block until we have been woken up
+         * by another thread or timeout is reached. */
+        switch_thread(true, list);
+    }
+    
+    /* Clear all flags to indicate we are up and running again. */
+    current->statearg = 0;
 }
 
+void wakeup_thread(struct thread_entry **list)
+{
+    struct thread_entry *thread;
+    
+    /* Check if there is a blocked thread at all. */
+    if (*list == NULL)
+        return ;
+    
+    /* Wake up the last thread first. */
+    thread = *list;
+    
+    /* Determine thread's current state. */
+    switch (GET_STATE(thread->statearg)) 
+    {
+        case STATE_BLOCKED:
+            /* Remove thread from the list of blocked threads and add it
+             * to the scheduler's list of running processes. */
+            remove_from_list(list, thread);
+            add_to_list(&cores[CURRENT_CORE].running, thread);
+            thread->statearg = 0;
+            break;
+        
+        case STATE_BLOCKED_W_TMO:
+            /* Just remove the timeout to cause scheduler to immediately
+             * wake up the thread. */
+            thread->statearg &= 0xC0000000;
+            *list = NULL;
+            break;
+        
+        default:
+            /* Nothing to do. Thread has already been woken up
+             * or it's state is not blocked or blocked with timeout. */
+            return ;
+    }
+}
 
 /*---------------------------------------------------------------------------
  * Create thread on the current core.
  * Return ID if context area could be allocated, else -1.
  *---------------------------------------------------------------------------
  */
-int create_thread(void (*function)(void), void* stack, int stack_size,
-                  const char *name)
+struct thread_entry* 
+    create_thread(void (*function)(void), void* stack, int stack_size,
+                  const char *name IF_PRIO(, int priority))
 {
     return create_thread_on_core(CURRENT_CORE, function, stack, stack_size,
-                  name);
+                  name IF_PRIO(, priority));
 }
 
 /*---------------------------------------------------------------------------
@@ -307,18 +590,28 @@ int create_thread(void (*function)(void), void* stack, int stack_size,
  * Return ID if context area could be allocated, else -1.
  *---------------------------------------------------------------------------
  */
-int create_thread_on_core(unsigned int core, void (*function)(void), void* stack, int stack_size,
-                  const char *name)
+struct thread_entry* 
+    create_thread_on_core(unsigned int core, void (*function)(void), 
+                          void* stack, int stack_size,
+                          const char *name IF_PRIO(, int priority))
 {
     unsigned int i;
     unsigned int stacklen;
     unsigned int *stackptr;
+    int n;
     struct regs *regs;
     struct thread_entry *thread;
 
-    if (cores[core].num_threads >= MAXTHREADS)
-        return -1;
-
+    for (n = 0; n < MAXTHREADS; n++)
+    {
+        if (cores[core].threads[n].name == NULL)
+            break;
+    }
+    
+    if (n == MAXTHREADS)
+        return NULL;
+    
+    
     /* Munge the stack to make it easy to spot stack overflows */
     stacklen = stack_size / sizeof(int);
     stackptr = stack;
@@ -328,10 +621,17 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
     }
 
     /* Store interesting information */
-    thread = &cores[core].threads[cores[core].num_threads];
+    thread = &cores[core].threads[n];
     thread->name = name;
     thread->stack = stack;
     thread->stack_size = stack_size;
+    thread->statearg = 0;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    thread->priority = priority;
+    highest_priority = 100;
+#endif
+    add_to_list(&cores[core].running, thread);
+    
     regs = &thread->context;
 #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || defined(CPU_ARM)
     /* Align stack to an even 32 bit boundary */
@@ -343,8 +643,7 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
 #endif
     regs->start = (void*)function;
 
-    wake_up_thread();
-    return cores[core].num_threads++; /* return the current ID, e.g for remove_thread() */
+    return thread;
 }
 
 /*---------------------------------------------------------------------------
@@ -352,44 +651,58 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
  * Parameter is the ID as returned from create_thread().
  *---------------------------------------------------------------------------
  */
-void remove_thread(int threadnum)
+void remove_thread(struct thread_entry *thread)
 {
-    remove_thread_on_core(CURRENT_CORE, threadnum);
+    if (thread == NULL)
+        thread = cores[CURRENT_CORE].running;
+    
+    /* Free the entry by removing thread name. */
+    thread->name = NULL;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    highest_priority = 100;
+#endif
+    
+    if (thread == cores[CURRENT_CORE].running)
+    {
+        remove_from_list(&cores[CURRENT_CORE].running, thread);
+        switch_thread(false, NULL);
+        return ;
+    }
+    
+    if (thread == cores[CURRENT_CORE].sleeping)
+        remove_from_list(&cores[CURRENT_CORE].sleeping, thread);
+    
+    remove_from_list(NULL, thread);
 }
 
-/*---------------------------------------------------------------------------
- * Remove a thread on the specified core from the scheduler.
- * Parameters are the core and the ID as returned from create_thread().
- *---------------------------------------------------------------------------
- */
-void remove_thread_on_core(unsigned int core, int threadnum)
+#ifdef HAVE_PRIORITY_SCHEDULING
+void thread_set_priority(struct thread_entry *thread, int priority)
 {
-    int i;
-
-    if (threadnum >= cores[core].num_threads)
-       return;
-
-    cores[core].num_threads--;
-    for (i=threadnum; i<cores[core].num_threads-1; i++)
-    {   /* move all entries which are behind */
-        cores[core].threads[i] = cores[core].threads[i+1];
-    }
-
-    if (cores[core].current_thread == threadnum) /* deleting the current one? */
-        cores[core].current_thread = cores[core].num_threads; /* set beyond last, avoid store harm */
-    else if (cores[core].current_thread > threadnum) /* within the moved positions? */
-        cores[core].current_thread--; /* adjust it, point to same context again */
+    if (thread == NULL)
+        thread = cores[CURRENT_CORE].running;
+    
+    thread->priority = priority;
+    highest_priority = 100;
 }
+#endif
 
 void init_threads(void)
 {
     unsigned int core = CURRENT_CORE;
 
-    cores[core].num_threads = 1; /* We have 1 thread to begin with */
-    cores[core].current_thread = 0; /* The current thread is number 0 */
+    memset(cores, 0, sizeof cores);
+    cores[core].sleeping = NULL;
+    cores[core].running = NULL;
     cores[core].threads[0].name = main_thread_name;
-/* In multiple core setups, each core has a different stack.  There is probably
-   a much better way to do this. */
+    cores[core].threads[0].statearg = 0;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    cores[core].threads[0].priority = PRIORITY_USER_INTERFACE;
+    highest_priority = 100;
+#endif
+    add_to_list(&cores[core].running, &cores[core].threads[0]);
+    
+    /* In multiple core setups, each core has a different stack.  There is probably
+     a much better way to do this. */
     if (core == CPU)
     {
         cores[CPU].threads[0].stack = stackbegin;
@@ -405,28 +718,24 @@ void init_threads(void)
 #else
     cores[core].threads[0].context.start = 0; /* thread 0 already running */
 #endif
-    cores[core].num_sleepers = 0;
-}
-
-int thread_stack_usage(int threadnum)
-{
-    return thread_stack_usage_on_core(CURRENT_CORE, threadnum);
 }
 
-int thread_stack_usage_on_core(unsigned int core, int threadnum)
+int thread_stack_usage(const struct thread_entry *thread)
 {
     unsigned int i;
-    unsigned int *stackptr = cores[core].threads[threadnum].stack;
-
-    if (threadnum >= cores[core].num_threads)
-        return -1;
+    unsigned int *stackptr = thread->stack;
 
-    for (i = 0;i < cores[core].threads[threadnum].stack_size/sizeof(int);i++)
+    for (i = 0;i < thread->stack_size/sizeof(int);i++)
     {
         if (stackptr[i] != DEADBEEF)
             break;
     }
 
-    return ((cores[core].threads[threadnum].stack_size - i * sizeof(int)) * 100) /
-        cores[core].threads[threadnum].stack_size;
+    return ((thread->stack_size - i * sizeof(int)) * 100) /
+        thread->stack_size;
+}
+
+int thread_get_status(const struct thread_entry *thread)
+{
+    return GET_STATE(thread->statearg);
 }
diff --git a/firmware/usb.c b/firmware/usb.c
index 6be5fda..0be6c40 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -558,8 +558,9 @@ void usb_init(void)
     last_usb_status = false;
 
 #ifndef BOOTLOADER
-    queue_init(&usb_queue);
-    create_thread(usb_thread, usb_stack, sizeof(usb_stack), usb_thread_name);
+    queue_init(&usb_queue, true);
+    create_thread(usb_thread, usb_stack, sizeof(usb_stack), 
+                  usb_thread_name IF_PRIO(, PRIORITY_SYSTEM));
 
     tick_add_task(usb_tick);
 #endif