rm48x50: turn on VFP support

This support Common VFPv2 sub-architecture.

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

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 }

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
 

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