reworking the handling of freq_alloc_bitmap_t to make it more efficient

Co-Authored-By: @schmidtr
Assisted-by: Gemini:3 Flash

common/utils/bits.h

@@ -7,17 +7,59 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include <string.h>
 #include "assertions.h"
 
 uint64_t reverse_bits(uint64_t in, int n_bits);
 void reverse_bits_u8(uint8_t const* in, size_t sz, uint8_t* out);
 
-static inline int count_bits(uint8_t *arr, int sz)
+static inline int get_last_bit_index(const uint32_t *arr, int sz)
+{
+  AssertFatal(sz > 0, "Invalid size %d to get the first bit of array\n", sz);
+  for (int i = sz - 1; i >= 0; i--) {
+    if (arr[i] != 0) {
+      // 31 - clz gives the index of the highest bit (0-31)
+      return (i * 32) + (31 - __builtin_clz(arr[i]));
+    }
+  }
+  return -1;
+}
+
+static uint32_t bit_mask(int from_bit, int num_bits)
+{
+  int total = from_bit + num_bits;
+  uint32_t right_mask = total < 32 ? (0xFFFFFFFF >> (32 - total)) : 0xFFFFFFFF;
+  return (0xFFFFFFFF << from_bit) & right_mask;
+}
+
+static inline int get_first_bit_index_mask(const uint32_t *arr, int sz, int from_bit, int num_bits)
+{
+  // start i from first_bit
+  int i = from_bit / 32;
+  from_bit %= 32;
+  for (; i < sz && num_bits > 0; i++) {
+    uint32_t a = arr[i] & bit_mask(from_bit, num_bits);
+    if (a != 0) {
+      // Find the first set bit in this 32-bit word
+      return (i * 32) + __builtin_ctz(a);
+    }
+    num_bits -= 32 - from_bit;
+    from_bit = 0;
+  }
+  return -1;
+}
+
+static inline int get_first_bit_index(const uint32_t *arr, int sz)
+{
+  return get_first_bit_index_mask(arr, sz, 0, sz * 32);
+}
+
+static inline int count_bits(const uint32_t *arr, int sz)
 {
-  AssertFatal(sz % sizeof(int) == 0, "to implement if needed\n");
   int ret = 0;
-  for (uint *ptr = (uint *)arr; (uint8_t *)ptr < arr + sz; ptr++)
-    ret += __builtin_popcount(*ptr);
+  // sz is the number of uint32_t elements
+  for (int i = 0; i < sz; i++)
+    ret += __builtin_popcount(arr[i]);
   return ret;
 }