diff --git a/os/ports/GCC/ARMCMx/chcore_v6m.c b/os/ports/GCC/ARMCMx/chcore_v6m.c
index e35e3a2d9..9a5da632a 100644
--- a/os/ports/GCC/ARMCMx/chcore_v6m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v6m.c
@@ -96,7 +96,8 @@ __attribute__((naked))
 #endif
 void _port_switch_from_isr(void) {
 
-  chSchDoRescheduleI();
+  if (chSchIsRescRequiredExI())
+    chSchDoRescheduleI();
 #if CORTEX_ALTERNATE_SWITCH
   SCB_ICSR = ICSR_PENDSVSET;
   port_unlock();
@@ -159,22 +160,18 @@ void _port_switch(Thread *ntp, Thread *otp) {
 void _port_irq_epilogue(regarm_t lr) {
 
   if (lr != (regarm_t)0xFFFFFFF1) {
-    port_lock_from_isr();
-    if (chSchIsRescRequiredExI()) {
-      register struct extctx *ctxp;
+    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();
+    port_lock_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.*/
   }
 }
 
diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index aa2c5bd0f..cd8cc9055 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -128,12 +128,12 @@ void PendSVVector(void) {
 #endif /* CORTEX_SIMPLIFIED_PRIORITY */
 
 /**
- * @brief   Reschedule verification and setup after an IRQ.
+ * @brief   Exception exit redirection to _port_switch_from_isr().
  */
 void _port_irq_epilogue(void) {
 
   port_lock_from_isr();
-  if ((SCB_ICSR & ICSR_RETTOBASE) && chSchIsRescRequiredExI()) {
+  if ((SCB_ICSR & ICSR_RETTOBASE)) {
     register struct extctx *ctxp;
 
     /* Adding an artificial exception return context, there is no need to
@@ -147,7 +147,6 @@ void _port_irq_epilogue(void) {
        order to keep the rest of the context switching atomic.*/
     return;
   }
-  /* ISR exit without context switching.*/
   port_unlock_from_isr();
 }
 
@@ -160,7 +159,8 @@ __attribute__((naked))
 #endif
 void _port_switch_from_isr(void) {
 
-  chSchDoRescheduleI();
+  if (chSchIsRescRequiredExI())
+    chSchDoRescheduleI();
 #if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__)
   asm volatile ("svc     #0");
 #else /* CORTEX_SIMPLIFIED_PRIORITY */
diff --git a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
index 33aafafc1..311e20030 100644
--- a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
@@ -116,7 +116,11 @@ PendSVVector:
  */
         PUBLIC  _port_switch_from_isr
 _port_switch_from_isr:
+        bl      chSchIsRescRequiredExI
+        cmp     r0, #0
+        beq     noresch
         bl      chSchDoRescheduleI
+noresch:
         ldr     r2, =SCB_ICSR
         movs    r3, #128
 #if CORTEX_ALTERNATE_SWITCH
@@ -135,25 +139,19 @@ waithere:
  */
         PUBLIC  _port_irq_epilogue
 _port_irq_epilogue:
-        push    {r3, lr}
+        push    {lr}
         adds    r0, r0, #15
-        beq     stillnested
+        beq     skipexit
         cpsid   i
-        bl      chSchIsRescRequiredExI
-        cmp     r0, #0
-        bne     doresch
-        cpsie   i
-stillnested
-        pop     {r3, pc}
-doresch
         mrs     r3, PSP
         subs    r3, r3, #32
         msr     PSP, r3
         ldr     r2, =_port_switch_from_isr
         str     r2, [r3, #24]
         movs    r2, #128
-        lsls     r2, r2, #17
+        lsls    r2, r2, #17
         str     r2, [r3, #28]
-        pop     {r3, pc}
+skipexit:
+        pop     {pc}
 
         END
diff --git a/os/ports/IAR/ARMCMx/chcoreasm_v7m.s b/os/ports/IAR/ARMCMx/chcoreasm_v7m.s
index b6eccaaa8..62c10c755 100644
--- a/os/ports/IAR/ARMCMx/chcoreasm_v7m.s
+++ b/os/ports/IAR/ARMCMx/chcoreasm_v7m.s
@@ -82,7 +82,10 @@ _port_thread_start:
  */
         PUBLIC  _port_switch_from_isr
 _port_switch_from_isr:
+        bl      chSchIsRescRequiredExI
+        cbz     r0, .L2
         bl      chSchDoRescheduleI
+.L2:
 #if CORTEX_SIMPLIFIED_PRIORITY
         mov     r3, #LWRD SCB_ICSR
         movt    r3, #HWRD SCB_ICSR
@@ -108,20 +111,16 @@ _port_irq_epilogue:
         mov     r3, #LWRD SCB_ICSR
         movt    r3, #HWRD SCB_ICSR
         ldr     r3, [r3, #0]
-        tst     r3, #ICSR_RETTOBASE
-        bne     .L7
+        ands    r3, r3, #ICSR_RETTOBASE
+        bne     .L8
 #if CORTEX_SIMPLIFIED_PRIORITY
         cpsie   i
 #else
-        movs    r3, #CORTEX_BASEPRI_DISABLED
+        /* Note, R3 is already zero.*/
         msr     BASEPRI, r3
 #endif
         bx      lr
-.L7:
-        push    {r3, lr}
-        bl      chSchIsRescRequiredExI
-        cmp     r0, #0
-        beq     .L4
+.L8:
         mrs     r3, PSP
         subs    r3, r3, #EXTCTX_SIZE
         msr     PSP, r3
@@ -129,15 +128,7 @@ _port_irq_epilogue:
         str     r2, [r3, #24]
         mov     r2, #0x01000000
         str     r2, [r3, #28]
-        pop     {r3, pc}
-.L4:
-#if CORTEX_SIMPLIFIED_PRIORITY
-        cpsie   i
-#else
-        movs    r3, #CORTEX_BASEPRI_DISABLED
-        msr     BASEPRI, r3
-#endif
-        pop     {r3, pc}
+        bx      lr
 
 /*
  * SVC vector.
diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
index 3ddfbbcf2..9c409d1cd 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
@@ -115,7 +115,11 @@ PendSVVector       PROC
  */
                 EXPORT  _port_switch_from_isr
 _port_switch_from_isr PROC
+                bl      chSchIsRescRequiredExI
+                cmp     r0, #0
+                beq     noresch
                 bl      chSchDoRescheduleI
+noresch
                 ldr     r2, =SCB_ICSR
                 movs    r3, #128
 #if CORTEX_ALTERNATE_SWITCH
@@ -134,26 +138,20 @@ waithere        b       waithere
  */
                 EXPORT  _port_irq_epilogue
 _port_irq_epilogue PROC
-                push    {r3, lr}
+                push    {lr}
                 adds    r0, r0, #15
-                beq     stillnested
+                beq     skipexit
                 cpsid   i
-                bl      chSchIsRescRequiredExI
-                cmp     r0, #0
-                bne     doresch
-                cpsie   i
-stillnested
-                pop     {r3, pc}
-doresch
                 mrs     r3, PSP
                 subs    r3, r3, #32
                 msr     PSP, r3
                 ldr     r2, =_port_switch_from_isr
                 str     r2, [r3, #24]
                 movs    r2, #128
-                lsls     r2, r2, #17
+                lsls    r2, r2, #17
                 str     r2, [r3, #28]
-                pop     {r3, pc}
+skipexit
+                pop     {pc}
                 ENDP
 
                 END
diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
index cf759c8e5..bd8947737 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
@@ -79,7 +79,10 @@ _port_thread_start PROC
  */
                 EXPORT  _port_switch_from_isr
 _port_switch_from_isr PROC
+                bl      chSchIsRescRequiredExI
+                cbz     r0, noreschedule
                 bl      chSchDoRescheduleI
+noreschedule
 #if CORTEX_SIMPLIFIED_PRIORITY
                 mov     r3, #SCB_ICSR :AND: 0xFFFF
                 movt    r3, #SCB_ICSR :SHR: 16
@@ -106,20 +109,16 @@ _port_irq_epilogue PROC
                 mov     r3, #SCB_ICSR :AND: 0xFFFF
                 movt    r3, #SCB_ICSR :SHR: 16
                 ldr     r3, [r3, #0]
-                tst     r3, #ICSR_RETTOBASE
+                ands    r3, r3, #ICSR_RETTOBASE
                 bne     skipexit
 #if CORTEX_SIMPLIFIED_PRIORITY
                 cpsie   i
 #else
-                movs    r3, #CORTEX_BASEPRI_DISABLED
+                /* Note, R3 is already zero.*/
                 msr     BASEPRI, r3
 #endif
                 bx      lr
 skipexit
-                push    {r3, lr}
-                bl      chSchIsRescRequiredExI
-                cmp     r0, #0
-                beq     noreschedule
                 mrs     r3, PSP
                 subs    r3, r3, #EXTCTX_SIZE
                 msr     PSP, r3
@@ -127,15 +126,7 @@ skipexit
                 str     r2, [r3, #24]
                 mov     r2, #0x01000000
                 str     r2, [r3, #28]
-                pop     {r3, pc}
-noreschedule
-#if CORTEX_SIMPLIFIED_PRIORITY
-                cpsie   i
-#else
-                movs    r3, #CORTEX_BASEPRI_DISABLED
-                msr     BASEPRI, r3
-#endif
-                pop     {r3, pc}
+                bx      lr
                 ENDP
 
 /*
@@ -152,6 +143,7 @@ SVCallVector    PROC
                 movs    r3, #CORTEX_BASEPRI_DISABLED
                 msr     BASEPRI, r3
                 bx      lr
+                nop
                 ENDP
 #endif
 
diff --git a/readme.txt b/readme.txt
index 710c24e92..78da1b1fc 100644
--- a/readme.txt
+++ b/readme.txt
@@ -69,6 +69,7 @@
 *****************************************************************************
 
 *** 2.2.6 ***
+- FIX: Fixed race condition in Cortex-Mx ports (bug 3317500).
 - FIX: Fixed wrong macro check in STM32 UART driver (bug 3311999).
 
 *** 2.2.5 ***