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