diff --git a/bsp/rm48x50/HALCoGen/source/sys_core.asm b/bsp/rm48x50/HALCoGen/source/sys_core.asm index f8d3ca6952f1f9b555722658278f0c637806a0d2..bfb4ad22a84122886c471303873c7ca5198629e1 100644 --- a/bsp/rm48x50/HALCoGen/source/sys_core.asm +++ b/bsp/rm48x50/HALCoGen/source/sys_core.asm @@ -67,11 +67,14 @@ _coreInitRegisters_ ; Switch back to Supervisor Mode (M = 10011) cps #19 - + ; Turn on FPV coprocessor mrc p15, #0x00, r2, c1, c0, #0x02 orr r2, r2, #0xF00000 mcr p15, #0x00, r2, c1, c0, #0x02 - mov r2, #0x40000000 + + ; Enable FPV + fmrx R2, fpexc + orr r2, r2, #0x40000000 fmxr fpexc, r2 fmdrr d0, r1, r1 diff --git a/bsp/rm48x50/application/reg_test.asm b/bsp/rm48x50/application/reg_test.asm index e8614c92cb0bca1ca1025acd972a2250114a3415..6b6816460419bcf96679802de8eb03ca5ea1b744 100644 --- a/bsp/rm48x50/application/reg_test.asm +++ b/bsp/rm48x50/application/reg_test.asm @@ -70,19 +70,11 @@ .def vRegTestTask1 .ref ulRegTest1Counter .ref rt_thread_delay - .if (0) - .ref vPortTaskUsesFPU - .endif ;__TI_VFP_SUPPORT__ .text .arm vRegTestTask1: - .if (0) - ; Let the port layer know that this task needs its FPU context saving. - BL vPortTaskUsesFPU - .endif - ; Fill each general purpose register with a known value. mov r0, #0xFF mov r1, #0x11 @@ -99,7 +91,7 @@ vRegTestTask1: mov r12, #0xCC mov r14, #0xEE - .if (0) + .if (__TI_VFP_SUPPORT__) ; Fill each FPU register with a known value. vmov d0, r0, r1 vmov d1, r2, r3 @@ -128,7 +120,7 @@ vRegTestLoop1: BL rt_thread_delay LDMFD sp!, {r0-r3, r12} - .if (0) + .if (__TI_VFP_SUPPORT__) ; Check all the VFP registers still contain the values set above. ; First save registers that are clobbered by the test. STMFD sp!, { r0-r1 } @@ -285,11 +277,6 @@ vRegTestError1: .arm ; vRegTestTask2: - .if (0) - ; Let the port layer know that this task needs its FPU context saving. - BL vPortTaskUsesFPU - .endif - ; Fill each general purpose register with a known value. mov r0, #0xFF000000 mov r1, #0x11000000 @@ -306,7 +293,7 @@ vRegTestTask2: mov r12, #0xCC000000 mov r14, #0xEE000000 - .if (0) + .if (__TI_VFP_SUPPORT__) ; Fill each FPU register with a known value. vmov d0, r0, r1 @@ -329,7 +316,7 @@ vRegTestTask2: vRegTestLoop2: - .if (0) + .if (__TI_VFP_SUPPORT__) ; Check all the VFP registers still contain the values set above. ; First save registers that are clobbered by the test. STMFD sp!, { r0-r1 } diff --git a/libcpu/arm/rm48x50/context_ccs.asm b/libcpu/arm/rm48x50/context_ccs.asm index 9d74e2d8496a1103b43853ee52b68407ee5fa990..7e677b9d5f9a634ac6f96931e0b914ec852c3131 100644 --- a/libcpu/arm/rm48x50/context_ccs.asm +++ b/libcpu/arm/rm48x50/context_ccs.asm @@ -57,9 +57,33 @@ rt_hw_context_switch STMFD sp!, {r4} ; push cpsr + .if (__TI_VFP_SUPPORT__) + VMRS r4, fpexc + TST r4, #0x40000000 + BEQ __no_vfp_frame1 + VSTMDB sp!, {d0-d15} + VMRS r5, fpscr + ; TODO: add support for Common VFPv3. + ; Save registers like FPINST, FPINST2 + STMFD sp!, {r5} +__no_vfp_frame1 + STMFD sp!, {r4} + .endif + STR sp, [r0] ; store sp in preempted tasks TCB LDR sp, [r1] ; get new task stack pointer + .if (__TI_VFP_SUPPORT__) + LDMFD sp!, {r0} ; get fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame2 + LDMFD sp!, {r1} ; get fpscr + VMSR fpscr, r1 + VLDMIA sp!, {d0-d15} +__no_vfp_frame2 + VMSR fpexc, r0 + .endif + LDMFD sp!, {r4} ; pop new task cpsr to spsr MSR spsr_cxsf, r4 @@ -73,6 +97,17 @@ rt_hw_context_switch rt_hw_context_switch_to LDR sp, [r0] ; get new task stack pointer + .if (__TI_VFP_SUPPORT__) + LDMFD sp!, {r0} ; get fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_to + LDMFD sp!, {r1} ; get fpscr + VMSR fpscr, r1 + VLDMIA sp!, {d0-d15} +__no_vfp_frame_to + VMSR fpexc, r0 + .endif + LDMFD sp!, {r4} ; pop new task cpsr to spsr MSR spsr_cxsf, r4 @@ -100,6 +135,20 @@ _reswitch .def IRQ_Handler IRQ_Handler STMFD sp!, {r0-r12,lr} + + .if (__TI_VFP_SUPPORT__) + VMRS r0, fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_str_irq + VSTMDB sp!, {d0-d15} + VMRS r1, fpscr + ; TODO: add support for Common VFPv3. + ; Save registers like FPINST, FPINST2 + STMFD sp!, {r1} +__no_vfp_frame_str_irq + STMFD sp!, {r0} + .endif + BL rt_interrupt_enter BL rt_hw_trap_irq BL rt_interrupt_leave @@ -111,6 +160,17 @@ IRQ_Handler CMP r1, #1 BEQ rt_hw_context_switch_interrupt_do + .if (__TI_VFP_SUPPORT__) + LDMFD sp!, {r0} ; get fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_ldr_irq + LDMFD sp!, {r1} ; get fpscr + VMSR fpscr, r1 + VLDMIA sp!, {d0-d15} +__no_vfp_frame_ldr_irq + VMSR fpexc, r0 + .endif + LDMFD sp!, {r0-r12,lr} SUBS pc, lr, #4 @@ -122,6 +182,17 @@ rt_hw_context_switch_interrupt_do MOV r1, #0 ; clear flag STR r1, [r0] + .if (__TI_VFP_SUPPORT__) + LDMFD sp!, {r0} ; get fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_do1 + LDMFD sp!, {r1} ; get fpscr + VMSR fpscr, r1 + VLDMIA sp!, {d0-d15} +__no_vfp_frame_do1 + VMSR fpexc, r0 + .endif + LDMFD sp!, {r0-r12,lr} ; reload saved registers STMFD sp, {r0-r3} ; save r0-r3. We will restore r0-r3 in the SVC ; mode so there is no need to update SP. @@ -141,6 +212,19 @@ rt_hw_context_switch_interrupt_do ; use them here. STMFD sp!, {r3} ; push old task's cpsr + .if (__TI_VFP_SUPPORT__) + VMRS r0, fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_do2 + VSTMDB sp!, {d0-d15} + VMRS r1, fpscr + ; TODO: add support for Common VFPv3. + ; Save registers like FPINST, FPINST2 + STMFD sp!, {r1} +__no_vfp_frame_do2 + STMFD sp!, {r0} + .endif + LDR r4, pfromthread LDR r5, [r4] STR sp, [r5] ; store sp in preempted tasks's TCB @@ -149,6 +233,17 @@ rt_hw_context_switch_interrupt_do LDR r6, [r6] LDR sp, [r6] ; get new task's stack pointer + .if (__TI_VFP_SUPPORT__) + LDMFD sp!, {r0} ; get fpexc + TST r0, #0x40000000 + BEQ __no_vfp_frame_do3 + LDMFD sp!, {r1} ; get fpscr + VMSR fpscr, r1 + VLDMIA sp!, {d0-d15} +__no_vfp_frame_do3 + VMSR fpexc, r0 + .endif + LDMFD sp!, {r4} ; pop new task's cpsr to spsr MSR spsr_cxsf, r4 diff --git a/libcpu/arm/rm48x50/stack.c b/libcpu/arm/rm48x50/stack.c index faa39b4c136d843c466c888c4803394aa24ede55..a11d8896f9ee66c3e1c952acdea1d1195186004c 100644 --- a/libcpu/arm/rm48x50/stack.c +++ b/libcpu/arm/rm48x50/stack.c @@ -57,6 +57,22 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, else *(--stk) = SVCMODE; /* arm mode */ +#ifdef __TI_VFP_SUPPORT__ + #define VFP_DATA_NR 32 + { + int i; + + for (i = 0; i < VFP_DATA_NR; i++) + { + *(--stk) = 0; + } + /* FPSCR TODO: do we need to set the values other than 0? */ + *(--stk) = 0; + /* FPEXC. Enable the FVP by default. */ + *(--stk) = 0x40000000; + } +#endif + /* return task's current stack address */ return (rt_uint8_t *)stk; }