diff --git a/libcpu/arm/cortex-m7/context_rvds.S b/libcpu/arm/cortex-m7/context_rvds.S
index 6caed09e38..d051f819c1 100644
--- a/libcpu/arm/cortex-m7/context_rvds.S
+++ b/libcpu/arm/cortex-m7/context_rvds.S
@@ -10,7 +10,6 @@
 ; * 2013-06-18     aozima       add restore MSP feature.
 ; * 2013-06-23     aozima       support lazy stack optimized.
 ; * 2018-07-24     aozima       enhancement hard fault exception handler.
-; * 2021-02-15     lizhirui     repair hard fault return bug
 ; */
 
 ;/**
@@ -236,6 +235,11 @@ MemManage_Handler
     ENDIF
     STMFD   r0!, {lr}               ; push exec_return register
 
+    TST     lr, #0x04               ; if(!EXC_RETURN[2])
+    ITE     EQ
+    MSREQ   msp, r0                 ; [2]=0 ==> Z=1, update stack pointer to MSP.
+    MSRNE   psp, r0                 ; [2]=1 ==> Z=0, update stack pointer to PSP.
+
     PUSH    {lr}
     BL      rt_hw_hard_fault_exception
     POP     {lr}