vppinfra: refactor uword bitmaps

Type: improvement
Change-Id: I4f05a0435825cd23b8ad8a6f8f1397e60c522319
Signed-off-by: Damjan Marion <damarion@cisco.com>

1 files changed, 73 insertions, 0 deletions

diff --git a/src/vppinfra/bitops.h b/src/vppinfra/bitops.h
index 7a4be3ce4c3..d19046e1d24 100644
--- a/src/vppinfra/bitops.h
+++ b/src/vppinfra/bitops.h
@@ -200,6 +200,79 @@ next_with_same_number_of_set_bits (uword x)
        _tmp;                                                                  \
        i = get_lowest_set_bit_index (_tmp = clear_lowest_set_bit (_tmp)))
 
+static_always_inline uword
+uword_bitmap_count_set_bits (uword *bmp, uword n_uwords)
+{
+  uword count = 0;
+  while (n_uwords--)
+    count += count_set_bits (bmp++[0]);
+  return count;
+}
+
+static_always_inline uword
+uword_bitmap_is_bit_set (uword *bmp, uword bit_index)
+{
+  bmp += bit_index / uword_bits;
+  bit_index %= uword_bits;
+  return (bmp[0] >> bit_index) & 1;
+}
+
+static_always_inline void
+uword_bitmap_set_bits_at_index (uword *bmp, uword bit_index, uword n_bits)
+{
+  bmp += bit_index / uword_bits;
+  bit_index %= uword_bits;
+  uword max_bits = uword_bits - bit_index;
+
+  if (n_bits < max_bits)
+    {
+      bmp[0] |= pow2_mask (n_bits) << bit_index;
+      return;
+    }
+
+  bmp++[0] |= pow2_mask (max_bits) << bit_index;
+  n_bits -= max_bits;
+
+  for (; n_bits >= uword_bits; bmp++, n_bits -= uword_bits)
+    bmp[0] = ~0ULL;
+
+  if (n_bits)
+    bmp[0] |= pow2_mask (n_bits);
+}
+
+static_always_inline void
+uword_bitmap_clear_bits_at_index (uword *bmp, uword bit_index, uword n_bits)
+{
+  bmp += bit_index / uword_bits;
+  bit_index %= uword_bits;
+  uword max_bits = uword_bits - bit_index;
+
+  if (n_bits < max_bits)
+    {
+      bmp[0] &= ~(pow2_mask (n_bits) << bit_index);
+      return;
+    }
+
+  bmp++[0] &= ~(pow2_mask (max_bits) << bit_index);
+  n_bits -= max_bits;
+
+  for (; n_bits >= uword_bits; bmp++, n_bits -= uword_bits)
+    bmp[0] = 0ULL;
+
+  if (n_bits)
+    bmp[0] &= ~pow2_mask (n_bits);
+}
+
+static_always_inline int
+uword_bitmap_find_first_set (uword *bmp)
+{
+  uword *b = bmp;
+  while (b[0] == 0)
+    b++;
+
+  return (b - bmp) * uword_bits + get_lowest_set_bit_index (b[0]);
+}
+
 #else
 #warning "already included"
 #endif /* included_clib_bitops_h */