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

apps/codecs.c

@@ -123,7 +123,7 @@ struct codec_api ci = {
     &current_tick,
     default_event_handler,
     default_event_handler_ex,
-    create_thread,
+    create_thread_on_core,
     remove_thread,
     reset_poweroff_timer,
 #ifndef SIMULATOR

apps/codecs.h

@@ -196,8 +196,10 @@ struct codec_api {
     long* current_tick;
     long (*default_event_handler)(long event);
     long (*default_event_handler_ex)(long event, void (*callback)(void *), void *parameter);
-    int (*create_thread)(void (*function)(void), void* stack, int stack_size, const char *name);
+    struct thread_entry* (*create_thread)(unsigned int core, void (*function)(void), 
-    void (*remove_thread)(int threadnum);
+                                          void* stack, int stack_size, const char *name
+                                          IF_PRIO(, int priority));
+    void (*remove_thread)(struct thread_entry *thread);
     void (*reset_poweroff_timer)(void);
 #ifndef SIMULATOR
     int (*system_memory_guard)(int newmode);

apps/debug_menu.c

@@ -80,13 +80,27 @@ extern char ata_device;
 extern int ata_io_address;
 extern struct core_entry cores[NUM_CORES];
 
+char thread_status_char(int status)
+{
+    switch (status)
+    {
+        case STATE_RUNNING      : return 'R';
+        case STATE_BLOCKED      : return 'B';
+        case STATE_SLEEPING     : return 'S';
+        case STATE_BLOCKED_W_TMO: return 'T';
+    }
+    
+    return '?';
+}
 #ifdef HAVE_LCD_BITMAP
 /* Test code!!! */
 bool dbg_os(void)
 {
+    struct thread_entry *thread;
     char buf[32];
     int i;
     int usage;
+    int status;
 #if NUM_CORES > 1
     unsigned int core;
     int line;
@@ -98,24 +112,54 @@ bool dbg_os(void)
 
     while(1)
     {
+#if 0 /* Enable to simulate UI lag. */
+        int _x;
+        for (_x = 0; _x < 1000000L; _x++) ;
+#endif
 #if NUM_CORES > 1
         lcd_puts(0, 0, "Core and stack usage:");
         line = 0;
         for(core = 0; core < NUM_CORES; core++)
         {
-            for(i = 0; i < num_threads[core]; i++)
+            for(i = 0; i < MAXTHREADS; i++)
             {
-                usage = thread_stack_usage_on_core(core, i);
+                thread = &cores[core].threads[i];
-                snprintf(buf, 32, "(%d) %s: %d%%", core, thread_name[core][i], usage);
+                if (thread->name == NULL)
+                    continue;
+                
+                usage = thread_stack_usage(thread);
+                status = thread_get_status(thread);
+                
+                snprintf(buf, 32, "(%d) %c%c %d %s: %d%%", core, 
+                         (status == STATE_RUNNING) ? '*' : ' ', 
+                         thread_status_char(status),
+                         cores[CURRENT_CORE].threads[i].priority,
+                         cores[core].threads[i].name, usage);
                 lcd_puts(0, ++line, buf);
             }
         }
 #else
         lcd_puts(0, 0, "Stack usage:");
-        for(i = 0; i < cores[CURRENT_CORE].num_threads;i++)
+        for(i = 0; i < MAXTHREADS; i++)
         {
-            usage = thread_stack_usage(i);
+            thread = &cores[CURRENT_CORE].threads[i];
-            snprintf(buf, 32, "%s: %d%%", cores[CURRENT_CORE].threads[i].name, usage);
+            if (thread->name == NULL)
+                continue;
+            
+            usage = thread_stack_usage(thread);
+            status = thread_get_status(thread);
+# ifdef HAVE_PRIORITY_SCHEDULING
+            snprintf(buf, 32, "%c%c %d %s: %d%%",
+                     (status == STATE_RUNNING) ? '*' : ' ', 
+                     thread_status_char(status),
+                     cores[CURRENT_CORE].threads[i].priority,
+                     cores[CURRENT_CORE].threads[i].name, usage);
+# else
+            snprintf(buf, 32, "%c%c %s: %d%%",
+                     (status == STATE_RUNNING) ? '*' : ' ', 
+                     (status == STATE_BLOCKED) ? 'B' : ' ', 
+                     cores[CURRENT_CORE].threads[i].name, usage);
+# endif
             lcd_puts(0, 1+i, buf);
         }
 #endif

apps/pcmbuf.c

@@ -35,9 +35,10 @@
 #include "settings.h"
 #include "audio.h"
 #include "dsp.h"
+#include "thread.h"
 
-/* 1.5s low mark */
+/* Keep watermark high for iPods at least (2s) */
-#define PCMBUF_WATERMARK     (NATIVE_FREQUENCY * 6)
+#define PCMBUF_WATERMARK     (NATIVE_FREQUENCY * 8)
 
 /* Structure we can use to queue pcm chunks in memory to be played
  * by the driver code. */
@@ -94,6 +95,8 @@ static size_t pcmbuf_mix_sample IDATA_ATTR;
 static bool low_latency_mode = false;
 static bool pcmbuf_flush;
 
+extern struct thread_entry *codec_thread_p;
+
 /* Helpful macros for use in conditionals this assumes some of the above
  * static variable names */
 #define NEED_FLUSH(position) \
@@ -109,14 +112,37 @@ static bool pcmbuf_flush_fillpos(void);
 void pcmbuf_boost(bool state)
 {
     static bool boost_state = false;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    static bool priority_modified = false;
+#endif
     
     if (crossfade_init || crossfade_active)
         return;
 
-    if (state != boost_state) {
+    if (state != boost_state)
+    {
         cpu_boost(state);
         boost_state = state;
     }
+    
+#ifdef HAVE_PRIORITY_SCHEDULING
+    if (state && LOW_DATA(2) && pcm_is_playing())
+    {
+        if (!priority_modified)
+        {
+            /* Buffer is critically low so override UI priority. */
+            priority_modified = true;
+            thread_set_priority(codec_thread_p, PRIORITY_REALTIME);
+        }
+    }
+    else if (priority_modified)
+    {
+        /* Set back the original priority. */
+        thread_set_priority(codec_thread_p, PRIORITY_PLAYBACK);
+        priority_modified = false;
+    }
+    
+#endif
 }
 #endif
 
@@ -244,7 +270,9 @@ static void pcmbuf_under_watermark(void)
     pcmbuf_boost(true);
     /* Disable crossfade if < .5s of audio */
     if (LOW_DATA(2))
+    {
         crossfade_active = false;
+    }
 }
 
 void pcmbuf_set_event_handler(void (*event_handler)(void))
@@ -270,8 +298,8 @@ bool pcmbuf_is_lowdata(void)
             crossfade_init || crossfade_active)
         return false;
 
-    /* 0.5 seconds of buffer is low data */
+    /* 1 seconds of buffer is low data */
-    return LOW_DATA(2);
+    return LOW_DATA(4);
 }
 
 /* Amount of bytes left in the buffer. */
@@ -443,7 +471,7 @@ static bool pcmbuf_flush_fillpos(void)
                 pcmbuf_play_start();
             }
             /* Let approximately one chunk of data playback */
-            sleep(PCMBUF_TARGET_CHUNK/(NATIVE_FREQUENCY * 4) / 5);
+            sleep(HZ*PCMBUF_TARGET_CHUNK/(NATIVE_FREQUENCY*4));
         }
         pcmbuf_add_chunk();
         return true;

