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, 0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff,
 197         0x0000000fffffffff, 0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff,
 198         0x000000ffffffffff, 0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff,
 199         0x00000fffffffffff, 0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff,
 200         0x0000ffffffffffff, 0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff,
 201         0x000fffffffffffff, 0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff,
 202         0x00ffffffffffffff, 0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff,
 203         0x0fffffffffffffff, 0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff,
 204         0xffffffffffffffff };
 205
 206 static int bitmap_set_fn(struct bitmap_level *bl, unsigned long offset,
 207                          unsigned int bit, void *__data)
 208 {
 209         struct bitmap_set_data *data = __data;
 210         unsigned long mask, overlap;
 211         unsigned int nr_bits;
 212
 213         nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);
 214
 215         mask = bit_masks[nr_bits] << bit;
 216
 217         /*
 218          * Mask off any potential overlap, only sets contig regions
 219          */
 220         overlap = bl->map[offset] & mask;
 221         if (overlap == mask) {
 222                 assert(data->fail_ok);
 223                 return 1;
 224         }
 225
 226         while (overlap) {
 227                 unsigned long clear_mask = ~(1UL << ffz(~overlap));
 228
 229                 mask &= clear_mask;
 230                 overlap &= clear_mask;
 231                 nr_bits--;
 232         }
 233
 234         assert(mask);
 235         assert(!(bl->map[offset] & mask));
 236
 237         bl->map[offset] |= mask;
 238
 239         if (!bl->level)
 240                 data->set_bits = nr_bits;
 241
 242         data->nr_bits = 1;
 243         return bl->map[offset] != -1UL;
 244 }
 245
 246 static void __bitmap_set(struct bitmap *bitmap, uint64_t bit_nr,
 247                          struct bitmap_set_data *data)
 248 {
 249         unsigned int set_bits, nr_bits = data->nr_bits;
 250
 251         if (bitmap->first_free >= bit_nr &&
 252             bitmap->first_free < bit_nr + data->nr_bits)
 253                 bitmap->first_free = -1ULL;
 254
 255         set_bits = 0;
 256         while (nr_bits) {
 257                 bitmap_handler(bitmap, bit_nr, bitmap_set_fn, data);
 258                 set_bits += data->set_bits;
 259
 260                 if (data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
 261                         break;
 262
 263                 nr_bits -= data->set_bits;
 264                 bit_nr += data->set_bits;
 265
 266                 data->nr_bits = nr_bits;
 267                 data->fail_ok = 1;
 268         }
 269
 270         data->set_bits = set_bits;
 271 }
 272
 273 void bitmap_set(struct bitmap *bitmap, uint64_t bit_nr)
 274 {
 275         struct bitmap_set_data data = { .nr_bits = 1, };
 276
 277         __bitmap_set(bitmap, bit_nr, &data);
 278 }
 279
 280 unsigned int bitmap_set_nr(struct bitmap *bitmap, uint64_t bit_nr, unsigned int nr_bits)
 281 {
 282         struct bitmap_set_data data = { .nr_bits = nr_bits, };
 283
 284         __bitmap_set(bitmap, bit_nr, &data);
 285         return data.set_bits;
 286 }
 287
 288 static int bitmap_isset_fn(struct bitmap_level *bl, unsigned long offset,
 289                             unsigned int bit, void *unused)
 290 {
 291         return (bl->map[offset] & (1UL << bit)) != 0;
 292 }
 293
 294 int bitmap_isset(struct bitmap *bitmap, uint64_t bit_nr)
 295 {
 296         return bitmap_handler_topdown(bitmap, bit_nr, bitmap_isset_fn, NULL);
 297 }
 298
 299 static uint64_t bitmap_find_first_free(struct bitmap *bitmap, unsigned int level,
 300                                        uint64_t index)
 301 {
 302         unsigned long j;
 303         int i;
 304
 305         /*
 306          * Start at the bottom, then converge towards first free bit at the top
 307          */
 308         for (i = level; i >= 0; i--) {
 309                 struct bitmap_level *bl = &bitmap->levels[i];
 310
 311                 if (index >= bl->map_size) {
 312                         index = -1ULL;
 313                         break;
 314                 }
 315
 316                 for (j = index; j < bl->map_size; j++) {
 317                         if (bl->map[j] == -1UL)
 318                                 continue;
 319
 320                         /*
 321                          * First free bit here is our index into the first
 322                          * free bit at the next higher level
 323                          */
 324                         index = (j << UNIT_SHIFT) + ffz(bl->map[j]);
 325                         break;
 326                 }
 327         }
 328
 329         return index;
 330 }
 331
 332 uint64_t bitmap_first_free(struct bitmap *bitmap)
 333 {
 334         if (firstfree_valid(bitmap))
 335                 return bitmap->first_free;
 336
 337         bitmap->first_free = bitmap_find_first_free(bitmap, bitmap->nr_levels - 1, 0);
 338         return bitmap->first_free;
 339 }
 340
 341 struct bitmap_next_free_data {
 342         unsigned int level;
 343         unsigned long offset;
 344         uint64_t bit;
 345 };
 346
 347 static int bitmap_next_free_fn(struct bitmap_level *bl, unsigned long offset,
 348                                unsigned int bit, void *__data)
 349 {
 350         struct bitmap_next_free_data *data = __data;
 351         uint64_t mask = ~((1UL << ((data->bit & BLOCKS_PER_UNIT_MASK) + 1)) - 1);
 352
 353         if (!(mask & bl->map[offset]))
 354                 return 0;
 355
 356         if (bl->map[offset] != -1UL) {
 357                 data->level = bl->level;
 358                 data->offset = offset;
 359                 return 1;
 360         }
 361
 362         data->bit = (data->bit + BLOCKS_PER_UNIT - 1) / BLOCKS_PER_UNIT;
 363         return 0;
 364 }
 365
 366 /*
 367  * 'bit_nr' is already set. Find the next free bit after this one.
 368  */
 369 uint64_t bitmap_next_free(struct bitmap *bitmap, uint64_t bit_nr)
 370 {
 371         struct bitmap_next_free_data data = { .level = -1U, .bit = bit_nr, };
 372
 373         if (firstfree_valid(bitmap) && bit_nr < bitmap->first_free)
 374                 return bitmap->first_free;
 375
 376         if (!bitmap_handler(bitmap, bit_nr, bitmap_next_free_fn, &data))
 377                 return bitmap_first_free(bitmap);
 378
 379         assert(data.level != -1U);
 380
 381         return bitmap_find_first_free(bitmap, data.level, data.offset);
 382 }