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