update libcpu/arm/cortex-m4: support lazy stack optimized.

4 files changed, 151 insertions, 17 deletions

diff --git a/libcpu/arm/cortex-m4/context_gcc.S b/libcpu/arm/cortex-m4/context_gcc.S
index b66e257051..a6ecaf9fe9 100644
--- a/libcpu/arm/cortex-m4/context_gcc.S
+++ b/libcpu/arm/cortex-m4/context_gcc.S
@@ -12,10 +12,11 @@
  * 2009-10-11     Bernard      first version
  * 2012-01-01     aozima       support context switch load/store FPU register.
  * 2013-06-18     aozima       add restore MSP feature.
+ * 2013-06-23     aozima       support lazy stack optimized.
  */
 
 /**
- * @addtogroup STM32
+ * @addtogroup cortex-m4
  */
 /*@{*/
 
@@ -108,10 +109,21 @@ PendSV_Handler:
     MRS r1, psp                 /* get from thread stack pointer */
     
 #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    TST     lr, #0x10           /* if(!EXC_RETURN[4]) */
     VSTMDB  r1!, {d8 - d15}     /* push FPU register s16~s31 */
 #endif
-    
+	
     STMFD   r1!, {r4 - r11}     /* push r4 - r11 register */
+
+#if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    MOV     r4, #0x00           /* flag = 0 */
+
+    TST     lr, #0x10           /* if(!EXC_RETURN[4]) */
+    MOVEQ   r4, #0x01           /* flag = 1 */
+
+    STMFD   r1!, {r4}           /* push flag */
+#endif
+
     LDR r0, [r0]
     STR r1, [r0]                /* update from thread stack pointer */
 
@@ -120,10 +132,15 @@ swtich_to_thread:
     LDR r1, [r1]
     LDR r1, [r1]                /* load thread stack pointer */
 
+#if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    LDMFD   r1!, {r3}           /* pop flag */
+#endif
+
     LDMFD   r1!, {r4 - r11}     /* pop r4 - r11 register */
 
 #if defined (__VFP_FP__) && !defined(__SOFTFP__)
-    VLDMIA  r1!, {d8 - d15}     /* pop FPU register s16~s31 */
+    CMP     r3,  #0             /* if(flag_r3 != 0) */
+    VLDMIANE  r1!, {d8 - d15}   /* pop FPU register s16~s31 */
 #endif
 
     MSR psp, r1                 /* update stack pointer */
@@ -132,6 +149,12 @@ pendsv_exit:
     /* restore interrupt */
     MSR PRIMASK, r2
 
+#if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    ORR     lr, lr, #0x10       /* lr |=  (1 << 4), clean FPCA. */
+    CMP     r3,  #0             /* if(flag_r3 != 0) */
+    BICNE   lr, lr, #0x10       /* lr &= ~(1 << 4), set FPCA. */
+#endif
+
     ORR lr, lr, #0x04
     BX  lr
 
@@ -145,6 +168,13 @@ rt_hw_context_switch_to:
     LDR r1, =rt_interrupt_to_thread
     STR r0, [r1]
 
+#if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    /* CLEAR CONTROL.FPCA */
+    MRS     r2, CONTROL         /* read */
+    BIC     r2, #0x04           /* modify */
+    MSR     CONTROL, r2         /* write-back */
+#endif
+
     /* set from thread to 0 */
     LDR r1, =rt_interrupt_from_thread
     MOV r0, #0x0
diff --git a/libcpu/arm/cortex-m4/context_iar.S b/libcpu/arm/cortex-m4/context_iar.S
index 7943a211b5..e882f36f64 100644
--- a/libcpu/arm/cortex-m4/context_iar.S
+++ b/libcpu/arm/cortex-m4/context_iar.S
@@ -13,10 +13,11 @@
 ; * 2009-09-27     Bernard      add protect when contex switch occurs
 ; * 2012-01-01     aozima       support context switch load/store FPU register.
 ; * 2013-06-18     aozima       add restore MSP feature.
+; * 2013-06-23     aozima       support lazy stack optimized.
 ; */
 
 ;/**
-; * @addtogroup STM32
+; * @addtogroup cortex-m4
 ; */
 ;/*@{*/
 
@@ -107,10 +108,25 @@ PendSV_Handler:
     MRS     r1, psp                 ; get from thread stack pointer
 
 #if defined ( __ARMVFP__ )
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    BNE     skip_push_fpu
     VSTMDB  r1!, {d8 - d15}         ; push FPU register s16~s31
+skip_push_fpu
 #endif
 
     STMFD   r1!, {r4 - r11}         ; push r4 - r11 register
