diff --git a/os/common/ports/ARMCAx-TZ/chcore.h b/os/common/ports/ARMCAx-TZ/chcore.h
index ba49735a2..d31f0f736 100644
--- a/os/common/ports/ARMCAx-TZ/chcore.h
+++ b/os/common/ports/ARMCAx-TZ/chcore.h
@@ -433,7 +433,7 @@ static inline bool port_is_isr_context(void) {
 
 /**
  * @brief   Kernel-lock action.
- * @details In this port it disables the FIQ and IRQ sources.
+ * @details In this port it disables the FIQ and keep IRQ state.
  */
 static inline void port_lock(void) {
 
@@ -445,11 +445,7 @@ static inline void port_lock(void) {
  * @details In this port it enables the FIQ and IRQ sources.
  */
 static inline void port_unlock(void) {
-extern thread_reference_t _ns_thread;
-  if (_ns_thread)
-    __asm volatile ("cpsie   if" : : : "memory");
-  else
-    __asm volatile ("cpsie   f" : : : "memory");
+  __asm volatile ("cpsie   if" : : : "memory");
 }
 
 /**
diff --git a/os/common/ports/ARMCAx-TZ/chsmc.c b/os/common/ports/ARMCAx-TZ/chsmc.c
index 4a0a505a1..e03d9bda6 100644
--- a/os/common/ports/ARMCAx-TZ/chsmc.c
+++ b/os/common/ports/ARMCAx-TZ/chsmc.c
@@ -181,9 +181,16 @@ msg_t smcEntry(smc_service_t *svc_handle, smc_params_area_t svc_data, size_t svc
   svcp->svc_data = svc_data;
   svcp->svc_datalen = svc_datalen;
 
+#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
+  _dbg_check_lock();
+#endif
+
   chThdResumeS(&svcp->svct, MSG_OK);
   r = chThdSuspendS(&_ns_thread);
 
+#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
+  _dbg_check_unlock();
+#endif
   return r;
 }
 
diff --git a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
index 66d3021f0..33ad2ca0a 100644
--- a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
+++ b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
@@ -145,7 +145,7 @@ _monitor_vectors:
  * SMC entry
  */
 sm_call:
-                stmfd   sp!, {r3}
+                stmfd   sp!, {r3, r12}
                 ldr     r12, =MON_S_SCR                     // enter in the secure world
                 mcr     p15, 0, r12, c1, c1, 0
                 ands    r0, r0                              // OS special service,
@@ -162,18 +162,9 @@ sm_call:
                 sm_load_ooctx_regs r3
 
                 msr     CPSR_c, #MODE_SYS | I_BIT | F_BIT   // switch to sys mode
-                stmfd   sp!, {lr}                           // save lr
 
-#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
-                bl      _dbg_check_lock
-#endif
                 bl      smcEntry                            // call the C smc handler
 
-#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
-                bl      _dbg_check_unlock
-#endif
-                ldmfd   sp!, {lr}                      // restore lr
-
                 ldr     r3, =sm_secctx
                 sm_store_ooctx_regs r3
 
@@ -184,14 +175,14 @@ sm_call:
                 msr     CPSR_c, #MODE_MON | I_BIT | F_BIT   // switch to monitor mode
                 ldr     r12, =MON_NS_SCR                    // enter in the non-secure world
                 mcr     p15, 0, r12, c1, c1, 0
-                ldmfd   sp!, {r3}
+                ldmfd   sp!, {r3, r12}
                 subs    pc, lr, #0                          // return from smc
 1:
                 mov     lr, r1                              // use the address in r1 as return address
                                                             // in the non secure world
                 ldr     r12, =MON_NS_SCR                    // enter in the non-secure world
                 mcr     p15, 0, r12, c1, c1, 0
-                ldmfd   sp!, {r3}
+                ldmfd   sp!, {r3, r12}
                 subs    pc, lr, #0                          // return from smc
 
 /*
@@ -207,7 +198,7 @@ sm_call:
  */
 sm_fiq:
                                                             // check point: SCR.NS == 1
-                stmfd   sp!, {r0}
+                stmfd   sp!, {r0, r12}
                 ldr     r0, =MON_S_SCR                      // enter in the secure world
                 mcr     p15, 0, r0, c1, c1, 0
 
@@ -232,7 +223,7 @@ sm_fiq:
                 msr     CPSR_c, #MODE_MON | I_BIT | F_BIT   // switch to monitor mode
                 ldr     r0, =MON_NS_SCR                     // set non-secure SCR before return
                 mcr     p15, 0, r0, c1, c1, 0
-                ldmfd   sp!, {r0}
+                ldmfd   sp!, {r0, r12}
                 subs    pc, lr, #4                          // return into non-secure world
 /*
  * IRQ entry
@@ -258,11 +249,11 @@ sm_irq:
                 msr     CPSR_c, #MODE_SYS | I_BIT | F_BIT
                 stmfd   sp!, {r0, r1}                       // push R0=SPSR, R1=LR_MON.
                                                             // check point: ns_tread != 0
-                ldr     r0, =_ns_thread
-                mov     r1, #MSG_TIMEOUT
 #if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
                 bl      _dbg_check_lock
 #endif
+                ldr     r0, =_ns_thread
+                mov     r1, #MSG_TIMEOUT
                 bl      chThdResumeS                        // resume the ns_thread and serve the IRQ
                                                             // into non-secure world
 #if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)