diff --git a/os/common/ports/ARMv7-M-ALT/compilers/GCC/chcoreasm.S b/os/common/ports/ARMv7-M-ALT/compilers/GCC/chcoreasm.S
index 41d1db1fe..481af96e1 100644
--- a/os/common/ports/ARMv7-M-ALT/compilers/GCC/chcoreasm.S
+++ b/os/common/ports/ARMv7-M-ALT/compilers/GCC/chcoreasm.S
@@ -219,7 +219,10 @@ frompriv:
                 ands    r3, #255
                 beq     ctxswitch
                 /* Called for non-privileged mode change.*/
-
+                msr     PSP, r0             /* Discarding SVC context.      */
+                orr     r2, #1
+                msr     CONTROL, r2         /* Switching to non-privileged. */
+                bx      lr
 
 ctxswitch:
                 /* Called for context switch.*/