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 /* Finds the first bit set plus 1, equivalent to gcc __builtin_ffs */ 11 static int 12 ba_ffs(bitalloc_word_t v) 13 { 14 int c; /* c will be the number of zero bits on the right plus 1 */ 15 16 v &= -v; 17 c = v ? 32 : 0; 18 19 if (v & 0x0000FFFF) 20 c -= 16; 21 if (v & 0x00FF00FF) 22 c -= 8; 23 if (v & 0x0F0F0F0F) 24 c -= 4; 25 if (v & 0x33333333) 26 c -= 2; 27 if (v & 0x55555555) 28 c -= 1; 29 30 return c; 31 } 32 33 int 34 ba_init(struct bitalloc *pool, int size) 35 { 36 bitalloc_word_t *mem = (bitalloc_word_t *)pool; 37 int i; 38 39 /* Initialize */ 40 pool->size = 0; 41 42 if (size < 1 || size > BITALLOC_MAX_SIZE) 43 return -1; 44 45 /* Zero structure */ 46 for (i = 0; 47 i < (int)(BITALLOC_SIZEOF(size) / sizeof(bitalloc_word_t)); 48 i++) 49 mem[i] = 0; 50 51 /* Initialize */ 52 pool->size = size; 53 54 /* Embed number of words of next level, after each level */ 55 int words[BITALLOC_MAX_LEVELS]; 56 int lev = 0; 57 int offset = 0; 58 59 words[0] = (size + 31) / 32; 60 while (words[lev] > 1) { 61 lev++; 62 words[lev] = (words[lev - 1] + 31) / 32; 63 } 64 65 while (lev) { 66 offset += words[lev]; 67 pool->storage[offset++] = words[--lev]; 68 } 69 70 /* Free the entire pool */ 71 for (i = 0; i < size; i++) 72 ba_free(pool, i); 73 74 return 0; 75 } 76 77 static int 78 ba_alloc_helper(struct bitalloc *pool, 79 int offset, 80 int words, 81 unsigned int size, 82 int index, 83 int *clear) 84 { 85 bitalloc_word_t *storage = &pool->storage[offset]; 86 int loc = ba_ffs(storage[index]); 87 int r; 88 89 if (loc == 0) 90 return -1; 91 92 loc--; 93 94 if (pool->size > size) { 95 r = ba_alloc_helper(pool, 96 offset + words + 1, 97 storage[words], 98 size * 32, 99 index * 32 + loc, 100 clear); 101 } else { 102 r = index * 32 + loc; 103 *clear = 1; 104 pool->free_count--; 105 } 106 107 if (*clear) { 108 storage[index] &= ~(1 << loc); 109 *clear = (storage[index] == 0); 110 } 111 112 return r; 113 } 114 115 int 116 ba_alloc(struct bitalloc *pool) 117 { 118 int clear = 0; 119 120 return ba_alloc_helper(pool, 0, 1, 32, 0, &clear); 121 } 122 123 static int 124 ba_alloc_index_helper(struct bitalloc *pool, 125 int offset, 126 int words, 127 unsigned int size, 128 int *index, 129 int *clear) 130 { 131 bitalloc_word_t *storage = &pool->storage[offset]; 132 int loc; 133 int r; 134 135 if (pool->size > size) 136 r = ba_alloc_index_helper(pool, 137 offset + words + 1, 138 storage[words], 139 size * 32, 140 index, 141 clear); 142 else 143 r = 1; /* Check if already allocated */ 144 145 loc = (*index % 32); 146 *index = *index / 32; 147 148 if (r == 1) { 149 r = (storage[*index] & (1 << loc)) ? 0 : -1; 150 if (r == 0) { 151 *clear = 1; 152 pool->free_count--; 153 } 154 } 155 156 if (*clear) { 157 storage[*index] &= ~(1 << loc); 158 *clear = (storage[*index] == 0); 159 } 160 161 return r; 162 } 163 164 int 165 ba_alloc_index(struct bitalloc *pool, int index) 166 { 167 int clear = 0; 168 int index_copy = index; 169 170 if (index < 0 || index >= (int)pool->size) 171 return -1; 172 173 if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0) 174 return index; 175 else 176 return -1; 177 } 178 179 static int 180 ba_inuse_helper(struct bitalloc *pool, 181 int offset, 182 int words, 183 unsigned int size, 184 int *index) 185 { 186 bitalloc_word_t *storage = &pool->storage[offset]; 187 int loc; 188 int r; 189 190 if (pool->size > size) 191 r = ba_inuse_helper(pool, 192 offset + words + 1, 193 storage[words], 194 size * 32, 195 index); 196 else 197 r = 1; /* Check if in use */ 198 199 loc = (*index % 32); 200 *index = *index / 32; 201 202 if (r == 1) 203 r = (storage[*index] & (1 << loc)) ? -1 : 0; 204 205 return r; 206 } 207 208 int 209 ba_inuse(struct bitalloc *pool, int index) 210 { 211 if (index < 0 || index >= (int)pool->size) 212 return -1; 213 214 return ba_inuse_helper(pool, 0, 1, 32, &index) == 0; 215 } 216 217 static int 218 ba_free_helper(struct bitalloc *pool, 219 int offset, 220 int words, 221 unsigned int size, 222 int *index) 223 { 224 bitalloc_word_t *storage = &pool->storage[offset]; 225 int loc; 226 int r; 227 228 if (pool->size > size) 229 r = ba_free_helper(pool, 230 offset + words + 1, 231 storage[words], 232 size * 32, 233 index); 234 else 235 r = 1; /* Check if already free */ 236 237 loc = (*index % 32); 238 *index = *index / 32; 239 240 if (r == 1) { 241 r = (storage[*index] & (1 << loc)) ? -1 : 0; 242 if (r == 0) 243 pool->free_count++; 244 } 245 246 if (r == 0) 247 storage[*index] |= (1 << loc); 248 249 return r; 250 } 251 252 int 253 ba_free(struct bitalloc *pool, int index) 254 { 255 if (index < 0 || index >= (int)pool->size) 256 return -1; 257 258 return ba_free_helper(pool, 0, 1, 32, &index); 259 } 260 261 int 262 ba_inuse_free(struct bitalloc *pool, int index) 263 { 264 if (index < 0 || index >= (int)pool->size) 265 return -1; 266 267 return ba_free_helper(pool, 0, 1, 32, &index) + 1; 268 } 269 270 int 271 ba_free_count(struct bitalloc *pool) 272 { 273 return (int)pool->free_count; 274 } 275 276 int ba_inuse_count(struct bitalloc *pool) 277 { 278 return (int)(pool->size) - (int)(pool->free_count); 279 } 280 281 static int 282 ba_find_next_helper(struct bitalloc *pool, 283 int offset, 284 int words, 285 unsigned int size, 286 int *index, 287 int free) 288 { 289 bitalloc_word_t *storage = &pool->storage[offset]; 290 int loc, r, bottom = 0; 291 292 if (pool->size > size) 293 r = ba_find_next_helper(pool, 294 offset + words + 1, 295 storage[words], 296 size * 32, 297 index, 298 free); 299 else 300 bottom = 1; /* Bottom of tree */ 301 302 loc = (*index % 32); 303 *index = *index / 32; 304 305 if (bottom) { 306 int bit_index = *index * 32; 307 308 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc)); 309 if (loc > 0) { 310 loc--; 311 r = (bit_index + loc); 312 if (r >= (int)pool->size) 313 r = -1; 314 } else { 315 /* Loop over array at bottom of tree */ 316 r = -1; 317 bit_index += 32; 318 *index = *index + 1; 319 while ((int)pool->size > bit_index) { 320 loc = ba_ffs(~storage[*index]); 321 322 if (loc > 0) { 323 loc--; 324 r = (bit_index + loc); 325 if (r >= (int)pool->size) 326 r = -1; 327 break; 328 } 329 bit_index += 32; 330 *index = *index + 1; 331 } 332 } 333 } 334 335 if (r >= 0 && (free)) { 336 if (bottom) 337 pool->free_count++; 338 storage[*index] |= (1 << loc); 339 } 340 341 return r; 342 } 343 344 int 345 ba_find_next_inuse(struct bitalloc *pool, int index) 346 { 347 if (index < 0 || 348 index >= (int)pool->size || 349 pool->free_count == pool->size) 350 return -1; 351 352 return ba_find_next_helper(pool, 0, 1, 32, &index, 0); 353 } 354 355 int 356 ba_find_next_inuse_free(struct bitalloc *pool, int index) 357 { 358 if (index < 0 || 359 index >= (int)pool->size || 360 pool->free_count == pool->size) 361 return -1; 362 363 return ba_find_next_helper(pool, 0, 1, 32, &index, 1); 364 } 365