diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S
index 97c3f1b44e2e810ea12a92aff0d3e1f5589f4e16..2fe9747d6f55a6fc31be4eb0ed482500b8c1b1df 100644
--- a/libcpu/arm/cortex-m4/context_gcc.S
+++ b/libcpu/arm/cortex-m4/context_gcc.S
@@ -11,6 +11,7 @@
  * Date           Author       Notes
  * 2009-10-11     Bernard      first version
  * 2012-01-01     aozima       support context switch load/store FPU register.
+ * 2013-06-18     aozima       add restore MSP feature.
  */
 
 /**
@@ -23,10 +24,11 @@
 .thumb
 .text
 
-.equ    NVIC_INT_CTRL,       0xE000ED04               /* interrupt control state register */
-.equ    NVIC_SYSPRI2,        0xE000ED20               /* system priority register (2) */
-.equ    NVIC_PENDSV_PRI,     0x00FF0000               /* PendSV priority value (lowest) */
-.equ    NVIC_PENDSVSET,      0x10000000               /* value to trigger PendSV exception */
+.equ    SCB_VTOR,           0xE000ED04              /* Vector Table Offset Register */
+.equ    NVIC_INT_CTRL,      0xE000ED04              /* interrupt control state register */
+.equ    NVIC_SYSPRI2,       0xE000ED20              /* system priority register (2) */
+.equ    NVIC_PENDSV_PRI,    0x00FF0000              /* PendSV priority value (lowest) */
+.equ    NVIC_PENDSVSET,     0x10000000              /* value to trigger PendSV exception */
 
 /*
  * rt_base_t rt_hw_interrupt_disable();
@@ -106,9 +108,9 @@ PendSV_Handler:
     MRS r1, psp                 /* get from thread stack pointer */
     
 #if defined (__VFP_FP__) && !defined(__SOFTFP__)
-	VSTMDB  r1!, {d8 - d15}     /* push FPU register s16~s31 */
+    VSTMDB  r1!, {d8 - d15}     /* push FPU register s16~s31 */
 #endif
-	
+    
     STMFD   r1!, {r4 - r11}     /* push r4 - r11 register */
     LDR r0, [r0]
     STR r1, [r0]                /* update from thread stack pointer */
@@ -164,6 +166,13 @@ rt_hw_context_switch_to:
     LDR r1, =NVIC_PENDSVSET
     STR r1, [r0]
 
+    /* restore MSP */
+    LDR     r0, =SCB_VTOR
+    LDR     r0, [r0]
+    LDR     r0, [r0]
+    NOP
+    MSR     msp, r0
+
     CPSIE   I                       /* enable interrupts at processor level */
 
     /* never reach here! */
diff --git a/libcpu/arm/cortex-m4/context_iar.S b/libcpu/arm/cortex-m4/context_iar.S
index db3f3704735f71e1ef65180acbde021bdcd20ce2..7943a211b508ff7fafdd8b8c22aba46cf5c986cd 100644
--- a/libcpu/arm/cortex-m4/context_iar.S
+++ b/libcpu/arm/cortex-m4/context_iar.S
@@ -12,6 +12,7 @@
 ; * 2009-01-17     Bernard      first version
 ; * 2009-09-27     Bernard      add protect when contex switch occurs
 ; * 2012-01-01     aozima       support context switch load/store FPU register.
+; * 2013-06-18     aozima       add restore MSP feature.
 ; */
 
 ;/**
@@ -19,6 +20,7 @@
 ; */
 ;/*@{*/
 
+SCB_VTOR        EQU     0xE000ED08               ; Vector Table Offset Register
 NVIC_INT_CTRL   EQU     0xE000ED04               ; interrupt control state register
 NVIC_SYSPRI2    EQU     0xE000ED20               ; system priority register (2)
 NVIC_PENDSV_PRI EQU     0x00FF0000               ; PendSV priority value (lowest)
@@ -162,6 +164,13 @@ rt_hw_context_switch_to:
     LDR     r1, =NVIC_PENDSVSET
     STR     r1, [r0]
 
+    ; restore MSP
+    LDR     r0, =SCB_VTOR
+    LDR     r0, [r0]
+    LDR     r0, [r0]
+    NOP
+    MSR     msp, r0
+
     CPSIE   I                       ; enable interrupts at processor level
 
     ; never reach here!
diff --git a/libcpu/arm/cortex-m4/context_rvds.S b/libcpu/arm/cortex-m4/context_rvds.S
index ee72c44d01d4841162c8478e6a05be2da6f2df31..768c17681069eee2367da19d445621efe6231e32 100644
--- a/libcpu/arm/cortex-m4/context_rvds.S
+++ b/libcpu/arm/cortex-m4/context_rvds.S
@@ -11,6 +11,7 @@
 ; * Date           Author       Notes
 ; * 2009-01-17     Bernard      first version.
 ; * 2012-01-01     aozima       support context switch load/store FPU register.
+; * 2013-06-18     aozima       add restore MSP feature.
 ; */
 
 ;/**
@@ -18,6 +19,7 @@
 ; */
 ;/*@{*/
 
+SCB_VTOR        EQU     0xE000ED08               ; Vector Table Offset Register
 NVIC_INT_CTRL   EQU     0xE000ED04               ; interrupt control state register
 NVIC_SYSPRI2    EQU     0xE000ED20               ; system priority register (2)
 NVIC_PENDSV_PRI EQU     0x00FF0000               ; PendSV priority value (lowest)
@@ -108,10 +110,10 @@ PendSV_Handler   PROC
     MRS     r1, psp                 ; get from thread stack pointer
 
     IF      {FPU} != "SoftVFP"
-	VSTMFD  r1!, {d8 - d15}         ; push FPU register s16~s31
+    VSTMFD  r1!, {d8 - d15}         ; push FPU register s16~s31
     ENDIF
 
-	STMFD   r1!, {r4 - r11}         ; push r4 - r11 register
+    STMFD   r1!, {r4 - r11}         ; push r4 - r11 register
     LDR     r0, [r0]
     STR     r1, [r0]                ; update from thread stack pointer
 
@@ -169,6 +171,13 @@ rt_hw_context_switch_to    PROC
     LDR     r1, =NVIC_PENDSVSET
     STR     r1, [r0]
 
+    ; restore MSP
+    LDR     r0, =SCB_VTOR
+    LDR     r0, [r0]
+    LDR     r0, [r0]
+    NOP
+    MSR     msp, r0
+
     ; enable interrupts at processor level
     CPSIE   I