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
ba_fls(bitalloc_word_t v)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
ba_ffs(bitalloc_word_t v)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
ba_init(struct bitalloc * pool,int size,bool free)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
ba_alloc_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int index,int * clear)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
ba_alloc(struct bitalloc * pool)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
ba_alloc_reverse_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int index,int * clear)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
ba_alloc_reverse(struct bitalloc * pool)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
ba_alloc_index_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int * index,int * clear)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
ba_alloc_index(struct bitalloc * pool,int index)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
ba_inuse_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int * index)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
ba_inuse(struct bitalloc * pool,int index)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
ba_free_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int * index)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
ba_free(struct bitalloc * pool,int index)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
ba_inuse_free(struct bitalloc * pool,int index)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
ba_free_count(struct bitalloc * pool)379 ba_free_count(struct bitalloc *pool)
380 {
381 return (int)pool->free_count;
382 }
383
ba_inuse_count(struct bitalloc * pool)384 int ba_inuse_count(struct bitalloc *pool)
385 {
386 return (int)(pool->size) - (int)(pool->free_count);
387 }
388
389 static int
ba_find_next_helper(struct bitalloc * pool,int offset,int words,unsigned int size,int * index,int free)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
ba_find_next_inuse(struct bitalloc * pool,int index)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
ba_find_next_inuse_free(struct bitalloc * pool,int index)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