[PATCH v3 1/2] lib: bitmap: add find_last_bit_from() and _find_last_bit_from()

[Yi Sun <yi.sun@unisoc.com>]

In some scenarios, it's not desirable to keep searching through the
beginning of the bitmap, but rather to search within a specific part.
The newly added function can accomplish this quickly.

Signed-off-by: Yi Sun <yi.sun@unisoc.com>
---
 include/linux/find.h | 33 +++++++++++++++++++++++++++++++++
 lib/find_bit.c       | 22 ++++++++++++++++++++++
 2 files changed, 55 insertions(+)

diff --git a/include/linux/find.h b/include/linux/find.h
index 6c2be8ca615d..17f1db7b41fb 100644
--- a/include/linux/find.h
+++ b/include/linux/find.h
@@ -33,6 +33,8 @@ unsigned long _find_first_and_and_bit(const unsigned long *addr1, const unsigned
 				      const unsigned long *addr3, unsigned long size);
 extern unsigned long _find_first_zero_bit(const unsigned long *addr, unsigned long size);
 extern unsigned long _find_last_bit(const unsigned long *addr, unsigned long size);
+extern unsigned long _find_last_bit_from(const unsigned long *addr, unsigned long size,
+					unsigned long offset);
 
 #ifdef __BIG_ENDIAN
 unsigned long _find_first_zero_bit_le(const unsigned long *addr, unsigned long size);
@@ -413,6 +415,37 @@ unsigned long find_last_bit(const unsigned long *addr, unsigned long size)
 }
 #endif
 
+/**
+ * find_last_bit_from - find the last set bit in a memory region
+ * @addr: The address to base the search on
+ * @size: The bitmap size in bits
+ * @offset: The bit number to start searching at
+ *
+ * Compared to the find_last_bit(),
+ * find_last_bit_from() has an additional parameter @offset,
+ * so it can search within a specific range of the bitmap,
+ * just like the find_next_bit().
+ *
+ * Returns the bit number of the last set bit, or size.
+ */
+static __always_inline
+unsigned long find_last_bit_from(const unsigned long *addr, unsigned long size,
+				unsigned long offset)
+{
+	if (small_const_nbits(size)) {
+		unsigned long val;
+
+		if (unlikely(offset >= size))
+			return size;
+
+		val = *addr & GENMASK(size - 1, offset);
+
+		return val ? __fls(val) : size;
+	}
+
+	return _find_last_bit_from(addr, size, offset);
+}
+
 /**
  * find_next_and_bit_wrap - find the next set bit in both memory regions
  * @addr1: The first address to base the search on
diff --git a/lib/find_bit.c b/lib/find_bit.c
index 5ac52dfce730..196b946dafff 100644
--- a/lib/find_bit.c
+++ b/lib/find_bit.c
@@ -237,6 +237,28 @@ unsigned long _find_last_bit(const unsigned long *addr, unsigned long size)
 EXPORT_SYMBOL(_find_last_bit);
 #endif
 
+unsigned long _find_last_bit_from(const unsigned long *addr, unsigned long size,
+				unsigned long offset)
+{
+	unsigned long val, idx, start_idx;
+
+	if (unlikely(offset >= size))
+		return size;
+
+	start_idx = offset / BITS_PER_LONG;
+	idx = (size - 1) / BITS_PER_LONG;
+	val = addr[idx] & BITMAP_LAST_WORD_MASK(size);
+
+	while (!val && idx > start_idx)
+		val = addr[--idx];
+
+	if (idx == start_idx)
+		val &= BITMAP_FIRST_WORD_MASK(offset);
+
+	return val ? idx * BITS_PER_LONG + __fls(val) : size;
+}
+EXPORT_SYMBOL(_find_last_bit_from);
+
 unsigned long find_next_clump8(unsigned long *clump, const unsigned long *addr,
 			       unsigned long size, unsigned long offset)
 {
-- 
2.34.1