Lib: Extend MPLS label allocator bitmap

Add function lmap_last_one_in_range() for finding the last active label
in a label range.

3 files changed, 88 insertions, 0 deletions

diff --git a/lib/bitmap.c b/lib/bitmap.c
index 60883cf2..c036f80d 100644
--- a/lib/bitmap.c
+++ b/lib/bitmap.c
@@ -398,6 +398,81 @@ lmap_first_zero_in_range(struct lmap *b, uint lo, uint hi)
   return hi;
 }
 
+static inline int
+b1024_last_one(u32 *p)
+{
+  for (int i = 31; i >= 0; i--)
+    if (p[i])
+      return 32*i + (31 - u32_clz(p[i]));
+
+  return 1024;
+}
+
+static uint
+b1024_last_one_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 = (1u << hi1) - 1;
+  u32 val;
+
+  for (int i = hi0; i > (int) lo0; i--)
+  {
+    val = p[i] & mask;
+    mask = ~0;
+
+    if (val)
+      return 32*i + (31 - u32_clz(val));
+  }
+
+  {
+    mask &= ~((1u << lo1) - 1);
+    val = p[lo0] & mask;
+
+    if (val)
+      return 32*lo0 + (31 - u32_clz(val));
+  }
+
+  return hi;
+}
+
+uint
+lmap_last_one_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 (hi1 && (hi0 < b->size) && b->data[hi0])
+  {
+    uint min = (lo0 == hi0) ? lo1 : 0;
+    uint n0 = hi0;
+    uint n1 = b1024_last_one_in_range(b->data[n0], min, hi1);
+
+    if (n1 < hi1)
+      return (n0 << 10) + n1;
+  }
+
+  for (int i = (int)MIN(hi0, b->size) - 1; i >= (int) lo0; i--)
+  {
+    if (! b->data[i])
+      continue;
+
+    uint n0 = i;
+    uint n1 = b1024_last_one(b->data[n0]);
+
+    if ((n0 == lo0) && (n1 < lo1))
+      return hi;
+
+    return (n0 << 10) + n1;
+  }
+
+  return hi;
+}
+
 void
 lmap_check(struct lmap *b)
 {
diff --git a/lib/bitmap.h b/lib/bitmap.h
index 4d2dc2a8..e3351ab1 100644
--- a/lib/bitmap.h
+++ b/lib/bitmap.h
@@ -79,6 +79,7 @@ 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);
+uint lmap_last_one_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 3aeeaef5..39fbd0ed 100644
--- a/lib/bitmap_test.c
+++ b/lib/bitmap_test.c
@@ -216,6 +216,18 @@ t_lmap_set_clear_fill(void)
       lmap_clear(&b, n);
       expected[n] = 0;
     }
+
+    {
+      n = lmap_last_one_in_range(&b, lo, hi);
+      bt_assert(n >= lo);
+      bt_assert(n <= hi);
+
+      for (last = n + 1; last < hi; last++)
+	bt_assert(!expected[last]);
+
+      if (n < hi)
+	bt_assert(expected[n]);
+    }
   }
 
   uint cnt = 0;