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

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

src/arch/i386/include/bits/strings.h

@@ -3,6 +3,50 @@
 
 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int __ffsl ( long value ) {
+	long lsb_minus_one;
+
+	/* If the input value is zero, the BSF instruction returns
+	 * ZF=0 and leaves an undefined value in the output register.
+	 * Perform this check in C rather than asm so that it can be
+	 * omitted in cases where the compiler is able to prove that
+	 * the input is non-zero.
+	 */
+	if ( value ) {
+		__asm__ ( "bsfl %1, %0"
+			  : "=r" ( lsb_minus_one )
+			  : "rm" ( value ) );
+		return ( lsb_minus_one + 1 );
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int __ffsll ( long long value ){
+	unsigned long high = ( value >> 32 );
+	unsigned long low = ( value >> 0 );
+
+	if ( low ) {
+		return ( __ffsl ( low ) );
+	} else if ( high ) {
+		return ( 32 + __ffsl ( high ) );
+	} else {
+		return 0;
+	}
+}
+
 /**
  * Find last (i.e. most significant) set bit
  *
@@ -13,7 +57,7 @@ static inline __attribute__ (( always_inline )) int __flsl ( long value ) {
 	long msb_minus_one;
 
 	/* If the input value is zero, the BSR instruction returns
-	 * ZF=1 and leaves an undefined value in the output register.
+	 * ZF=0 and leaves an undefined value in the output register.
 	 * Perform this check in C rather than asm so that it can be
 	 * omitted in cases where the compiler is able to prove that
 	 * the input is non-zero.

src/arch/x86_64/include/bits/strings.h

@@ -3,6 +3,42 @@
 
 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int __ffsll ( long long value ){
+	long long lsb_minus_one;
+
+	/* If the input value is zero, the BSF instruction returns
+	 * ZF=0 and leaves an undefined value in the output register.
+	 * Perform this check in C rather than asm so that it can be
+	 * omitted in cases where the compiler is able to prove that
+	 * the input is non-zero.
+	 */
+	if ( value ) {
+		__asm__ ( "bsfq %1, %0"
+			  : "=r" ( lsb_minus_one )
+			  : "rm" ( value ) );
+		return ( lsb_minus_one + 1 );
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Find first (i.e. least significant) set bit
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+static inline __attribute__ (( always_inline )) int __ffsl ( long value ) {
+
+	return __ffsll ( value );
+}
+
 /**
  * Find last (i.e. most significant) set bit
  *
@@ -13,7 +49,7 @@ static inline __attribute__ (( always_inline )) int __flsll ( long long value ){
 	long long msb_minus_one;
 
 	/* If the input value is zero, the BSR instruction returns
-	 * ZF=1 and leaves an undefined value in the output register.
+	 * ZF=0 and leaves an undefined value in the output register.
 	 * Perform this check in C rather than asm so that it can be
 	 * omitted in cases where the compiler is able to prove that
 	 * the input is non-zero.

src/include/strings.h

@@ -12,6 +12,54 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #include <string.h>
 #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
  *
@@ -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,9 +108,37 @@ __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
  *

src/tests/math_test.c

@@ -38,6 +38,26 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #include <ipxe/test.h>
 #include <ipxe/isqrt.h>
 
+/**
+ * Force a call to the non-constant implementation of ffsl()
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+__attribute__ (( noinline )) int ffsl_var ( long value ) {
+	return ffsl ( value );
+}
+
+/**
+ * Force a call to the non-constant implementation of ffsll()
+ *
+ * @v value		Value
+ * @ret lsb		Least significant bit set in value (LSB=1), or zero
+ */
+__attribute__ (( noinline )) int ffsll_var ( long long value ) {
+	return ffsll ( value );
+}
+
 /**
  * Force a call to the non-constant implementation of flsl()
  *
@@ -176,6 +196,44 @@ __attribute__ (( noinline )) int64_t s64mod_var ( int64_t dividend,
 	return check_divmod ( dividend, divisor, % );
 }
 
+/**
+ * Report a ffsl() test result
+ *
+ * @v value		Value
+ * @v lsb		Expected LSB
+ * @v file		Test code file
+ * @v line		Test code line
+ */
+static inline __attribute__ (( always_inline )) void
+ffsl_okx ( long value, int lsb, const char *file, unsigned int line ) {
+
+	/* Verify as a constant (requires to be inlined) */
+	okx ( ffsl ( value ) == lsb, file, line );
+
+	/* Verify as a non-constant */
+	okx ( ffsl_var ( value ) == lsb, file, line );
+}
+#define ffsl_ok( value, lsb ) ffsl_okx ( value, lsb, __FILE__, __LINE__ )
+
+/**
+ * Report a ffsll() test result
+ *
+ * @v value		Value
+ * @v lsb		Expected LSB
+ * @v file		Test code file
+ * @v line		Test code line
+ */
+static inline __attribute__ (( always_inline )) void
+ffsll_okx ( long long value, int lsb, const char *file, unsigned int line ) {
+
+	/* Verify as a constant (requires to be inlined) */
+	okx ( ffsll ( value ) == lsb, file, line );
+
+	/* Verify as a non-constant */
+	okx ( ffsll_var ( value ) == lsb, file, line );
+}
+#define ffsll_ok( value, lsb ) ffsll_okx ( value, lsb, __FILE__, __LINE__ )
+
 /**
  * Report a flsl() test result
  *
@@ -274,6 +332,22 @@ static void s64divmod_okx ( int64_t dividend, int64_t divisor,
  */
 static void math_test_exec ( void ) {
 
+	/* Test ffsl() */
+	ffsl_ok ( 0, 0 );
+	ffsl_ok ( 1, 1 );
+	ffsl_ok ( 255, 1 );
+	ffsl_ok ( 256, 9 );
+	ffsl_ok ( 257, 1 );
+	ffsl_ok ( 0x54850596, 2 );
+	ffsl_ok ( 0x80000000, 32 );
+
+	/* Test ffsll() */
+	ffsll_ok ( 0, 0 );
+	ffsll_ok ( 1, 1 );
+	ffsll_ok ( 0x6d63623330ULL, 5 );
+	ffsll_ok ( 0x80000000UL, 32 );
+	ffsll_ok ( 0x8000000000000000ULL, 64 );
+
 	/* Test flsl() */
 	flsl_ok ( 0, 0 );
 	flsl_ok ( 1, 1 );