diff --git a/libcpu/arm/cortex-m0/context_gcc.S b/libcpu/arm/cortex-m0/context_gcc.S index bd59a01e534fc240eaa3e3df8aef9e41fd844403..f875a07fd88efdb0bac3949785e43cd45b7b0cb2 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 9b8a7dddb12e56093bb23e7186d011506fa5018c..f1318e552d7e6800f3e2192f477e90729961ea61 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 1194d8f4aeacbef618b432e375f7f0c5eb21760d..9c2f42ac83102eaac2d2d50c2640a86b0163f0eb 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 c0cf140dafa1dd360be2613fdffb8b38bbd27945..fad274aea825f45777c901433a022f6dc915ed26 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 - - .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) */ + .cpu cortex-m3 + .fpu softvfp + .syntax unified + .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 */ + + .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 8152e5f40d0e356e452a53777b5fb5d74a95ddbb..c27f02b512f885b95d7f09fcfe7d5d8b38227986 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 7f2e8e67d16432c08235d7fc9f9524aac7ea8a47..8b40c7cc48af4b0ca183e5873aa7c07f22db402a 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 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