7 files changed, 54 insertions, 1 deletions

diff --git a/firmware/export/system.h b/firmware/export/system.h
index e83ee53d96..d33a35f6fa 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -204,6 +204,16 @@ enum {
 #include "bitswap.h"
 #include "rbendian.h"
 
+#ifndef ASSERT_CPU_MODE
+/* Very useful to have defined properly for your architecture */
+#define ASSERT_CPU_MODE(mode, rstatus...) \
+    ({ (mode); rstatus; })
+#endif
+
+#ifndef CPU_MODE_THREAD_CONTEXT
+#define CPU_MODE_THREAD_CONTEXT 0
+#endif
+
 #ifndef BIT_N
 #define BIT_N(n) (1U << (n))
 #endif
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);
 
diff --git a/firmware/target/arm/system-arm.h b/firmware/target/arm/system-arm.h
index 719ec82f1b..2d8c6f2c9f 100644
--- a/firmware/target/arm/system-arm.h
+++ b/firmware/target/arm/system-arm.h
@@ -76,9 +76,32 @@ void __div0(void);
 #define ints_enabled_checkval(val) \
     (((val) & IRQ_FIQ_STATUS) == 0)
 
+#define CPU_MODE_USER   0x10
+#define CPU_MODE_FIQ    0x11
+#define CPU_MODE_IRQ    0x12
+#define CPU_MODE_SVC    0x13
+#define CPU_MODE_ABT    0x17
+#define CPU_MODE_UNDEF  0x1b
+#define CPU_MODE_SYS    0x1f
+
 /* We run in SYS mode */
+#define CPU_MODE_THREAD_CONTEXT CPU_MODE_SYS
+
 #define is_thread_context() \
-    (get_processor_mode() == 0x1f)
+    (get_processor_mode() == CPU_MODE_THREAD_CONTEXT)
+
+/* Assert that the processor is in the desired execution mode
+ * mode:       Processor mode value to test for
+ * rstatus...: Provide if you already have the value saved, otherwise leave
+ *             blank to get it automatically.
+ */
+#define ASSERT_CPU_MODE(mode, rstatus...) \
+    ({ unsigned long __massert = (mode);                         \
+       unsigned long __mproc = *#rstatus ?                       \
+            ((rstatus +0) & 0x1f) : get_processor_mode();        \
+       if (__mproc != __massert)                                 \
+           panicf("Incorrect CPU mode in %s (0x%02lx!=0x%02lx)", \
+                  __func__, __mproc, __massert); })
 
 /* Core-level interrupt masking */