diff --git a/Ubiquitous/XiUOS/arch/arm/armv6m/arm32_switch.c b/Ubiquitous/XiUOS/arch/arm/armv6m/arm32_switch.c
index cd2bd883c..a35ad27ea 100644
--- a/Ubiquitous/XiUOS/arch/arm/armv6m/arm32_switch.c
+++ b/Ubiquitous/XiUOS/arch/arm/armv6m/arm32_switch.c
@@ -23,7 +23,6 @@
 
 void __attribute__((naked)) HwInterruptcontextSwitch(x_ubase from, x_ubase to, struct TaskDescriptor *to_task, void *context)
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("PUSH {R4}");
     asm volatile ("PUSH {R5}");
 
@@ -44,14 +43,15 @@ void __attribute__((naked)) HwInterruptcontextSwitch(x_ubase from, x_ubase to, s
     asm volatile ("STR r5, [r4]");
     asm volatile ("LDR r4, =InterruptFromKtask");
     asm volatile ("STR r0, [r4]");
+
     asm volatile ("POP {R5}");
     asm volatile ("POP {R4}");
+
     asm volatile ("B Arm32SwitchReswitch");
 }
 
 void __attribute__((naked)) Arm32SwitchReswitch()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("PUSH {R4}");
     asm volatile ("LDR r4, =InterruptToKtask");
     asm volatile ("STR r1, [r4]");
@@ -66,13 +66,11 @@ void __attribute__((naked)) Arm32SwitchReswitch()
 
 void __attribute__((naked)) SwitchKtaskContext(x_ubase from, x_ubase to, struct TaskDescriptor *to_task)
 {
-    // KPrintf("%s\n", __func__);
     asm volatile("B HwInterruptcontextSwitch");
 }
 
 void SwitchKtaskContextTo(x_ubase to, struct TaskDescriptor *to_task)
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("LDR r2, =InterruptToKtask");
     asm volatile ("STR r0, [r2]");
     asm volatile ("LDR r2, =InterruptToKtaskDescriptor");
@@ -109,7 +107,6 @@ void SwitchKtaskContextTo(x_ubase to, struct TaskDescriptor *to_task)
 
 void __attribute__((naked)) HardFaultHandler()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("MRS     r0, msp");
 
     // asm volatile ("TST     lr, #0x04");
@@ -124,7 +121,6 @@ void __attribute__((naked)) HardFaultHandler()
 
 void __attribute__((naked)) Arm32SwitchGetSpDone()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("MRS     r3, primask");
 
     // asm volatile ("STMFD   r0!, {r3 - r11}");
@@ -163,14 +159,12 @@ void __attribute__((naked)) Arm32SwitchGetSpDone()
 
 void __attribute__((naked)) Arm32SwitchUpdateMsp()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("MSR     msp, r0");
     asm volatile ("B       Arm32SwitchUpdateDone");
 }
 
 void __attribute__((naked)) Arm32SwitchUpdateDone()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("PUSH    {LR}");
     asm volatile ("BL      HwHardFaultException");
 
@@ -189,7 +183,6 @@ void __attribute__((naked)) Arm32SwitchUpdateDone()
 
 void __attribute__((naked)) MemFaultHandler()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("MRS     r0, msp");
 
     // asm volatile ("TST     lr, #0x04");
@@ -204,7 +197,6 @@ void __attribute__((naked)) MemFaultHandler()
 
 void __attribute__((naked)) Arm32Switch1()
 {
-    // KPrintf("%s\n", __func__);
     asm volatile ("MRS     r3, primask");
 
     // asm volatile ("STMFD   r0!, {r3 - r11}");
diff --git a/Ubiquitous/XiUOS/arch/arm/armv7m/arm32_switch.c b/Ubiquitous/XiUOS/arch/arm/armv7m/arm32_switch.c
index 5fd08dab0..09ee4bb1f 100644
--- a/Ubiquitous/XiUOS/arch/arm/armv7m/arm32_switch.c
+++ b/Ubiquitous/XiUOS/arch/arm/armv7m/arm32_switch.c
@@ -21,19 +21,37 @@
 
 void __attribute__((naked)) HwInterruptcontextSwitch(x_ubase from, x_ubase to, struct TaskDescriptor *to_task, void *context)
 {
+    asm volatile ("PUSH {R4}");
+    asm volatile ("PUSH {R5}");
+
     asm volatile ("LDR r4, =KtaskSwitchInterruptFlag");
     asm volatile ("LDR r5, [r4]");
     asm volatile ("CMP r5, #1");
+
+    asm volatile ("POP {R5}");
+    asm volatile ("POP {R4}");
+
     asm volatile ("BEQ Arm32SwitchReswitch");
+
+    asm volatile ("PUSH {R4}");
+    asm volatile ("PUSH {R5}");
+    asm volatile ("LDR r4, =KtaskSwitchInterruptFlag");
+
     asm volatile ("MOV r5, #1");
     asm volatile ("STR r5, [r4]");
     asm volatile ("LDR r4, =InterruptFromKtask");
     asm volatile ("STR r0, [r4]");
+
+    asm volatile ("POP {R5}");
+    asm volatile ("POP {R4}");
+
     asm volatile ("B Arm32SwitchReswitch");
 }
 
 void __attribute__((naked)) Arm32SwitchReswitch()
 {
+    asm volatile ("PUSH {R4}");
+
     asm volatile ("LDR r4, =InterruptToKtask");
     asm volatile ("STR r1, [r4]");
     asm volatile ("LDR r4, =InterruptToKtaskDescriptor");
@@ -41,6 +59,9 @@ void __attribute__((naked)) Arm32SwitchReswitch()
     asm volatile ("LDR r0, =" NVIC_INT_CTRL);
     asm volatile ("LDR r1, =" NVIC_PENDSVSET);
     asm volatile ("STR r1, [r0]");
+
+    asm volatile ("POP {R4}");
+
     asm volatile ("BX LR");
 }