Lib: Indirect bitmap for MPLS label allocator

4 files changed, 301 insertions, 0 deletions

diff --git a/lib/bitmap.c b/lib/bitmap.c
index b6ea5a38..60883cf2 100644
--- a/lib/bitmap.c
+++ b/lib/bitmap.c
@@ -195,3 +195,217 @@ hmap_check(struct hmap *b)
     }
   }
 }
+
+
+/*
+ *	Indirect bitmap for MPLS labels (20 bit range)
+ */
+
+void
+lmap_init(struct lmap *b, pool *p)
+{
+  b->slab = sl_new(p, 128);
+  b->size = 8;
+  b->data = mb_allocz(p, b->size * sizeof(u32 *));
+  b->root = sl_allocz(b->slab);
+}
+
+static void
+lmap_grow(struct lmap *b, uint need)
+{
+  uint old_size = b->size;
+
+  while (b->size < need)
+    b->size *= 2;
+
+  b->data = mb_realloc(b->data, b->size * sizeof(u32 *));
+
+  memset(b->data + old_size, 0, (b->size - old_size) * sizeof(u32 *));
+}
+
+void
+lmap_free(struct lmap *b)
+{
+  rfree(b->slab);
+  mb_free(b->data);
+  memset(b, 0, sizeof(struct lmap));
+}
+
+static inline int
+b1024_and(u32 *p)
+{
+  for (int i = 0; i < 32; i++)
+    if (~p[i])
+      return 0;
+
+  return 1;
+}
+
+static inline int
+b1024_or(u32 *p)
+{
+  for (int i = 0; i < 32; i++)
+    if (p[i])
+      return 1;
+
+  return 0;
+}
+
+int
+lmap_test(struct lmap *b, uint n)
+{
+  uint n0 = n >> 10;
+  uint n1 = n & 0x3ff;
+
+  return (n0 < b->size) && b->data[n0] && BIT32_TEST(b->data[n0], n1);
+}
+
+void
+lmap_set(struct lmap *b, uint n)
+{
+  uint n0 = n >> 10;
+  uint n1 = n & 0x3ff;
+
+  if (n0 >= b->size)
+    lmap_grow(b, n0 + 1);
+
+  if (! b->data[n0])
+    b->data[n0] = sl_allocz(b->slab);
+
+  BIT32_SET(b->data[n0], n1);
+
+  if (b1024_and(b->data[n0]))
+    BIT32_SET(b->root, n0);
+}
+
+void
+lmap_clear(struct lmap *b, uint n)
+{
+  uint n0 = n >> 10;
+  uint n1 = n & 0x3ff;
+
+  if (n0 >= b->size)
+    return;
+
+  if (! b->data[n0])
+    return;
+
+  BIT32_CLR(b->data[n0], n1);
+  BIT32_CLR(b->root, n0);
+
+  if (!b1024_or(b->data[n0]))
+  {
+    sl_free(b->data[n0]);
+    b->data[n0] = NULL;
+  }
+}
+
+static inline int
+b1024_first_zero(u32 *p)
+{
+  for (int i = 0; i < 32; i++)
+    if (~p[i])
+      return 32*i + u32_ctz(~p[i]);
+
+  return 1024;
+}
+
+uint
+lmap_first_zero(struct lmap *b)
+{
+  uint n0 = b1024_first_zero(b->root);
+  uint n1 = ((n0 < b->size) && b->data[n0]) ?
+    b1024_first_zero(b->data[n0]) : 0;
+
+  return (n0 << 10) + n1;
+}
+
+static uint
+b1024_first_zero_in_range(u32 *p, uint lo, uint hi)
+{
+  uint lo0 = lo >> 5;
+  uint lo1 = lo & 0x1f;
+  uint hi0 = hi >> 5;
+  uint hi1 = hi & 0x1f;
+  u32 mask = (1 << lo1) - 1;
+  u32 val;
+
+  for (uint i = lo0; i < hi0; i++)
+  {
+    val = p[i] | mask;
+    mask = 0;
+
+    if (~val)
+      return 32*i + u32_ctz(~val);
+  }
+
+  if (hi1)
+  {
+    mask |= ~((1u << hi1) - 1);
+    val = p[hi0] | mask;
+
+    if (~val)
+      return 32*hi0 + u32_ctz(~val);
+  }
+
+  return hi;
+}
+
+uint
+lmap_first_zero_in_range(struct lmap *b, uint lo, uint hi)
+{
+  uint lo0 = lo >> 10;
+  uint lo1 = lo & 0x3ff;
+  uint hi0 = hi >> 10;
+  uint hi1 = hi & 0x3ff;
+
+  if (lo1)
+  {
+    uint max = (lo0 == hi0) ? hi1 : 1024;
+    uint n0 = lo0;
+    uint n1 = ((n0 < b->size) && b->data[n0]) ?
+      b1024_first_zero_in_range(b->data[n0], lo1, max) : lo1;
+
+    if (n1 < 1024)
+      return (n0 << 10) + n1;
+
+    lo0++;
+    lo1 = 0;
+  }
+
+  if (lo0 < hi0)
+  {
+    uint n0 = b1024_first_zero_in_range(b->root, lo0, hi0);
+
+    if (n0 < hi0)
+    {
+      uint n1 = ((n0 < b->size) && b->data[n0]) ?
+	b1024_first_zero(b->data[n0]) : 0;
+
+      return (n0 << 10) + n1;
+    }
+  }
+
+  if (hi1)
+  {
+    uint n0 = hi0;
+    uint n1 = ((n0 < b->size) && b->data[n0]) ?
+      b1024_first_zero_in_range(b->data[n0], 0, hi1) : 0;
+
+    return (n0 << 10) + n1;
+  }
+
+  return hi;
+}
+
+void
+lmap_check(struct lmap *b)
+{
+  for (int i = 0; i < (int) b->size; i++)
+  {
+    int x = b->data[i] && b1024_and(b->data[i]);
+    int y = !!BIT32_TEST(b->root, i);
+    if (x != y)
+      bug("Inconsistent data on %d (%d vs %d)", i, x, y);
+  }
+}
diff --git a/lib/bitmap.h b/lib/bitmap.h
index 0093cd18..4d2dc2a8 100644
--- a/lib/bitmap.h
+++ b/lib/bitmap.h
@@ -60,4 +60,25 @@ void hmap_clear(struct hmap *b, uint n);
 u32 hmap_first_zero(struct hmap *b);
 void hmap_check(struct hmap *b);
 