apps/playback.c

@@ -176,7 +176,7 @@ static char *voicebuf;
 static size_t voice_remaining;
 static bool voice_is_playing;
 static void (*voice_getmore)(unsigned char** start, int* size);
-static int voice_thread_num = -1;
+static struct thread_entry *voice_thread_p = NULL;
 
 /* Is file buffer currently being refilled? */
 static volatile bool filling IDATA_ATTR;
@@ -267,6 +267,8 @@ static struct event_queue codec_queue;
 static long codec_stack[(DEFAULT_STACK_SIZE + 0x2000)/sizeof(long)]
 IBSS_ATTR;
 static const char codec_thread_name[] = "codec";
+/* For modifying thread priority later. */
+struct thread_entry *codec_thread_p;
 
 /* Voice thread */
 static struct event_queue voice_queue;
@@ -628,13 +630,13 @@ void audio_preinit(void)
 
     mutex_init(&mutex_codecthread);
 
-    queue_init(&audio_queue);
+    queue_init(&audio_queue, true);
-    queue_init(&codec_queue);
+    queue_init(&codec_queue, true);
-    /* clear, not init to create a private queue */
+    /* create a private queue */
-    queue_clear(&codec_callback_queue);
+    queue_init(&codec_callback_queue, false);
 
     create_thread(audio_thread, audio_stack, sizeof(audio_stack),
-                  audio_thread_name);
+                  audio_thread_name IF_PRIO(, PRIORITY_BUFFERING));
 }
 
 void audio_init(void)
@@ -648,14 +650,14 @@ void voice_init(void)
     if (!filebuf)
         return;     /* Audio buffers not yet set up */
 
-    if (voice_thread_num >= 0)
+    if (voice_thread_p)
     {
         logf("Terminating voice codec");
-        remove_thread(voice_thread_num);
+        remove_thread(voice_thread_p);
         if (current_codec == CODEC_IDX_VOICE)
             mutex_unlock(&mutex_codecthread);
         queue_delete(&voice_queue);
-        voice_thread_num = -1;
+        voice_thread_p = NULL;
         voice_codec_loaded = false;
     }
 
@@ -663,9 +665,10 @@ void voice_init(void)
         return;
 
     logf("Starting voice codec");
-    queue_init(&voice_queue);
+    queue_init(&voice_queue, true);
-    voice_thread_num = create_thread(voice_thread, voice_stack,
+    voice_thread_p = create_thread(voice_thread, voice_stack,
-            sizeof(voice_stack), voice_thread_name);
+            sizeof(voice_stack), voice_thread_name 
+            IF_PRIO(, PRIORITY_PLAYBACK));
     
     while (!voice_codec_loaded)
         yield();
