From d68220d8667462339640cfda2f3d80484bd1f8db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=98=8E=E5=BE=B7=E6=97=A0=E6=95=8C=E8=B5=B5=E6=99=93?=
 =?UTF-8?q?=E8=96=87?= <1755923166@qq.com>
Date: Wed, 3 Jul 2019 19:31:54 +0800
Subject: [PATCH] [libcpu][c28x]Add __rt_ffs support

Use a native instruction "Count Sign Bits" to support fast ffs function, then add __rt_ffs support in C28x.
---
 libcpu/ti-dsp/c28x/context.s | 22 ++++++++++++++++++++++
 libcpu/ti-dsp/c28x/cpuport.c | 15 +++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/libcpu/ti-dsp/c28x/context.s b/libcpu/ti-dsp/c28x/context.s
index f0555d6d35..f21e0ddb6e 100644
--- a/libcpu/ti-dsp/c28x/context.s
+++ b/libcpu/ti-dsp/c28x/context.s
@@ -6,6 +6,8 @@
 ; Change Logs:
 ; Date           Author       Notes
 ; 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.
 ;
 
     .ref   _rt_interrupt_to_thread
@@ -15,6 +17,7 @@
     .def   _RTOSINT_Handler
     .def   _rt_hw_get_st0
     .def   _rt_hw_get_st1
+    .def   _rt_hw_calc_csb
     .def   _rt_hw_context_switch_interrupt
     .def   _rt_hw_context_switch
     .def   _rt_hw_context_switch_to
@@ -229,6 +232,25 @@ _rt_hw_get_st1:
     LRETR
     .endasmfunc
 
+; C28x do not have a build-in "__ffs" func in its C compiler.
+; We can use the "Count Sign Bits" (CSB) instruction to make one.
+; CSB will return the number of 0's minus 1 above the highest set bit.
+; The count is placed in T. For example:
+;    ACC        T     maxbit
+; 0x00000001    30      0
+; 0x00000010    26      4
+; 0x000001FF    22      8
+; 0x000001F0    22      8
+    .asmfunc
+_rt_hw_calc_csb:
+    MOV     AH, #0
+    CSB     ACC                   ; T = no. of sign bits - 1
+    MOVU    ACC, T                ; ACC = no. of sign bits - 1
+    SUBB    ACC, #30              ; ACC = ACC - 30
+    ABS     ACC                   ; ACC = |ACC|
+    lretr
+    .endasmfunc
+
 ;
 ; * void rt_hw_context_switch_to(rt_uint32 to);
 ; * r0 --> to
diff --git a/libcpu/ti-dsp/c28x/cpuport.c b/libcpu/ti-dsp/c28x/cpuport.c
index 2d42884bed..e34556c524 100644
--- a/libcpu/ti-dsp/c28x/cpuport.c
+++ b/libcpu/ti-dsp/c28x/cpuport.c
@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2018-09-01     xuzhuoyi     the first version.
+ * 2019-07-03     zhaoxiaowei  add support for __rt_ffs.
  */
 
 #include <rtthread.h>
@@ -19,6 +20,7 @@ static rt_err_t (*rt_exception_hook)(void *context) = RT_NULL;
 
 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
 {
@@ -102,6 +104,19 @@ struct exception_info
     struct stack_frame stack_frame;
 };
 
+#ifdef RT_USING_CPU_FFS
+/*
+ * This function called rt_hw_calc_csb to finds the first bit set in value.
+ * rt_hw_calc_csb is a native assembly program that use "CSB" instruction in C28x.
+ * When you use this function, remember that "int" is only 16-bit in C28x's C compiler.
+ * If value is a number bigger that 0xFFFF, trouble may be caused.
+ * Maybe change "int __rt_ffs(int value)" to "rt_int32_t __rt_ffs(rt_int32_t value)" will be better.
+ */
+int __rt_ffs(int value)
+{
+    return rt_hw_calc_csb(value);
+}
+#endif
 
 /**
  * shutdown CPU