From 01b27b9e9117e8a5eba3bac64f02213b1d4734bc Mon Sep 17 00:00:00 2001
From: Xiao Lifan <xiao605401969@live.com>
Date: Thu, 5 Dec 2019 21:50:48 +0800
Subject: [PATCH] [libcpu][c28x]add support for c28x mcu hardware fpu

---
 libcpu/ti-dsp/c28x/context.s | 38 +++++++++++++++++++++++++++++++++++-
 libcpu/ti-dsp/c28x/cpuport.c | 19 ++++++++++++++++++
 2 files changed, 56 insertions(+), 1 deletion(-)

diff --git a/libcpu/ti-dsp/c28x/context.s b/libcpu/ti-dsp/c28x/context.s
index f21e0ddb6e..460ad701e5 100644
--- a/libcpu/ti-dsp/c28x/context.s
+++ b/libcpu/ti-dsp/c28x/context.s
@@ -8,6 +8,7 @@
 ; 2018-09-01     xuzhuoyi     the first version.
 ; 2019-06-17     zhaoxiaowei  fix bugs of old c28x interrupt api.
 ; 2019-07-03     zhaoxiaowei  add _rt_hw_calc_csb function to support __rt_ffs.
+; 2019-12-05     xiaolifan    add support for hardware fpu32
 ;
 
     .ref   _rt_interrupt_to_thread
@@ -24,6 +25,16 @@
     .def   _rt_hw_interrupt_thread_switch
     .def   _rt_hw_interrupt_disable
     .def   _rt_hw_interrupt_enable
+    
+;workaround for importing fpu settings from the compiler
+	.cdecls C,NOLIST
+	%{
+		#ifdef __TMS320C28XX_FPU32__
+			#define __FPU32__ 1
+		#else
+			#define __FPU32__ 0
+		#endif
+	%}
 
 
 RT_CTX_SAVE  .macro      
@@ -38,12 +49,37 @@ RT_CTX_SAVE  .macro
     PUSH    XAR7
     PUSH    XT
     PUSH    RPC
-
+    
+	.if __FPU32__
+	PUSH	RB
+	MOV32	*SP++, STF
+	MOV32	*SP++, R0H
+	MOV32	*SP++, R1H
+	MOV32	*SP++, R2H
+	MOV32	*SP++, R3H
+	MOV32	*SP++, R4H
+	MOV32	*SP++, R5H
+	MOV32	*SP++, R6H
+	MOV32	*SP++, R7H
+	.endif
  
     .endm
 
 
 RT_CTX_RESTORE  .macro
+
+	.if __FPU32__
+	MOV32	R7H, *--SP, UNCF
+	MOV32	R6H, *--SP, UNCF
+	MOV32	R5H, *--SP, UNCF
+	MOV32	R4H, *--SP, UNCF
+	MOV32	R3H, *--SP, UNCF
+	MOV32	R2H, *--SP, UNCF
+	MOV32	R1H, *--SP, UNCF
+	MOV32	R0H, *--SP, UNCF
+	MOV32	STF, *--SP
+	POP		RB
+	.endif
                                   
     POP     RPC
     POP     XT
diff --git a/libcpu/ti-dsp/c28x/cpuport.c b/libcpu/ti-dsp/c28x/cpuport.c
index 3f88606c65..e16210f447 100644
--- a/libcpu/ti-dsp/c28x/cpuport.c
+++ b/libcpu/ti-dsp/c28x/cpuport.c
@@ -7,6 +7,7 @@
  * Date           Author       Notes
  * 2018-09-01     xuzhuoyi     the first version.
  * 2019-07-03     zhaoxiaowei  add support for __rt_ffs.
+ * 2019-12-05     xiaolifan    add support for hardware fpu32
  */
 
 #include <rtthread.h>
@@ -22,6 +23,7 @@ extern rt_uint16_t rt_hw_get_st0(void);
 extern rt_uint16_t rt_hw_get_st1(void);
 extern int rt_hw_calc_csb(int value);
 
+
 struct exception_stack_frame
 {
     rt_uint32_t t_st0;
@@ -49,6 +51,18 @@ struct stack_frame
     rt_uint32_t xt;
     rt_uint32_t rpc;
 
+#ifdef __TMS320C28XX_FPU32__
+    rt_uint32_t rb;
+    rt_uint32_t stf;
+    rt_uint32_t r0h;
+    rt_uint32_t r1h;
+    rt_uint32_t r2h;
+    rt_uint32_t r3h;
+    rt_uint32_t r4h;
+    rt_uint32_t r5h;
+    rt_uint32_t r6h;
+    rt_uint32_t r7h;
+#endif
 
 };
 
@@ -83,6 +97,11 @@ rt_uint8_t *rt_hw_stack_init(void       *tentry,
     stack_frame->exception_stack_frame.return_address = (unsigned long)tentry;          /* return_address */
     stack_frame->rpc = (unsigned long)texit;
 
+#ifdef __TMS320C28XX_FPU32__
+    stack_frame->stf = 0x00000200;
+    stack_frame->rb = 0;
+#endif
+
     /* return task's current stack address */
     return stk + sizeof(struct stack_frame);
 }