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