From aad32f8546706727a1e37f72f6ddc16f279dfbfd Mon Sep 17 00:00:00 2001 From: wuyangyong Date: Sat, 31 Dec 2011 20:25:44 +0000 Subject: [PATCH] support context switch load/store FPU register. git-svn-id: https://rt-thread.googlecode.com/svn/trunk@1901 bbd45198-f89e-11dd-88c7-29a3b14d5316 --- libcpu/arm/cortex-m4/context_gcc.S | 11 +- libcpu/arm/cortex-m4/context_iar.S | 3 + libcpu/arm/cortex-m4/context_rvds.S | 5 +- libcpu/arm/cortex-m4/cpuport.c | 239 +++++++++++++++++++--------- 4 files changed, 174 insertions(+), 84 deletions(-) diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S index 189e7e6246..7936a106fc 100644 --- a/libcpu/arm/cortex-m4/context_gcc.S +++ b/libcpu/arm/cortex-m4/context_gcc.S @@ -10,6 +10,7 @@ * Change Logs: * Date Author Notes * 2009-10-11 Bernard first version + * 2012-01-01 aozima support context switch load/store FPU register. */ /** @@ -17,8 +18,8 @@ */ /*@{*/ -.cpu cortex-m3 -.fpu softvfp +.cpu cortex-m4 +.fpu vfpv4 .syntax unified .thumb .text @@ -104,7 +105,8 @@ PendSV_Handler: 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 */ + VSTMDB r1!, {d8 - d15} /* push FPU register s16~s31 */ + STMFD r1!, {r4 - r11} /* push r4 - r11 register */ LDR r0, [r0] STR r1, [r0] /* update from thread stack pointer */ @@ -113,7 +115,8 @@ swtich_to_thread: LDR r1, [r1] LDR r1, [r1] /* load thread stack pointer */ - LDMFD r1!, {r4 - r11} /* pop r4 - r11 register */ + LDMFD r1!, {r4 - r11} /* pop r4 - r11 register */ + VLDMIA r1!, {d8 - d15} /* pop FPU register s16~s31 */ MSR psp, r1 /* update stack pointer */ pendsv_exit: diff --git a/libcpu/arm/cortex-m4/context_iar.S b/libcpu/arm/cortex-m4/context_iar.S index c7d5c61d70..4de0580177 100644 --- a/libcpu/arm/cortex-m4/context_iar.S +++ b/libcpu/arm/cortex-m4/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 +; * 2012-01-01 aozima support context switch load/store FPU register. ; */ ;/** @@ -102,6 +103,7 @@ PendSV_Handler: CBZ r1, swtich_to_thread ; skip register save at the first time MRS r1, psp ; get from thread stack pointer + VSTMFD r1!, {d8 - d15} ; push FPU register s16~s31 STMFD r1!, {r4 - r11} ; push r4 - r11 register LDR r0, [r0] STR r1, [r0] ; update from thread stack pointer @@ -112,6 +114,7 @@ swtich_to_thread LDR r1, [r1] ; load thread stack pointer LDMFD r1!, {r4 - r11} ; pop r4 - r11 register + VLDMFD r1!, {d8 - d15} ; pop FPU register s16~s31 MSR psp, r1 ; update stack pointer pendsv_exit diff --git a/libcpu/arm/cortex-m4/context_rvds.S b/libcpu/arm/cortex-m4/context_rvds.S index b38113239e..ab73619e2b 100644 --- a/libcpu/arm/cortex-m4/context_rvds.S +++ b/libcpu/arm/cortex-m4/context_rvds.S @@ -9,7 +9,8 @@ ; * ; * Change Logs: ; * Date Author Notes -; * 2009-01-17 Bernard first version +; * 2009-01-17 Bernard first version. +; * 2012-01-01 aozima support context switch load/store FPU register. ; */ ;/** @@ -105,6 +106,7 @@ PendSV_Handler PROC CBZ r1, swtich_to_thread ; skip register save at the first time MRS r1, psp ; get from thread stack pointer + VSTMFD r1!, {d8 - d15} ; push FPU register s16~s31 STMFD r1!, {r4 - r11} ; push r4 - r11 register LDR r0, [r0] STR r1, [r0] ; update from thread stack pointer @@ -115,6 +117,7 @@ swtich_to_thread LDR r1, [r1] ; load thread stack pointer LDMFD r1!, {r4 - r11} ; pop r4 - r11 register + VLDMFD r1!, {d8 - d15} ; pop FPU register s16~s31 MSR psp, r1 ; update stack pointer pendsv_exit diff --git a/libcpu/arm/cortex-m4/cpuport.c b/libcpu/arm/cortex-m4/cpuport.c index a282a3be77..dd55ba3df0 100644 --- a/libcpu/arm/cortex-m4/cpuport.c +++ b/libcpu/arm/cortex-m4/cpuport.c @@ -1,7 +1,7 @@ /* * File : cpuport.c * This file is part of RT-Thread RTOS - * COPYRIGHT (C) 2009 - 2011, RT-Thread Development Team + * COPYRIGHT (C) 2006 - 2011, RT-Thread Development Team * * The license and distribution terms for this file may be * found in the file LICENSE in this distribution or at @@ -9,114 +9,195 @@ * * Change Logs: * Date Author Notes - * 2006-08-23 Bernard the first version - * 2011-06-03 Bernard merge all of C source code into cpuport.c - */ + * 2011-10-21 Bernard the first version. + * 2011-10-27 aozima update for cortex-M4 FPU. + * 2011-12-31 aozima fixed stack align issues. + * 2012-01-01 aozima support context switch load/store FPU register. +*/ #include +#define _CPACR (*(rt_uint32_t *)0xE000ED88) /* Coprocessor Access Control Register */ + /* exception and interrupt handler table */ rt_uint32_t rt_interrupt_from_thread, rt_interrupt_to_thread; rt_uint32_t rt_thread_switch_interrupt_flag; -struct stack_contex +struct exception_stack_frame { - rt_uint32_t r0; - rt_uint32_t r1; - rt_uint32_t r2; - rt_uint32_t r3; - rt_uint32_t r12; - rt_uint32_t lr; - rt_uint32_t pc; - rt_uint32_t psr; + rt_uint32_t r0; + rt_uint32_t r1; + rt_uint32_t r2; + rt_uint32_t r3; + rt_uint32_t r12; + rt_uint32_t lr; + rt_uint32_t pc; + rt_uint32_t psr; }; -struct stack_contex_fpu +struct exception_stack_frame_fpu { - rt_uint32_t r0; - rt_uint32_t r1; - rt_uint32_t r2; - rt_uint32_t r3; - rt_uint32_t r12; - rt_uint32_t lr; - rt_uint32_t pc; - rt_uint32_t psr; - /* FPU register */ - rt_uint32_t S0; - rt_uint32_t S1; - rt_uint32_t S2; - rt_uint32_t S3; - rt_uint32_t S4; - rt_uint32_t S5; - rt_uint32_t S6; - rt_uint32_t S7; - rt_uint32_t S8; - rt_uint32_t S9; - rt_uint32_t S10; - rt_uint32_t S11; - rt_uint32_t S12; - rt_uint32_t S13; - rt_uint32_t S14; - rt_uint32_t S15; - rt_uint32_t FPSCR; - rt_uint32_t NO_NAME; + rt_uint32_t r0; + rt_uint32_t r1; + rt_uint32_t r2; + rt_uint32_t r3; + rt_uint32_t r12; + rt_uint32_t lr; + rt_uint32_t pc; + rt_uint32_t psr; + + /* FPU register */ + rt_uint32_t S0; + rt_uint32_t S1; + rt_uint32_t S2; + rt_uint32_t S3; + rt_uint32_t S4; + rt_uint32_t S5; + rt_uint32_t S6; + rt_uint32_t S7; + rt_uint32_t S8; + rt_uint32_t S9; + rt_uint32_t S10; + rt_uint32_t S11; + rt_uint32_t S12; + rt_uint32_t S13; + rt_uint32_t S14; + rt_uint32_t S15; + rt_uint32_t FPSCR; + rt_uint32_t NO_NAME; +}; + +struct stack_frame +{ + /* r4 ~ r11 register */ + rt_uint32_t r4; + rt_uint32_t r5; + rt_uint32_t r6; + rt_uint32_t r7; + rt_uint32_t r8; + rt_uint32_t r9; + rt_uint32_t r10; + rt_uint32_t r11; + + struct exception_stack_frame exception_stack_frame; +}; + +struct stack_frame_fpu +{ + /* r4 ~ r11 register */ + rt_uint32_t r4; + rt_uint32_t r5; + rt_uint32_t r6; + rt_uint32_t r7; + rt_uint32_t r8; + rt_uint32_t r9; + rt_uint32_t r10; + rt_uint32_t r11; + + /* FPU register s16 ~ s31 */ + rt_uint32_t s16; + rt_uint32_t s17; + rt_uint32_t s18; + rt_uint32_t s19; + rt_uint32_t s20; + rt_uint32_t s21; + rt_uint32_t s22; + rt_uint32_t s23; + rt_uint32_t s24; + rt_uint32_t s25; + rt_uint32_t s26; + rt_uint32_t s27; + rt_uint32_t s28; + rt_uint32_t s29; + rt_uint32_t s30; + rt_uint32_t s31; + + struct exception_stack_frame_fpu exception_stack_frame; }; rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, - rt_uint8_t *stack_addr, void *texit) + rt_uint8_t *stack_addr, void *texit) { - unsigned long *stk; - struct stack_contex_fpu * stack_contex_fpu; + rt_uint8_t * stk; + unsigned long i; - stk = (unsigned long *)stack_addr + sizeof(rt_uint32_t); + stk = stack_addr + sizeof(rt_uint32_t); - stk -= sizeof(struct stack_contex_fpu); - stack_contex_fpu = (struct stack_contex_fpu *)stk; - stack_contex_fpu->r0 = (unsigned long)parameter; /* r0 : argument */ - stack_contex_fpu->r1 = 0; /* r1 */ - stack_contex_fpu->r2 = 0; /* r2 */ - stack_contex_fpu->r3 = 0; /* r3 */ - stack_contex_fpu->r12 = 0; /* r12 */ - stack_contex_fpu->lr = (unsigned long)texit; /* lr */ - stack_contex_fpu->pc = (unsigned long)tentry; /* entry point, pc */ - stack_contex_fpu->psr = 0x01000000L; /* PSR */ + /* check FPU enable ? */ + if((_CPACR & (0xF << 20)) == (0xF << 20)) + { + /* FPU is enable */ + struct stack_frame_fpu * stack_frame_fpu; - *(--stk) = 0; /* r11 */ - *(--stk) = 0; /* r10 */ - *(--stk) = 0; /* r9 */ - *(--stk) = 0; /* r8 */ - *(--stk) = 0; /* r7 */ - *(--stk) = 0; /* r6 */ - *(--stk) = 0; /* r5 */ - *(--stk) = 0; /* r4 */ + stk -= sizeof(struct stack_frame_fpu); + stack_frame_fpu = (struct stack_frame_fpu *)stk; - /* return task's current stack address */ - return (rt_uint8_t *)stk; + /* init all register */ + for(i=0; iexception_stack_frame.r0 = (unsigned long)parameter; /* r0 : argument */ + stack_frame_fpu->exception_stack_frame.r1 = 0; /* r1 */ + stack_frame_fpu->exception_stack_frame.r2 = 0; /* r2 */ + stack_frame_fpu->exception_stack_frame.r3 = 0; /* r3 */ + stack_frame_fpu->exception_stack_frame.r12 = 0; /* r12 */ + stack_frame_fpu->exception_stack_frame.lr = (unsigned long)texit; /* lr */ + stack_frame_fpu->exception_stack_frame.pc = (unsigned long)tentry; /* entry point, pc */ + stack_frame_fpu->exception_stack_frame.psr = 0x01000000L; /* PSR */ + } + else + { + /* FPU is disable */ + struct stack_frame * stack_frame; + + stk -= sizeof(struct stack_frame); + stack_frame = (struct stack_frame *)stk; + + /* init all register */ + for(i=0; iexception_stack_frame.r0 = (unsigned long)parameter; /* r0 : argument */ + stack_frame->exception_stack_frame.r1 = 0; /* r1 */ + stack_frame->exception_stack_frame.r2 = 0; /* r2 */ + stack_frame->exception_stack_frame.r3 = 0; /* r3 */ + stack_frame->exception_stack_frame.r12 = 0; /* r12 */ + stack_frame->exception_stack_frame.lr = (unsigned long)texit; /* lr */ + stack_frame->exception_stack_frame.pc = (unsigned long)tentry; /* entry point, pc */ + stack_frame->exception_stack_frame.psr = 0x01000000L; /* PSR */ + } + + /* return task's current stack address */ + return stk; } extern void rt_hw_interrupt_thread_switch(void); extern void list_thread(void); extern rt_thread_t rt_current_thread; -void rt_hw_hard_fault_exception(struct stack_contex* contex) +void rt_hw_hard_fault_exception(struct exception_stack_frame * exception_stack) { - rt_kprintf("psr: 0x%08x\n", contex->psr); - rt_kprintf(" pc: 0x%08x\n", contex->pc); - rt_kprintf(" lr: 0x%08x\n", contex->lr); - rt_kprintf("r12: 0x%08x\n", contex->r12); - rt_kprintf("r03: 0x%08x\n", contex->r3); - rt_kprintf("r02: 0x%08x\n", contex->r2); - rt_kprintf("r01: 0x%08x\n", contex->r1); - rt_kprintf("r00: 0x%08x\n", contex->r0); + rt_kprintf("psr: 0x%08x\n", exception_stack->psr); + rt_kprintf(" pc: 0x%08x\n", exception_stack->pc); + rt_kprintf(" lr: 0x%08x\n", exception_stack->lr); + rt_kprintf("r12: 0x%08x\n", exception_stack->r12); + rt_kprintf("r03: 0x%08x\n", exception_stack->r3); + rt_kprintf("r02: 0x%08x\n", exception_stack->r2); + rt_kprintf("r01: 0x%08x\n", exception_stack->r1); + rt_kprintf("r00: 0x%08x\n", exception_stack->r0); - rt_kprintf("hard fault on thread: %s\n", rt_current_thread->name); + rt_kprintf("hard fault on thread: %s\n", rt_current_thread->name); #ifdef RT_USING_FINSH - list_thread(); + list_thread(); #endif - while (1); + while (1); } void rt_hw_cpu_shutdown() { - rt_kprintf("shutdown...\n"); + rt_kprintf("shutdown...\n"); - RT_ASSERT(0); + RT_ASSERT(0); }