[libcpu] Add builtin ffs implementation for Cortex-M4.

1 files changed, 44 insertions, 1 deletions

diff --git a/libcpu/arm/cortex-m4/cpuport.c b/libcpu/arm/cortex-m4/cpuport.c
index ef5fcc8b7c..47d8473997 100644
--- a/libcpu/arm/cortex-m4/cpuport.c
+++ b/libcpu/arm/cortex-m4/cpuport.c
@@ -1,7 +1,7 @@
 /*
  * File      : cpuport.c
  * This file is part of RT-Thread RTOS
- * COPYRIGHT (C) 2006 - 2012, RT-Thread Development Team
+ * COPYRIGHT (C) 2006 - 2014, RT-Thread Development Team
  *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
@@ -212,6 +212,9 @@ void rt_hw_hard_fault_exception(struct exception_stack_frame *exception_stack)
     while (1);
 }
 
+/**
+ * shutdown CPU
+ */
 void rt_hw_cpu_shutdown(void)
 {
     rt_kprintf("shutdown...\n");
@@ -219,3 +222,43 @@ void rt_hw_cpu_shutdown(void)
     RT_ASSERT(0);
 }
 
+#ifdef RT_USING_CPU_FFS
+/**
+ * This function finds the first bit set (beginning with the least significant bit)
+ * in value and return the index of that bit.
+ *
+ * Bits are numbered starting at 1 (the least significant bit).  A return value of
+ * zero from any of these functions means that the argument was zero.
+ *
+ * @return return the index of the first bit set. If value is 0, then this function
+ * shall return 0.
+ */
+#if defined(__CC_ARM)
+__asm int __rt_ffs(int value)
+{
+    CMP     r0, #0x00
+    BEQ     exit
+    RBIT    r0, r0
+    CLZ     r0, r0
+    ADDS    r0, r0, #0x01
+
+exit
+    BX      lr
+}
+#elif defined(__IAR_SYSTEMS_ICC__)
+int __rt_ffs(int value)
+{
+    if (value == 0) return value;
+
+    __ASM("RBIT r0, r0");
+    __ASM("CLZ  r0, r0");
+    __ASM("ADDS r0, r0, #0x01");
+}
+#elif defined(__GNUC__)
+int __rt_ffs(int value)
+{
+    return __builtin_ffs(value);
+}
+#endif
+
+#endif