rm48x50: VFP lazy stacking

You need to turn on RT_VFP_LAZY_STACKING in rtconfig.h. By default, RTT
will turn on VFP for all threads and stack all the VFP registers. When
doing lazy stacking, VFP will only be turned on for the thread who
issued VFP instructions.

Currently, if a thread turned on VFP, it cannot be turned off. RTT will
never know what time the thread doesn't need VFP any more. The thread
might could turn off the VFP for it self in proper time.

5 files changed, 27 insertions, 13 deletions

diff --git a/bsp/rm48x50/HALCoGen/source/sys_core.asm b/bsp/rm48x50/HALCoGen/source/sys_core.asm
index bfb4ad22a8..ae2a9ebd93 100644
--- a/bsp/rm48x50/HALCoGen/source/sys_core.asm
+++ b/bsp/rm48x50/HALCoGen/source/sys_core.asm
@@ -72,8 +72,8 @@ _coreInitRegisters_
         orr   r2,      r2,         #0xF00000
         mcr   p15,     #0x00,      r2,       c1, c0, #0x02
 
-        ; Enable FPV
-        fmrx  R2,      fpexc
+        .if (RT_VFP_LAZY_STACKING) = 0
+        fmrx  r2,      fpexc
         orr   r2,      r2,   #0x40000000
         fmxr  fpexc,   r2
 
@@ -93,6 +93,7 @@ _coreInitRegisters_
         fmdrr d13,        r1,     r1
         fmdrr d14,        r1,     r1
         fmdrr d15,        r1,     r1
+        .endif
         bl    next1
 next1
         bl    next2
diff --git a/bsp/rm48x50/HALCoGen/source/sys_intvecs.asm b/bsp/rm48x50/HALCoGen/source/sys_intvecs.asm
index 16cd338fa0..e97ba448ac 100644
--- a/bsp/rm48x50/HALCoGen/source/sys_intvecs.asm
+++ b/bsp/rm48x50/HALCoGen/source/sys_intvecs.asm
@@ -21,8 +21,7 @@
 
 resetEntry
         b   _c_int00
-undefEntry
-        b   undefEntry
+        b   turnon_VFP
 svcEntry
         b   svcEntry
 prefetchEntry
