5 files changed, 20 insertions, 0 deletions

diff --git a/firmware/kernel/mrsw_lock.c b/firmware/kernel/mrsw_lock.c
index b683f63d5f..6120666d05 100644
--- a/firmware/kernel/mrsw_lock.c
+++ b/firmware/kernel/mrsw_lock.c
@@ -159,6 +159,8 @@ void mrsw_init(struct mrsw_lock *mrsw)
  * access recursively. The current writer is ignored and gets access. */
 void mrsw_read_acquire(struct mrsw_lock *mrsw)
 {
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT);
+
     struct thread_entry *current = __running_self_entry();
 
     if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 ))
@@ -268,6 +270,8 @@ void mrsw_read_release(struct mrsw_lock *mrsw)
  * safely call recursively. */
 void mrsw_write_acquire(struct mrsw_lock *mrsw)
 {
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT);
+
     struct thread_entry *current = __running_self_entry();
 
     if (current == mrsw->splay.blocker.thread)
diff --git a/firmware/kernel/mutex.c b/firmware/kernel/mutex.c
index cb9e6816b8..b1ae3e9e54 100644
--- a/firmware/kernel/mutex.c
+++ b/firmware/kernel/mutex.c
@@ -39,6 +39,8 @@ void mutex_init(struct mutex *m)
 /* Gain ownership of a mutex object or block until it becomes free */
 void mutex_lock(struct mutex *m)
 {
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT);
+
     struct thread_entry *current = __running_self_entry();
 
     if(current == m->blocker.thread)
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c
index 927e55274c..70dba46c0a 100644
--- a/firmware/kernel/queue.c
+++ b/firmware/kernel/queue.c
@@ -283,6 +283,9 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
 #endif
 
     oldlevel = disable_irq_save();
+
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
+
     corelock_lock(&q->cl);
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
@@ -335,6 +338,10 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 #endif
 
     oldlevel = disable_irq_save();
+
+    if (ticks != TIMEOUT_NOBLOCK)
+        ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
+
     corelock_lock(&q->cl);
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
@@ -421,6 +428,9 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
     unsigned int wr;
 
     oldlevel = disable_irq_save();
+
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
+
     corelock_lock(&q->cl);
 
     wr = q->write++ & QUEUE_LENGTH_MASK;
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c
index 5e9e46798f..6b58fa3d8a 100644
--- a/firmware/kernel/semaphore.c
+++ b/firmware/kernel/semaphore.c
@@ -57,6 +57,8 @@ int semaphore_wait(struct semaphore *s, int timeout)
     }
     else if(timeout != 0)
     {
+        ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
+
         /* too many waits - block until count is upped... */
         struct thread_entry *current = __running_self_entry();
 
diff --git a/firmware/kernel/thread.c b/firmware/kernel/thread.c
index ea76421389..29ab9db873 100644
--- a/firmware/kernel/thread.c
+++ b/firmware/kernel/thread.c
@@ -1234,6 +1234,8 @@ unsigned int create_thread(void (*function)(void),
  */
 void thread_wait(unsigned int thread_id)
 {
+    ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT);
+
     struct thread_entry *current = __running_self_entry();
     struct thread_entry *thread = __thread_id_entry(thread_id);