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