diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S
index 908c95786d7..188482ce090 100644
--- a/libcpu/arm/cortex-m4/context_gcc.S
+++ b/libcpu/arm/cortex-m4/context_gcc.S
@@ -238,10 +238,23 @@ HardFault_Handler:
     MRS     r0, psp                 /* get fault context from thread. */
 _get_sp_done:
 
+#if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    TST     lr, #0x10               /* if(!EXC_RETURN[4]) */
+    IT      EQ
+    VSTMDBEQ r0!, {d8 - d15}        /* push FPU register s16~s31 */
+#endif
+
     STMFD   r0!, {r4 - r11}         /* push r4 - r11 register */
+
 #if defined (__VFP_FP__) && !defined(__SOFTFP__)
-    STMFD   r0!, {lr}               /* push dummy for flag */
+    MOV     r4, #0x00               /* flag = 0 */
+
+    TST     lr, #0x10               /* if(!EXC_RETURN[4]) */
+    IT      EQ
+    MOVEQ   r4, #0x01               /* flag = 1 */
+    STMFD   r0!, {r4}               /* push flag */
 #endif
+
     STMFD   r0!, {lr}               /* push exec_return register */
 
     TST     lr, #0x04               /* if(!EXC_RETURN[2]) */
diff --git a/libcpu/arm/cortex-m4/context_iar.S b/libcpu/arm/cortex-m4/context_iar.S
index fc87c82aa5a..c1c90b0c8f6 100644
--- a/libcpu/arm/cortex-m4/context_iar.S
+++ b/libcpu/arm/cortex-m4/context_iar.S
@@ -237,14 +237,28 @@ HardFault_Handler:
     MRS     r0, psp                 ; get fault context from thread.
 _get_sp_done
 
+#if defined ( __ARMVFP__ )
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    BNE     skip_push_fpu
+    VSTMDB  r0!, {d8 - d15}         ; push FPU register s16~s31
+skip_push_fpu
+#endif
+
     STMFD   r0!, {r4 - r11}         ; push r4 - r11 register
-    ;STMFD   r0!, {lr}               ; push exec_return register
+
 #if defined ( __ARMVFP__ )
-    SUB     r0, r0, #0x04           ; push dummy for flag
-    STR     lr, [r0]
+    MOV     r4, #0x00               ; flag = 0
+
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    BNE     push_flag
+    MOV     r4, #0x01               ; flag = 1
+push_flag
+    SUB     r0, r0, #0x04
+    STR     r4, [r0]                ; push flag
 #endif
+
     SUB     r0, r0, #0x04
-    STR     lr, [r0]
+    STR     lr, [r0]                ; push exec_return register
 
     TST     lr, #0x04               ; if(!EXC_RETURN[2])
     BEQ     _update_msp
diff --git a/libcpu/arm/cortex-m4/context_rvds.S b/libcpu/arm/cortex-m4/context_rvds.S
index 3f6e2e83d60..de7d71306bc 100644
--- a/libcpu/arm/cortex-m4/context_rvds.S
+++ b/libcpu/arm/cortex-m4/context_rvds.S
@@ -236,10 +236,21 @@ HardFault_Handler    PROC
     MRSEQ   r0, msp                 ; [2]=0 ==> Z=1, get fault context from handler.
     MRSNE   r0, psp                 ; [2]=1 ==> Z=0, get fault context from thread.
 
+    IF      {FPU} != "SoftVFP"
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    VSTMFDEQ r0!, {d8 - d15}        ; push FPU register s16~s31
+    ENDIF
+
     STMFD   r0!, {r4 - r11}         ; push r4 - r11 register
+
     IF      {FPU} != "SoftVFP"
-    STMFD   r0!, {lr}               ; push dummy for flag
+    MOV     r4, #0x00               ; flag = 0
+
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    MOVEQ   r4, #0x01               ; flag = 1
+    STMFD   r0!, {r4}               ; push flag
     ENDIF
+
     STMFD   r0!, {lr}               ; push exec_return register
 
     TST     lr, #0x04               ; if(!EXC_RETURN[2])