summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--bsp/rm48x50/HALCoGen/source/sys_core.asm7
-rw-r--r--bsp/rm48x50/application/reg_test.asm21
-rw-r--r--libcpu/arm/rm48x50/context_ccs.asm95
-rw-r--r--libcpu/arm/rm48x50/stack.c16
4 files changed, 120 insertions, 19 deletions
diff --git a/bsp/rm48x50/HALCoGen/source/sys_core.asm b/bsp/rm48x50/HALCoGen/source/sys_core.asm
index f8d3ca6952..bfb4ad22a8 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 e8614c92cb..6b68164604 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 9d74e2d849..7e677b9d5f 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 faa39b4c13..a11d8896f9 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;
}
generated by cgit v1.3.1 (git 2.54.0) at 2026-10-30 22:24:42 +0000