rm48x50: turn on VFP support

This support Common VFPv2 sub-architecture.

2 files changed, 111 insertions, 0 deletions

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