From 14144030fab0c88a592815591b6601236d7b81ea Mon Sep 17 00:00:00 2001
From: Benjamin Valentin <benjamin.valentin@ml-pa.com>
Date: Tue, 4 Aug 2020 15:06:07 +0200
Subject: [PATCH] core/bitarithm: use __builtin_clz() for bitarithm_msb()

The `clz` instruction pretty much implements getting the most significant bit
in hardware, so use it instead of the software implementation.

This reults in both a reduction in code size as in a speedup:

master:

  text    data     bss     dec     hex filename
 14816     136    2424   17376    43e0 tests/bitarithm_timings/bin/same54-xpro/tests_bitarithm_timings.elf

 + bitarithm_msb: 3529411 iterations per second

this patch:

  text    data     bss     dec     hex filename
 14768     136    2424   17328    43b0 tests/bitarithm_timings/bin/same54-xpro/tests_bitarithm_timings.elf

 + bitarithm_msb: 9230761 iterations per second
---
 core/bitarithm.c         | 11 +----------
 core/include/bitarithm.h | 27 ++++++++++++++++++++++++++-
 2 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/core/bitarithm.c b/core/bitarithm.c
index e64b50cc86..93ffc763d7 100644
--- a/core/bitarithm.c
+++ b/core/bitarithm.c
@@ -23,11 +23,9 @@
 
 #include "bitarithm.h"
 
-unsigned bitarithm_msb(unsigned v)
+unsigned bitarith_msb_32bit_no_native_clz(unsigned v)
 {
     register unsigned r; /* result of log2(v) will go here */
-
-#if ARCH_32_BIT
     register unsigned shift;
 
     /* begin{code-style-ignore} */
@@ -37,13 +35,6 @@ unsigned bitarithm_msb(unsigned v)
     shift = (v > 0x3   ) << 1; v >>= shift; r |= shift;
                                             r |= (v >> 1);
     /* end{code-style-ignore} */
-#else
-    r = 0;
-    while (v >>= 1) { /* unroll for more speed... */
-        r++;
-    }
-
-#endif
 
     return r;
 }
diff --git a/core/include/bitarithm.h b/core/include/bitarithm.h
index 1278f35435..510b5c6a02 100644
--- a/core/include/bitarithm.h
+++ b/core/include/bitarithm.h
@@ -102,7 +102,7 @@ extern "C" {
  *
  * Source: http://graphics.stanford.edu/~seander/bithacks.html#IntegerLogObvious
  */
-unsigned bitarithm_msb(unsigned v);
+static inline unsigned bitarithm_msb(unsigned v);
 
 /**
  * @brief   Returns the number of the lowest '1' bit in a value
@@ -136,8 +136,33 @@ static inline uint8_t bitarithm_bits_set_u32(uint32_t v)
 uint8_t bitarithm_bits_set_u32(uint32_t v);
 #endif
 
+/**
+ * @brief   Returns the number of the highest '1' bit in a value
+ *
+ *          Internal software implementation for 32 bit platforms,
+ *          use @see bitarithm_msb in application code.
+ * @param[in]   v   Input value
+ * @return          Bit Number
+ */
+unsigned bitarith_msb_32bit_no_native_clz(unsigned v);
+
 /* implementations */
 
+static inline unsigned bitarithm_msb(unsigned v)
+{
+#if defined(BITARITHM_HAS_CLZ)
+    return 8 * sizeof(v) - __builtin_clz(v) - 1;
+#elif ARCH_32_BIT
+    return bitarith_msb_32bit_no_native_clz(v);
+#else
+    unsigned r = 0;
+    while (v >>= 1) { /* unroll for more speed... */
+        ++r;
+    }
+    return r;
+#endif
+}
+
 static inline unsigned bitarithm_lsb(unsigned v)
 #if defined(BITARITHM_LSB_BUILTIN)
 {