+
+#if defined ( __ARMVFP__ )
+    MOV     r4, #0x00               ; flag = 0
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    BNE     push_flag
+    MOV     r4, #0x01               ; flag = 1
+push_flag
+    ;STMFD   r1!, {r4}              ; push flag
+    SUB     r1, r1, #0x04
+    STR     r4, [r1]
+#endif
+
     LDR     r0, [r0]
     STR     r1, [r0]                ; update from thread stack pointer
 
@@ -119,10 +135,16 @@ swtich_to_thread
     LDR     r1, [r1]
     LDR     r1, [r1]                ; load thread stack pointer
 
+#if defined ( __ARMVFP__ )
+    LDMFD   r1!, {r3}               ; pop flag
+#endif
+
     LDMFD   r1!, {r4 - r11}         ; pop r4 - r11 register
 
 #if defined ( __ARMVFP__ )
+    CBZ     r3, skip_pop_fpu
     VLDMIA  r1!, {d8 - d15}         ; pop FPU register s16~s31
+skip_pop_fpu
 #endif
 
     MSR     psp, r1                 ; update stack pointer
@@ -131,6 +153,13 @@ pendsv_exit
     ; restore interrupt
     MSR     PRIMASK, r2
 
+#if defined ( __ARMVFP__ )
+    ORR     lr, lr, #0x10           ; lr |=  (1 << 4), clean FPCA.
+    CBZ     r3, return_without_fpu  ; if(flag_r3 != 0)
+    BIC     lr, lr, #0x10           ; lr &= ~(1 << 4), set FPCA.
+return_without_fpu
+#endif
+
     ORR     lr, lr, #0x04
     BX      lr
 
@@ -143,6 +172,13 @@ rt_hw_context_switch_to:
     LDR     r1, =rt_interrupt_to_thread
     STR     r0, [r1]
 
+#if defined ( __ARMVFP__ )
+    ; CLEAR CONTROL.FPCA
+    MRS     r2, CONTROL             ; read
+    BIC     r2, r2, #0x04           ; modify
+    MSR     CONTROL, r2             ; write-back
+#endif
+
     ; set from thread to 0
     LDR     r1, =rt_interrupt_from_thread
     MOV     r0, #0x0
diff --git a/libcpu/arm/cortex-m4/context_rvds.S b/libcpu/arm/cortex-m4/context_rvds.S
index 768c176810..189ae33db1 100644
--- a/libcpu/arm/cortex-m4/context_rvds.S
+++ b/libcpu/arm/cortex-m4/context_rvds.S
@@ -12,10 +12,11 @@
 ; * 2009-01-17     Bernard      first version.
 ; * 2012-01-01     aozima       support context switch load/store FPU register.
 ; * 2013-06-18     aozima       add restore MSP feature.
+; * 2013-06-23     aozima       support lazy stack optimized.
 ; */
 
 ;/**
-; * @addtogroup STM32
+; * @addtogroup cortex-m4
 ; */
 ;/*@{*/
 
@@ -110,10 +111,21 @@ PendSV_Handler   PROC
     MRS     r1, psp                 ; get from thread stack pointer
 
     IF      {FPU} != "SoftVFP"
-    VSTMFD  r1!, {d8 - d15}         ; push FPU register s16~s31
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    VSTMFDEQ  r1!, {d8 - d15}       ; push FPU register s16~s31
     ENDIF
 
     STMFD   r1!, {r4 - r11}         ; push r4 - r11 register
+
+    IF      {FPU} != "SoftVFP"
+    MOV     r4, #0x00               ; flag = 0
+
+    TST     lr, #0x10               ; if(!EXC_RETURN[4])
+    MOVEQ   r4, #0x01               ; flag = 1
+
+    STMFD   r1!, {r4}               ; push flag
+    ENDIF
+
     LDR     r0, [r0]
     STR     r1, [r0]                ; update from thread stack pointer
 
@@ -122,10 +134,15 @@ swtich_to_thread
     LDR     r1, [r1]
     LDR     r1, [r1]                ; load thread stack pointer
 
+    IF      {FPU} != "SoftVFP"
+    LDMFD   r1!, {r3}               ; pop flag
+    ENDIF
+
     LDMFD   r1!, {r4 - r11}         ; pop r4 - r11 register
 
     IF      {FPU} != "SoftVFP"
-    VLDMFD  r1!, {d8 - d15}         ; pop FPU register s16~s31
+    CMP     r3,  #0                 ; if(flag_r3 != 0)
+    VLDMFDNE  r1!, {d8 - d15}       ; pop FPU register s16~s31
     ENDIF
 
     MSR     psp, r1                 ; update stack pointer
@@ -134,6 +151,12 @@ pendsv_exit
     ; restore interrupt
     MSR     PRIMASK, r2
 
