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"