[v2 1/5] lib/bitmap: add bitmap_{set,get}_value_unaligned()

[Alexander Potapenko <glider@google.com>]

The two new functions allow setting/getting values of length up to
BITS_PER_LONG bits at arbitrary position in the bitmap.

Suggested-by: Yury Norov <yury.norov@gmail.com>
Signed-off-by: Alexander Potapenko <glider@google.com>
---
 include/linux/bitmap.h | 63 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 63 insertions(+)

diff --git a/include/linux/bitmap.h b/include/linux/bitmap.h
index 03644237e1efb..8e36ce07bafd4 100644
--- a/include/linux/bitmap.h
+++ b/include/linux/bitmap.h
@@ -77,6 +77,8 @@ struct device;
  *  bitmap_to_arr64(buf, src, nbits)            Copy nbits from buf to u64[] dst
  *  bitmap_get_value8(map, start)               Get 8bit value from map at start
  *  bitmap_set_value8(map, value, start)        Set 8bit value to map at start
+ *  bitmap_get_value_unaligned(map, start, nbits)  Get value up to BITS_PER_LONG
+ *  bitmap_set_value_unaligned(map, value, start, nbits)  Set value up to BITS_PER_LONG
  *
  * Note, bitmap_zero() and bitmap_fill() operate over the region of
  * unsigned longs, that is, bits behind bitmap till the unsigned long
@@ -583,6 +585,35 @@ static inline unsigned long bitmap_get_value8(const unsigned long *map,
 	return (map[index] >> offset) & 0xFF;
 }
 
+/**
+ * bitmap_get_value_unaligned - get an @nbits-bit value within a memory region
+ * @map: address to the bitmap memory region
+ * @start: bit offset of the value; may not be a multiple of 8
+ * @nbits: number of bits to get
+ *
+ * Returns the @nbits-sized value located at the @start bit offset within the
+ * @map memory region.
+ */
+static inline unsigned long bitmap_get_value_unaligned(const unsigned long *map,
+						       unsigned long start,
+						       unsigned long nbits)
+{
+	const size_t index = BIT_WORD(start);
+	const unsigned long offset = start % BITS_PER_LONG;
+	const unsigned long carry = (offset + nbits) % BITS_PER_LONG;
+	unsigned long hi, lo, result;
+
+	if (offset + nbits <= BITS_PER_LONG) {
+		result = map[index] >> (BITS_PER_LONG - offset - nbits);
+		return result & BITMAP_LAST_WORD_MASK(nbits);
+	}
+
+	hi = map[index] & BITMAP_LAST_WORD_MASK(BITS_PER_LONG - offset);
+	lo = map[index + 1] & BITMAP_FIRST_WORD_MASK(BITS_PER_LONG - carry);
+	lo >>= (BITS_PER_LONG - carry);
+	return (hi << carry) | lo;
+}
+
 /**
  * bitmap_set_value8 - set an 8-bit value within a memory region
  * @map: address to the bitmap memory region
@@ -599,6 +630,38 @@ static inline void bitmap_set_value8(unsigned long *map, unsigned long value,
 	map[index] |= value << offset;
 }
 
+/**
+ * bitmap_set_value_unaligned - set an @nbits-bit value within a memory region
+ * @map: address to the bitmap memory region
+ * @value: the value up to BITS_PER_LONG bits (will be clamped to @nbits)
+ * @start: bit offset of the value; may not be a multiple of 8
+ * @nbits: number of bits to set
+ */
+static inline void bitmap_set_value_unaligned(unsigned long *map,
+					      unsigned long value,
+					      unsigned long start,
+					      unsigned long nbits)
+{
+	const size_t index = BIT_WORD(start);
+	const unsigned long offset = start % BITS_PER_LONG;
+	unsigned long mask = BITMAP_LAST_WORD_MASK(nbits);
+	const unsigned long carry = (offset + nbits) % BITS_PER_LONG;
+
+	value &= mask;
+	if (offset + nbits <= BITS_PER_LONG) {
+		value <<= (BITS_PER_LONG - offset - nbits);
+		mask <<= (BITS_PER_LONG - offset - nbits);
+		map[index] &= ~mask;
+		map[index] |= value;
+		return;
+	}
+	map[index] &= ~BITMAP_LAST_WORD_MASK(BITS_PER_LONG - offset);
+	map[index] |= (value >> (carry));
+	value &= BITMAP_LAST_WORD_MASK(carry);
+	map[index + 1] &= ~BITMAP_FIRST_WORD_MASK(BITS_PER_LONG - carry);
+	map[index + 1] |= value << (BITS_PER_LONG - carry);
+}
+
 #endif /* __ASSEMBLY__ */
 
 #endif /* __LINUX_BITMAP_H */
-- 
2.41.0.255.g8b1d071c50-goog