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