1 /* SPDX-License-Identifier: BSD-3-Clause 2 * Copyright(c) 2019-2021 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, bool free) 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 if it is required*/ 105 if (free) { 106 for (i = 0; i < size; i++) 107 ba_free(pool, i); 108 } 109 110 return 0; 111 } 112 113 static int 114 ba_alloc_helper(struct bitalloc *pool, 115 int offset, 116 int words, 117 unsigned int size, 118 int index, 119 int *clear) 120 { 121 bitalloc_word_t *storage = &pool->storage[offset]; 122 int loc = ba_ffs(storage[index]); 123 int r; 124 125 if (loc == 0) 126 return -1; 127 128 loc--; 129 130 if (pool->size > size) { 131 r = ba_alloc_helper(pool, 132 offset + words + 1, 133 storage[words], 134 size * 32, 135 index * 32 + loc, 136 clear); 137 } else { 138 r = index * 32 + loc; 139 *clear = 1; 140 pool->free_count--; 141 } 142 143 if (*clear) { 144 storage[index] &= ~(1 << loc); 145 *clear = (storage[index] == 0); 146 } 147 148 return r; 149 } 150 151 int 152 ba_alloc(struct bitalloc *pool) 153 { 154 int clear = 0; 155 156 return ba_alloc_helper(pool, 0, 1, 32, 0, &clear); 157 } 158 159 /** 160 * Help function to alloc entry from highest available index 161 * 162 * Searching the pool from highest index for the empty entry. 163 * 164 * [in] pool 165 * Pointer to the resource pool 166 * 167 * [in] offset 168 * Offset of the storage in the pool 169 * 170 * [in] words 171 * Number of words in this level 172 * 173 * [in] size 174 * Number of entries in this level 175 * 176 * [in] index 177 * Index of words that has the entry 178 * 179 * [in] clear 180 * Indicate if a bit needs to be clear due to the entry is allocated 181 * 182 * Returns: 183 * 0 - Success 184 * -1 - Failure 185 */ 186 static int 187 ba_alloc_reverse_helper(struct bitalloc *pool, 188 int offset, 189 int words, 190 unsigned int size, 191 int index, 192 int *clear) 193 { 194 bitalloc_word_t *storage = &pool->storage[offset]; 195 int loc = ba_fls(storage[index]); 196 int r; 197 198 if (loc == 0) 199 return -1; 200 201 loc--; 202 203 if (pool->size > size) { 204 r = ba_alloc_reverse_helper(pool, 205 offset + words + 1, 206 storage[words], 207 size * 32, 208 index * 32 + loc, 209 clear); 210 } else { 211 r = index * 32 + loc; 212 *clear = 1; 213 pool->free_count--; 214 } 215 216 if (*clear) { 217 storage[index] &= ~(1 << loc); 218 *clear = (storage[index] == 0); 219 } 220 221 return r; 222 } 223 224 int 225 ba_alloc_reverse(struct bitalloc *pool) 226 { 227 int clear = 0; 228 229 return ba_alloc_reverse_helper(pool, 0, 1, 32, 0, &clear); 230 } 231 232 static int 233 ba_alloc_index_helper(struct bitalloc *pool, 234 int offset, 235 int words, 236 unsigned int size, 237 int *index, 238 int *clear) 239 { 240 bitalloc_word_t *storage = &pool->storage[offset]; 241 int loc; 242 int r; 243 244 if (pool->size > size) 245 r = ba_alloc_index_helper(pool, 246 offset + words + 1, 247 storage[words], 248 size * 32, 249 index, 250 clear); 251 else 252 r = 1; /* Check if already allocated */ 253 254 loc = (*index % 32); 255 *index = *index / 32; 256 257 if (r == 1) { 258 r = (storage[*index] & (1 << loc)) ? 0 : -1; 259 if (r == 0) { 260 *clear = 1; 261 pool->free_count--; 262 } 263 } 264 265 if (*clear) { 266 storage[*index] &= ~(1 << loc); 267 *clear = (storage[*index] == 0); 268 } 269 270 return r; 271 } 272 273 int 274 ba_alloc_index(struct bitalloc *pool, int index) 275 { 276 int clear = 0; 277 int index_copy = index; 278 279 if (index < 0 || index >= (int)pool->size) 280 return -1; 281 282 if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0) 283 return index; 284 else 285 return -1; 286 } 287 288 static int 289 ba_inuse_helper(struct bitalloc *pool, 290 int offset, 291 int words, 292 unsigned int size, 293 int *index) 294 { 295 bitalloc_word_t *storage = &pool->storage[offset]; 296 int loc; 297 int r; 298 299 if (pool->size > size) 300 r = ba_inuse_helper(pool, 301 offset + words + 1, 302 storage[words], 303 size * 32, 304 index); 305 else 306 r = 1; /* Check if in use */ 307 308 loc = (*index % 32); 309 *index = *index / 32; 310 311 if (r == 1) 312 r = (storage[*index] & (1 << loc)) ? -1 : 0; 313 314 return r; 315 } 316 317 int 318 ba_inuse(struct bitalloc *pool, int index) 319 { 320 if (index < 0 || index >= (int)pool->size) 321 return -1; 322 323 return ba_inuse_helper(pool, 0, 1, 32, &index) == 0; 324 } 325 326 static int 327 ba_free_helper(struct bitalloc *pool, 328 int offset, 329 int words, 330 unsigned int size, 331 int *index) 332 { 333 bitalloc_word_t *storage = &pool->storage[offset]; 334 int loc; 335 int r; 336 337 if (pool->size > size) 338 r = ba_free_helper(pool, 339 offset + words + 1, 340 storage[words], 341 size * 32, 342 index); 343 else 344 r = 1; /* Check if already free */ 345 346 loc = (*index % 32); 347 *index = *index / 32; 348 349 if (r == 1) { 350 r = (storage[*index] & (1 << loc)) ? -1 : 0; 351 if (r == 0) 352 pool->free_count++; 353 } 354 355 if (r == 0) 356 storage[*index] |= (1 << loc); 357 358 return r; 359 } 360 361 int 362 ba_free(struct bitalloc *pool, int index) 363 { 364 if (index < 0 || index >= (int)pool->size) 365 return -1; 366 367 return ba_free_helper(pool, 0, 1, 32, &index); 368 } 369 370 int 371 ba_inuse_free(struct bitalloc *pool, int index) 372 { 373 if (index < 0 || index >= (int)pool->size) 374 return -1; 375 376 return ba_free_helper(pool, 0, 1, 32, &index) + 1; 377 } 378 379 int 380 ba_free_count(struct bitalloc *pool) 381 { 382 return (int)pool->free_count; 383 } 384 385 int ba_inuse_count(struct bitalloc *pool) 386 { 387 return (int)(pool->size) - (int)(pool->free_count); 388 } 389 390 static int 391 ba_find_next_helper(struct bitalloc *pool, 392 int offset, 393 int words, 394 unsigned int size, 395 int *index, 396 int free) 397 { 398 bitalloc_word_t *storage = &pool->storage[offset]; 399 int loc, r, bottom = 0; 400 401 if (pool->size > size) 402 r = ba_find_next_helper(pool, 403 offset + words + 1, 404 storage[words], 405 size * 32, 406 index, 407 free); 408 else 409 bottom = 1; /* Bottom of tree */ 410 411 loc = (*index % 32); 412 *index = *index / 32; 413 414 if (bottom) { 415 int bit_index = *index * 32; 416 417 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc)); 418 if (loc > 0) { 419 loc--; 420 r = (bit_index + loc); 421 if (r >= (int)pool->size) 422 r = -1; 423 } else { 424 /* Loop over array at bottom of tree */ 425 r = -1; 426 bit_index += 32; 427 *index = *index + 1; 428 while ((int)pool->size > bit_index) { 429 loc = ba_ffs(~storage[*index]); 430 431 if (loc > 0) { 432 loc--; 433 r = (bit_index + loc); 434 if (r >= (int)pool->size) 435 r = -1; 436 break; 437 } 438 bit_index += 32; 439 *index = *index + 1; 440 } 441 } 442 } 443 444 if (r >= 0 && (free)) { 445 if (bottom) 446 pool->free_count++; 447 storage[*index] |= (1 << loc); 448 } 449 450 return r; 451 } 452 453 int 454 ba_find_next_inuse(struct bitalloc *pool, int index) 455 { 456 if (index < 0 || 457 index >= (int)pool->size || 458 pool->free_count == pool->size) 459 return -1; 460 461 return ba_find_next_helper(pool, 0, 1, 32, &index, 0); 462 } 463 464 int 465 ba_find_next_inuse_free(struct bitalloc *pool, int index) 466 { 467 if (index < 0 || 468 index >= (int)pool->size || 469 pool->free_count == pool->size) 470 return -1; 471 472 return ba_find_next_helper(pool, 0, 1, 32, &index, 1); 473 } 474