vppinfra: refactor uword bitmaps

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

4 files changed, 92 insertions, 18 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 */
diff --git a/src/vppinfra/clib.h b/src/vppinfra/clib.h
index 7a8029a30bd..b8257bf1164 100644
--- a/src/vppinfra/clib.h
+++ b/src/vppinfra/clib.h
@@ -197,6 +197,15 @@
   decl __attribute ((destructor));		\
   decl
 
+always_inline uword
+pow2_mask (uword x)
+{
+#ifdef __BMI2__
+  return _bzhi_u64 (-1ULL, x);
+#endif
+  return ((uword) 1 << x) - (uword) 1;
+}
+
 #include <vppinfra/bitops.h>
 
 always_inline uword
@@ -237,15 +246,6 @@ min_log2_u64 (u64 x)
 }
 
 always_inline uword
-pow2_mask (uword x)
-{
-#ifdef __BMI2__
-  return _bzhi_u64 (-1ULL, x);
-#endif
-  return ((uword) 1 << x) - (uword) 1;
-}
-
-always_inline uword
 max_pow2 (uword x)
 {
   word y = (word) 1 << min_log2 (x);
diff --git a/src/vppinfra/format.h b/src/vppinfra/format.h
index 2cd636d0be8..a359328005f 100644
--- a/src/vppinfra/format.h
+++ b/src/vppinfra/format.h
@@ -322,8 +322,8 @@ u8 *format_c_identifier (u8 * s, va_list * va);
 /* Format hexdump with both hex and printable chars - compatible with text2pcap */
 u8 *format_hexdump (u8 * s, va_list * va);
 
-/* Format bitmap of array of u64 numbers */
-u8 *format_u64_bitmap (u8 *s, va_list *va);
+/* Format bitmap of array of uword numbers */
+u8 *format_uword_bitmap (u8 *s, va_list *va);
 
 /* Unix specific formats. */
 #ifdef CLIB_UNIX
diff --git a/src/vppinfra/std-formats.c b/src/vppinfra/std-formats.c
index bfe9844c832..4605ed5d75c 100644
--- a/src/vppinfra/std-formats.c
+++ b/src/vppinfra/std-formats.c
@@ -456,29 +456,30 @@ format_hexdump (u8 * s, va_list * args)
 }
 
 __clib_export u8 *
-format_u64_bitmap (u8 *s, va_list *args)
+format_uword_bitmap (u8 *s, va_list *args)
 {
-  u64 *bitmap = va_arg (*args, u64 *);
+  uword *bitmap = va_arg (*args, uword *);
   int n_uword = va_arg (*args, int);
-  u32 indent = format_get_indent (s);
+  uword indent = format_get_indent (s);
 
   s = format (s, "%6s", "");
 
-  for (int i = 60; i >= 0; i -= 4)
+  for (int i = uword_bits - 4; i >= 0; i -= 4)
     s = format (s, "%5d", i);
 
   vec_add1 (s, '\n');
 
   for (int j = n_uword - 1; j >= 0; j--)
     {
-      s = format (s, "%U0x%04x ", format_white_space, indent, j * 8);
-      for (int i = 63; i >= 0; i--)
+      s = format (s, "%U0x%04x ", format_white_space, indent,
+		  j * uword_bits / 8);
+      for (int i = uword_bits - 1; i >= 0; i--)
 	{
 	  vec_add1 (s, (1ULL << i) & bitmap[j] ? '1' : '.');
 	  if (i % 4 == 0)
 	    vec_add1 (s, ' ');
 	}
-      s = format (s, "0x%016lx", bitmap[j]);
+      s = format (s, uword_bits == 64 ? "0x%016lx" : "0x%08lx", bitmap[j]);
       if (j)
 	vec_add1 (s, '\n');
     }