Merge pull request #108 from aozima/pulls

回收main函数所用的栈空间

9 files changed, 191 insertions, 110 deletions

diff --git a/libcpu/arm/cortex-m0/context_gcc.S b/libcpu/arm/cortex-m0/context_gcc.S
index bd59a01e53..f875a07fd8 100644
--- a/libcpu/arm/cortex-m0/context_gcc.S
+++ b/libcpu/arm/cortex-m0/context_gcc.S
@@ -13,6 +13,7 @@
  * 2012-06-01   aozima       set pendsv priority to 0xFF.
  * 2012-08-17   aozima       fixed bug: store r8 - r11.
  * 2013-02-20   aozima       port to gcc.
+ * 2013-06-18   aozima       add restore MSP feature.
  */
  
 	.cpu 	cortex-m3
@@ -21,6 +22,7 @@
 	.thumb
 	.text
 
+    .equ    SCB_VTOR, 0xE000ED04            /* Vector Table Offset Register */
 	.equ	ICSR, 0xE000ED04 				/* interrupt control state register */
 	.equ	PENDSVSET_BIT, 0x10000000 		/* value to trigger PendSV exception */
 	
@@ -152,6 +154,13 @@ rt_hw_context_switch_to:
 	LDR		R1, =PENDSVSET_BIT
 	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-m0/context_iar.S b/libcpu/arm/cortex-m0/context_iar.S
index 9b8a7dddb1..f1318e552d 100644
--- a/libcpu/arm/cortex-m0/context_iar.S
+++ b/libcpu/arm/cortex-m0/context_iar.S
@@ -12,6 +12,7 @@
 ; * 2010-01-25     Bernard      first version
 ; * 2012-06-01     aozima       set pendsv priority to 0xFF.
 ; * 2012-08-17     aozima       fixed bug: store r8 - r11.
+; * 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_SHPR3      EQU     0xE000ED20               ; system priority register (2)
 NVIC_PENDSV_PRI EQU     0x00FF0000               ; PendSV priority value (lowest)
@@ -178,6 +180,13 @@ rt_hw_context_switch_to:
     STR     r1, [r0]
     NOP
 
+    ; restore MSP
+    LDR     r0, =SCB_VTOR
+    LDR     r0, [r0]
+    LDR     r0, [r0]
+    NOP
+    MSR     msp, r0
+
     ; enable interrupts at processor level
     CPSIE   I
 
diff --git a/libcpu/arm/cortex-m0/context_rvds.S b/libcpu/arm/cortex-m0/context_rvds.S
index 1194d8f4ae..9c2f42ac83 100644
--- a/libcpu/arm/cortex-m0/context_rvds.S
+++ b/libcpu/arm/cortex-m0/context_rvds.S
@@ -12,6 +12,7 @@
 ; * 2010-01-25     Bernard      first version
 ; * 2012-06-01     aozima       set pendsv priority to 0xFF.
 ; * 2012-08-17     aozima       fixed bug: store r8 - r11.
+; * 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_SHPR3      EQU     0xE000ED20               ; system priority register (2)
 NVIC_PENDSV_PRI EQU     0x00FF0000               ; PendSV priority value (lowest)
@@ -183,6 +185,13 @@ rt_hw_context_switch_to    PROC
     STR     r1, [r0]
     NOP
 
+    ; restore MSP
+    LDR     r0, =SCB_VTOR
+    LDR     r0, [r0]
+    LDR     r0, [r0]
+    NOP
+    MSR     msp, r0
+
     ; enable interrupts at processor level
     CPSIE   I
 
diff --git a/libcpu/arm/cortex-m3/context_gcc.S b/libcpu/arm/cortex-m3/context_gcc.S
index c0cf140daf..fad274aea8 100644
--- a/libcpu/arm/cortex-m3/context_gcc.S
+++ b/libcpu/arm/cortex-m3/context_gcc.S
@@ -10,161 +10,170 @@
  * Change Logs:
  * Date         Author    Notes
  * 2009-10-11   Bernard   First version
- * 2010-12-29	onelife	  Modify for EFM32
- * 2011-06-17	onelife	  Merge all of the assembly source code into context_gcc.S
- * 2011-07-12	onelife	  Add interrupt context check function
+ * 2010-12-29   onelife   Modify for EFM32
+ * 2011-06-17   onelife   Merge all of the assembly source code into context_gcc.S
+ * 2011-07-12   onelife   Add interrupt context check function
+ * 2013-06-18   aozima    add restore MSP feature.
  */
  
-	.cpu 	cortex-m3
-	.fpu 	softvfp
-	.syntax	unified
-	.thumb
-	.text
+    .cpu    cortex-m3
+    .fpu    softvfp
+    .syntax unified
+    .thumb
+    .text
 
