8 files changed, 158 insertions, 90 deletions

diff --git a/firmware/backlight.c b/firmware/backlight.c
index 07cc9532be..66cc6df569 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -130,7 +130,7 @@ static long backlight_stack[DEFAULT_STACK_SIZE/sizeof(long)];
 static const char backlight_thread_name[] = "backlight";
 static struct event_queue backlight_queue;
 #ifdef BACKLIGHT_DRIVER_CLOSE
-static struct thread_entry *backlight_thread_p = NULL;
+static unsigned int backlight_thread_id = 0;
 #endif
 
 static int backlight_timer SHAREDBSS_ATTR;
@@ -744,7 +744,7 @@ void backlight_init(void)
      * call the appropriate backlight_set_*() functions, only changing light
      * status if necessary. */
 #ifdef BACKLIGHT_DRIVER_CLOSE
-    backlight_thread_p =
+    backlight_thread_id =
 #endif
     create_thread(backlight_thread, backlight_stack,
                   sizeof(backlight_stack), 0, backlight_thread_name
@@ -756,13 +756,13 @@ void backlight_init(void)
 #ifdef BACKLIGHT_DRIVER_CLOSE
 void backlight_close(void)
 {
-    struct thread_entry *thread = backlight_thread_p;
+    unsigned int thread = backlight_thread_id;
 
     /* Wait for thread to exit */
-    if (thread == NULL)
+    if (thread == 0)
         return;
 
-    backlight_thread_p = NULL;
+    backlight_thread_id = 0;
 
     queue_post(&backlight_queue, BACKLIGHT_QUIT, 0);
     thread_wait(thread);
diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 00a7c3e19a..e3fa3e8958 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -71,7 +71,7 @@
 #endif
 
 #ifdef ATA_DRIVER_CLOSE
-static struct thread_entry *ata_thread_p = NULL;
+static unsigned int ata_thread_id = 0;
 #endif
 
 #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64
@@ -94,7 +94,8 @@ static void ata_lock_init(struct ata_lock *l)
 
 static void ata_lock_lock(struct ata_lock *l)
 {
-    struct thread_entry * const current = thread_get_current();
+    struct thread_entry * const current =
+        thread_id_entry(THREAD_ID_CURRENT);
 
     if (current == l->thread)
     {
@@ -1350,7 +1351,7 @@ int ata_init(void)
 
         last_disk_activity = current_tick;
 #ifdef ATA_DRIVER_CLOSE
-        ata_thread_p =
+        ata_thread_id =
 #endif
         create_thread(ata_thread, ata_stack,
                       sizeof(ata_stack), 0, ata_thread_name
@@ -1370,15 +1371,15 @@ int ata_init(void)
 #ifdef ATA_DRIVER_CLOSE
 void ata_close(void)
 {
-    struct thread_entry *thread = ata_thread_p;
+    unsigned int thread_id = ata_thread_id;
     
-    if (thread == NULL)
+    if (thread_id == 0)
         return;
 
-    ata_thread_p = NULL;
+    ata_thread_id = 0;
 
     queue_post(&ata_queue, Q_CLOSE, 0);
-    thread_wait(thread);
+    thread_wait(thread_id);
 }
 #endif /* ATA_DRIVER_CLOSE */
 
diff --git a/firmware/export/kernel.h b/firmware/export/kernel.h
index beba58eb21..ef65463e5d 100644
--- a/firmware/export/kernel.h
+++ b/firmware/export/kernel.h
@@ -261,7 +261,7 @@ extern void queue_post(struct event_queue *q, long id, intptr_t data);
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 extern void queue_enable_queue_send(struct event_queue *q,
                                     struct queue_sender_list *send,
-                                    struct thread_entry *owner);
+                                    unsigned int owner_id);
 extern intptr_t queue_send(struct event_queue *q, long id, intptr_t data);
 extern void queue_reply(struct event_queue *q, intptr_t retval);
 extern bool queue_in_queue_send(struct event_queue *q);
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index c4dfbf4ed3..4c1e952347 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -58,9 +58,6 @@
 #define NUM_PRIORITIES           32
 #define PRIORITY_IDLE            32  /* Priority representative of no tasks */
 
-/* TODO: Only a minor tweak to create_thread would be needed to let
- * thread slots be caller allocated - no essential threading functionality
- * depends upon an array */
 #if CONFIG_CODEC == SWCODEC
 
 #ifdef HAVE_RECORDING
@@ -280,6 +277,7 @@ struct thread_entry
     int skip_count;            /* Number of times skipped if higher priority
                                   thread was running */
 #endif
+    uint16_t id;               /* Current slot id */
     unsigned short stack_size; /* Size of stack in bytes */
 #ifdef HAVE_PRIORITY_SCHEDULING
     unsigned char base_priority; /* Base priority (set explicitly during
@@ -298,6 +296,16 @@ struct thread_entry
 #endif
 };
 
+/*** Macros for internal use ***/
+/* Thread ID, 16 bits = |VVVVVVVV|SSSSSSSS| */
+#define THREAD_ID_VERSION_SHIFT      8
+#define THREAD_ID_VERSION_MASK  0xff00
+#define THREAD_ID_SLOT_MASK     0x00ff
+#define THREAD_ID_INIT(n)       ((1u << THREAD_ID_VERSION_SHIFT) | (n))
+
+/* Specify current thread in a function taking an ID. */
+#define THREAD_ID_CURRENT ((unsigned int)-1)
+
 #if NUM_CORES > 1
 /* Operations to be performed just before stopping a thread and starting
    a new one if specified before calling switch_thread */
@@ -475,11 +483,11 @@ void init_threads(void);
 
 /* Allocate a thread in the scheduler */
 #define CREATE_THREAD_FROZEN   0x00000001 /* Thread is frozen at create time */
-struct thread_entry*
-    create_thread(void (*function)(void), void* stack, size_t stack_size,
-                  unsigned flags, const char *name
-                  IF_PRIO(, int priority)
-                          IF_COP(, unsigned int core));
+unsigned int create_thread(void (*function)(void),
+                           void* stack, size_t stack_size,
+                           unsigned flags, const char *name
+                           IF_PRIO(, int priority)
+                           IF_COP(, unsigned int core));
 
 /* Set and clear the CPU frequency boost flag for the calling thread */
 #ifdef HAVE_SCHEDULER_BOOSTCTRL
@@ -489,17 +497,19 @@ void cancel_cpu_boost(void);
 #define trigger_cpu_boost()
 #define cancel_cpu_boost()
 #endif
+/* Return thread entry from id */
+struct thread_entry *thread_id_entry(unsigned int thread_id);
 /* Make a frozed thread runnable (when started with CREATE_THREAD_FROZEN).
  * Has no effect on a thread not frozen. */
-void thread_thaw(struct thread_entry *thread);
+void thread_thaw(unsigned int thread_id);
 /* Wait for a thread to exit */
-void thread_wait(struct thread_entry *thread);
+void thread_wait(unsigned int thread_id);
 /* Exit the current thread */
 void thread_exit(void);
 #if defined(DEBUG) || defined(ROCKBOX_HAS_LOGF)
 #define ALLOW_REMOVE_THREAD
 /* Remove a thread from the scheduler */
-void remove_thread(struct thread_entry *thread);
+void remove_thread(unsigned int thread_id);
 #endif
 
 /* Switch to next runnable thread */
@@ -526,13 +536,13 @@ unsigned int thread_queue_wake(struct thread_entry **list);
 unsigned int wakeup_thread(struct thread_entry **list);
 
 #ifdef HAVE_PRIORITY_SCHEDULING
-int thread_set_priority(struct thread_entry *thread, int priority);
-int thread_get_priority(struct thread_entry *thread);
+int thread_set_priority(unsigned int thread_id, int priority);
+int thread_get_priority(unsigned int thread_id);
 #endif /* HAVE_PRIORITY_SCHEDULING */
 #if NUM_CORES > 1
 unsigned int switch_core(unsigned int new_core);
 #endif
-struct thread_entry * thread_get_current(void);
+unsigned int thread_get_current(void);
 
 /* Debugging info - only! */
 int thread_stack_usage(const struct thread_entry *thread);
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 920893818a..553f6721a1 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -352,11 +352,12 @@ static void queue_remove_sender_thread_cb(struct thread_entry *thread)
  * specified for priority inheritance to operate.
  *
  * Use of queue_wait(_w_tmo) by multiple threads on a queue using synchronous
- * messages results in an undefined order of message replies.
+ * messages results in an undefined order of message replies or possible default
+ * replies if two or more waits happen before a reply is done.
  */
 void queue_enable_queue_send(struct event_queue *q,
                              struct queue_sender_list *send,
-                             struct thread_entry *owner)
+                             unsigned int owner_id)
 {
     int oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
@@ -367,9 +368,11 @@ void queue_enable_queue_send(struct event_queue *q,
 #ifdef HAVE_PRIORITY_SCHEDULING
         send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
         send->blocker.priority = PRIORITY_IDLE;
-        send->blocker.thread = owner;
-        if(owner != NULL)
+        if(owner_id != 0)
+        {
+            send->blocker.thread = thread_id_entry(owner_id);
             q->blocker_p = &send->blocker;
+        }
 #endif
         q->send = send;
     }
@@ -377,7 +380,7 @@ void queue_enable_queue_send(struct event_queue *q,
     corelock_unlock(&q->cl);
     restore_irq(oldlevel);
 
-    (void)owner;
+    (void)owner_id;
 }
 
 /* Unblock a blocked thread at a given event index */
@@ -532,7 +535,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
 
 #ifdef HAVE_PRIORITY_SCHEDULING
     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
-                  QUEUE_GET_THREAD(q) == thread_get_current(),
+                  QUEUE_GET_THREAD(q) == cores[CURRENT_CORE].running,
                   "queue_wait->wrong thread\n");
 #endif
 
@@ -579,7 +582,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
-                  QUEUE_GET_THREAD(q) == thread_get_current(),
+                  QUEUE_GET_THREAD(q) == cores[CURRENT_CORE].running,
                   "queue_wait_w_tmo->wrong thread\n");
 #endif
 
@@ -914,10 +917,10 @@ void mutex_lock(struct mutex *m)
 void mutex_unlock(struct mutex *m)
 {
     /* unlocker not being the owner is an unlocking violation */
-    KERNEL_ASSERT(MUTEX_GET_THREAD(m) == thread_get_current(),
+    KERNEL_ASSERT(MUTEX_GET_THREAD(m) == cores[CURRENT_CORE].running,
                   "mutex_unlock->wrong thread (%s != %s)\n",
                   MUTEX_GET_THREAD(m)->name,
-                  thread_get_current()->name);
+                  cores[CURRENT_CORE].running->name);
 
     if(m->count > 0)
     {
@@ -990,7 +993,7 @@ void spinlock_lock(struct spinlock *l)
 void spinlock_unlock(struct spinlock *l)
 {
     /* unlocker not being the owner is an unlocking violation */
-    KERNEL_ASSERT(l->thread == thread_get_current(),
+    KERNEL_ASSERT(l->thread == cores[CURRENT_CORE].running,
                   "spinlock_unlock->wrong thread\n");
 
     if(l->count > 0)
diff --git a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
index c185994bfc..81849d0852 100644
--- a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
@@ -69,7 +69,7 @@ static const unsigned char pmic_ints_regs[2] =
 
 #ifdef PMIC_DRIVER_CLOSE
 static bool pmic_close = false;
-static struct thread_entry *mc13783_thread_p = NULL;
+static unsigned int mc13783_thread_id = 0;
 #endif
 
 static void mc13783_interrupt_thread(void)
@@ -149,7 +149,7 @@ void mc13783_init(void)
     MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
 
 #ifdef PMIC_DRIVER_CLOSE
-    mc13783_thread_p =
+    mc13783_thread_id =
 #endif
         create_thread(mc13783_interrupt_thread,
             mc13783_thread_stack, sizeof(mc13783_thread_stack), 0,
@@ -159,16 +159,16 @@ void mc13783_init(void)
 #ifdef PMIC_DRIVER_CLOSE
 void mc13783_close(void)
 {
-    struct thread_entry *thread = mc13783_thread_p;
+    unsigned int thread_id = mc13783_thread_p;
 
-    if (thread == NULL)
+    if (thread_id == 0)
         return;
 
-    mc13783_thread_p = NULL;
+    mc13783_thread_id = 0;
 
     pmic_close = true;
     wakeup_signal(&mc13783_wake);
-    thread_wait(thread);
+    thread_wait(thread_id);
 }
 #endif /* PMIC_DRIVER_CLOSE */
 
diff --git a/firmware/thread.c b/firmware/thread.c
index c500fc4818..377c3355b4 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -1691,8 +1691,8 @@ struct thread_entry *
     struct thread_entry *next;
     int bl_pr;
 
-    THREAD_ASSERT(thread_get_current() == bl_t,
-                  "UPPT->wrong thread", thread_get_current());
+    THREAD_ASSERT(cores[CURRENT_CORE].running == bl_t,
+                  "UPPT->wrong thread", cores[CURRENT_CORE].running);
 
     LOCK_THREAD(bl_t);
 
@@ -2031,7 +2031,7 @@ void switch_thread(void)
     }
 
 #ifdef RB_PROFILE
-    profile_thread_stopped(thread - threads);
+    profile_thread_stopped(thread->id & THREAD_ID_SLOT_MASK);
 #endif
 
     /* Begin task switching by saving our current context so that we can
@@ -2136,7 +2136,7 @@ void switch_thread(void)
     load_context(&thread->context);
 
 #ifdef RB_PROFILE
-    profile_thread_started(thread - threads);
+    profile_thread_started(thread->id & THREAD_ID_SLOT_MASK);
 #endif
 
 }
@@ -2316,6 +2316,24 @@ unsigned int thread_queue_wake(struct thread_entry **list)
 }
 
 /*---------------------------------------------------------------------------
+ * Assign the thread slot a new ID. Version is 1-255.
+ *---------------------------------------------------------------------------
+ */
+static void new_thread_id(unsigned int slot_num,
+                          struct thread_entry *thread)
+{
+    unsigned int version =
+        (thread->id + (1u << THREAD_ID_VERSION_SHIFT))
+                & THREAD_ID_VERSION_MASK;
+
+    /* If wrapped to 0, make it 1 */
+    if (version == 0)
+        version = 1u << THREAD_ID_VERSION_SHIFT;
+
+    thread->id = version | (slot_num & THREAD_ID_SLOT_MASK);
+}
+
+/*---------------------------------------------------------------------------
  * Find an empty thread slot or MAXTHREADS if none found. The slot returned
  * will be locked on multicore.
  *---------------------------------------------------------------------------
@@ -2349,6 +2367,17 @@ static struct thread_entry * find_empty_thread_slot(void)
     return thread;
 }
 
+/*---------------------------------------------------------------------------
+ * Return the thread_entry pointer for a thread_id. Return the current
+ * thread if the ID is 0 (alias for current).
+ *---------------------------------------------------------------------------
+ */
+struct thread_entry * thread_id_entry(unsigned int thread_id)
+{
+    return (thread_id == THREAD_ID_CURRENT) ?
+        cores[CURRENT_CORE].running :
+        &threads[thread_id & THREAD_ID_SLOT_MASK];
+}
 
 /*---------------------------------------------------------------------------
  * Place the current core in idle mode - woken up on interrupt or wake
@@ -2369,11 +2398,11 @@ void core_idle(void)
  * Return ID if context area could be allocated, else NULL.
  *---------------------------------------------------------------------------
  */
-struct thread_entry* 
-    create_thread(void (*function)(void), void* stack, size_t stack_size,
-                  unsigned flags, const char *name
-                  IF_PRIO(, int priority)
-                  IF_COP(, unsigned int core))
+unsigned int create_thread(void (*function)(void),
+                           void* stack, size_t stack_size,
+                           unsigned flags, const char *name
+                           IF_PRIO(, int priority)
+                           IF_COP(, unsigned int core))
 {
     unsigned int i;
     unsigned int stack_words;
@@ -2385,7 +2414,7 @@ struct thread_entry*
     thread = find_empty_thread_slot();
     if (thread == NULL)
     {
-        return NULL;
+        return 0;
     }
 
     oldlevel = disable_irq_save();
@@ -2443,15 +2472,15 @@ struct thread_entry*
     THREAD_STARTUP_INIT(core, thread, function);
 
     thread->state = state;
+    i = thread->id; /* Snapshot while locked */
 
     if (state == STATE_RUNNING)
         core_schedule_wakeup(thread);
 
     UNLOCK_THREAD(thread);
-
     restore_irq(oldlevel);
 
-    return thread;
+    return i;
 }
 
 #ifdef HAVE_SCHEDULER_BOOSTCTRL
@@ -2489,18 +2518,17 @@ void cancel_cpu_boost(void)
  * Parameter is the ID as returned from create_thread().
  *---------------------------------------------------------------------------
  */
-void thread_wait(struct thread_entry *thread)
+void thread_wait(unsigned int thread_id)
 {
     struct thread_entry *current = cores[CURRENT_CORE].running;
-
-    if (thread == NULL)
-        thread = current;
+    struct thread_entry *thread = thread_id_entry(thread_id);
 
     /* Lock thread-as-waitable-object lock */
     corelock_lock(&thread->waiter_cl);
 
     /* Be sure it hasn't been killed yet */
-    if (thread->state != STATE_KILLED)
+    if (thread_id == THREAD_ID_CURRENT ||
+        (thread->id == thread_id && thread->state != STATE_KILLED))
     {
         IF_COP( current->obj_cl = &thread->waiter_cl; )
         current->bqp = &thread->queue;
@@ -2538,9 +2566,10 @@ void thread_exit(void)
     if (current->name == THREAD_DESTRUCT)
     {
         /* Thread being killed - become a waiter */
+        unsigned int id = current->id;
         UNLOCK_THREAD(current);
         corelock_unlock(&current->waiter_cl);
-        thread_wait(current);
+        thread_wait(id);
         THREAD_PANICF("thread_exit->WK:*R", current);
     }
 #endif
@@ -2568,7 +2597,13 @@ void thread_exit(void)
     }
 
     flush_icache();
+
+    /* At this point, this thread isn't using resources allocated for
+     * execution except the slot itself. */
 #endif
+
+    /* Update ID for this slot */
+    new_thread_id(current->id, current);
     current->name = NULL;
 
     /* Signal this thread */
@@ -2593,7 +2628,7 @@ void thread_exit(void)
  * leave various objects in an undefined state.
  *---------------------------------------------------------------------------
  */
-void remove_thread(struct thread_entry *thread)
+void remove_thread(unsigned int thread_id)
 {
 #if NUM_CORES > 1
     /* core is not constant here because of core switching */
@@ -2604,13 +2639,11 @@ void remove_thread(struct thread_entry *thread)
     const unsigned int core = CURRENT_CORE;
 #endif
     struct thread_entry *current = cores[core].running;
+    struct thread_entry *thread = thread_id_entry(thread_id);
 
     unsigned state;
     int oldlevel;
 
-    if (thread == NULL)
-        thread = current;
-
     if (thread == current)
         thread_exit(); /* Current thread - do normal exit */
 
@@ -2621,10 +2654,8 @@ void remove_thread(struct thread_entry *thread)
 
     state = thread->state;
 
-    if (state == STATE_KILLED)
-    {
+    if (thread->id != thread_id || state == STATE_KILLED)
         goto thread_killed;
-    }
 
 #if NUM_CORES > 1
     if (thread->name == THREAD_DESTRUCT)
@@ -2633,7 +2664,7 @@ void remove_thread(struct thread_entry *thread)
         UNLOCK_THREAD(thread);
         corelock_unlock(&thread->waiter_cl);
         restore_irq(oldlevel);
-        thread_wait(thread);
+        thread_wait(thread_id);
         return;
     }
 
@@ -2741,6 +2772,7 @@ IF_COP( retry_state: )
     /* Otherwise thread is frozen and hasn't run yet */
     }
 
+    new_thread_id(thread_id, thread);
     thread->state = STATE_KILLED;
 
     /* If thread was waiting on itself, it will have been removed above.
@@ -2773,17 +2805,15 @@ thread_killed: /* Thread was already killed */
  * needed inheritance changes also may happen.
  *---------------------------------------------------------------------------
  */
-int thread_set_priority(struct thread_entry *thread, int priority)
+int thread_set_priority(unsigned int thread_id, int priority)
 {
     int old_base_priority = -1;
+    struct thread_entry *thread = thread_id_entry(thread_id);
 
     /* A little safety measure */
     if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
         return -1;
 
-    if (thread == NULL)
-        thread = cores[CURRENT_CORE].running;
-
     /* Thread could be on any list and therefore on an interrupt accessible
        one - disable interrupts */
     int oldlevel = disable_irq_save();
@@ -2791,7 +2821,8 @@ int thread_set_priority(struct thread_entry *thread, int priority)
     LOCK_THREAD(thread);
 
     /* Make sure it's not killed */
-    if (thread->state != STATE_KILLED)
+    if (thread_id == THREAD_ID_CURRENT ||
+        (thread->id == thread_id && thread->state != STATE_KILLED))
     {
         int old_priority = thread->priority;
 
@@ -2908,13 +2939,19 @@ int thread_set_priority(struct thread_entry *thread, int priority)
  * Returns the current base priority for a thread.
  *---------------------------------------------------------------------------
  */
-int thread_get_priority(struct thread_entry *thread)
+int thread_get_priority(unsigned int thread_id)
 {
-    /* Simple, quick probe. */
-    if (thread == NULL)
-        thread = cores[CURRENT_CORE].running;
+    struct thread_entry *thread = thread_id_entry(thread_id);
+    int base_priority = thread->base_priority;
 
-    return thread->base_priority;
+    /* Simply check without locking slot. It may or may not be valid by the
+     * time the function returns anyway. If all tests pass, it is the
+     * correct value for when it was valid. */
+    if (thread_id != THREAD_ID_CURRENT &&
+        (thread->id != thread_id || thread->state == STATE_KILLED))
+        base_priority = -1;
+
+    return base_priority;
 }
 #endif /* HAVE_PRIORITY_SCHEDULING */
 
@@ -2924,12 +2961,16 @@ int thread_get_priority(struct thread_entry *thread)
  * virtue of the slot having a state of STATE_FROZEN.
  *---------------------------------------------------------------------------
  */
-void thread_thaw(struct thread_entry *thread)
+void thread_thaw(unsigned int thread_id)
 {
+    struct thread_entry *thread = thread_id_entry(thread_id);
     int oldlevel = disable_irq_save();
+
     LOCK_THREAD(thread);
 
-    if (thread->state == STATE_FROZEN)
+    /* If thread is the current one, it cannot be frozen, therefore
+     * there is no need to check that. */
+    if (thread->id == thread_id && thread->state == STATE_FROZEN)
         core_schedule_wakeup(thread);
 
     UNLOCK_THREAD(thread);
@@ -2940,9 +2981,9 @@ void thread_thaw(struct thread_entry *thread)
  * Return the ID of the currently executing thread.
  *---------------------------------------------------------------------------
  */
-struct thread_entry * thread_get_current(void)
+unsigned int thread_get_current(void)
 {
-    return cores[CURRENT_CORE].running;
+    return cores[CURRENT_CORE].running->id;
 }
 
 #if NUM_CORES > 1
@@ -2967,9 +3008,10 @@ unsigned int switch_core(unsigned int new_core)
     if (current->name == THREAD_DESTRUCT)
     {
         /* Thread being killed - deactivate and let process complete */
+        unsigned int id = current->id;
         UNLOCK_THREAD(current);
         restore_irq(oldlevel);
-        thread_wait(current);
+        thread_wait(id);
         /* Should never be reached */
         THREAD_PANICF("switch_core->D:*R", current);
     }
@@ -3034,6 +3076,19 @@ void init_threads(void)
     const unsigned int core = CURRENT_CORE;
     struct thread_entry *thread;
 
+    if (core == CPU)
+    {
+        /* Initialize core locks and IDs in all slots */
+        int n;
+        for (n = 0; n < MAXTHREADS; n++)
+        {
+            thread = &threads[n];
+            corelock_init(&thread->waiter_cl);
+            corelock_init(&thread->slot_cl);
+            thread->id = THREAD_ID_INIT(n);
+        }
+    }
+
     /* CPU will initialize first and then sleep */
     thread = find_empty_thread_slot();
 
@@ -3060,8 +3115,6 @@ void init_threads(void)
     thread->priority = PRIORITY_USER_INTERFACE;
     rtr_add_entry(core, PRIORITY_USER_INTERFACE);
 #endif
-    corelock_init(&thread->waiter_cl);
-    corelock_init(&thread->slot_cl);
 
     add_to_list_l(&cores[core].running, thread);
 
@@ -3070,6 +3123,7 @@ void init_threads(void)
         thread->stack = stackbegin;
         thread->stack_size = (uintptr_t)stackend - (uintptr_t)stackbegin;
 #if NUM_CORES > 1  /* This code path will not be run on single core targets */
+        /* Initialize all locking for the slots */
         /* Wait for other processors to finish their inits since create_thread
          * isn't safe to call until the kernel inits are done. The first
          * threads created in the system must of course be created by CPU. */
diff --git a/firmware/usb.c b/firmware/usb.c
index f9bfbc4dbf..2bff53e5d6 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -78,7 +78,7 @@ static int usb_mmc_countdown = 0;
 #ifdef USB_FULL_INIT
 static long usb_stack[(DEFAULT_STACK_SIZE + 0x800)/sizeof(long)];
 static const char usb_thread_name[] = "usb";
-static struct thread_entry *usb_thread_entry;
+static unsigned int usb_thread_entry = 0;
 #endif
 static struct event_queue usb_queue;
 static int last_usb_status;
@@ -539,10 +539,10 @@ void usb_start_monitoring(void)
 #ifdef USB_DRIVER_CLOSE
 void usb_close(void)
 {
-    struct thread_entry *thread = usb_thread_entry;
-    usb_thread_entry = NULL;
+    uintptr_t thread = usb_thread_entry;
+    usb_thread_entry = 0;
 
-    if (thread == NULL)
+    if (thread == 0)
         return;
 
     tick_remove_task(usb_tick);