24 files changed, 555 insertions, 208 deletions

diff --git a/firmware/backlight.c b/firmware/backlight.c
index bf88cbe9bc..4e76072822 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 1eaff33de1..6af8eb9432 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 17033e1827..0bdd0657bd 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 8593eebea1..f57088504b 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 b2e79c419a..6303ca2851 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 daad4f17de..6536a34037 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 6f87e15b3e..62761b3aa7 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 71cb9cf8b0..2b439c23ad 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 e6ae28bc19..47c02ea7b7 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 33f483d38d..dc49a37c8a 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 ed5816cebf..5db6d548ff 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 8407876d34..ada6f29216 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 16012470fa..050258d1f8 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 1987d9a3ed..e74cad7f03 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 48f8b8a25f..bae2824050 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 18adaeeca2..1b756cc6bd 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 26b1cbe2e7..482516b9dc 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 e102997dae..7e053bc507 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 889f950252..4a6d61515a 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 df0cbbad12..61b0a22d87 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 68fc22b937..bce6fb5f25 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 8f832964b5..f1e1aac57f 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 eb39c7ad32..e4dcbbcf9a 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 6be5fda093..0be6c4011e 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