1 files changed, 13 insertions, 16 deletions
diff --git a/os/ports/GCC/ARMCMx/chcore_v6m.c b/os/ports/GCC/ARMCMx/chcore_v6m.c
index 18082e86f..2183a9ded 100644
--- a/os/ports/GCC/ARMCMx/chcore_v6m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v6m.c
@@ -90,7 +90,8 @@ __attribute__((naked))
 #endif
 void _port_switch_from_isr(void) {
 
-  chSchDoRescheduleI();
+  if (chSchIsRescRequiredExI())
+    chSchDoRescheduleI();
 #if CORTEX_ALTERNATE_SWITCH
   SCB_ICSR = ICSR_PENDSVSET;
   port_unlock();
@@ -153,22 +154,18 @@ void _port_switch(Thread *ntp, Thread *otp) {
 void _port_irq_epilogue(regarm_t lr) {
 
   if (lr != (regarm_t)0xFFFFFFF1) {
+    register struct extctx *ctxp;
+
     port_lock_from_isr();
-    if (chSchIsRescRequiredExI()) {
-      register struct extctx *ctxp;
-
-      /* Adding an artificial exception return context, there is no need to
-         populate it fully.*/
-      asm volatile ("mrs     %0, PSP" : "=r" (ctxp) : : "memory");
-      ctxp--;
-      asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");
-      ctxp->pc = _port_switch_from_isr;
-      ctxp->xpsr = (regarm_t)0x01000000;
-      /* Note, returning without unlocking is intentional, this is done in
-        order to keep the rest of the context switching atomic.*/
-      return;
-    }
-    port_unlock_from_isr();
+    /* Adding an artificial exception return context, there is no need to
+       populate it fully.*/
+    asm volatile ("mrs     %0, PSP" : "=r" (ctxp) : : "memory");
+    ctxp--;
+    asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");
+    ctxp->pc = _port_switch_from_isr;
+    ctxp->xpsr = (regarm_t)0x01000000;
+    /* Note, returning without unlocking is intentional, this is done in
+      order to keep the rest of the context switching atomic.*/
   }
 }