2 files changed, 28 insertions, 2 deletions
diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
index cf67d82c2..83878805e 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
@@ -36,6 +36,10 @@ SCB_ICSR        EQU     0xE000ED04
                 IMPORT  chThdExit
                 IMPORT  chSchIsPreemptionRequired
                 IMPORT  chSchDoReschedule
+#if CH_DBG_SYSTEM_STATE_CHECK
+                IMPORT  dbg_check_unlock
+                IMPORT  dbg_check_lock
+#endif
 
 /*
  * Performs a context switch between two threads.
@@ -66,6 +70,9 @@ _port_switch    PROC
  */
                 EXPORT  _port_thread_start
 _port_thread_start PROC
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_unlock
+#endif
                 cpsie   i
                 mov     r0, r5
                 blx     r4
@@ -109,11 +116,17 @@ PendSVVector       PROC
  */
                 EXPORT  _port_switch_from_isr
 _port_switch_from_isr PROC
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_lock
+#endif
                 bl      chSchIsPreemptionRequired
                 cmp     r0, #0
-                beq     noresch
+                beq     noreschedule
                 bl      chSchDoReschedule
-noresch
+noreschedule
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_unlock
+#endif
                 ldr     r2, =SCB_ICSR
                 movs    r3, #128
 #if CORTEX_ALTERNATE_SWITCH
diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
index db2c747ca..6be194737 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
@@ -38,6 +38,10 @@ ICSR_PENDSVSET  EQU     0x10000000
                 IMPORT  chThdExit
                 IMPORT  chSchIsPreemptionRequired
                 IMPORT  chSchDoReschedule
+#if CH_DBG_SYSTEM_STATE_CHECK
+                IMPORT  dbg_check_unlock
+                IMPORT  dbg_check_lock
+#endif
 
 /*
  * Performs a context switch between two threads.
@@ -56,6 +60,9 @@ _port_switch    PROC
  */
                 EXPORT  _port_thread_start
 _port_thread_start PROC
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_unlock
+#endif
 #if CORTEX_SIMPLIFIED_PRIORITY
                 cpsie   i
 #else
@@ -73,10 +80,16 @@ _port_thread_start PROC
  */
                 EXPORT  _port_switch_from_isr
 _port_switch_from_isr PROC
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_lock
+#endif
                 bl      chSchIsPreemptionRequired
                 cbz     r0, noreschedule
                 bl      chSchDoReschedule
 noreschedule
+#if CH_DBG_SYSTEM_STATE_CHECK
+                bl      dbg_check_unlock
+#endif
 #if CORTEX_SIMPLIFIED_PRIORITY
                 mov     r3, #SCB_ICSR :AND: 0xFFFF
                 movt    r3, #SCB_ICSR :SHR: 16