diff options
Diffstat (limited to 'libcpu')
| -rw-r--r-- | libcpu/arm/cortex-m4/context_gcc.S | 21 | ||||
| -rw-r--r-- | libcpu/arm/cortex-m4/context_iar.S | 9 | ||||
| -rw-r--r-- | libcpu/arm/cortex-m4/context_rvds.S | 13 |
3 files changed, 35 insertions, 8 deletions
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 |