find_first_set_bit tweaks

Correct sizing in m68k asm.
Make the gerneric version far more generic.

Change-Id: I32945f7431771979b3fe0da5472bdb110fd054ae

2 files changed, 9 insertions, 24 deletions

diff --git a/firmware/asm/ffs.c b/firmware/asm/ffs.c
index f68a9cef8c..6826f42e94 100644
--- a/firmware/asm/ffs.c
+++ b/firmware/asm/ffs.c
@@ -21,9 +21,7 @@
 #include "config.h"
 #include <inttypes.h>
 
-/* find_first_set_bit() - this is a C version of the ffs algorithm devised
- * by D.Seal and posted to comp.sys.arm on  16 Feb 1994.
- *
+/*
  * Find the index of the least significant set bit in the word.
  * return values:
  *   0  - bit 0 is set
@@ -32,25 +30,12 @@
  *   31 - bit 31 is set
  *   32 - no bits set
  */
-
-/* Table shared with assembly code */
-const uint8_t L_ffs_table[64] ICONST_ATTR =
-{
-/*   0   1   2   3   4   5   6   7           */
-/* ----------------------------------------- */
-    32,  0,  1, 12,  2,  6,  0, 13, /*  0- 7 */
-     3,  0,  7,  0,  0,  0,  0, 14, /*  8-15 */
-    10,  4,  0,  0,  8,  0,  0, 25, /* 16-23 */
-     0,  0,  0,  0,  0, 21, 27, 15, /* 24-31 */
-    31, 11,  5,  0,  0,  0,  0,  0, /* 32-39 */
-     9,  0,  0, 24,  0,  0, 20, 26, /* 40-47 */
-    30,  0,  0,  0,  0, 23,  0, 19, /* 48-55 */
-    29,  0, 22, 18, 28, 17, 16,  0, /* 56-63 */
-};
-
-#if !defined(CPU_COLDFIRE)
 int find_first_set_bit(uint32_t val)
 {
-    return L_ffs_table[((val & -val)*0x0450fbaf) >> 26];
+    if (val == 0)
+        return 32;
+
+    /* __builtin_ffs(l(l)): Returns one plus the index of the least significant
+       1-bit of x, or if x is zero, returns zero. */
+    return __builtin_ffs(val) - 1;
 }
-#endif
diff --git a/firmware/asm/m68k/ffs.S b/firmware/asm/m68k/ffs.S
index 2a39ae5bcd..87421ffe10 100644
--- a/firmware/asm/m68k/ffs.S
+++ b/firmware/asm/m68k/ffs.S
@@ -61,6 +61,7 @@ find_first_set_bit:
     | the top 24 bits of d0 are 0 so just load the value over it
     move.b  (%a0, %d1.l), %d0    |
     rts                          |
+    .size   find_first_set_bit, .-find_first_set_bit
 
     .section .irodata
 L_ffs_table:
@@ -74,5 +75,4 @@ L_ffs_table:
     .byte    9,  0,  0, 24,  0,  0, 20, 26  | 40-47
     .byte   30,  0,  0,  0,  0, 23,  0, 19  | 48-55
     .byte   29,  0, 22, 18, 28, 17, 16,  0  | 56-63
-
-    .size   find_first_set_bit, .-find_first_set_bit
+    .size    L_ffs_table, .-L_ffs_table