-	.equ	ICSR, 0xE000ED04 				/* interrupt control state register */
-	.equ	PENDSVSET_BIT, 0x10000000 		/* value to trigger PendSV exception */
-	
-	.equ	SHPR3, 0xE000ED20 				/* system priority register (3) */
-	.equ	PENDSV_PRI_LOWEST, 0x00FF0000 	/* PendSV priority value (lowest) */
+    .equ    SCB_VTOR, 0xE000ED04            /* Vector Table Offset Register */
+    .equ    ICSR, 0xE000ED04                /* interrupt control state register */
+    .equ    PENDSVSET_BIT, 0x10000000       /* value to trigger PendSV exception */
+    
+    .equ    SHPR3, 0xE000ED20               /* system priority register (3) */
+    .equ    PENDSV_PRI_LOWEST, 0x00FF0000   /* PendSV priority value (lowest) */
 
 /*
  * rt_base_t rt_hw_interrupt_disable();
  */
-	.global rt_hw_interrupt_disable
-	.type rt_hw_interrupt_disable, %function
+    .global rt_hw_interrupt_disable
+    .type rt_hw_interrupt_disable, %function
 rt_hw_interrupt_disable:
-	MRS		R0, PRIMASK
-	CPSID	I
-	BX		LR
+    MRS     R0, PRIMASK
+    CPSID   I
+    BX      LR
 
 /*
  * void rt_hw_interrupt_enable(rt_base_t level);
  */
-	.global rt_hw_interrupt_enable
-	.type rt_hw_interrupt_enable, %function
+    .global rt_hw_interrupt_enable
+    .type rt_hw_interrupt_enable, %function
 rt_hw_interrupt_enable:
-	MSR		PRIMASK, R0
-	BX		LR
+    MSR     PRIMASK, R0
+    BX      LR
 
 /*
  * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
  * R0 --> from
  * R1 --> to
  */
-	.global rt_hw_context_switch_interrupt
-	.type rt_hw_context_switch_interrupt, %function
-	.global rt_hw_context_switch
-	.type rt_hw_context_switch, %function
+    .global rt_hw_context_switch_interrupt
+    .type rt_hw_context_switch_interrupt, %function
+    .global rt_hw_context_switch
+    .type rt_hw_context_switch, %function
 rt_hw_context_switch_interrupt:
 rt_hw_context_switch:
-	/* set rt_thread_switch_interrupt_flag to 1 */
-	LDR 	R2, =rt_thread_switch_interrupt_flag
-	LDR 	R3, [R2]
-	CMP 	R3, #1
-	BEQ 	_reswitch
-	MOV 	R3, #1
-	STR 	R3, [R2]
+    /* set rt_thread_switch_interrupt_flag to 1 */
+    LDR     R2, =rt_thread_switch_interrupt_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, =ICSR      		/* trigger the PendSV exception (causes context switch) */
-	LDR		R1, =PENDSVSET_BIT
-	STR		R1, [R0]
-	BX		LR
+    LDR     R0, =ICSR           /* trigger the PendSV exception (causes context switch) */
+    LDR     R1, =PENDSVSET_BIT
+    STR     R1, [R0]
+    BX      LR
 
 /* R0 --> swith from thread stack
  * R1 --> swith to thread stack
  * psr, pc, LR, R12, R3, R2, R1, R0 are pushed into [from] stack
  */
-	.global PendSV_Handler
-	.type PendSV_Handler, %function
+    .global PendSV_Handler
+    .type PendSV_Handler, %function
 PendSV_Handler:
-	/* disable interrupt to protect context switch */
-	MRS		R2, PRIMASK
-	CPSID	I
+    /* disable interrupt to protect context switch */
+    MRS     R2, PRIMASK
+    CPSID   I
 
-	/* get rt_thread_switch_interrupt_flag */
-	LDR		R0, =rt_thread_switch_interrupt_flag
-	LDR		R1, [R0]
-	CBZ		R1, pendsv_exit			/* pendsv aLReady handled */
+    /* get rt_thread_switch_interrupt_flag */
+    LDR     R0, =rt_thread_switch_interrupt_flag
+    LDR     R1, [R0]
+    CBZ     R1, pendsv_exit         /* pendsv aLReady handled */
 
-	/* clear rt_thread_switch_interrupt_flag to 0 */
-	MOV		R1, #0
-	STR		R1, [R0]
+    /* clear rt_thread_switch_interrupt_flag to 0 */
+    MOV     R1, #0
+    STR     R1, [R0]
 
-	LDR		R0, =rt_interrupt_from_thread
-	LDR		R1, [R0]
-	CBZ		R1, swtich_to_thread    /* skip register save at the first time */
+    LDR     R0, =rt_interrupt_from_thread
+    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 */
-	LDR		R0, [R0]
-	STR		R1, [R0]				/* update from thread stack pointer */
+    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:
-	LDR		R1, =rt_interrupt_to_thread
-	LDR		R1, [R1]
-	LDR		R1, [R1]				/* load thread stack pointer */
+    LDR     R1, =rt_interrupt_to_thread
+    LDR     R1, [R1]
+    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
+    /* restore interrupt */
+    MSR     PRIMASK, R2
 
