diff --git a/Ports/ARM-Cortex-M/ARMv7-M/ARM/os_cpu_a.asm b/Ports/ARM-Cortex-M/ARMv7-M/ARM/os_cpu_a.asm
index f4b838b..3510259 100644
--- a/Ports/ARM-Cortex-M/ARMv7-M/ARM/os_cpu_a.asm
+++ b/Ports/ARM-Cortex-M/ARMv7-M/ARM/os_cpu_a.asm
@@ -255,6 +255,11 @@ OSStartHighRdy
     LDMFD    SP!, {R0-R3}                                       ; Restore r0, r3
     LDMFD    SP!, {R12, LR}                                     ; Load R12 and LR
     LDMFD    SP!, {R1, R2}                                      ; Load PC and discard xPSR
+
+    IF {FPU} != "SoftVFP"
+    VLDMIA   SP!, {S0-S15, S16}					; Restore s0-s15 and discard FPSCR
+    ENDIF
+
     CPSIE    I
     BX       R1