@@ -33,5 +32,13 @@ reservedEntry
         b   IRQ_Handler
         ldr pc,[pc,#-0x1b0]
 
-    
+    .sect ".text"
+turnon_VFP
+        ; Enable FPV
+        STMDB sp!,     {r0}
+        fmrx  r0,      fpexc
+        orr   r0,      r0,   #0x40000000
+        fmxr  fpexc,   r0
+        LDMIA sp!,     {r0}
+        subs  pc,      lr,   #4
 ;-------------------------------------------------------------------------------
diff --git a/bsp/rm48x50/rtconfig.h b/bsp/rm48x50/rtconfig.h
index 0ae5168fba..86fc630c8c 100644
--- a/bsp/rm48x50/rtconfig.h
+++ b/bsp/rm48x50/rtconfig.h
@@ -211,6 +211,7 @@
 // #define RTGUI_USING_MOUSE_CURSOR
 // </section>
 
+#define RT_VFP_LAZY_STACKING
 // </RDTConfigurator>
 
 #endif
diff --git a/libcpu/arm/rm48x50/context_ccs.asm b/libcpu/arm/rm48x50/context_ccs.asm
index 695844e300..dc4113f5e4 100644
--- a/libcpu/arm/rm48x50/context_ccs.asm
+++ b/libcpu/arm/rm48x50/context_ccs.asm
@@ -75,13 +75,13 @@ __no_vfp_frame1
 
     .if (__TI_VFP_SUPPORT__)
         LDMIA   sp!, {r0}       ; get fpexc
+        VMSR    fpexc,  r0      ; restore fpexc
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame2
         LDMIA   sp!, {r1}       ; get fpscr
         VMSR    fpscr, r1
 		VLDMIA  sp!, {d0-d15}
 __no_vfp_frame2
-        VMSR    fpexc, r0
     .endif
 
     LDMIA   sp!, {r4}           ; pop new task cpsr to spsr
@@ -99,13 +99,13 @@ rt_hw_context_switch_to
 
     .if (__TI_VFP_SUPPORT__)
         LDMIA   sp!, {r0}       ; get fpexc
+        VMSR    fpexc, r0
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame_to
         LDMIA   sp!, {r1}       ; get fpscr
         VMSR    fpscr, r1
 		VLDMIA  sp!, {d0-d15}
 __no_vfp_frame_to
-        VMSR    fpexc, r0
     .endif
 
     LDMIA   sp!, {r4}           ; pop new task cpsr to spsr
@@ -162,13 +162,13 @@ __no_vfp_frame_str_irq
 
     .if (__TI_VFP_SUPPORT__)
         LDMIA   sp!, {r0}       ; get fpexc
+        VMSR    fpexc, r0
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame_ldr_irq
         LDMIA   sp!, {r1}       ; get fpscr
         VMSR    fpscr, r1
 		VLDMIA  sp!, {d0-d15}
 __no_vfp_frame_ldr_irq
-        VMSR    fpexc, r0
     .endif
 
     LDMIA   sp!, {r0-r12,lr}
@@ -184,13 +184,13 @@ rt_hw_context_switch_interrupt_do
 
     .if (__TI_VFP_SUPPORT__)
         LDMIA   sp!, {r0}       ; get fpexc
+        VMSR    fpexc, r0
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame_do1
         LDMIA   sp!, {r1}       ; get fpscr
         VMSR    fpscr, r1
 		VLDMIA  sp!, {d0-d15}
 __no_vfp_frame_do1
-        VMSR    fpexc, r0
     .endif
 
     LDMIA   sp!, {r0-r12,lr}  ; reload saved registers
@@ -216,7 +216,7 @@ __no_vfp_frame_do1
 		VMRS    r0,  fpexc
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame_do2
-		VSTMDB  sp!, {d0-d15}
+        VSTMDB  sp!, {d0-d15}
         VMRS    r1, fpscr
         ; TODO: add support for Common VFPv3.
         ;       Save registers like FPINST, FPINST2
@@ -235,13 +235,13 @@ __no_vfp_frame_do2
 
     .if (__TI_VFP_SUPPORT__)
         LDMIA   sp!, {r0}       ; get fpexc
+        VMSR    fpexc, r0
         TST     r0,  #0x40000000
         BEQ     __no_vfp_frame_do3
         LDMIA   sp!, {r1}       ; get fpscr
         VMSR    fpscr, r1
 		VLDMIA  sp!, {d0-d15}
 __no_vfp_frame_do3
-        VMSR    fpexc, r0
     .endif
 
     LDMIA   sp!, {r4}         ; pop new task's cpsr to spsr
diff --git a/libcpu/arm/rm48x50/stack.c b/libcpu/arm/rm48x50/stack.c
index a11d8896f9..02dc84e6f1 100644
--- a/libcpu/arm/rm48x50/stack.c
+++ b/libcpu/arm/rm48x50/stack.c
@@ -58,8 +58,9 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
 		*(--stk) = SVCMODE;					/* arm mode   */
 
 #ifdef __TI_VFP_SUPPORT__
-    #define VFP_DATA_NR 32
+#ifndef RT_VFP_LAZY_STACKING
     {
+        #define VFP_DATA_NR 32
         int i;
 
         for (i = 0; i < VFP_DATA_NR; i++)
@@ -68,9 +69,13 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
         }
         /* FPSCR TODO: do we need to set the values other than 0? */
         *(--stk) = 0;
-        /* FPEXC. Enable the FVP by default. */
+        /* FPEXC. Enable the FVP if no lazy stacking. */
         *(--stk) = 0x40000000;
     }
+#else
+        /* FPEXC. Disable the FVP by default. */
+        *(--stk) = 0x00000000;
+#endif
 #endif
 
 	/* return task's current stack address */