From 34d59ccb0f9f64aee3b85a44c37a8b77a840c6e7 Mon Sep 17 00:00:00 2001 From: aozima Date: Sun, 23 Jun 2013 16:28:55 +0800 Subject: [PATCH] update libcpu/arm/cortex-m4: support lazy stack optimized. --- libcpu/arm/cortex-m4/context_gcc.S | 36 ++++++++++++++++-- libcpu/arm/cortex-m4/context_iar.S | 38 ++++++++++++++++++- libcpu/arm/cortex-m4/context_rvds.S | 36 ++++++++++++++++-- libcpu/arm/cortex-m4/cpuport.c | 58 ++++++++++++++++++++++++----- 4 files changed, 151 insertions(+), 17 deletions(-) diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S index b66e257051..a6ecaf9fe9 100644 --- a/libcpu/arm/cortex-m4/context_gcc.S +++ b/libcpu/arm/cortex-m4/context_gcc.S @@ -12,10 +12,11 @@ * 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. + * 2013-06-23 aozima support lazy stack optimized. */ /** - * @addtogroup STM32 + * @addtogroup cortex-m4 */ /*@{*/ @@ -108,10 +109,21 @@ PendSV_Handler: MRS r1, psp /* get from thread stack pointer */ #if defined (__VFP_FP__) && !defined(__SOFTFP__) + TST lr, #0x10 /* if(!EXC_RETURN[4]) */ VSTMDB r1!, {d8 - d15} /* push FPU register s16~s31 */ #endif - + STMFD r1!, {r4 - r11} /* push r4 - r11 register */ + +#if defined (__VFP_FP__) && !defined(__SOFTFP__) + MOV r4, #0x00 /* flag = 0 */ + + TST lr, #0x10 /* if(!EXC_RETURN[4]) */ + MOVEQ r4, #0x01 /* flag = 1 */ + + STMFD r1!, {r4} /* push flag */ +#endif + LDR r0, [r0] STR r1, [r0] /* update from thread stack pointer */ @@ -120,10 +132,15 @@ swtich_to_thread: LDR r1, [r1] LDR r1, [r1] /* load thread stack pointer */ +#if defined (__VFP_FP__) && !defined(__SOFTFP__) + LDMFD r1!, {r3} /* pop flag */ +#endif + LDMFD r1!, {r4 - r11} /* pop r4 - r11 register */ #if defined (__VFP_FP__) && !defined(__SOFTFP__) - VLDMIA r1!, {d8 - d15} /* pop FPU register s16~s31 */ + CMP r3, #0 /* if(flag_r3 != 0) */ + VLDMIANE r1!, {d8 - d15} /* pop FPU register s16~s31 */ #endif MSR psp, r1 /* update stack pointer */ @@ -132,6 +149,12 @@ pendsv_exit: /* restore interrupt */ MSR PRIMASK, r2 +#if defined (__VFP_FP__) && !defined(__SOFTFP__) + ORR lr, lr, #0x10 /* lr |= (1 << 4), clean FPCA. */ + CMP r3, #0 /* if(flag_r3 != 0) */ + BICNE lr, lr, #0x10 /* lr &= ~(1 << 4), set FPCA. */ +#endif + ORR lr, lr, #0x04 BX lr @@ -145,6 +168,13 @@ rt_hw_context_switch_to: LDR r1, =rt_interrupt_to_thread STR r0, [r1] +#if defined (__VFP_FP__) && !defined(__SOFTFP__) + /* CLEAR CONTROL.FPCA */ + MRS r2, CONTROL /* read */ + BIC r2, #0x04 /* modify */ + MSR CONTROL, r2 /* write-back */ +#endif + /* set from thread to 0 */ LDR r1, =rt_interrupt_from_thread MOV r0, #0x0 diff --git a/libcpu/arm/cortex-m4/context_iar.S b/libcpu/arm/cortex-m4/context_iar.S index 7943a211b5..e882f36f64 100644 --- a/libcpu/arm/cortex-m4/context_iar.S +++ b/libcpu/arm/cortex-m4/context_iar.S @@ -13,10 +13,11 @@ ; * 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. +; * 2013-06-23 aozima support lazy stack optimized. ; */ ;/** -; * @addtogroup STM32 +; * @addtogroup cortex-m4 ; */ ;/*@{*/ @@ -107,10 +108,25 @@ PendSV_Handler: MRS r1, psp ; get from thread stack pointer #if defined ( __ARMVFP__ ) + TST lr, #0x10 ; if(!EXC_RETURN[4]) + BNE skip_push_fpu VSTMDB r1!, {d8 - d15} ; push FPU register s16~s31 +skip_push_fpu #endif STMFD r1!, {r4 - r11} ; push r4 - r11 register + +#if defined ( __ARMVFP__ ) + MOV r4, #0x00 ; flag = 0 + TST lr, #0x10 ; if(!EXC_RETURN[4]) + BNE push_flag + MOV r4, #0x01 ; flag = 1 +push_flag + ;STMFD r1!, {r4} ; push flag + SUB r1, r1, #0x04 + STR r4, [r1] +#endif + LDR r0, [r0] STR r1, [r0] ; update from thread stack pointer @@ -119,10 +135,16 @@ swtich_to_thread LDR r1, [r1] LDR r1, [r1] ; load thread stack pointer +#if defined ( __ARMVFP__ ) + LDMFD r1!, {r3} ; pop flag +#endif + LDMFD r1!, {r4 - r11} ; pop r4 - r11 register #if defined ( __ARMVFP__ ) + CBZ r3, skip_pop_fpu VLDMIA r1!, {d8 - d15} ; pop FPU register s16~s31 +skip_pop_fpu #endif MSR psp, r1 ; update stack pointer @@ -131,6 +153,13 @@ pendsv_exit ; restore interrupt MSR PRIMASK, r2 +#if defined ( __ARMVFP__ ) + ORR lr, lr, #0x10 ; lr |= (1 << 4), clean FPCA. + CBZ r3, return_without_fpu ; if(flag_r3 != 0) + BIC lr, lr, #0x10 ; lr &= ~(1 << 4), set FPCA. +return_without_fpu +#endif + ORR lr, lr, #0x04 BX lr @@ -143,6 +172,13 @@ rt_hw_context_switch_to: LDR r1, =rt_interrupt_to_thread STR r0, [r1] +#if defined ( __ARMVFP__ ) + ; CLEAR CONTROL.FPCA + MRS r2, CONTROL ; read + BIC r2, r2, #0x04 ; modify + MSR CONTROL, r2 ; write-back +#endif + ; set from thread to 0 LDR r1, =rt_interrupt_from_thread MOV r0, #0x0 diff --git a/libcpu/arm/cortex-m4/context_rvds.S b/libcpu/arm/cortex-m4/context_rvds.S index 768c176810..189ae33db1 100644 --- a/libcpu/arm/cortex-m4/context_rvds.S +++ b/libcpu/arm/cortex-m4/context_rvds.S @@ -12,10 +12,11 @@ ; * 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. +; * 2013-06-23 aozima support lazy stack optimized. ; */ ;/** -; * @addtogroup STM32 +; * @addtogroup cortex-m4 ; */ ;/*@{*/ @@ -110,10 +111,21 @@ PendSV_Handler PROC MRS r1, psp ; get from thread stack pointer IF {FPU} != "SoftVFP" - VSTMFD r1!, {d8 - d15} ; push FPU register s16~s31 + TST lr, #0x10 ; if(!EXC_RETURN[4]) + VSTMFDEQ r1!, {d8 - d15} ; push FPU register s16~s31 ENDIF STMFD r1!, {r4 - r11} ; push r4 - r11 register + + IF {FPU} != "SoftVFP" + MOV r4, #0x00 ; flag = 0 + + TST lr, #0x10 ; if(!EXC_RETURN[4]) + MOVEQ r4, #0x01 ; flag = 1 + + STMFD r1!, {r4} ; push flag + ENDIF + LDR r0, [r0] STR r1, [r0] ; update from thread stack pointer @@ -122,10 +134,15 @@ swtich_to_thread LDR r1, [r1] LDR r1, [r1] ; load thread stack pointer + IF {FPU} != "SoftVFP" + LDMFD r1!, {r3} ; pop flag + ENDIF + LDMFD r1!, {r4 - r11} ; pop r4 - r11 register IF {FPU} != "SoftVFP" - VLDMFD r1!, {d8 - d15} ; pop FPU register s16~s31 + CMP r3, #0 ; if(flag_r3 != 0) + VLDMFDNE r1!, {d8 - d15} ; pop FPU register s16~s31 ENDIF MSR psp, r1 ; update stack pointer @@ -134,6 +151,12 @@ pendsv_exit ; restore interrupt MSR PRIMASK, r2 + IF {FPU} != "SoftVFP" + ORR lr, lr, #0x10 ; lr |= (1 << 4), clean FPCA. + CMP r3, #0 ; if(flag_r3 != 0) + BICNE lr, lr, #0x10 ; lr &= ~(1 << 4), set FPCA. + ENDIF + ORR lr, lr, #0x04 BX lr ENDP @@ -149,6 +172,13 @@ rt_hw_context_switch_to PROC LDR r1, =rt_interrupt_to_thread STR r0, [r1] + IF {FPU} != "SoftVFP" + ; CLEAR CONTROL.FPCA + MRS r2, CONTROL ; read + BIC r2, #0x04 ; modify + MSR CONTROL, r2 ; write-back + ENDIF + ; set from thread to 0 LDR r1, =rt_interrupt_from_thread MOV r0, #0x0 diff --git a/libcpu/arm/cortex-m4/cpuport.c b/libcpu/arm/cortex-m4/cpuport.c index 8f97758741..ef5fcc8b7c 100644 --- a/libcpu/arm/cortex-m4/cpuport.c +++ b/libcpu/arm/cortex-m4/cpuport.c @@ -16,6 +16,7 @@ * 2012-12-11 lgnq fixed the coding style. * 2012-12-23 aozima stack addr align to 8byte. * 2012-12-29 Bernard Add exception hook. + * 2013-06-23 aozima support lazy stack optimized. */ #include @@ -32,6 +33,37 @@ rt_uint32_t rt_thread_switch_interrupt_flag; static rt_err_t (*rt_exception_hook)(void *context) = RT_NULL; 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; +}; + +struct stack_frame +{ +#if USE_FPU + rt_uint32_t flag; +#endif /* USE_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; + + struct exception_stack_frame exception_stack_frame; +}; + +struct exception_stack_frame_fpu { rt_uint32_t r0; rt_uint32_t r1; @@ -65,8 +97,10 @@ struct exception_stack_frame #endif }; -struct stack_frame +struct stack_frame_fpu { + rt_uint32_t flag; + /* r4 ~ r11 register */ rt_uint32_t r4; rt_uint32_t r5; @@ -97,7 +131,7 @@ struct stack_frame rt_uint32_t s31; #endif - struct exception_stack_frame exception_stack_frame; + struct exception_stack_frame_fpu exception_stack_frame; }; rt_uint8_t *rt_hw_stack_init(void *tentry, @@ -130,6 +164,10 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, stack_frame->exception_stack_frame.pc = (unsigned long)tentry; /* entry point, pc */ stack_frame->exception_stack_frame.psr = 0x01000000L; /* PSR */ +#if USE_FPU + stack_frame->flag = 0; +#endif /* USE_FPU */ + /* return task's current stack address */ return stk; } @@ -141,20 +179,20 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, */ void rt_hw_exception_install(rt_err_t (*exception_handle)(void* context)) { - rt_exception_hook = exception_handle; + rt_exception_hook = exception_handle; } void rt_hw_hard_fault_exception(struct exception_stack_frame *exception_stack) { - extern long list_thread(void); + extern long list_thread(void); - if (rt_exception_hook != RT_NULL) - { - rt_err_t result; + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; - result = rt_exception_hook(exception_stack); - if (result == RT_EOK) return; - } + result = rt_exception_hook(exception_stack); + if (result == RT_EOK) return; + } rt_kprintf("psr: 0x%08x\n", exception_stack->psr); rt_kprintf(" pc: 0x%08x\n", exception_stack->pc); -- GitLab