1 /*
2 * Copyright (c) 2021 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <errno.h>
8 #include <stddef.h>
9 #include <stdbool.h>
10 #include <stdio.h>
11 #include <zephyr/sys/bitarray.h>
12 #include <zephyr/sys/check.h>
13 #include <zephyr/sys/sys_io.h>
14
15 /* Number of bits represented by one bundle */
16 #define bundle_bitness(ba) (sizeof(ba->bundles[0]) * 8)
17
18 struct bundle_data {
19 /* Start and end index of bundles */
20 size_t sidx, eidx;
21
22 /* Offset inside start and end bundles */
23 size_t soff, eoff;
24
25 /* Masks for start/end bundles */
26 uint32_t smask, emask;
27 };
28
setup_bundle_data(sys_bitarray_t * bitarray,struct bundle_data * bd,size_t offset,size_t num_bits)29 static void setup_bundle_data(sys_bitarray_t *bitarray,
30 struct bundle_data *bd,
31 size_t offset, size_t num_bits)
32 {
33 bd->sidx = offset / bundle_bitness(bitarray);
34 bd->soff = offset % bundle_bitness(bitarray);
35
36 bd->eidx = (offset + num_bits - 1) / bundle_bitness(bitarray);
37 bd->eoff = (offset + num_bits - 1) % bundle_bitness(bitarray);
38
39 bd->smask = ~(BIT(bd->soff) - 1);
40 bd->emask = (BIT(bd->eoff) - 1) | BIT(bd->eoff);
41
42 if (bd->sidx == bd->eidx) {
43 /* The region lies within the same bundle. So combine the masks. */
44 bd->smask &= bd->emask;
45 }
46 }
47
48 /*
49 * Find out if the bits in a region is all set or all clear.
50 *
51 * @param[in] bitarray Bitarray struct
52 * @param[in] offset Starting bit location
53 * @param[in] num_bits Number of bits in the region
54 * @param[in] match_set True if matching all set bits,
55 * False if matching all cleared bits
56 * @param[out] bd Data related to matching which can be
57 * used later to find out where the region
58 * lies in the bitarray bundles.
59 * @param[out] mismatch Offset to the mismatched bit.
60 * Can be NULL.
61 *
62 * @retval true If all bits are set or cleared
63 * @retval false Not all bits are set or cleared
64 */
match_region(sys_bitarray_t * bitarray,size_t offset,size_t num_bits,bool match_set,struct bundle_data * bd,size_t * mismatch)65 static bool match_region(sys_bitarray_t *bitarray, size_t offset,
66 size_t num_bits, bool match_set,
67 struct bundle_data *bd,
68 size_t *mismatch)
69 {
70 size_t idx;
71 uint32_t bundle;
72 uint32_t mismatch_bundle;
73 size_t mismatch_bundle_idx;
74 size_t mismatch_bit_off;
75
76 setup_bundle_data(bitarray, bd, offset, num_bits);
77
78 if (bd->sidx == bd->eidx) {
79 bundle = bitarray->bundles[bd->sidx];
80 if (!match_set) {
81 bundle = ~bundle;
82 }
83
84 if ((bundle & bd->smask) != bd->smask) {
85 /* Not matching to mask. */
86 mismatch_bundle = ~bundle & bd->smask;
87 mismatch_bundle_idx = bd->sidx;
88 goto mismatch;
89 } else {
90 /* Matching to mask. */
91 goto out;
92 }
93 }
94
95 /* Region lies in a number of bundles. Need to loop through them. */
96
97 /* Start of bundles */
98 bundle = bitarray->bundles[bd->sidx];
99 if (!match_set) {
100 bundle = ~bundle;
101 }
102
103 if ((bundle & bd->smask) != bd->smask) {
104 /* Start bundle not matching to mask. */
105 mismatch_bundle = ~bundle & bd->smask;
106 mismatch_bundle_idx = bd->sidx;
107 goto mismatch;
108 }
109
110 /* End of bundles */
111 bundle = bitarray->bundles[bd->eidx];
112 if (!match_set) {
113 bundle = ~bundle;
114 }
115
116 if ((bundle & bd->emask) != bd->emask) {
117 /* End bundle not matching to mask. */
118 mismatch_bundle = ~bundle & bd->emask;
119 mismatch_bundle_idx = bd->eidx;
120 goto mismatch;
121 }
122
123 /* In-between bundles */
124 for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
125 /* Note that this is opposite from above so that
126 * we are simply checking if bundle == 0.
127 */
128 bundle = bitarray->bundles[idx];
129 if (match_set) {
130 bundle = ~bundle;
131 }
132
133 if (bundle != 0U) {
134 /* Bits in "between bundles" do not match */
135 mismatch_bundle = bundle;
136 mismatch_bundle_idx = idx;
137 goto mismatch;
138 }
139 }
140
141 out:
142 /* All bits in region matched. */
143 return true;
144
145 mismatch:
146 if (mismatch != NULL) {
147 /* Must have at least 1 bit set to indicate
148 * where the mismatch is.
149 */
150 __ASSERT_NO_MSG(mismatch_bundle != 0);
151
152 mismatch_bit_off = find_lsb_set(mismatch_bundle) - 1;
153 mismatch_bit_off += mismatch_bundle_idx *
154 bundle_bitness(bitarray);
155 *mismatch = (uint32_t)mismatch_bit_off;
156 }
157 return false;
158 }
159
160 /*
161 * Set or clear a region of bits.
162 *
163 * @param bitarray Bitarray struct
164 * @param offset Starting bit location
165 * @param num_bits Number of bits in the region
166 * @param to_set True if to set all bits.
167 * False if to clear all bits.
168 * @param bd Bundle data. Can reuse the output from
169 * match_region(). NULL if there is no
170 * prior call to match_region().
171 */
set_region(sys_bitarray_t * bitarray,size_t offset,size_t num_bits,bool to_set,struct bundle_data * bd)172 static void set_region(sys_bitarray_t *bitarray, size_t offset,
173 size_t num_bits, bool to_set,
174 struct bundle_data *bd)
175 {
176 size_t idx;
177 struct bundle_data bdata;
178
179 if (bd == NULL) {
180 bd = &bdata;
181 setup_bundle_data(bitarray, bd, offset, num_bits);
182 }
183
184 if (bd->sidx == bd->eidx) {
185 /* Start/end at same bundle */
186 if (to_set) {
187 bitarray->bundles[bd->sidx] |= bd->smask;
188 } else {
189 bitarray->bundles[bd->sidx] &= ~bd->smask;
190 }
191 } else {
192 /* Start/end at different bundle.
193 * So set/clear the bits in start and end bundles
194 * separately. For in-between bundles,
195 * set/clear all bits.
196 */
197 if (to_set) {
198 bitarray->bundles[bd->sidx] |= bd->smask;
199 bitarray->bundles[bd->eidx] |= bd->emask;
200 for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
201 bitarray->bundles[idx] = ~0U;
202 }
203 } else {
204 bitarray->bundles[bd->sidx] &= ~bd->smask;
205 bitarray->bundles[bd->eidx] &= ~bd->emask;
206 for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
207 bitarray->bundles[idx] = 0U;
208 }
209 }
210 }
211 }
212
sys_bitarray_set_bit(sys_bitarray_t * bitarray,size_t bit)213 int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit)
214 {
215 k_spinlock_key_t key;
216 int ret;
217 size_t idx, off;
218
219 key = k_spin_lock(&bitarray->lock);
220
221 __ASSERT_NO_MSG(bitarray != NULL);
222 __ASSERT_NO_MSG(bitarray->num_bits > 0);
223
224 if (bit >= bitarray->num_bits) {
225 ret = -EINVAL;
226 goto out;
227 }
228
229 idx = bit / bundle_bitness(bitarray);
230 off = bit % bundle_bitness(bitarray);
231
232 bitarray->bundles[idx] |= BIT(off);
233
234 ret = 0;
235
236 out:
237 k_spin_unlock(&bitarray->lock, key);
238 return ret;
239 }
240
sys_bitarray_clear_bit(sys_bitarray_t * bitarray,size_t bit)241 int sys_bitarray_clear_bit(sys_bitarray_t *bitarray, size_t bit)
242 {
243 k_spinlock_key_t key;
244 int ret;
245 size_t idx, off;
246
247 __ASSERT_NO_MSG(bitarray != NULL);
248 __ASSERT_NO_MSG(bitarray->num_bits > 0);
249
250 key = k_spin_lock(&bitarray->lock);
251
252 if (bit >= bitarray->num_bits) {
253 ret = -EINVAL;
254 goto out;
255 }
256
257 idx = bit / bundle_bitness(bitarray);
258 off = bit % bundle_bitness(bitarray);
259
260 bitarray->bundles[idx] &= ~BIT(off);
261
262 ret = 0;
263
264 out:
265 k_spin_unlock(&bitarray->lock, key);
266 return ret;
267 }
268
sys_bitarray_test_bit(sys_bitarray_t * bitarray,size_t bit,int * val)269 int sys_bitarray_test_bit(sys_bitarray_t *bitarray, size_t bit, int *val)
270 {
271 k_spinlock_key_t key;
272 int ret;
273 size_t idx, off;
274
275 __ASSERT_NO_MSG(bitarray != NULL);
276 __ASSERT_NO_MSG(bitarray->num_bits > 0);
277
278 key = k_spin_lock(&bitarray->lock);
279
280 CHECKIF(val == NULL) {
281 ret = -EINVAL;
282 goto out;
283 }
284
285 if (bit >= bitarray->num_bits) {
286 ret = -EINVAL;
287 goto out;
288 }
289
290 idx = bit / bundle_bitness(bitarray);
291 off = bit % bundle_bitness(bitarray);
292
293 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
294 *val = 1;
295 } else {
296 *val = 0;
297 }
298
299 ret = 0;
300
301 out:
302 k_spin_unlock(&bitarray->lock, key);
303 return ret;
304 }
305
sys_bitarray_test_and_set_bit(sys_bitarray_t * bitarray,size_t bit,int * prev_val)306 int sys_bitarray_test_and_set_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
307 {
308 k_spinlock_key_t key;
309 int ret;
310 size_t idx, off;
311
312 __ASSERT_NO_MSG(bitarray != NULL);
313 __ASSERT_NO_MSG(bitarray->num_bits > 0);
314
315 key = k_spin_lock(&bitarray->lock);
316
317 CHECKIF(prev_val == NULL) {
318 ret = -EINVAL;
319 goto out;
320 }
321
322 if (bit >= bitarray->num_bits) {
323 ret = -EINVAL;
324 goto out;
325 }
326
327 idx = bit / bundle_bitness(bitarray);
328 off = bit % bundle_bitness(bitarray);
329
330 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
331 *prev_val = 1;
332 } else {
333 *prev_val = 0;
334 }
335
336 bitarray->bundles[idx] |= BIT(off);
337
338 ret = 0;
339
340 out:
341 k_spin_unlock(&bitarray->lock, key);
342 return ret;
343 }
344
sys_bitarray_test_and_clear_bit(sys_bitarray_t * bitarray,size_t bit,int * prev_val)345 int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
346 {
347 k_spinlock_key_t key;
348 int ret;
349 size_t idx, off;
350
351 __ASSERT_NO_MSG(bitarray != NULL);
352 __ASSERT_NO_MSG(bitarray->num_bits > 0);
353
354 key = k_spin_lock(&bitarray->lock);
355
356 CHECKIF(prev_val == NULL) {
357 ret = -EINVAL;
358 goto out;
359 }
360
361 if (bit >= bitarray->num_bits) {
362 ret = -EINVAL;
363 goto out;
364 }
365
366 idx = bit / bundle_bitness(bitarray);
367 off = bit % bundle_bitness(bitarray);
368
369 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
370 *prev_val = 1;
371 } else {
372 *prev_val = 0;
373 }
374
375 bitarray->bundles[idx] &= ~BIT(off);
376
377 ret = 0;
378
379 out:
380 k_spin_unlock(&bitarray->lock, key);
381 return ret;
382 }
383
sys_bitarray_alloc(sys_bitarray_t * bitarray,size_t num_bits,size_t * offset)384 int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits,
385 size_t *offset)
386 {
387 k_spinlock_key_t key;
388 uint32_t bit_idx;
389 int ret;
390 struct bundle_data bd;
391 size_t off_start, off_end;
392 size_t mismatch;
393
394 __ASSERT_NO_MSG(bitarray != NULL);
395 __ASSERT_NO_MSG(bitarray->num_bits > 0);
396
397 key = k_spin_lock(&bitarray->lock);
398
399 CHECKIF(offset == NULL) {
400 ret = -EINVAL;
401 goto out;
402 }
403
404 if ((num_bits == 0) || (num_bits > bitarray->num_bits)) {
405 ret = -EINVAL;
406 goto out;
407 }
408
409 bit_idx = 0;
410
411 /* Find the first non-allocated bit by looking at bundles
412 * instead of individual bits.
413 *
414 * On RISC-V 64-bit, it complains about undefined reference to `ffs`.
415 * So don't use this on RISCV64.
416 */
417 for (size_t idx = 0; idx < bitarray->num_bundles; idx++) {
418 if (~bitarray->bundles[idx] == 0U) {
419 /* bundle is all 1s => all allocated, skip */
420 bit_idx += bundle_bitness(bitarray);
421 continue;
422 }
423
424 if (bitarray->bundles[idx] != 0U) {
425 /* Find the first free bit in bundle if not all free */
426 off_start = find_lsb_set(~bitarray->bundles[idx]) - 1;
427 bit_idx += off_start;
428 }
429
430 break;
431 }
432
433 off_end = bitarray->num_bits - num_bits;
434 ret = -ENOSPC;
435 while (bit_idx <= off_end) {
436 if (match_region(bitarray, bit_idx, num_bits, false,
437 &bd, &mismatch)) {
438 set_region(bitarray, bit_idx, num_bits, true, &bd);
439
440 *offset = bit_idx;
441 ret = 0;
442 break;
443 }
444
445 /* Fast-forward to the bit just after
446 * the mismatched bit.
447 */
448 bit_idx = mismatch + 1;
449 }
450
451 out:
452 k_spin_unlock(&bitarray->lock, key);
453 return ret;
454 }
455
sys_bitarray_free(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)456 int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
457 size_t offset)
458 {
459 k_spinlock_key_t key;
460 int ret;
461 size_t off_end = offset + num_bits - 1;
462 struct bundle_data bd;
463
464 __ASSERT_NO_MSG(bitarray != NULL);
465 __ASSERT_NO_MSG(bitarray->num_bits > 0);
466
467 key = k_spin_lock(&bitarray->lock);
468
469 if ((num_bits == 0)
470 || (num_bits > bitarray->num_bits)
471 || (offset >= bitarray->num_bits)
472 || (off_end >= bitarray->num_bits)) {
473 ret = -EINVAL;
474 goto out;
475 }
476
477 /* Note that we need to make sure the bits in specified region
478 * (offset to offset + num_bits) are all allocated before we clear
479 * them.
480 */
481 if (match_region(bitarray, offset, num_bits, true, &bd, NULL)) {
482 set_region(bitarray, offset, num_bits, false, &bd);
483 ret = 0;
484 } else {
485 ret = -EFAULT;
486 }
487
488 out:
489 k_spin_unlock(&bitarray->lock, key);
490 return ret;
491 }
492
is_region_set_clear(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)493 static bool is_region_set_clear(sys_bitarray_t *bitarray, size_t num_bits,
494 size_t offset, bool to_set)
495 {
496 bool ret;
497 struct bundle_data bd;
498 size_t off_end = offset + num_bits - 1;
499 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
500
501 __ASSERT_NO_MSG(bitarray != NULL);
502 __ASSERT_NO_MSG(bitarray->num_bits > 0);
503
504 if ((num_bits == 0)
505 || (num_bits > bitarray->num_bits)
506 || (offset >= bitarray->num_bits)
507 || (off_end >= bitarray->num_bits)) {
508 ret = false;
509 goto out;
510 }
511
512 ret = match_region(bitarray, offset, num_bits, to_set, &bd, NULL);
513
514 out:
515 k_spin_unlock(&bitarray->lock, key);
516 return ret;
517 }
518
sys_bitarray_is_region_set(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)519 bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
520 size_t offset)
521 {
522 return is_region_set_clear(bitarray, num_bits, offset, true);
523 }
524
sys_bitarray_is_region_cleared(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)525 bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
526 size_t offset)
527 {
528 return is_region_set_clear(bitarray, num_bits, offset, false);
529 }
530
set_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)531 static int set_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
532 size_t offset, bool to_set)
533 {
534 int ret;
535 size_t off_end = offset + num_bits - 1;
536 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
537
538 __ASSERT_NO_MSG(bitarray != NULL);
539 __ASSERT_NO_MSG(bitarray->num_bits > 0);
540
541 if ((num_bits == 0)
542 || (num_bits > bitarray->num_bits)
543 || (offset >= bitarray->num_bits)
544 || (off_end >= bitarray->num_bits)) {
545 ret = -EINVAL;
546 goto out;
547 }
548
549 set_region(bitarray, offset, num_bits, to_set, NULL);
550 ret = 0;
551
552 out:
553 k_spin_unlock(&bitarray->lock, key);
554 return ret;
555 }
556
sys_bitarray_test_and_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)557 int sys_bitarray_test_and_set_region(sys_bitarray_t *bitarray, size_t num_bits,
558 size_t offset, bool to_set)
559 {
560 int ret;
561 bool region_clear;
562 struct bundle_data bd;
563
564 __ASSERT_NO_MSG(bitarray != NULL);
565 __ASSERT_NO_MSG(bitarray->num_bits > 0);
566
567 size_t off_end = offset + num_bits - 1;
568 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
569
570
571 if ((num_bits == 0)
572 || (num_bits > bitarray->num_bits)
573 || (offset >= bitarray->num_bits)
574 || (off_end >= bitarray->num_bits)) {
575 ret = -EINVAL;
576 goto out;
577 }
578
579 region_clear = match_region(bitarray, offset, num_bits, !to_set, &bd, NULL);
580 if (region_clear) {
581 set_region(bitarray, offset, num_bits, to_set, &bd);
582 ret = 0;
583 } else {
584 ret = -EEXIST;
585 }
586
587 out:
588 k_spin_unlock(&bitarray->lock, key);
589 return ret;
590 }
591
sys_bitarray_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)592 int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
593 size_t offset)
594 {
595 return set_clear_region(bitarray, num_bits, offset, true);
596 }
597
sys_bitarray_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)598 int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
599 size_t offset)
600 {
601 return set_clear_region(bitarray, num_bits, offset, false);
602 }
603
