diff --git a/os/ports/GCC/ARMCMx/chcore_v6m.c b/os/ports/GCC/ARMCMx/chcore_v6m.c
index b5289bf72..3435d186f 100644
--- a/os/ports/GCC/ARMCMx/chcore_v6m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v6m.c
@@ -40,7 +40,7 @@
 regarm_t _port_saved_pc;
 
 /**
- * @brief   IRQ nesting counter.
+ * @brief   IRQ nesting counter.
  */
 unsigned _port_irq_nesting;
 
@@ -82,7 +82,7 @@ void _port_switch_from_isr(void) {
 
   chSchDoRescheduleI();
 
-  /* Note, the last registers are restored alone after re-enabling the
+  /* Note, the last register is restored alone after re-enabling the
      interrupts in order to minimize the (very remote and unlikely)
      possibility that the stack is filled by continuous and saturating
      interrupts that would not allow that last words to be pulled out of
@@ -91,8 +91,9 @@ void _port_switch_from_isr(void) {
                 "mov     r12, r1                                \n\t"
                 "msr     APSR, r0                               \n\t"
                 "mov     lr, r2                                 \n\t"
+                "pop     {r0, r1, r2, r3}                       \n\t"
                 "cpsie   i                                      \n\t"
-                "pop     {r0, r1, r2, r3, pc}" : : : "memory");
+                "pop     {pc}" : : : "memory");
 }
 
 #define PUSH_CONTEXT(sp) {                                                  \
diff --git a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
index 3a1ebda57..1591fb2de 100644
--- a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
@@ -103,8 +103,9 @@ _port_switch_from_isr:
         mov     r12, r1
         msr     APSR, r0
         mov     lr, r2
+        pop     {r0, r1, r2, r3}
         cpsie   i
-        pop     {r0, r1, r2, r3, pc}
+        pop     {pc}
 
 /*
  * Reschedule verification and setup after an IRQ.
diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
index 0a38a0a6e..af04a476e 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v6m.s
@@ -100,8 +100,9 @@ _port_switch_from_isr PROC
                 mov     r12, r1
                 msr     APSR, r0
                 mov     lr, r2
+                pop     {r0, r1, r2, r3}
                 cpsie   i
-                pop     {r0, r1, r2, r3, pc}
+                pop     {pc}
                 ENDP
 
 /*
diff --git a/readme.txt b/readme.txt
index 2121a490e..01c63a02c 100644
--- a/readme.txt
+++ b/readme.txt
@@ -69,6 +69,8 @@
 *****************************************************************************
 
 *** 2.2.1 ***
+- FIX: Stack overflow in CM0 ports when nearing interrupts saturation (bug
+  3187105).
 - FIX: Fixed missing e200z test report (bug 3182611).
 - FIX: Fixed error in _BSEMAPHORE_DATA macro (bug 3184139).
 - FIX: Error in MAC driver (bug 3179783).