@@ -1982,13 +1985,15 @@ static bool audio_yield_codecs(void)
 {
     yield();
     
-    if (!queue_empty(&audio_queue)) return true;
+    if (!queue_empty(&audio_queue)) 
+        return true;
 
     while ((pcmbuf_is_crossfade_active() || pcmbuf_is_lowdata())
             && !ci.stop_codec && playing && !audio_filebuf_is_lowdata())
     {
         sleep(1);
-        if (!queue_empty(&audio_queue)) return true;
+        if (!queue_empty(&audio_queue)) 
+            return true;
     }
     
     return false;
@@ -3178,8 +3183,9 @@ static void audio_playback_init(void)
     id3_voice.frequency = 11200;
     id3_voice.length = 1000000L;
 
-    create_thread(codec_thread, codec_stack, sizeof(codec_stack),
+    codec_thread_p = create_thread(codec_thread, codec_stack, 
-            codec_thread_name);
+                                   sizeof(codec_stack),
+                                   codec_thread_name IF_PRIO(, PRIORITY_PLAYBACK));
 
     while (1)
     {
@@ -3213,7 +3219,8 @@ static void audio_thread(void)
     /* At first initialize audio system in background. */
     audio_playback_init();
 
-    while (1) {
+    while (1) 
+    {
         if (filling)
         {
             queue_wait_w_tmo(&audio_queue, &ev, 0);
@@ -3221,7 +3228,7 @@ static void audio_thread(void)
                 ev.id = Q_AUDIO_FILL_BUFFER;
         }
         else
-            queue_wait_w_tmo(&audio_queue, &ev, HZ);
+            queue_wait_w_tmo(&audio_queue, &ev, HZ/2);
 
         switch (ev.id) {
             case Q_AUDIO_FILL_BUFFER:

apps/playlist.c

@@ -1777,8 +1777,8 @@ void playlist_init(void)
     memset(playlist->filenames, 0,
            playlist->max_playlist_size * sizeof(int));
     create_thread(playlist_thread, playlist_stack, sizeof(playlist_stack),
-                  playlist_thread_name);
+                  playlist_thread_name IF_PRIO(, PRIORITY_BACKGROUND));
-    queue_init(&playlist_queue);
+    queue_init(&playlist_queue, true);
 #endif
 }
 

apps/plugin.c

@@ -108,6 +108,7 @@ static const struct plugin_api rockbox_api = {
     PREFIX(lcd_icon),
     lcd_double_height,
 #else
+    lcd_setmargins,
     lcd_set_drawmode,
     lcd_get_drawmode,
     lcd_setfont,
@@ -132,6 +133,9 @@ static const struct plugin_api rockbox_api = {
     lcd_bitmap_transparent_part,
     lcd_bitmap_transparent,
 #endif
+    bidi_l2v,
+    font_get_bits,
+    font_load,
     lcd_putsxy,
     lcd_puts_style,
     lcd_puts_scroll_style,
@@ -178,6 +182,45 @@ static const struct plugin_api rockbox_api = {
     remote_backlight_on,
     remote_backlight_off,
 #endif
+#if NB_SCREENS == 2
+    {&screens[SCREEN_MAIN], &screens[SCREEN_REMOTE]},
+#else
+    {&screens[SCREEN_MAIN]},
+#endif
+#if defined(HAVE_REMOTE_LCD) && (LCD_REMOTE_DEPTH > 1)
+    lcd_remote_set_foreground,
+    lcd_remote_get_foreground,
+    lcd_remote_set_background,
+    lcd_remote_get_background,
+    lcd_remote_bitmap_part,
+    lcd_remote_bitmap,
+#endif
+
+#if defined(HAVE_LCD_COLOR) && !defined(SIMULATOR)
+    lcd_yuv_blit,
+#endif
+    /* list */
+    gui_synclist_init,
+    gui_synclist_set_nb_items,
+    gui_synclist_set_icon_callback,
+    gui_synclist_get_nb_items,
+    gui_synclist_get_sel_pos,
+    gui_synclist_draw,
+    gui_synclist_select_item,
+    gui_synclist_select_next,
+    gui_synclist_select_previous,
+    gui_synclist_select_next_page,
+    gui_synclist_select_previous_page,
+    gui_synclist_add_item,
+    gui_synclist_del_item,
+    gui_synclist_limit_scroll,
+    gui_synclist_flash,
+#ifdef HAVE_LCD_BITMAP
+    gui_synclist_scroll_right,
+    gui_synclist_scroll_left,
+#endif
+    gui_synclist_do_button,
+    
     /* button */
     button_get,
     button_get_w_tmo,
@@ -205,12 +248,14 @@ static const struct plugin_api rockbox_api = {
     ata_sleep,
     ata_disk_is_active,
 #endif
+    reload_directory,
 
     /* dir */
     PREFIX(opendir),
     PREFIX(closedir),
     PREFIX(readdir),
     PREFIX(mkdir),
+    PREFIX(rmdir),
 
     /* kernel/ system */
     PREFIX(sleep),
@@ -253,6 +298,7 @@ static const struct plugin_api rockbox_api = {
 
     /* strings and memory */
     snprintf,
+    vsnprintf,
     strcpy,
     strncpy,
     strlen,
@@ -268,6 +314,7 @@ static const struct plugin_api rockbox_api = {
     atoi,
     strchr,
     strcat,
+    memchr,
     memcmp,
     strcasestr,
     /* unicode stuff */
@@ -277,9 +324,12 @@ static const struct plugin_api rockbox_api = {
     utf16BEdecode,
     utf8encode,
     utf8length,
+    utf8seek,
 
     /* sound */
     sound_set,
+    set_sound,
+
     sound_min,
     sound_max,
 #ifndef SIMULATOR
@@ -334,6 +384,9 @@ static const struct plugin_api rockbox_api = {
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
     mas_codec_writereg,
     mas_codec_readreg,
+    i2c_begin,
+    i2c_end,
+    i2c_write,
 #endif
 #endif /* !SIMULATOR && CONFIG_CODEC != SWCODEC */
 
@@ -352,6 +405,14 @@ static const struct plugin_api rockbox_api = {
     menu_insert,
     menu_set_cursor,
     set_option,
+    set_int,
+    set_bool,
+
+    /* action handling */
+    get_custom_action,
+    get_action,
+    action_signalscreenchange,
+    action_userabort,
     
     /* power */
     battery_level,
@@ -405,74 +466,6 @@ static const struct plugin_api rockbox_api = {
 
     /* new stuff at the end, sort into place next time
        the API gets incompatible */
-    set_sound,
-#if ((CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)) && !defined(SIMULATOR)
-    i2c_begin,
-    i2c_end,
-    i2c_write,
-#endif
-
-    vsnprintf,
-    memchr,
-    /* list */
-    gui_synclist_init,
-    gui_synclist_set_nb_items,
-    gui_synclist_set_icon_callback,
-    gui_synclist_get_nb_items,
-    gui_synclist_get_sel_pos,
-    gui_synclist_draw,
-    gui_synclist_select_item,
-    gui_synclist_select_next,
-    gui_synclist_select_previous,
-    gui_synclist_select_next_page,
-    gui_synclist_select_previous_page,
-    gui_synclist_add_item,
-    gui_synclist_del_item,
-    gui_synclist_limit_scroll,
-    gui_synclist_flash,
-#ifdef HAVE_LCD_BITMAP
-    gui_synclist_scroll_right,
-    gui_synclist_scroll_left,
-#endif
-    gui_synclist_do_button,
-
-#ifdef HAVE_LCD_BITMAP
-    lcd_setmargins,
-#endif
-    utf8seek,
-
-    set_int,
-    reload_directory,
-    set_bool,
-#if NB_SCREENS == 2
-    {&screens[SCREEN_MAIN], &screens[SCREEN_REMOTE]},
-#else
-    {&screens[SCREEN_MAIN]},
-#endif
-#ifdef HAVE_LCD_BITMAP
-    bidi_l2v,
-    font_get_bits,
-    font_load,
-#endif
-#if defined(HAVE_REMOTE_LCD) && (LCD_REMOTE_DEPTH > 1)
-    lcd_remote_set_foreground,
-    lcd_remote_get_foreground,
-    lcd_remote_set_background,
-    lcd_remote_get_background,
-    lcd_remote_bitmap_part,
-    lcd_remote_bitmap,
-#endif
-
-#if defined(HAVE_LCD_COLOR) && !defined(SIMULATOR)
-    lcd_yuv_blit,
-#endif
-
-    PREFIX(rmdir),
-    /* action handling */
-    get_custom_action,
-    get_action,
-    action_signalscreenchange,
-    action_userabort,
 };
 
 int plugin_load(const char* plugin, void* parameter)

apps/plugin.h

@@ -105,12 +105,12 @@
 #define PLUGIN_MAGIC 0x526F634B /* RocK */
 
 /* increase this every time the api struct changes */
-#define PLUGIN_API_VERSION 29
+#define PLUGIN_API_VERSION 30
 
 /* update this to latest version if a change to the api struct breaks
    backwards compatibility (and please take the opportunity to sort in any
    new function which are "waiting" at the end of the function table) */
-#define PLUGIN_MIN_API_VERSION 14
+#define PLUGIN_MIN_API_VERSION 30
 
 /* plugin return codes */
 enum plugin_status {
@@ -143,6 +143,7 @@ struct plugin_api {
     void (*PREFIX(lcd_icon))(int icon, bool enable);
     void (*lcd_double_height)(bool on);
 #else
+    void (*lcd_setmargins)(int x, int y);
     void (*lcd_set_drawmode)(int mode);
     int  (*lcd_get_drawmode)(void);
     void (*lcd_setfont)(int font);
@@ -174,6 +175,9 @@ struct plugin_api {
     void (*lcd_bitmap_transparent)(const fb_data *src, int x, int y,
             int width, int height);
 #endif
+    unsigned short *(*bidi_l2v)( const unsigned char *str, int orientation );
+    const unsigned char *(*font_get_bits)( struct font *pf, unsigned short char_code );
+    struct font* (*font_load)(const char *path);
     void (*lcd_putsxy)(int x, int y, const unsigned char *string);
     void (*lcd_puts_style)(int x, int y, const unsigned char *str, int style);
     void (*lcd_puts_scroll_style)(int x, int y, const unsigned char* string,
@@ -229,6 +233,49 @@ struct plugin_api {
     void (*remote_backlight_on)(void);
     void (*remote_backlight_off)(void);
 #endif
+    struct screen* screens[NB_SCREENS];
+#if defined(HAVE_REMOTE_LCD) && (LCD_REMOTE_DEPTH > 1)
+    void     (*lcd_remote_set_foreground)(unsigned foreground);
+    unsigned (*lcd_remote_get_foreground)(void);
+    void     (*lcd_remote_set_background)(unsigned foreground);
+    unsigned (*lcd_remote_get_background)(void);
+    void (*lcd_remote_bitmap_part)(const fb_remote_data *src, int src_x, int src_y,
+                                   int stride, int x, int y, int width, int height);
+    void (*lcd_remote_bitmap)(const fb_remote_data *src, int x, int y, int width,
+                              int height);
+#endif
+#if defined(HAVE_LCD_COLOR) && !defined(SIMULATOR)
+    void (*lcd_yuv_blit)(unsigned char * const src[3],
+                         int src_x, int src_y, int stride,
+                         int x, int y, int width, int height);
+#endif
+
+    /* list */
+    void (*gui_synclist_init)(struct gui_synclist * lists,
+            list_get_name callback_get_item_name,void * data,
+            bool scroll_all,int selected_size);
+    void (*gui_synclist_set_nb_items)(struct gui_synclist * lists, int nb_items);
+    void (*gui_synclist_set_icon_callback)(struct gui_synclist * lists, list_get_icon icon_callback);
+    int (*gui_synclist_get_nb_items)(struct gui_synclist * lists);
+    int  (*gui_synclist_get_sel_pos)(struct gui_synclist * lists);
+    void (*gui_synclist_draw)(struct gui_synclist * lists);
+    void (*gui_synclist_select_item)(struct gui_synclist * lists,
+    int item_number);
+    void (*gui_synclist_select_next)(struct gui_synclist * lists);
+    void (*gui_synclist_select_previous)(struct gui_synclist * lists);
+    void (*gui_synclist_select_next_page)(struct gui_synclist * lists,
+    enum screen_type screen);
+    void (*gui_synclist_select_previous_page)(struct gui_synclist * lists,
+    enum screen_type screen);
+    void (*gui_synclist_add_item)(struct gui_synclist * lists);
+    void (*gui_synclist_del_item)(struct gui_synclist * lists);
+    void (*gui_synclist_limit_scroll)(struct gui_synclist * lists, bool scroll);
+    void (*gui_synclist_flash)(struct gui_synclist * lists);
+#ifdef HAVE_LCD_BITMAP
+    void (*gui_synclist_scroll_right)(struct gui_synclist * lists);
+    void (*gui_synclist_scroll_left)(struct gui_synclist * lists);
+#endif
+    unsigned (*gui_synclist_do_button)(struct gui_synclist * lists, unsigned button);
     
     /* button */
     long (*button_get)(bool block);
@@ -257,12 +304,14 @@ struct plugin_api {
     void (*ata_sleep)(void);
     bool (*ata_disk_is_active)(void);
 #endif
+    void (*reload_directory)(void);
 
     /* dir */
     DIR* (*PREFIX(opendir))(const char* name);
     int (*PREFIX(closedir))(DIR* dir);
     struct dirent* (*PREFIX(readdir))(DIR* dir);
     int (*PREFIX(mkdir))(const char *name, int mode);
+    int (*PREFIX(rmdir))(const char *name);
 
     /* kernel/ system */
     void (*PREFIX(sleep))(int ticks);
@@ -270,8 +319,10 @@ struct plugin_api {
     long* current_tick;
     long (*default_event_handler)(long event);
     long (*default_event_handler_ex)(long event, void (*callback)(void *), void *parameter);
-    int (*create_thread)(void (*function)(void), void* stack, int stack_size, const char *name);
+    struct thread_entry* (*create_thread)(void (*function)(void), void* stack, 
-    void (*remove_thread)(int threadnum);
+                                          int stack_size, const char *name 
+                                          IF_PRIO(, int priority));
+    void (*remove_thread)(struct thread_entry *thread);
     void (*reset_poweroff_timer)(void);
 #ifndef SIMULATOR
     int (*system_memory_guard)(int newmode);
@@ -285,7 +336,7 @@ struct plugin_api {
     void (*timer_unregister)(void);
     bool (*timer_set_period)(long count);
 #endif
-    void (*queue_init)(struct event_queue *q);
+    void (*queue_init)(struct event_queue *q, bool register_queue);
     void (*queue_delete)(struct event_queue *q);
     void (*queue_post)(struct event_queue *q, long id, void *data);
     void (*queue_wait_w_tmo)(struct event_queue *q, struct event *ev,
@@ -308,6 +359,7 @@ struct plugin_api {
 
     /* strings and memory */
     int (*snprintf)(char *buf, size_t size, const char *fmt, ...);
+    int (*vsnprintf)(char *buf, int size, const char *fmt, va_list ap);
     char* (*strcpy)(char *dst, const char *src);
     char* (*strncpy)(char *dst, const char *src, size_t length);
     size_t (*strlen)(const char *str);
@@ -323,6 +375,7 @@ struct plugin_api {
     int (*atoi)(const char *str);
     char *(*strchr)(const char *s, int c);
     char *(*strcat)(char *s1, const char *s2);
+    void *(*memchr)(const void *s1, int c, size_t n);
     int (*memcmp)(const void *s1, const void *s2, size_t n);
     char *(*strcasestr) (const char* phaystack, const char* pneedle);
     /* unicode stuff */
@@ -332,9 +385,12 @@ struct plugin_api {
     unsigned char* (*utf16BEdecode)(const unsigned char *utf16, unsigned char *utf8, int count);
     unsigned char* (*utf8encode)(unsigned long ucs, unsigned char *utf8);
     unsigned long (*utf8length)(const unsigned char *utf8);
+    int (*utf8seek)(const unsigned char* utf8, int offset);
 
     /* sound */
     void (*sound_set)(int setting, int value);
+    bool (*set_sound)(const unsigned char * string,
+                      int* variable, int setting);
     int (*sound_min)(int setting);
     int (*sound_max)(int setting);
 #ifndef SIMULATOR
@@ -390,6 +446,9 @@ struct plugin_api {
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
     int (*mas_codec_writereg)(int reg, unsigned int val);
     int (*mas_codec_readreg)(int reg);
+    void (*i2c_begin)(void);
+    void (*i2c_end)(void);
+    int  (*i2c_write)(int address, unsigned char* buf, int count );
 #endif
 #endif
 
@@ -413,7 +472,17 @@ struct plugin_api {
     bool (*set_option)(const char* string, void* variable,
                        enum optiontype type, const struct opt_items* options,
                        int numoptions, void (*function)(int));
+    bool (*set_int)(const unsigned char* string, const char* unit, int voice_unit,
+                    int* variable, void (*function)(int), int step, int min,
+                    int max, void (*formatter)(char*, int, int, const char*) );
+    bool (*set_bool)(const char* string, bool* variable );
 
+    /* action handling */
+    int (*get_custom_action)(int context,int timeout,
+                          const struct button_mapping* (*get_context_map)(int));
+    int (*get_action)(int context, int timeout);
+    void (*action_signalscreenchange)(void);
+    bool (*action_userabort)(int timeout);
 
     /* power */
     int (*battery_level)(void);
@@ -476,83 +545,6 @@ struct plugin_api {
 
     /* new stuff at the end, sort into place next time
        the API gets incompatible */
-    bool (*set_sound)(const unsigned char * string,
-                      int* variable, int setting);
-#if ((CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)) && !defined(SIMULATOR)
-    void (*i2c_begin)(void);
-    void (*i2c_end)(void);
-    int  (*i2c_write)(int address, unsigned char* buf, int count );
-#endif
-
-    int (*vsnprintf)(char *buf, int size, const char *fmt, va_list ap);
-    void *(*memchr)(const void *s1, int c, size_t n);
-
-    /* list */
-    void (*gui_synclist_init)(struct gui_synclist * lists,
-            list_get_name callback_get_item_name,void * data,
-            bool scroll_all,int selected_size);
-    void (*gui_synclist_set_nb_items)(struct gui_synclist * lists, int nb_items);
-    void (*gui_synclist_set_icon_callback)(struct gui_synclist * lists, list_get_icon icon_callback);
-    int (*gui_synclist_get_nb_items)(struct gui_synclist * lists);
-    int  (*gui_synclist_get_sel_pos)(struct gui_synclist * lists);
-    void (*gui_synclist_draw)(struct gui_synclist * lists);
-    void (*gui_synclist_select_item)(struct gui_synclist * lists,
-    int item_number);
-    void (*gui_synclist_select_next)(struct gui_synclist * lists);
-    void (*gui_synclist_select_previous)(struct gui_synclist * lists);
-    void (*gui_synclist_select_next_page)(struct gui_synclist * lists,
-    enum screen_type screen);
-    void (*gui_synclist_select_previous_page)(struct gui_synclist * lists,
-    enum screen_type screen);
-    void (*gui_synclist_add_item)(struct gui_synclist * lists);
-    void (*gui_synclist_del_item)(struct gui_synclist * lists);
-    void (*gui_synclist_limit_scroll)(struct gui_synclist * lists, bool scroll);
-    void (*gui_synclist_flash)(struct gui_synclist * lists);
-#ifdef HAVE_LCD_BITMAP
-    void (*gui_synclist_scroll_right)(struct gui_synclist * lists);
-    void (*gui_synclist_scroll_left)(struct gui_synclist * lists);
-#endif
-    unsigned (*gui_synclist_do_button)(struct gui_synclist * lists, unsigned button);
-
-#ifdef HAVE_LCD_BITMAP
-    void (*lcd_setmargins)(int x, int y);
-#endif
-    int (*utf8seek)(const unsigned char* utf8, int offset);
-
-    bool (*set_int)(const unsigned char* string, const char* unit, int voice_unit,
-                    int* variable, void (*function)(int), int step, int min,
-                    int max, void (*formatter)(char*, int, int, const char*) );
-    void (*reload_directory)(void);
-    bool (*set_bool)(const char* string, bool* variable );
-    struct screen* screens[NB_SCREENS];
-#ifdef HAVE_LCD_BITMAP
-    unsigned short *(*bidi_l2v)( const unsigned char *str, int orientation );
-    const unsigned char *(*font_get_bits)( struct font *pf, unsigned short char_code );
-    struct font* (*font_load)(const char *path);
-#endif
-#if defined(HAVE_REMOTE_LCD) && (LCD_REMOTE_DEPTH > 1)
-    void     (*lcd_remote_set_foreground)(unsigned foreground);
-    unsigned (*lcd_remote_get_foreground)(void);
-    void     (*lcd_remote_set_background)(unsigned foreground);
-    unsigned (*lcd_remote_get_background)(void);
-    void (*lcd_remote_bitmap_part)(const fb_remote_data *src, int src_x, int src_y,
-                                   int stride, int x, int y, int width, int height);
-    void (*lcd_remote_bitmap)(const fb_remote_data *src, int x, int y, int width,
-                              int height);
-#endif
-#if defined(HAVE_LCD_COLOR) && !defined(SIMULATOR)
-    void (*lcd_yuv_blit)(unsigned char * const src[3],
-                         int src_x, int src_y, int stride,
-                         int x, int y, int width, int height);
-#endif
-
-    int (*PREFIX(rmdir))(const char *name);
-    /* action handling */
-    int (*get_custom_action)(int context,int timeout,
-                          const struct button_mapping* (*get_context_map)(int));
-    int (*get_action)(int context, int timeout);
-    void (*action_signalscreenchange)(void);
-    bool (*action_userabort)(int timeout);
 };
 
 /* plugin header */

apps/plugins/alpine_cdc.c

@@ -202,7 +202,7 @@ struct
 /* communication to the worker thread */
 struct 
 {
-    int id; /* ID of the thread */
+    struct thread_entry *id; /* Pointer of the thread */
     bool foreground; /* set as long as we're owning the UI */
     bool exiting; /* signal to the thread that we want to exit */
     bool ended; /* response from the thread, that is has exited */
@@ -1169,7 +1169,8 @@ int main(void* parameter)
 
     rb->memset(&gTread, 0, sizeof(gTread));
     gTread.foreground = true;
-    gTread.id = rb->create_thread(thread, stack, stacksize, "CDC");
+    gTread.id = rb->create_thread(thread, stack, stacksize, "CDC"
+                                  IF_PRIO(, PRIORITY_BACKGROUND));
 
 #ifdef DEBUG
     do

apps/plugins/battery_bench.c

@@ -102,7 +102,7 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
 
 struct 
 {
-    int id;
+    struct thread_entry *id;
     bool ended;
 } s_thread;
 
@@ -454,10 +454,11 @@ int main(void)
         rb->close(fd);
     }
     
-    rb->queue_init(&thread_q); /* put the thread's queue in the bcast list */
+    rb->queue_init(&thread_q, true); /* put the thread's queue in the bcast list */
     rb->memset(&s_thread, 0, sizeof(s_thread)); /* zero the struct */
     if((s_thread.id = rb->create_thread(thread, thread_stack,
-        sizeof(thread_stack), "Battery Benchmark"))<0)
+        sizeof(thread_stack), "Battery Benchmark" 
+        IF_PRIO(, PRIORITY_BACKGROUND))) == NULL)
     {
         rb->splash(HZ,true,"Cannot create thread!");
         return PLUGIN_ERROR;

apps/tagcache.c

@@ -54,6 +54,7 @@
  */
 
 #include <stdio.h>
+#include "config.h"
 #include "thread.h"
 #include "kernel.h"
 #include "system.h"
@@ -1062,7 +1063,6 @@ static bool get_next(struct tagcache_search *tcs)
     
     if (tagcache_is_numeric_tag(tcs->type))
     {
-        logf("r:%d", tcs->position);
         snprintf(buf, sizeof(buf), "%d", tcs->position);
         tcs->result_seek = tcs->position;
         tcs->result = buf;
@@ -3455,7 +3455,6 @@ static void tagcache_thread(void)
     sleep(HZ);
     stat.ready = check_all_headers();
     
-    
     while (1)
     {
         queue_wait_w_tmo(&tagcache_queue, &ev, HZ);
@@ -3503,6 +3502,7 @@ static void tagcache_thread(void)
             
 
                 logf("tagcache check done");
+    
                 check_done = true;
                 break ;
 
@@ -3612,9 +3612,10 @@ void tagcache_init(void)
     current_serial = 0;
     write_lock = read_lock = 0;
     
-    queue_init(&tagcache_queue);
+    queue_init(&tagcache_queue, true);
     create_thread(tagcache_thread, tagcache_stack,
-                  sizeof(tagcache_stack), tagcache_thread_name);
+                  sizeof(tagcache_stack), tagcache_thread_name 
+                  IF_PRIO(, PRIORITY_BACKGROUND));
 }
 
 bool tagcache_is_initialized(void)

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 */

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;
 }
+

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));
 }
 
 /**

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;
 
     }

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;

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);

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();
+        sleep_thread(0);
-    wake_up_thread();
 
     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();
+        sleep_thread(0);
-    wake_up_thread();
     
     if (SDA)
         /* ack failed */

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();
+        sleep_thread(1);
-    wake_up_thread();
 
     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();
+        sleep_thread(1);
-    wake_up_thread();
     
     if (SDA)
         /* ack failed */

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 ***/

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 ***/

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 ***/

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 ***/

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 */

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 ***/

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

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_DIRCACHE
-#define HAVE_TC_RAMCACHE 1
+#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 */

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);

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,
+struct thread_entry*
-                  const char *name);
+    create_thread(void (*function)(void), void* stack, int stack_size,
-int create_thread_on_core(unsigned int core, void (*function)(void), void* stack, int stack_size,
+                  const char *name IF_PRIO(, int priority));
-                  const char *name);
+
-void remove_thread(int threadnum);
+struct thread_entry*
-void remove_thread_on_core(unsigned int core, int threadnum);
+    create_thread_on_core(unsigned int core, void (*function)(void), 
-void switch_thread(void);
+                          void* stack, int stack_size,
-void sleep_thread(void);
+                          const char *name
-void wake_up_thread(void);
+                          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(const struct thread_entry *thread);
-int thread_stack_usage_on_core(unsigned int core, int threadnum);
+int thread_get_status(const struct thread_entry *thread);
 #ifdef RB_PROFILE
 void profile_thread(void);
 #endif

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 */
+    sleep_thread(ticks);
-    int timeout = current_tick + ticks + 1;
-
-    while (TIME_BEFORE( current_tick, timeout )) {
-        sleep_thread();
-    }
-    wake_up_thread();
 #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();
+    switch_thread(true, NULL);
-    wake_up_thread();
 #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;
+    q->thread = NULL;
     
-    /* Add it to the all_queues array */
+    if (register_queue)
-    all_queues[num_queues++] = q;
+    {
+        /* 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;
+    if (q->read == q->write && ticks > 0)
-
-    while(q->read == q->write && TIME_BEFORE( current_tick, timeout ))
     {
-        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,14 +505,16 @@ 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... */
+    if (m->locked)
-    while(m->locked)
+    {
-        sleep_thread();
+        /* Wait until the lock is open... */
-    wake_up_thread();
+        block_thread(&m->thread, 0);
+    }
     
     /* ...and lock it */
     m->locked = true;
@@ -528,7 +522,10 @@ void mutex_lock(struct mutex *m)
 
 void mutex_unlock(struct mutex *m)
 {
-    m->locked = false;
+    if (m->thread == NULL)
+        m->locked = false;
+    else
+        wakeup_thread(&m->thread);
 }
 
 #endif

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();
+        sleep_thread(1);
-    wake_up_thread();
 }
 
 
@@ -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();
+        sleep_thread(1);
-    wake_up_thread();
 }
 
 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);
 

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();
+        sleep_thread(1);
-    wake_up_thread();
 }
 
 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();
+        sleep_thread(1);
-    wake_up_thread();
 
     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();
+        sleep_thread(1);
-    wake_up_thread();    
 }
 
 
@@ -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();
+        sleep_thread(1);
-    wake_up_thread();    
     
     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();
+        sleep_thread(1);
-    wake_up_thread();    
 
     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();
+        sleep_thread(1);
-    wake_up_thread();    
 }
 
 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();
+        sleep_thread(1);
-    wake_up_thread();    
 }
 
 /* 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 */

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 */

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
 
-/*---------------------------------------------------------------------------
+static void add_to_list(struct thread_entry **list,
- * Switch thread in round robin fashion.
+                        struct thread_entry *thread)
- *---------------------------------------------------------------------------
- */
-void switch_thread(void)
 {
-#ifdef RB_PROFILE
+    if (*list == NULL)
-    profile_thread_stopped(cores[CURRENT_CORE].current_thread);
-#endif
-    int current;
-    unsigned int *stackptr;
-
-#ifdef SIMULATOR
-    /* Do nothing */
-#else
-    while (cores[CURRENT_CORE].num_sleepers == cores[CURRENT_CORE].num_threads)
     {
-        /* Enter sleep mode, woken up on interrupt */
+        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;
+         */
+    }
+}
+
+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 (;;)
+    {
+        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);
-#endif
-
-    if (++current >= cores[CURRENT_CORE].num_threads)
-        current = 0;
-
-    cores[CURRENT_CORE].current_thread = current;
-    load_context(&cores[CURRENT_CORE].threads[current].context);
 #ifdef RB_PROFILE
-    profile_thread_started(cores[CURRENT_CORE].current_thread);
+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
+
+/* 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
 }
 
-void sleep_thread(void)
+/*---------------------------------------------------------------------------
+ * Switch thread in round robin fashion.
+ *---------------------------------------------------------------------------
+ */
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
 {
-    ++cores[CURRENT_CORE].num_sleepers;
+#ifdef RB_PROFILE
-    switch_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 wake_up_thread(void)
+void sleep_thread(int ticks)
 {
-    cores[CURRENT_CORE].num_sleepers = 0;
+    /* 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 block_thread(struct thread_entry **list, int timeout)
+{
+    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,
+struct thread_entry* 
-                  const char *name)
+    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,17 +590,27 @@ 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,
+struct thread_entry* 
-                  const char *name)
+    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)
+    for (n = 0; n < MAXTHREADS; n++)
-        return -1;
+    {
+        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);
@@ -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 thread;
-    return cores[core].num_threads++; /* return the current ID, e.g for remove_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. */
- * Remove a thread on the specified core from the scheduler.
+    thread->name = NULL;
- * Parameters are the core and the ID as returned from create_thread().
+#ifdef HAVE_PRIORITY_SCHEDULING
- *---------------------------------------------------------------------------
+    highest_priority = 100;
- */
+#endif
-void remove_thread_on_core(unsigned int core, int threadnum)
-{
-    int i;
     
-    if (threadnum >= cores[core].num_threads)
+    if (thread == cores[CURRENT_CORE].running)
-       return;
+    {
-
+        remove_from_list(&cores[CURRENT_CORE].running, thread);
-    cores[core].num_threads--;
+        switch_thread(false, NULL);
-    for (i=threadnum; i<cores[core].num_threads-1; i++)
+        return ;
-    {   /* 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? */
+    if (thread == cores[CURRENT_CORE].sleeping)
-        cores[core].current_thread = cores[core].num_threads; /* set beyond last, avoid store harm */
+        remove_from_list(&cores[CURRENT_CORE].sleeping, thread);
-    else if (cores[core].current_thread > threadnum) /* within the moved positions? */
+    
-        cores[core].current_thread--; /* adjust it, point to same context again */
+    remove_from_list(NULL, thread);
 }
 
+#ifdef HAVE_PRIORITY_SCHEDULING
+void thread_set_priority(struct thread_entry *thread, int priority)
+{
+    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 */
+    memset(cores, 0, sizeof cores);
-    cores[core].current_thread = 0; /* The current thread is number 0 */
+    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
+    cores[core].threads[0].statearg = 0;
-   a much better way to do this. */
+#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)
+int thread_stack_usage(const struct thread_entry *thread)
-{
-    return thread_stack_usage_on_core(CURRENT_CORE, threadnum);
-}
-
-int thread_stack_usage_on_core(unsigned int core, int threadnum)
 {
     unsigned int i;
-    unsigned int *stackptr = cores[core].threads[threadnum].stack;
+    unsigned int *stackptr = thread->stack;
 
-    if (threadnum >= cores[core].num_threads)
+    for (i = 0;i < thread->stack_size/sizeof(int);i++)
-        return -1;
-
-    for (i = 0;i < cores[core].threads[threadnum].stack_size/sizeof(int);i++)
     {
         if (stackptr[i] != DEADBEEF)
             break;
     }
 
-    return ((cores[core].threads[threadnum].stack_size - i * sizeof(int)) * 100) /
+    return ((thread->stack_size - i * sizeof(int)) * 100) /
-        cores[core].threads[threadnum].stack_size;
+        thread->stack_size;
+}
+
+int thread_get_status(const struct thread_entry *thread)
+{
+    return GET_STATE(thread->statearg);
 }

firmware/usb.c

@@ -558,8 +558,9 @@ void usb_init(void)
     last_usb_status = false;
 
 #ifndef BOOTLOADER
-    queue_init(&usb_queue);
+    queue_init(&usb_queue, true);
-    create_thread(usb_thread, usb_stack, sizeof(usb_stack), usb_thread_name);
+    create_thread(usb_thread, usb_stack, sizeof(usb_stack), 
+                  usb_thread_name IF_PRIO(, PRIORITY_SYSTEM));
 
     tick_add_task(usb_tick);
 #endif

uisimulator/sdl/kernel.c

@@ -32,8 +32,10 @@ int set_irq_level (int level)
     return (_lv = level);
 }
 
-void queue_init(struct event_queue *q)
+void queue_init(struct event_queue *q, bool register_queue)
 {
+    (void)register_queue;
+    
     q->read = 0;
     q->write = 0;
 }
@@ -47,7 +49,7 @@ void queue_wait(struct event_queue *q, struct event *ev)
 {
     while(q->read == q->write)
     {
-        switch_thread();
+        switch_thread(true, NULL);
     }
 
     *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
@@ -97,8 +99,11 @@ void queue_clear(struct event_queue* q)
     q->write = 0;
 }
 
-void switch_thread (void)
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
 {
+    (void)save_context;
+    (void)blocked_list;
+    
     yield ();
 }
 
@@ -160,7 +165,7 @@ void mutex_init(struct mutex *m)
 void mutex_lock(struct mutex *m)
 {
     while(m->locked)
-        switch_thread();
+        switch_thread(true, NULL);
     m->locked = true;
 }