1 /* SPDX-License-Identifier: BSD-3-Clause 2 * Copyright(c) 2019-2020 Broadcom 3 * All rights reserved. 4 */ 5 6 #include "bitalloc.h" 7 8 #define BITALLOC_MAX_LEVELS 6 9 10 11 /* Finds the last bit set plus 1, equivalent to gcc __builtin_fls */ 12 static int 13 ba_fls(bitalloc_word_t v) 14 { 15 int c = 32; 16 17 if (!v) 18 return 0; 19 20 if (!(v & 0xFFFF0000u)) { 21 v <<= 16; 22 c -= 16; 23 } 24 if (!(v & 0xFF000000u)) { 25 v <<= 8; 26 c -= 8; 27 } 28 if (!(v & 0xF0000000u)) { 29 v <<= 4; 30 c -= 4; 31 } 32 if (!(v & 0xC0000000u)) { 33 v <<= 2; 34 c -= 2; 35 } 36 if (!(v & 0x80000000u)) { 37 v <<= 1; 38 c -= 1; 39 } 40 41 return c; 42 } 43 44 /* Finds the first bit set plus 1, equivalent to gcc __builtin_ffs */ 45 static int 46 ba_ffs(bitalloc_word_t v) 47 { 48 int c; /* c will be the number of zero bits on the right plus 1 */ 49 50 v &= -v; 51 c = v ? 32 : 0; 52 53 if (v & 0x0000FFFF) 54 c -= 16; 55 if (v & 0x00FF00FF) 56 c -= 8; 57 if (v & 0x0F0F0F0F) 58 c -= 4; 59 if (v & 0x33333333) 60 c -= 2; 61 if (v & 0x55555555) 62 c -= 1; 63 64 return c; 65 } 66 67 int 68 ba_init(struct bitalloc *pool, int size) 69 { 70 bitalloc_word_t *mem = (bitalloc_word_t *)pool; 71 int i; 72 73 /* Initialize */ 74 pool->size = 0; 75 76 if (size < 1 || size > BITALLOC_MAX_SIZE) 77 return -1; 78 79 /* Zero structure */ 80 for (i = 0; 81 i < (int)(BITALLOC_SIZEOF(size) / sizeof(bitalloc_word_t)); 82 i++) 83 mem[i] = 0; 84 85 /* Initialize */ 86 pool->size = size; 87 88 /* Embed number of words of next level, after each level */ 89 int words[BITALLOC_MAX_LEVELS]; 90 int lev = 0; 91 int offset = 0; 92 93 words[0] = (size + 31) / 32; 94 while (words[lev] > 1) { 95 lev++; 96 words[lev] = (words[lev - 1] + 31) / 32; 97 } 98 99 while (lev) { 100 offset += words[lev]; 101 pool->storage[offset++] = words[--lev]; 102 } 103 104 /* Free the entire pool */ 105 for (i = 0; i < size; i++) 106 ba_free(pool, i); 107 108 return 0; 109 } 110 111 static int 112 ba_alloc_helper(struct bitalloc *pool, 113 int offset, 114 int words, 115 unsigned int size, 116 int index, 117 int *clear) 118 { 119 bitalloc_word_t *storage = &pool->storage[offset]; 120 int loc = ba_ffs(storage[index]); 121 int r; 122 123 if (loc == 0) 124 return -1; 125 126 loc--; 127 128 if (pool->size > size) { 129 r = ba_alloc_helper(pool, 130 offset + words + 1, 131 storage[words], 132 size * 32, 133 index * 32 + loc, 134 clear); 135 } else { 136 r = index * 32 + loc; 137 *clear = 1; 138 pool->free_count--; 139 } 140 141 if (*clear) { 142 storage[index] &= ~(1 << loc); 143 *clear = (storage[index] == 0); 144 } 145 146 return r; 147 } 148 149 int 150 ba_alloc(struct bitalloc *pool) 151 { 152 int clear = 0; 153 154 return ba_alloc_helper(pool, 0, 1, 32, 0, &clear); 155 } 156 157 /** 158 * Help function to alloc entry from highest available index 159 * 160 * Searching the pool from highest index for the empty entry. 161 * 162 * [in] pool 163 * Pointer to the resource pool 164 * 165 * [in] offset 166 * Offset of the storage in the pool 167 * 168 * [in] words 169 * Number of words in this level 170 * 171 * [in] size 172 * Number of entries in this level 173 * 174 * [in] index 175 * Index of words that has the entry 176 * 177 * [in] clear 178 * Indicate if a bit needs to be clear due to the entry is allocated 179 * 180 * Returns: 181 * 0 - Success 182 * -1 - Failure 183 */ 184 static int 185 ba_alloc_reverse_helper(struct bitalloc *pool, 186 int offset, 187 int words, 188 unsigned int size, 189 int index, 190 int *clear) 191 { 192 bitalloc_word_t *storage = &pool->storage[offset]; 193 int loc = ba_fls(storage[index]); 194 int r; 195 196 if (loc == 0) 197 return -1; 198 199 loc--; 200 201 if (pool->size > size) { 202 r = ba_alloc_reverse_helper(pool, 203 offset + words + 1, 204 storage[words], 205 size * 32, 206 index * 32 + loc, 207 clear); 208 } else { 209 r = index * 32 + loc; 210 *clear = 1; 211 pool->free_count--; 212 } 213 214 if (*clear) { 215 storage[index] &= ~(1 << loc); 216 *clear = (storage[index] == 0); 217 } 218 219 return r; 220 } 221 222 int 223 ba_alloc_reverse(struct bitalloc *pool) 224 { 225 int clear = 0; 226 227 return ba_alloc_reverse_helper(pool, 0, 1, 32, 0, &clear); 228 } 229 230 static int 231 ba_alloc_index_helper(struct bitalloc *pool, 232 int offset, 233 int words, 234 unsigned int size, 235 int *index, 236 int *clear) 237 { 238 bitalloc_word_t *storage = &pool->storage[offset]; 239 int loc; 240 int r; 241 242 if (pool->size > size) 243 r = ba_alloc_index_helper(pool, 244 offset + words + 1, 245 storage[words], 246 size * 32, 247 index, 248 clear); 249 else 250 r = 1; /* Check if already allocated */ 251 252 loc = (*index % 32); 253 *index = *index / 32; 254 255 if (r == 1) { 256 r = (storage[*index] & (1 << loc)) ? 0 : -1; 257 if (r == 0) { 258 *clear = 1; 259 pool->free_count--; 260 } 261 } 262 263 if (*clear) { 264 storage[*index] &= ~(1 << loc); 265 *clear = (storage[*index] == 0); 266 } 267 268 return r; 269 } 270 271 int 272 ba_alloc_index(struct bitalloc *pool, int index) 273 { 274 int clear = 0; 275 int index_copy = index; 276 277 if (index < 0 || index >= (int)pool->size) 278 return -1; 279 280 if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0) 281 return index; 282 else 283 return -1; 284 } 285 286 static int 287 ba_inuse_helper(struct bitalloc *pool, 288 int offset, 289 int words, 290 unsigned int size, 291 int *index) 292 { 293 bitalloc_word_t *storage = &pool->storage[offset]; 294 int loc; 295 int r; 296 297 if (pool->size > size) 298 r = ba_inuse_helper(pool, 299 offset + words + 1, 300 storage[words], 301 size * 32, 302 index); 303 else 304 r = 1; /* Check if in use */ 305 306 loc = (*index % 32); 307 *index = *index / 32; 308 309 if (r == 1) 310 r = (storage[*index] & (1 << loc)) ? -1 : 0; 311 312 return r; 313 } 314 315 int 316 ba_inuse(struct bitalloc *pool, int index) 317 { 318 if (index < 0 || index >= (int)pool->size) 319 return -1; 320 321 return ba_inuse_helper(pool, 0, 1, 32, &index) == 0; 322 } 323 324 static int 325 ba_free_helper(struct bitalloc *pool, 326 int offset, 327 int words, 328 unsigned int size, 329 int *index) 330 { 331 bitalloc_word_t *storage = &pool->storage[offset]; 332 int loc; 333 int r; 334 335 if (pool->size > size) 336 r = ba_free_helper(pool, 337 offset + words + 1, 338 storage[words], 339 size * 32, 340 index); 341 else 342 r = 1; /* Check if already free */ 343 344 loc = (*index % 32); 345 *index = *index / 32; 346 347 if (r == 1) { 348 r = (storage[*index] & (1 << loc)) ? -1 : 0; 349 if (r == 0) 350 pool->free_count++; 351 } 352 353 if (r == 0) 354 storage[*index] |= (1 << loc); 355 356 return r; 357 } 358 359 int 360 ba_free(struct bitalloc *pool, int index) 361 { 362 if (index < 0 || index >= (int)pool->size) 363 return -1; 364 365 return ba_free_helper(pool, 0, 1, 32, &index); 366 } 367 368 int 369 ba_inuse_free(struct bitalloc *pool, int index) 370 { 371 if (index < 0 || index >= (int)pool->size) 372 return -1; 373 374 return ba_free_helper(pool, 0, 1, 32, &index) + 1; 375 } 376 377 int 378 ba_free_count(struct bitalloc *pool) 379 { 380 return (int)pool->free_count; 381 } 382 383 int ba_inuse_count(struct bitalloc *pool) 384 { 385 return (int)(pool->size) - (int)(pool->free_count); 386 } 387 388 static int 389 ba_find_next_helper(struct bitalloc *pool, 390 int offset, 391 int words, 392 unsigned int size, 393 int *index, 394 int free) 395 { 396 bitalloc_word_t *storage = &pool->storage[offset]; 397 int loc, r, bottom = 0; 398 399 if (pool->size > size) 400 r = ba_find_next_helper(pool, 401 offset + words + 1, 402 storage[words], 403 size * 32, 404 index, 405 free); 406 else 407 bottom = 1; /* Bottom of tree */ 408 409 loc = (*index % 32); 410 *index = *index / 32; 411 412 if (bottom) { 413 int bit_index = *index * 32; 414 415 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc)); 416 if (loc > 0) { 417 loc--; 418 r = (bit_index + loc); 419 if (r >= (int)pool->size) 420 r = -1; 421 } else { 422 /* Loop over array at bottom of tree */ 423 r = -1; 424 bit_index += 32; 425 *index = *index + 1; 426 while ((int)pool->size > bit_index) { 427 loc = ba_ffs(~storage[*index]); 428 429 if (loc > 0) { 430 loc--; 431 r = (bit_index + loc); 432 if (r >= (int)pool->size) 433 r = -1; 434 break; 435 } 436 bit_index += 32; 437 *index = *index + 1; 438 } 439 } 440 } 441 442 if (r >= 0 && (free)) { 443 if (bottom) 444 pool->free_count++; 445 storage[*index] |= (1 << loc); 446 } 447 448 return r; 449 } 450 451 int 452 ba_find_next_inuse(struct bitalloc *pool, int index) 453 { 454 if (index < 0 || 455 index >= (int)pool->size || 456 pool->free_count == pool->size) 457 return -1; 458 459 return ba_find_next_helper(pool, 0, 1, 32, &index, 0); 460 } 461 462 int 463 ba_find_next_inuse_free(struct bitalloc *pool, int index) 464 { 465 if (index < 0 || 466 index >= (int)pool->size || 467 pool->free_count == pool->size) 468 return -1; 469 470 return ba_find_next_helper(pool, 0, 1, 32, &index, 1); 471 } 472