[newlib 06/22] bitset: add BIT_FFS_AT() for finding the first bit set greater than a start bit

[Sebastian Huber]

From: D Scott Phillips <scottph at FreeBSD.org>

Reviewed by:	kib
Approved by:	scottl (implicit)
MFC after:	1 week
Sponsored by:	Ampere Computing, Inc.
Differential Revision:	https://reviews.freebsd.org/D26128
---
 newlib/libc/sys/rtems/include/sys/bitset.h | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/newlib/libc/sys/rtems/include/sys/bitset.h b/newlib/libc/sys/rtems/include/sys/bitset.h
index 9ff18f4f3..2e00dab7b 100644
--- a/newlib/libc/sys/rtems/include/sys/bitset.h
+++ b/newlib/libc/sys/rtems/include/sys/bitset.h
@@ -210,21 +210,32 @@
 		    (f)->__bits[__i]);					\
 } while (0)
 
-#define	BIT_FFS(_s, p) __extension__ ({					\
+/*
+ * Note that `start` and the returned value from BIT_FFS_AT are
+ * 1-based bit indices.
+ */
+#define	BIT_FFS_AT(_s, p, start) __extension__ ({			\
 	__size_t __i;							\
+	long __mask;							\
 	int __bit;							\
 									\
+	__mask = ~0UL << ((start) % _BITSET_BITS);			\
 	__bit = 0;							\
-	for (__i = 0; __i < __bitset_words((_s)); __i++) {		\
-		if ((p)->__bits[__i] != 0) {				\
-			__bit = ffsl((p)->__bits[__i]);			\
+	for (__i = __bitset_word((_s), (start));			\
+	    __i < __bitset_words((_s));					\
+	    __i++) {							\
+		if (((p)->__bits[__i] & __mask) != 0) {			\
+			__bit = ffsl((p)->__bits[__i] & __mask);	\
 			__bit += __i * _BITSET_BITS;			\
 			break;						\
 		}							\
+		__mask = ~0UL;						\
 	}								\
 	__bit;								\
 })
 
+#define	BIT_FFS(_s, p) BIT_FFS_AT((_s), (p), 0)
+
 #define	BIT_FLS(_s, p) __extension__ ({					\
 	__size_t __i;							\
 	int __bit;							\
-- 
2.35.3