diff --git a/bsp/rm48x50/HALCoGen/source/sys_core.asm b/bsp/rm48x50/HALCoGen/source/sys_core.asm
index bfb4ad22a84122886c471303873c7ca5198629e1..ae2a9ebd930415cefbc86eb2686edac2b0143fb6 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 16cd338fa03c12f1106117d1003d6dd6feff568d..e97ba448ac3a5e538d584792ba28faa50bda0c17 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 0ae5168fbac750a99bd5300f70ff29a3600c11f4..86fc630c8cd86e157987012899fc5ace7a96ef53 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 695844e300266dc49b06a78dd02a2063c5771c55..dc4113f5e47bb7b8f4afc46b0d93dbb6639436e6 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 a11d8896f9ee66c3e1c952acdea1d1195186004c..02dc84e6f10449e1886063c487fc4010da5b8008 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 */