common: Add __fls function

Returns the most significant bit set.
Replace 31 - __builtin_clz(x), so x must be different from 0.
Use get_next_bit when not on the performance path,
on performance path set the bit field just after reading it.

BRANCH=smaug
BUG=none
TEST=compile, check Ryu still works.

Change-Id: Ie1a4cda4188f45b4bf92d0549d5c8fb401a30e5d
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/301300

7 files changed, 16 insertions, 13 deletions

diff --git a/chip/npcx/flash.c b/chip/npcx/flash.c
index 9a043f67ab..54e1db21b3 100644
--- a/chip/npcx/flash.c
+++ b/chip/npcx/flash.c
@@ -292,7 +292,7 @@ static int protect_to_reg(unsigned int start, unsigned int len,
 	blocks = len / size;
 
 	/* Determine bp = log2(blocks) with log2(0) = 0 */
-	bp = blocks ? (31 - __builtin_clz(blocks)) : 0;
+	bp = blocks ? __fls(blocks) : 0;
 
 	/* Clear bits */
 	*sr1 &= ~(SPI_FLASH_SR1_SEC | SPI_FLASH_SR1_TB
diff --git a/common/timer.c b/common/timer.c
index f6c69ac5d2..289314e74a 100644
--- a/common/timer.c
+++ b/common/timer.c
@@ -65,8 +65,8 @@ void process_timers(int overflow)
 			/* read atomically the current state of timer running */
 			check_timer = running_t0 = timer_running;
 			while (check_timer) {
-				int tskid = 31 - __builtin_clz(check_timer);
 
+				int tskid = __fls(check_timer);
 				/* timer has expired ? */
 				if (timer_deadline[tskid].val <= now.val)
 					expire_timer(tskid);
diff --git a/core/cortex-m/task.c b/core/cortex-m/task.c
index f7fbfded19..510e4cba32 100644
--- a/core/cortex-m/task.c
+++ b/core/cortex-m/task.c
@@ -266,7 +266,7 @@ void svc_handler(int desched, task_id_t resched)
 	tasks_ready |= 1 << resched;
 
 	ASSERT(tasks_ready);
-	next = __task_id_to_ptr(31 - __builtin_clz(tasks_ready));
+	next = __task_id_to_ptr(__fls(tasks_ready));
 
 #ifdef CONFIG_TASK_PROFILING
 	/* Track time in interrupts */
@@ -536,11 +536,11 @@ void mutex_unlock(struct mutex *mtx)
 			     : "r" (&mtx->lock), "r" (&mtx->waiters), "r" (0)
 			     : "cc");
 	while (waiters) {
-		task_id_t id = 31 - __builtin_clz(waiters);
+		task_id_t id = __fls(waiters);
+		waiters &= ~(1 << id);
 
 		/* Somebody is waiting on the mutex */
 		task_set_event(id, TASK_EVENT_MUTEX, 0);
-		waiters &= ~(1 << id);
 	}
 
 	/* Ensure no event is remaining from mutex wake-up */
diff --git a/core/cortex-m0/task.c b/core/cortex-m0/task.c
index 501659a48c..7179d05629 100644
--- a/core/cortex-m0/task.c
+++ b/core/cortex-m0/task.c
@@ -231,7 +231,7 @@ task_  __attribute__((noinline)) *__svc_handler(int desched, task_id_t resched)
 	tasks_ready |= 1 << resched;
 
 	ASSERT(tasks_ready);
-	next = __task_id_to_ptr(31 - __builtin_clz(tasks_ready));
+	next = __task_id_to_ptr(__fls(tasks_ready));
 
 #ifdef CONFIG_TASK_PROFILING
 	/* Track additional time in re-sched exception context */
@@ -530,11 +530,11 @@ void mutex_unlock(struct mutex *mtx)
 			     : "r" (&mtx->lock), "r" (&mtx->waiters), "r" (0)
 			     : "cc");
 	while (waiters) {
-		task_id_t id = 31 - __builtin_clz(waiters);
+		task_id_t id = __fls(waiters);
+		waiters &= ~(1 << id);
 
 		/* Somebody is waiting on the mutex */
 		task_set_event(id, TASK_EVENT_MUTEX, 0);
-		waiters &= ~(1 << id);
 	}
 
 	/* Ensure no event is remaining from mutex wake-up */
diff --git a/core/nds32/task.c b/core/nds32/task.c
index 5543d83cb1..3c7358abe6 100644
--- a/core/nds32/task.c
+++ b/core/nds32/task.c
@@ -272,7 +272,7 @@ void syscall_handler(int desched, task_id_t resched, int swirq)
 
 task_ *next_sched_task(void)
 {
-	return __task_id_to_ptr(31 - __builtin_clz(tasks_ready));
+	return __task_id_to_ptr(__fls(tasks_ready));
 }
 
 static inline void __schedule(int desched, int resched, int swirq)
@@ -453,11 +453,11 @@ void mutex_unlock(struct mutex *mtx)
 	mtx->lock = 0;
 
 	while (waiters) {
-		task_id_t id = 31 - __builtin_clz(waiters);
+		task_id_t id = __fls(waiters);
+		waiters &= ~(1 << id);
 
 		/* Somebody is waiting on the mutex */
 		task_set_event(id, TASK_EVENT_MUTEX, 0);
-		waiters &= ~(1 << id);
 	}
 
 	/* Ensure no event is remaining from mutex wake-up */
diff --git a/include/gpio.h b/include/gpio.h
index 2e7555d241..31f68b1c4b 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -47,7 +47,7 @@
 #define GPIO_INT_BOTH_DSLEEP (GPIO_INT_BOTH | GPIO_INT_DSLEEP)
 
 /* Convert GPIO mask to GPIO number / index. */
-#define GPIO_MASK_TO_NUM(mask) (31 - __builtin_clz(mask))
+#define GPIO_MASK_TO_NUM(mask) (__fls(mask))
 
 /* Convert a GPIO to a port + mask pair */
 #define GPIO_TO_PORT_MASK_PAIR(gpio) \
diff --git a/include/util.h b/include/util.h
index 86c7a8dc0c..8fec4a14e8 100644
--- a/include/util.h
+++ b/include/util.h
@@ -77,7 +77,10 @@
 /* True of x is a power of two */
 #define POWER_OF_TWO(x) (x && !(x & (x - 1)))
 
-/**
+/* find the most significant bit. Not defined in n == 0. */
+#define __fls(n) (31 - __builtin_clz(n))
+
+/*
  * macros for integer division with various rounding variants
  * default integer division rounds down.
  */