+
+struct lmap
+{
+  slab *slab;
+  uint size;
+  u32 **data;
+  u32 *root;
+};
+
+static inline uint lmap_max(struct lmap *b)
+{ return b->size << 10; }
+
+void lmap_init(struct lmap *b, pool *p);
+void lmap_free(struct lmap *b);
+int lmap_test(struct lmap *b, uint n);
+void lmap_set(struct lmap *b, uint n);
+void lmap_clear(struct lmap *b, uint n);
+uint lmap_first_zero(struct lmap *b);
+uint lmap_first_zero_in_range(struct lmap *b, uint lo, uint hi);
+void lmap_check(struct lmap *b);
+
 #endif
diff --git a/lib/bitmap_test.c b/lib/bitmap_test.c
index 07860c94..3aeeaef5 100644
--- a/lib/bitmap_test.c
+++ b/lib/bitmap_test.c
@@ -170,6 +170,70 @@ t_hmap_set_clear_fill(void)
   return 1;
 }
 
+static int
+t_lmap_set_clear_fill(void)
+{
+  struct lmap b;
+
+  lmap_init(&b, &root_pool);
+
+  char expected[MAX_NUM] = {};
+  uint i, j, n;
+
+  for (i = 0; i < STEP_NUM; i++)
+  {
+    uint last = 0;
+    uint lo = bt_random() % (1 << 19);
+    uint hi = lo + 2 * STEP_SET;
+    uint step_set = bt_random() % STEP_SET;
+    uint step_clr = bt_random() % STEP_CLR;
+
+    for (j = 0; j < step_set; j++)
+    {
+      n = lmap_first_zero_in_range(&b, lo, hi);
+      bt_assert(n >= lo);
+      bt_assert(n <= hi);
+
+      for (last = lo; last < n; last++)
+	bt_assert(expected[last]);
+
+      if (n >= hi)
+	break;
+
+      bt_assert(!expected[n]);
+
+      lmap_set(&b, n);
+      expected[n] = 1;
+    }
+
+    for (j = 0; j < step_clr; j++)
+    {
+      n = lo + bt_random() % (step_set + 1);
+
+      if (!expected[n])
+	continue;
+
+      lmap_clear(&b, n);
+      expected[n] = 0;
+    }
+  }
+
+  uint cnt = 0;
+  for (i = 0; i < MAX_NUM; i++)
+  {
+    if (lmap_test(&b, i) != expected[i])
+      bt_abort_msg("Bitmap mismatch on %d (should be %d %d)", i, lmap_test(&b, i), expected[i]);
+
+    if (expected[i])
+      cnt++;
+  }
+  // bt_log("Total %u", cnt);
+
+  lmap_check(&b);
+
+  return 1;
+}
+
 int
 main(int argc, char *argv[])
 {
@@ -178,6 +242,7 @@ main(int argc, char *argv[])
   bt_test_suite(t_bmap_set_clear_random, "BMap - random sequence of sets / clears");
   bt_test_suite(t_hmap_set_clear_random, "HMap - random sequence of sets / clears");
   bt_test_suite(t_hmap_set_clear_fill, "HMap - linear sets and random clears");
+  bt_test_suite(t_lmap_set_clear_fill, "LMap - linear sets and random clears");
 
   return bt_exit_value();
 }
diff --git a/test/bt-utils.c b/test/bt-utils.c
index fb42cd35..57b8b824 100644
--- a/test/bt-utils.c
+++ b/test/bt-utils.c
@@ -16,6 +16,7 @@
 #include "nest/bird.h"
 #include "nest/route.h"
 #include "nest/protocol.h"
+#include "nest/mpls.h"
 
 #include "sysdep/unix/unix.h"
 #include "sysdep/unix/krt.h"