rm48x50: turn on VFP support

This support Common VFPv2 sub-architecture.
This commit is contained in:
Grissiom 2013-05-31 18:32:21 +08:00
parent 939c58c295
commit ec1203bfab
4 changed files with 120 additions and 19 deletions

View File

@ -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

View File

@ -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 }

View File

@ -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

View File

@ -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;
}