-	ORR		LR, LR, #0x04
-	BX		LR
+    ORR     LR, LR, #0x04
+    BX      LR
 
 /*
  * void rt_hw_context_switch_to(rt_uint32 to);
  * R0 --> to
  */
-	.global rt_hw_context_switch_to
-	.type rt_hw_context_switch_to, %function
+    .global rt_hw_context_switch_to
+    .type rt_hw_context_switch_to, %function
 rt_hw_context_switch_to:
-	LDR		R1, =rt_interrupt_to_thread
-	STR		R0, [R1]
+    LDR     R1, =rt_interrupt_to_thread
+    STR     R0, [R1]
+
+    /* set from thread to 0 */
+    LDR     R1, =rt_interrupt_from_thread
+    MOV     R0, #0
+    STR     R0, [R1]
 
-	/* set from thread to 0 */
-	LDR		R1, =rt_interrupt_from_thread
-	MOV		R0, #0
-	STR		R0, [R1]
+    /* set interrupt flag to 1 */
+    LDR     R1, =rt_thread_switch_interrupt_flag
+    MOV     R0, #1
+    STR     R0, [R1]
 
-	/* set interrupt flag to 1 */
-	LDR 	R1, =rt_thread_switch_interrupt_flag
-	MOV 	R0, #1
-	STR 	R0, [R1]
+    /* set the PendSV exception priority */
+    LDR     R0, =SHPR3
+    LDR     R1, =PENDSV_PRI_LOWEST
+    LDR.W   R2, [R0,#0]             /* read */
+    ORR     R1, R1, R2              /* modify */
+    STR     R1, [R0]                /* write-back */
 
-	/* set the PendSV exception priority */
-	LDR		R0, =SHPR3
-	LDR		R1, =PENDSV_PRI_LOWEST
-	LDR.W   R2, [R0,#0]				/* read */
-	ORR     R1, R1, R2				/* modify */
-	STR     R1, [R0]				/* write-back */
+    LDR     R0, =ICSR               /* trigger the PendSV exception (causes context switch) */
+    LDR     R1, =PENDSVSET_BIT
+    STR     R1, [R0]
 
-	LDR		R0, =ICSR				/* trigger the PendSV exception (causes context switch) */
-	LDR		R1, =PENDSVSET_BIT
-	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 */
+    CPSIE   I                       /* enable interrupts at processor level */
 
-	/* never reach here! */
+    /* never reach here! */
 
 /* compatible with old version */
-	.global rt_hw_interrupt_thread_switch
-	.type rt_hw_interrupt_thread_switch, %function
+    .global rt_hw_interrupt_thread_switch
+    .type rt_hw_interrupt_thread_switch, %function
 rt_hw_interrupt_thread_switch:
-	BX		LR
-	NOP
+    BX      LR
+    NOP
 
-	.global HardFault_Handler
-	.type HardFault_Handler, %function
+    .global HardFault_Handler
+    .type HardFault_Handler, %function
 HardFault_Handler:
     /* get current context */
     MRS     R0, PSP                 /* get fault thread stack pointer */
@@ -179,8 +188,8 @@ HardFault_Handler:
  * rt_uint32_t rt_hw_interrupt_check(void);
  * R0 --> state
  */
-	.global rt_hw_interrupt_check
-	.type rt_hw_interrupt_check, %function
+    .global rt_hw_interrupt_check
+    .type rt_hw_interrupt_check, %function
 rt_hw_interrupt_check:
-	MRS     R0, IPSR
-	BX		LR
+    MRS     R0, IPSR
+    BX      LR
diff --git a/libcpu/arm/cortex-m3/context_iar.S b/libcpu/arm/cortex-m3/context_iar.S
index 8152e5f40d..c27f02b512 100644
--- a/libcpu/arm/cortex-m3/context_iar.S
+++ b/libcpu/arm/cortex-m3/context_iar.S
@@ -11,6 +11,7 @@
 ; * Date           Author       Notes
 ; * 2009-01-17     Bernard      first version
 ; * 2009-09-27     Bernard      add protect when contex switch occurs
+; * 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)
@@ -151,6 +153,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-m3/context_rvds.S b/libcpu/arm/cortex-m3/context_rvds.S
index 7f2e8e67d1..8b40c7cc48 100644
--- a/libcpu/arm/cortex-m3/context_rvds.S
+++ b/libcpu/arm/cortex-m3/context_rvds.S
@@ -10,6 +10,7 @@
 ; * Change Logs:
 ; * Date           Author       Notes
 ; * 2009-01-17     Bernard      first version
+; * 2013-06-18     aozima       add restore MSP feature.
 ; */
 
 ;/**
@@ -17,6 +18,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)
@@ -158,6 +160,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
 
diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S
index 97c3f1b44e..2fe9747d6f 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 db3f370473..7943a211b5 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 ee72c44d01..768c176810 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