lib/bitmap.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <assert.h>
   5
   6 #include "../arch/arch.h"
   7 #include "bitmap.h"
   8 #include "../smalloc.h"
   9 #include "../minmax.h"
  10
  11 #if BITS_PER_LONG == 64
  12 #define UNIT_SIZE               8
  13 #define UNIT_SHIFT              6
  14 #elif BITS_PER_LONG == 32
  15 #define UNIT_SIZE               4
  16 #define UNIT_SHIFT              5
  17 #else
  18 #error "Number of arch bits unknown"
  19 #endif
  20
  21 #define BLOCKS_PER_UNIT         (1UL << UNIT_SHIFT)
  22 #define BLOCKS_PER_UNIT_MASK    (BLOCKS_PER_UNIT - 1)
  23
  24 #define firstfree_valid(b)      ((b)->first_free != (uint64_t) -1)
  25
  26 struct bitmap_level {
  27         int level;
  28         unsigned long map_size;
  29         unsigned long *map;
  30 };
  31
  32 struct bitmap {
  33         unsigned int nr_levels;
  34         struct bitmap_level *levels;
  35         uint64_t first_free;
  36 };
  37
  38 static unsigned long ulog64(unsigned long val, unsigned int log)
  39 {
  40         while (log-- && val)
  41                 val >>= UNIT_SHIFT;
  42
  43         return val;
  44 }
  45
  46 void bitmap_reset(struct bitmap *bitmap)
  47 {
  48         int i;
  49
  50         for (i = 0; i < bitmap->nr_levels; i++) {
  51                 struct bitmap_level *bl = &bitmap->levels[i];
  52
  53                 memset(bl->map, 0, bl->map_size * UNIT_SIZE);
  54         }
  55 }
  56
  57 void bitmap_free(struct bitmap *bitmap)
  58 {
  59         unsigned int i;
  60
  61         if (!bitmap)
  62                 return;
  63
  64         for (i = 0; i < bitmap->nr_levels; i++)
  65                 sfree(bitmap->levels[i].map);
  66
  67         sfree(bitmap->levels);
  68         sfree(bitmap);
  69 }
  70
  71 struct bitmap *bitmap_new(unsigned long nr_bits)
  72 {
  73         struct bitmap *bitmap;
  74         unsigned int i, levels;
  75
  76         bitmap = smalloc(sizeof(*bitmap));
  77         if (!bitmap)
  78                 return NULL;
  79
  80         levels = 1;
  81         i = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
  82         while (i > 1) {
  83                 i = (i + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
  84                 levels++;
  85         }
  86
  87         bitmap->nr_levels = levels;
  88         bitmap->levels = smalloc(bitmap->nr_levels * sizeof(struct bitmap_level));
  89         bitmap->first_free = 0;
  90
  91         for (i = 0; i < bitmap->nr_levels; i++) {
  92                 struct bitmap_level *bl = &bitmap->levels[i];
  93
  94                 bl->level = i;
  95                 bl->map_size = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
  96                 bl->map = smalloc(bl->map_size << UNIT_SHIFT);
  97                 if (!bl->map)
  98                         goto err;
  99
 100                 nr_bits = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
 101         }
 102
 103         bitmap_reset(bitmap);
 104         return bitmap;
 105 err:
 106         for (i = 0; i < bitmap->nr_levels; i++)
 107                 if (bitmap->levels[i].map)
 108                         sfree(bitmap->levels[i].map);
 109
 110         sfree(bitmap->levels);
 111         return NULL;
 112 }
 113
 114 static int bitmap_handler(struct bitmap *bitmap, uint64_t bit_nr,
 115                           int (*func)(struct bitmap_level *, unsigned long, unsigned int,
 116                           void *), void *data)
 117 {
 118         struct bitmap_level *bl;
 119         int i;
 120
 121         for (i = 0; i < bitmap->nr_levels; i++) {
 122                 unsigned long index = ulog64(bit_nr, i);
 123                 unsigned long offset = index >> UNIT_SHIFT;
 124                 unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
 125
 126                 bl = &bitmap->levels[i];
 127
 128                 if (func(bl, offset, bit, data))
 129                         return 1;
 130         }
 131
 132         return 0;
 133 }
 134
 135 static int bitmap_handler_topdown(struct bitmap *bitmap, uint64_t bit_nr,
 136         int (*func)(struct bitmap_level *, unsigned long, unsigned int, void *),
 137         void *data)
 138 {
 139         struct bitmap_level *bl;
 140         int i, level = bitmap->nr_levels;
 141
 142         for (i = bitmap->nr_levels - 1; i >= 0; i--) {
 143                 unsigned long index = ulog64(bit_nr, --level);
 144                 unsigned long offset = index >> UNIT_SHIFT;
 145                 unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
 146
 147                 bl = &bitmap->levels[i];
 148
 149                 if (func(bl, offset, bit, data))
 150                         return 1;
 151         }
 152
 153         return 0;
 154 }
 155
 156 static int bitmap_clear_fn(struct bitmap_level *bl, unsigned long offset,
 157                            unsigned int bit, void *unused)
 158 {
 159         if (!(bl->map[offset] & (1UL << bit)))
 160                 return 1;
 161
 162         bl->map[offset] &= ~(1UL << bit);
 163         return 0;
 164 }
 165
 166 void bitmap_clear(struct bitmap *bitmap, uint64_t bit_nr)
 167 {
 168         bitmap_handler(bitmap, bit_nr, bitmap_clear_fn, NULL);
 169 }
 170
 171 struct bitmap_set_data {
 172         unsigned int nr_bits;
 173         unsigned int set_bits;
 174         unsigned int fail_ok;
 175 };
 176
 177 static unsigned long bit_masks[] = {
 178         0x0000000000000000, 0x0000000000000001, 0x0000000000000003, 0x0000000000000007,
 179         0x000000000000000f, 0x000000000000001f, 0x000000000000003f, 0x000000000000007f,
 180         0x00000000000000ff, 0x00000000000001ff, 0x00000000000003ff, 0x00000000000007ff,
 181         0x0000000000000fff, 0x0000000000001fff, 0x0000000000003fff, 0x0000000000007fff,
 182         0x000000000000ffff, 0x000000000001ffff, 0x000000000003ffff, 0x000000000007ffff,
 183         0x00000000000fffff, 0x00000000001fffff, 0x00000000003fffff, 0x00000000007fffff,
 184         0x0000000000ffffff, 0x0000000001ffffff, 0x0000000003ffffff, 0x0000000007ffffff,
 185         0x000000000fffffff, 0x000000001fffffff, 0x000000003fffffff, 0x000000007fffffff,
 186         0x00000000ffffffff,
 187 #if BITS_PER_LONG == 64
 188         0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff, 0x0000000fffffffff,
 189         0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff, 0x000000ffffffffff,
 190         0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff, 0x00000fffffffffff,
 191         0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff, 0x0000ffffffffffff,
 192         0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff, 0x000fffffffffffff,
 193         0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff, 0x00ffffffffffffff,
 194         0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff, 0x0fffffffffffffff,
 195         0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff, 0xffffffffffffffff
 196 #endif
 197 };
 198
 199 static int bitmap_set_fn(struct bitmap_level *bl, unsigned long offset,
 200                          unsigned int bit, void *__data)
 201 {
 202         struct bitmap_set_data *data = __data;
 203         unsigned long mask, overlap;
 204         unsigned int nr_bits;
 205
 206         nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);
 207
 208         mask = bit_masks[nr_bits] << bit;
 209
 210         /*
 211          * Mask off any potential overlap, only sets contig regions
 212          */
 213         overlap = bl->map[offset] & mask;
 214         if (overlap == mask) {
 215                 assert(data->fail_ok);
 216                 return 1;
 217         }
 218
 219         while (overlap) {
 220                 unsigned long clear_mask = ~(1UL << ffz(~overlap));
 221
 222                 mask &= clear_mask;
 223                 overlap &= clear_mask;
 224                 nr_bits--;
 225         }
 226
 227         assert(mask);
 228         assert(!(bl->map[offset] & mask));
 229
 230         bl->map[offset] |= mask;
 231
 232         if (!bl->level)
 233                 data->set_bits = nr_bits;
 234
 235         data->nr_bits = 1;
 236         return bl->map[offset] != -1UL;
 237 }
 238
 239 static void __bitmap_set(struct bitmap *bitmap, uint64_t bit_nr,
 240                          struct bitmap_set_data *data)
 241 {
 242         unsigned int set_bits, nr_bits = data->nr_bits;
 243
 244         if (bitmap->first_free >= bit_nr &&
 245             bitmap->first_free < bit_nr + data->nr_bits)
 246                 bitmap->first_free = -1ULL;
 247
 248         set_bits = 0;
 249         while (nr_bits) {
 250                 bitmap_handler(bitmap, bit_nr, bitmap_set_fn, data);
 251                 set_bits += data->set_bits;
 252
 253                 if (data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
 254                         break;
 255
 256                 nr_bits -= data->set_bits;
 257                 bit_nr += data->set_bits;
 258
 259                 data->nr_bits = nr_bits;
 260                 data->fail_ok = 1;
 261         }
 262
 263         data->set_bits = set_bits;
 264 }
 265
 266 void bitmap_set(struct bitmap *bitmap, uint64_t bit_nr)
 267 {
 268         struct bitmap_set_data data = { .nr_bits = 1, };
 269
 270         __bitmap_set(bitmap, bit_nr, &data);
 271 }
 272
 273 unsigned int bitmap_set_nr(struct bitmap *bitmap, uint64_t bit_nr, unsigned int nr_bits)
 274 {
 275         struct bitmap_set_data data = { .nr_bits = nr_bits, };
 276
 277         __bitmap_set(bitmap, bit_nr, &data);
 278         return data.set_bits;
 279 }
 280
 281 static int bitmap_isset_fn(struct bitmap_level *bl, unsigned long offset,
 282                             unsigned int bit, void *unused)
 283 {
 284         return (bl->map[offset] & (1UL << bit)) != 0;
 285 }
 286
 287 int bitmap_isset(struct bitmap *bitmap, uint64_t bit_nr)
 288 {
 289         return bitmap_handler_topdown(bitmap, bit_nr, bitmap_isset_fn, NULL);
 290 }
 291
 292 static uint64_t bitmap_find_first_free(struct bitmap *bitmap, unsigned int level,
 293                                        uint64_t index)
 294 {
 295         unsigned long j;
 296         int i;
 297
 298         /*
 299          * Start at the bottom, then converge towards first free bit at the top
 300          */
 301         for (i = level; i >= 0; i--) {
 302                 struct bitmap_level *bl = &bitmap->levels[i];
 303
 304                 if (index >= bl->map_size) {
 305                         index = -1ULL;
 306                         break;
 307                 }
 308
 309                 for (j = index; j < bl->map_size; j++) {
 310                         if (bl->map[j] == -1UL)
 311                                 continue;
 312
 313                         /*
 314                          * First free bit here is our index into the first
 315                          * free bit at the next higher level
 316                          */
 317                         index = (j << UNIT_SHIFT) + ffz(bl->map[j]);
 318                         break;
 319                 }
 320         }
 321
 322         return index;
 323 }
 324
 325 uint64_t bitmap_first_free(struct bitmap *bitmap)
 326 {
 327         if (firstfree_valid(bitmap))
 328                 return bitmap->first_free;
 329
 330         bitmap->first_free = bitmap_find_first_free(bitmap, bitmap->nr_levels - 1, 0);
 331         return bitmap->first_free;
 332 }
 333
 334 struct bitmap_next_free_data {
 335         unsigned int level;
 336         unsigned long offset;
 337         uint64_t bit;
 338 };
 339
 340 static int bitmap_next_free_fn(struct bitmap_level *bl, unsigned long offset,
 341                                unsigned int bit, void *__data)
 342 {
 343         struct bitmap_next_free_data *data = __data;
 344         uint64_t mask = ~((1UL << ((data->bit & BLOCKS_PER_UNIT_MASK) + 1)) - 1);
 345
 346         if (!(mask & bl->map[offset]))
 347                 return 0;
 348
 349         if (bl->map[offset] != -1UL) {
 350                 data->level = bl->level;
 351                 data->offset = offset;
 352                 return 1;
 353         }
 354
 355         data->bit = (data->bit + BLOCKS_PER_UNIT - 1) / BLOCKS_PER_UNIT;
 356         return 0;
 357 }
 358
 359 /*
 360  * 'bit_nr' is already set. Find the next free bit after this one.
 361  */
 362 uint64_t bitmap_next_free(struct bitmap *bitmap, uint64_t bit_nr)
 363 {
 364         struct bitmap_next_free_data data = { .level = -1U, .bit = bit_nr, };
 365
 366         if (firstfree_valid(bitmap) && bit_nr < bitmap->first_free)
 367                 return bitmap->first_free;
 368
 369         if (!bitmap_handler(bitmap, bit_nr, bitmap_next_free_fn, &data))
 370                 return bitmap_first_free(bitmap);
 371
 372         assert(data.level != -1U);
 373
 374         return bitmap_find_first_free(bitmap, data.level, data.offset);
 375 }