diff --git a/libcpu/arm/stm32/context_gcc.S b/libcpu/arm/stm32/context_gcc.S
index 9fe6c23882ae982ec0186c79ce7e262a1f9eb537..1fbcfd72018ade556f303a7799db585fd2aa0808 100644
--- a/libcpu/arm/stm32/context_gcc.S
+++ b/libcpu/arm/stm32/context_gcc.S
@@ -35,7 +35,7 @@
 .type rt_hw_interrupt_disable, %function
 rt_hw_interrupt_disable:
 	MRS		r0, PRIMASK
-	CPSID		I
+	CPSID	I
 	BX		LR
 
 /*
@@ -60,21 +60,21 @@ rt_hw_interrupt_enable:
 rt_hw_context_switch_interrupt:
 rt_hw_context_switch:
 	/* set rt_thread_switch_interrput_flag to 1 */
-	LDR 		r2, =rt_thread_switch_interrput_flag
-	LDR 		r3, [r2]
-	CMP 		r3, #1
-	BEQ 		_reswitch
-	MOV 		r3, #1
-	STR 		r3, [r2]
+	LDR 	r2, =rt_thread_switch_interrput_flag
+	LDR 	r3, [r2]
+	CMP 	r3, #1
+	BEQ 	_reswitch
+	MOV 	r3, #1
+	STR 	r3, [r2]
 
-	LDR 		r2, =rt_interrupt_from_thread	/* set rt_interrupt_from_thread */
-	STR 		r0, [r2]
+	LDR 	r2, =rt_interrupt_from_thread	/* set rt_interrupt_from_thread */
+	STR 	r0, [r2]
 
 _reswitch:
-	LDR 		r2, =rt_interrupt_to_thread	/* set rt_interrupt_to_thread */
-	STR 		r1, [r2]
+	LDR 	r2, =rt_interrupt_to_thread		/* set rt_interrupt_to_thread */
+	STR 	r1, [r2]
 
-	LDR		r0, =NVIC_INT_CTRL      	/* trigger the PendSV exception (causes context switch) */
+	LDR		r0, =NVIC_INT_CTRL      		/* trigger the PendSV exception (causes context switch) */
 	LDR		r1, =NVIC_PENDSVSET
 	STR		r1, [r0]
 	BX		LR
@@ -88,10 +88,14 @@ _reswitch:
 rt_hw_pend_sv:
 	/* disable interrupt to protect context switch */
 	MRS		r2, PRIMASK
-	CPSID		I
+	CPSID	I
 
-	/* clear rt_thread_switch_interrput_flag to 0 */
+	/* get rt_thread_switch_interrupt_flag */
 	LDR		r0, =rt_thread_switch_interrput_flag
+	LDR		r1, [r0]
+	CBZ		r1, pendsv_exit			/* pendsv already handled */
+
+	/* clear rt_thread_switch_interrput_flag to 0 */
 	MOV		r1, #0x00
 	STR		r1, [r0]
 
@@ -99,19 +103,20 @@ rt_hw_pend_sv:
 	LDR		r1, [r0]
 	CBZ		r1, swtich_to_thread    /* skip register save at the first time */
 
-	MRS		r1, psp			/* get from thread stack pointer */
-	STMFD		r1!, {r4 - r11}		/* push r4 - r11 register */
+	MRS		r1, psp					/* get from thread stack pointer */
+	STMFD	r1!, {r4 - r11}			/* push r4 - r11 register */
 	LDR		r0, [r0]
-	STR		r1, [r0]		/* update from thread stack pointer */
+	STR		r1, [r0]				/* update from thread stack pointer */
 
 swtich_to_thread:
 	LDR		r1, =rt_interrupt_to_thread
 	LDR		r1, [r1]
-	LDR		r1, [r1]		/* load thread stack pointer */
+	LDR		r1, [r1]				/* load thread stack pointer */
 
-	LDMFD		r1!, {r4 - r11}		/* pop r4 - r11 register */
-	MSR		psp, r1			/* update stack pointer */
+	LDMFD	r1!, {r4 - r11}			/* pop r4 - r11 register */
+	MSR		psp, r1					/* update stack pointer */
 
+pendsv_exit:
 	/* restore interrupt */
 	MSR		PRIMASK, r2
 
@@ -133,6 +138,11 @@ rt_hw_context_switch_to:
 	MOV		r0, #0x0
 	STR		r0, [r1]
 
+	/* set interrupt flag to 1 */
+	LDR 	r1, =rt_thread_switch_interrput_flag
+	MOV 	r0, #1
+	STR 	r0, [r1]
+
 	/* set the PendSV exception priority */
 	LDR		r0, =NVIC_SYSPRI2
 	LDR		r1, =NVIC_PENDSV_PRI
@@ -142,7 +152,7 @@ rt_hw_context_switch_to:
 	LDR		r1, =NVIC_PENDSVSET
 	STR		r1, [r0]
 	
-	CPSIE		I                       /* enable interrupts at processor level */
+	CPSIE	I                       /* enable interrupts at processor level */
 	
 	/* never reach here! */
 
diff --git a/libcpu/arm/stm32/context_iar.S b/libcpu/arm/stm32/context_iar.S
index f62d3c36dde5fee7f634db24ac6fd4fa376e6866..c6f8660bd39d158ba143fd6f2b650fffcb4d8dd4 100644
--- a/libcpu/arm/stm32/context_iar.S
+++ b/libcpu/arm/stm32/context_iar.S
@@ -88,8 +88,12 @@ rt_hw_pend_sv:
 	MRS		r2, PRIMASK
 	CPSID   I
 
-	; clear rt_thread_switch_interrput_flag to 0
+	; get rt_thread_switch_interrupt_flag
 	LDR		r0, =rt_thread_switch_interrput_flag
+	LDR		r1, [r0]
+	CBZ		r1, pendsv_exit			; pendsv already handled
+
+	; clear rt_thread_switch_interrput_flag to 0
 	MOV		r1, #0x00
 	STR		r1, [r0]
 
@@ -110,6 +114,7 @@ swtich_to_thread
 	LDMFD	r1!, {r4 - r11}			; pop r4 - r11 register
 	MSR		psp, r1					; update stack pointer
 
+pendsv_exit
 	; restore interrupt
 	MSR		PRIMASK, r2
 
@@ -130,6 +135,11 @@ rt_hw_context_switch_to:
 	MOV		r0, #0x0
 	STR		r0, [r1]
 
+	; set interrupt flag to 1
+	LDR 	r1, =rt_thread_switch_interrput_flag
+	MOV 	r0, #1
+	STR 	r0, [r1]
+
 	; set the PendSV exception priority
     LDR     r0, =NVIC_SYSPRI2
     LDR     r1, =NVIC_PENDSV_PRI
diff --git a/libcpu/arm/stm32/context_rvds.S b/libcpu/arm/stm32/context_rvds.S
index 65818909db4f1a34465d6f47beff70dc6ba6dd11..cf07b71bff04a202188445c6472d15df4c160a9e 100644
--- a/libcpu/arm/stm32/context_rvds.S
+++ b/libcpu/arm/stm32/context_rvds.S
@@ -107,7 +107,6 @@ rt_hw_pend_sv	PROC
 	MRS     r1, psp                 ; get from thread stack pointer
 	STMFD	r1!, {r4 - r11}			; push r4 - r11 register
 	LDR		r0, [r0]
-
 	STR		r1, [r0]				; update from thread stack pointer
 
 swtich_to_thread