[libc] Add ffs(), ffsl(), and ffsll()

Signed-off-by: Michael Brown <mcb30@ipxe.org>

1 files changed, 77 insertions, 4 deletions

diff --git a/src/include/strings.h b/src/include/strings.h
index dec756fe..fab26dc2 100644
--- a/src/include/strings.h
+++ b/src/include/strings.h
@@ -13,6 +13,54 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #include <bits/strings.h>
 
 /**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v x			Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int
+__constant_ffsll ( unsigned long long x ) {
+	int r = 0;
+
+	if ( ! ( x & 0x00000000ffffffffULL ) ) {
+		x >>= 32;
+		r += 32;
+	}
+	if ( ! ( x & 0x0000ffffUL ) ) {
+		x >>= 16;
+		r += 16;
+	}
+	if ( ! ( x & 0x00ff ) ) {
+		x >>= 8;
+		r += 8;
+	}
+	if ( ! ( x & 0x0f ) ) {
+		x >>= 4;
+		r += 4;
+	}
+	if ( ! ( x & 0x3 ) ) {
+		x >>= 2;
+		r += 2;
+	}
+	if ( ! ( x & 0x1 ) ) {
+		x >>= 1;
+		r += 1;
+	}
+	return ( x ? ( r + 1 ) : 0 );
+}
+
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v x			Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int
+__constant_ffsl ( unsigned long x ) {
+	return __constant_ffsll ( x );
+}
+
+/**
  * Find last (i.e. most significant) set bit
  *
  * @v x			Value
@@ -46,10 +94,7 @@ __constant_flsll ( unsigned long long x ) {
 		x >>= 1;
 		r += 1;
 	}
-	if ( x & 0x1 ) {
-		r += 1;
-	}
-	return r;
+	return ( x ? ( r + 1 ) : 0 );
 }
 
 /**
@@ -63,10 +108,38 @@ __constant_flsl ( unsigned long x ) {
 	return __constant_flsll ( x );
 }
 
+int __ffsll ( long long x );
+int __ffsl ( long x );
 int __flsll ( long long x );
 int __flsl ( long x );
 
 /**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v x			Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+#define ffsll( x ) \
+	( __builtin_constant_p ( x ) ? __constant_ffsll ( x ) : __ffsll ( x ) )
+
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v x			Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+#define ffsl( x ) \
+	( __builtin_constant_p ( x ) ? __constant_ffsl ( x ) : __ffsl ( x ) )
+
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v x			Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+#define ffs( x ) ffsl ( x )
+
+/**
  * Find last (i.e. most significant) set bit
  *
  * @v x			Value