+    IF      {FPU} != "SoftVFP"
+    ORR     lr, lr, #0x10           ; lr |=  (1 << 4), clean FPCA.
+    CMP     r3,  #0                 ; if(flag_r3 != 0)
+    BICNE   lr, lr, #0x10           ; lr &= ~(1 << 4), set FPCA.
+    ENDIF
+
     ORR     lr, lr, #0x04
     BX      lr
     ENDP
@@ -149,6 +172,13 @@ rt_hw_context_switch_to    PROC
     LDR     r1, =rt_interrupt_to_thread
     STR     r0, [r1]
 
+    IF      {FPU} != "SoftVFP"
+    ; CLEAR CONTROL.FPCA
+    MRS     r2, CONTROL             ; read
+    BIC     r2, #0x04               ; modify
+    MSR     CONTROL, r2             ; write-back
+    ENDIF
+
     ; set from thread to 0
     LDR     r1, =rt_interrupt_from_thread
     MOV     r0, #0x0
diff --git a/libcpu/arm/cortex-m4/cpuport.c b/libcpu/arm/cortex-m4/cpuport.c
index 8f97758741..ef5fcc8b7c 100644
--- a/libcpu/arm/cortex-m4/cpuport.c
+++ b/libcpu/arm/cortex-m4/cpuport.c
@@ -16,6 +16,7 @@
  * 2012-12-11     lgnq         fixed the coding style.
  * 2012-12-23     aozima       stack addr align to 8byte.
  * 2012-12-29     Bernard      Add exception hook.
+ * 2013-06-23     aozima       support lazy stack optimized.
  */
 
 #include <rtthread.h>
@@ -41,6 +42,37 @@ struct exception_stack_frame
     rt_uint32_t lr;
     rt_uint32_t pc;
     rt_uint32_t psr;
+};
+
+struct stack_frame
+{
+#if USE_FPU
+    rt_uint32_t flag;
+#endif /* USE_FPU */
+
+    /* r4 ~ r11 register */
+    rt_uint32_t r4;
+    rt_uint32_t r5;
+    rt_uint32_t r6;
+    rt_uint32_t r7;
+    rt_uint32_t r8;
+    rt_uint32_t r9;
+    rt_uint32_t r10;
+    rt_uint32_t r11;
+
+    struct exception_stack_frame exception_stack_frame;
+};
+
+struct exception_stack_frame_fpu
+{
+    rt_uint32_t r0;
+    rt_uint32_t r1;
+    rt_uint32_t r2;
+    rt_uint32_t r3;
+    rt_uint32_t r12;
+    rt_uint32_t lr;
+    rt_uint32_t pc;
+    rt_uint32_t psr;
 
 #if USE_FPU
     /* FPU register */
@@ -65,8 +97,10 @@ struct exception_stack_frame
 #endif
 };
 
-struct stack_frame
+struct stack_frame_fpu
 {
+    rt_uint32_t flag;
+
     /* r4 ~ r11 register */
     rt_uint32_t r4;
     rt_uint32_t r5;
@@ -97,7 +131,7 @@ struct stack_frame
     rt_uint32_t s31;
 #endif
 
-    struct exception_stack_frame exception_stack_frame;
+    struct exception_stack_frame_fpu exception_stack_frame;
 };
 
 rt_uint8_t *rt_hw_stack_init(void       *tentry,
@@ -130,6 +164,10 @@ rt_uint8_t *rt_hw_stack_init(void       *tentry,
     stack_frame->exception_stack_frame.pc  = (unsigned long)tentry;    /* entry point, pc */
     stack_frame->exception_stack_frame.psr = 0x01000000L;              /* PSR */
 
+#if USE_FPU
+    stack_frame->flag = 0;
+#endif /* USE_FPU */
+
     /* return task's current stack address */
     return stk;
 }
@@ -141,20 +179,20 @@ rt_uint8_t *rt_hw_stack_init(void       *tentry,
  */
 void rt_hw_exception_install(rt_err_t (*exception_handle)(void* context))
 {
-	rt_exception_hook = exception_handle;
+    rt_exception_hook = exception_handle;
 }
 
 void rt_hw_hard_fault_exception(struct exception_stack_frame *exception_stack)
 {
-	extern long list_thread(void);
+    extern long list_thread(void);
 
-	if (rt_exception_hook != RT_NULL)
-	{
-		rt_err_t result;
+    if (rt_exception_hook != RT_NULL)
+    {
+        rt_err_t result;
 
-		result = rt_exception_hook(exception_stack);
-		if (result == RT_EOK) return;
-	}
+        result = rt_exception_hook(exception_stack);
+        if (result == RT_EOK) return;
+    }
 
     rt_kprintf("psr: 0x%08x\n", exception_stack->psr);
     rt_kprintf(" pc: 0x%08x\n", exception_stack->pc);