update libcpu/arm/cortex-m4: restore MSP.

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,6 +24,7 @@
 .thumb
 .text
 
+.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) */
@@ -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! */

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!

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)
@@ -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