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_popcount_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,size_t * count)213 int sys_bitarray_popcount_region(sys_bitarray_t *bitarray, size_t num_bits, size_t offset,
214 size_t *count)
215 {
216 k_spinlock_key_t key;
217 size_t idx;
218 struct bundle_data bd;
219 int ret;
220
221 __ASSERT_NO_MSG(bitarray != NULL);
222 __ASSERT_NO_MSG(bitarray->num_bits > 0);
223
224 key = k_spin_lock(&bitarray->lock);
225
226 if (num_bits == 0 || offset + num_bits > bitarray->num_bits) {
227 ret = -EINVAL;
228 goto out;
229 }
230
231 CHECKIF(count == NULL) {
232 ret = -EINVAL;
233 goto out;
234 }
235
236 setup_bundle_data(bitarray, &bd, offset, num_bits);
237
238 if (bd.sidx == bd.eidx) {
239 /* Start/end at same bundle */
240 *count = POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
241 } else {
242 /* Start/end at different bundle.
243 * So count the bits in start and end bundles
244 * separately with correct mask applied. For in-between bundles,
245 * count all bits.
246 */
247 *count = 0;
248 *count += POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
249 *count += POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask);
250 for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
251 *count += POPCOUNT(bitarray->bundles[idx]);
252 }
253 }
254
255 ret = 0;
256
257 out:
258 k_spin_unlock(&bitarray->lock, key);
259 return ret;
260 }
261
sys_bitarray_xor(sys_bitarray_t * dst,sys_bitarray_t * other,size_t num_bits,size_t offset)262 int sys_bitarray_xor(sys_bitarray_t *dst, sys_bitarray_t *other, size_t num_bits, size_t offset)
263 {
264 k_spinlock_key_t key_dst, key_other;
265 int ret;
266 size_t idx;
267 struct bundle_data bd;
268
269 __ASSERT_NO_MSG(dst != NULL);
270 __ASSERT_NO_MSG(dst->num_bits > 0);
271 __ASSERT_NO_MSG(other != NULL);
272 __ASSERT_NO_MSG(other->num_bits > 0);
273
274 key_dst = k_spin_lock(&dst->lock);
275 key_other = k_spin_lock(&other->lock);
276
277
278 if (dst->num_bits != other->num_bits) {
279 ret = -EINVAL;
280 goto out;
281 }
282
283 if (num_bits == 0 || offset + num_bits > dst->num_bits) {
284 ret = -EINVAL;
285 goto out;
286 }
287
288 setup_bundle_data(other, &bd, offset, num_bits);
289
290 if (bd.sidx == bd.eidx) {
291 /* Start/end at same bundle */
292 dst->bundles[bd.sidx] =
293 ((other->bundles[bd.sidx] ^ dst->bundles[bd.sidx]) & bd.smask) |
294 (dst->bundles[bd.sidx] & ~bd.smask);
295 } else {
296 /* Start/end at different bundle.
297 * So xor the bits in start and end bundles according to their bitmasks
298 * separately. For in-between bundles,
299 * xor all bits.
300 */
301 dst->bundles[bd.sidx] =
302 ((other->bundles[bd.sidx] ^ dst->bundles[bd.sidx]) & bd.smask) |
303 (dst->bundles[bd.sidx] & ~bd.smask);
304 dst->bundles[bd.eidx] =
305 ((other->bundles[bd.eidx] ^ dst->bundles[bd.eidx]) & bd.emask) |
306 (dst->bundles[bd.eidx] & ~bd.emask);
307 for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
308 dst->bundles[idx] ^= other->bundles[idx];
309 }
310 }
311
312 ret = 0;
313
314 out:
315 k_spin_unlock(&other->lock, key_other);
316 k_spin_unlock(&dst->lock, key_dst);
317 return ret;
318 }
319
sys_bitarray_set_bit(sys_bitarray_t * bitarray,size_t bit)320 int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit)
321 {
322 k_spinlock_key_t key;
323 int ret;
324 size_t idx, off;
325
326 __ASSERT_NO_MSG(bitarray != NULL);
327 __ASSERT_NO_MSG(bitarray->num_bits > 0);
328
329 key = k_spin_lock(&bitarray->lock);
330
331 if (bit >= bitarray->num_bits) {
332 ret = -EINVAL;
333 goto out;
334 }
335
336 idx = bit / bundle_bitness(bitarray);
337 off = bit % bundle_bitness(bitarray);
338
339 bitarray->bundles[idx] |= BIT(off);
340
341 ret = 0;
342
343 out:
344 k_spin_unlock(&bitarray->lock, key);
345 return ret;
346 }
347
sys_bitarray_clear_bit(sys_bitarray_t * bitarray,size_t bit)348 int sys_bitarray_clear_bit(sys_bitarray_t *bitarray, size_t bit)
349 {
350 k_spinlock_key_t key;
351 int ret;
352 size_t idx, off;
353
354 __ASSERT_NO_MSG(bitarray != NULL);
355 __ASSERT_NO_MSG(bitarray->num_bits > 0);
356
357 key = k_spin_lock(&bitarray->lock);
358
359 if (bit >= bitarray->num_bits) {
360 ret = -EINVAL;
361 goto out;
362 }
363
364 idx = bit / bundle_bitness(bitarray);
365 off = bit % bundle_bitness(bitarray);
366
367 bitarray->bundles[idx] &= ~BIT(off);
368
369 ret = 0;
370
371 out:
372 k_spin_unlock(&bitarray->lock, key);
373 return ret;
374 }
375
sys_bitarray_test_bit(sys_bitarray_t * bitarray,size_t bit,int * val)376 int sys_bitarray_test_bit(sys_bitarray_t *bitarray, size_t bit, int *val)
377 {
378 k_spinlock_key_t key;
379 int ret;
380 size_t idx, off;
381
382 __ASSERT_NO_MSG(bitarray != NULL);
383 __ASSERT_NO_MSG(bitarray->num_bits > 0);
384
385 key = k_spin_lock(&bitarray->lock);
386
387 CHECKIF(val == NULL) {
388 ret = -EINVAL;
389 goto out;
390 }
391
392 if (bit >= bitarray->num_bits) {
393 ret = -EINVAL;
394 goto out;
395 }
396
397 idx = bit / bundle_bitness(bitarray);
398 off = bit % bundle_bitness(bitarray);
399
400 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
401 *val = 1;
402 } else {
403 *val = 0;
404 }
405
406 ret = 0;
407
408 out:
409 k_spin_unlock(&bitarray->lock, key);
410 return ret;
411 }
412
sys_bitarray_test_and_set_bit(sys_bitarray_t * bitarray,size_t bit,int * prev_val)413 int sys_bitarray_test_and_set_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
414 {
415 k_spinlock_key_t key;
416 int ret;
417 size_t idx, off;
418
419 __ASSERT_NO_MSG(bitarray != NULL);
420 __ASSERT_NO_MSG(bitarray->num_bits > 0);
421
422 key = k_spin_lock(&bitarray->lock);
423
424 CHECKIF(prev_val == NULL) {
425 ret = -EINVAL;
426 goto out;
427 }
428
429 if (bit >= bitarray->num_bits) {
430 ret = -EINVAL;
431 goto out;
432 }
433
434 idx = bit / bundle_bitness(bitarray);
435 off = bit % bundle_bitness(bitarray);
436
437 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
438 *prev_val = 1;
439 } else {
440 *prev_val = 0;
441 }
442
443 bitarray->bundles[idx] |= BIT(off);
444
445 ret = 0;
446
447 out:
448 k_spin_unlock(&bitarray->lock, key);
449 return ret;
450 }
451
sys_bitarray_test_and_clear_bit(sys_bitarray_t * bitarray,size_t bit,int * prev_val)452 int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
453 {
454 k_spinlock_key_t key;
455 int ret;
456 size_t idx, off;
457
458 __ASSERT_NO_MSG(bitarray != NULL);
459 __ASSERT_NO_MSG(bitarray->num_bits > 0);
460
461 key = k_spin_lock(&bitarray->lock);
462
463 CHECKIF(prev_val == NULL) {
464 ret = -EINVAL;
465 goto out;
466 }
467
468 if (bit >= bitarray->num_bits) {
469 ret = -EINVAL;
470 goto out;
471 }
472
473 idx = bit / bundle_bitness(bitarray);
474 off = bit % bundle_bitness(bitarray);
475
476 if ((bitarray->bundles[idx] & BIT(off)) != 0) {
477 *prev_val = 1;
478 } else {
479 *prev_val = 0;
480 }
481
482 bitarray->bundles[idx] &= ~BIT(off);
483
484 ret = 0;
485
486 out:
487 k_spin_unlock(&bitarray->lock, key);
488 return ret;
489 }
490
sys_bitarray_alloc(sys_bitarray_t * bitarray,size_t num_bits,size_t * offset)491 int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits,
492 size_t *offset)
493 {
494 k_spinlock_key_t key;
495 uint32_t bit_idx;
496 int ret;
497 struct bundle_data bd;
498 size_t off_start, off_end;
499 size_t mismatch;
500
501 __ASSERT_NO_MSG(bitarray != NULL);
502 __ASSERT_NO_MSG(bitarray->num_bits > 0);
503
504 key = k_spin_lock(&bitarray->lock);
505
506 CHECKIF(offset == NULL) {
507 ret = -EINVAL;
508 goto out;
509 }
510
511 if ((num_bits == 0) || (num_bits > bitarray->num_bits)) {
512 ret = -EINVAL;
513 goto out;
514 }
515
516 if (bitarray->num_bits <= 32) {
517 int off;
518
519 off = bitmask_find_gap(bitarray->bundles[0], num_bits, bitarray->num_bits, false);
520 if (off < 0) {
521 ret = -ENOSPC;
522 } else {
523 bitarray->bundles[0] |= BIT_MASK(num_bits) << off;
524 *offset = off;
525 ret = 0;
526 }
527 goto out;
528 }
529
530 bit_idx = 0;
531
532 /* Find the first non-allocated bit by looking at bundles
533 * instead of individual bits.
534 *
535 * On RISC-V 64-bit, it complains about undefined reference to `ffs`.
536 * So don't use this on RISCV64.
537 */
538 for (size_t idx = 0; idx < bitarray->num_bundles; idx++) {
539 if (~bitarray->bundles[idx] == 0U) {
540 /* bundle is all 1s => all allocated, skip */
541 bit_idx += bundle_bitness(bitarray);
542 continue;
543 }
544
545 if (bitarray->bundles[idx] != 0U) {
546 /* Find the first free bit in bundle if not all free */
547 off_start = find_lsb_set(~bitarray->bundles[idx]) - 1;
548 bit_idx += off_start;
549 }
550
551 break;
552 }
553
554 off_end = bitarray->num_bits - num_bits;
555 ret = -ENOSPC;
556 while (bit_idx <= off_end) {
557 if (match_region(bitarray, bit_idx, num_bits, false,
558 &bd, &mismatch)) {
559 set_region(bitarray, bit_idx, num_bits, true, &bd);
560
561 *offset = bit_idx;
562 ret = 0;
563 break;
564 }
565
566 /* Fast-forward to the bit just after
567 * the mismatched bit.
568 */
569 bit_idx = mismatch + 1;
570 }
571
572 out:
573 k_spin_unlock(&bitarray->lock, key);
574 return ret;
575 }
576
sys_bitarray_find_nth_set(sys_bitarray_t * bitarray,size_t n,size_t num_bits,size_t offset,size_t * found_at)577 int sys_bitarray_find_nth_set(sys_bitarray_t *bitarray, size_t n, size_t num_bits, size_t offset,
578 size_t *found_at)
579 {
580 k_spinlock_key_t key;
581 size_t count, idx;
582 uint32_t mask;
583 struct bundle_data bd;
584 int ret;
585
586 __ASSERT_NO_MSG(bitarray != NULL);
587 __ASSERT_NO_MSG(bitarray->num_bits > 0);
588
589 key = k_spin_lock(&bitarray->lock);
590
591 if (n == 0 || num_bits == 0 || offset + num_bits > bitarray->num_bits) {
592 ret = -EINVAL;
593 goto out;
594 }
595
596 ret = 1;
597 mask = 0;
598 setup_bundle_data(bitarray, &bd, offset, num_bits);
599
600 count = POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
601 /* If we already found more bits set than n, we found the target bundle */
602 if (count >= n) {
603 idx = bd.sidx;
604 mask = bd.smask;
605 goto found;
606 }
607 /* Keep looking if there are more bundles */
608 if (bd.sidx != bd.eidx) {
609 /* We are now only looking for the remaining bits */
610 n -= count;
611 /* First bundle was already checked, keep looking in middle (complete)
612 * bundles.
613 */
614 for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
615 count = POPCOUNT(bitarray->bundles[idx]);
616 if (count >= n) {
617 mask = ~(mask & 0);
618 goto found;
619 }
620 n -= count;
621 }
622 /* Continue searching in last bundle */
623 count = POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask);
624 if (count >= n) {
625 idx = bd.eidx;
626 mask = bd.emask;
627 goto found;
628 }
629 }
630
631 goto out;
632
633 found:
634 /* The bit we are looking for must be in the current bundle idx.
635 * Find out the exact index of the bit.
636 */
637 for (size_t j = 0; j <= bundle_bitness(bitarray) - 1; j++) {
638 if (bitarray->bundles[idx] & mask & BIT(j)) {
639 if (--n <= 0) {
640 *found_at = idx * bundle_bitness(bitarray) + j;
641 ret = 0;
642 break;
643 }
644 }
645 }
646
647 out:
648 k_spin_unlock(&bitarray->lock, key);
649 return ret;
650 }
651
sys_bitarray_free(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)652 int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
653 size_t offset)
654 {
655 k_spinlock_key_t key;
656 int ret;
657 size_t off_end = offset + num_bits - 1;
658 struct bundle_data bd;
659
660 __ASSERT_NO_MSG(bitarray != NULL);
661 __ASSERT_NO_MSG(bitarray->num_bits > 0);
662
663 key = k_spin_lock(&bitarray->lock);
664
665 if ((num_bits == 0)
666 || (num_bits > bitarray->num_bits)
667 || (offset >= bitarray->num_bits)
668 || (off_end >= bitarray->num_bits)) {
669 ret = -EINVAL;
670 goto out;
671 }
672
673 /* Note that we need to make sure the bits in specified region
674 * (offset to offset + num_bits) are all allocated before we clear
675 * them.
676 */
677 if (bitarray->num_bits <= 32) {
678 uint32_t mask = BIT_MASK(num_bits) << offset;
679
680 if ((mask & bitarray->bundles[0]) != mask) {
681 ret = -EFAULT;
682 } else {
683 bitarray->bundles[0] &= ~mask;
684 ret = 0;
685 }
686 } else if (match_region(bitarray, offset, num_bits, true, &bd, NULL)) {
687 set_region(bitarray, offset, num_bits, false, &bd);
688 ret = 0;
689 } else {
690 ret = -EFAULT;
691 }
692
693 out:
694 k_spin_unlock(&bitarray->lock, key);
695 return ret;
696 }
697
is_region_set_clear(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)698 static bool is_region_set_clear(sys_bitarray_t *bitarray, size_t num_bits,
699 size_t offset, bool to_set)
700 {
701 bool ret;
702 struct bundle_data bd;
703 size_t off_end = offset + num_bits - 1;
704 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
705
706 __ASSERT_NO_MSG(bitarray != NULL);
707 __ASSERT_NO_MSG(bitarray->num_bits > 0);
708
709 if ((num_bits == 0)
710 || (num_bits > bitarray->num_bits)
711 || (offset >= bitarray->num_bits)
712 || (off_end >= bitarray->num_bits)) {
713 ret = false;
714 goto out;
715 }
716
717 ret = match_region(bitarray, offset, num_bits, to_set, &bd, NULL);
718
719 out:
720 k_spin_unlock(&bitarray->lock, key);
721 return ret;
722 }
723
sys_bitarray_is_region_set(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)724 bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
725 size_t offset)
726 {
727 return is_region_set_clear(bitarray, num_bits, offset, true);
728 }
729
sys_bitarray_is_region_cleared(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)730 bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
731 size_t offset)
732 {
733 return is_region_set_clear(bitarray, num_bits, offset, false);
734 }
735
set_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)736 static int set_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
737 size_t offset, bool to_set)
738 {
739 int ret;
740 size_t off_end = offset + num_bits - 1;
741 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
742
743 __ASSERT_NO_MSG(bitarray != NULL);
744 __ASSERT_NO_MSG(bitarray->num_bits > 0);
745
746 if ((num_bits == 0)
747 || (num_bits > bitarray->num_bits)
748 || (offset >= bitarray->num_bits)
749 || (off_end >= bitarray->num_bits)) {
750 ret = -EINVAL;
751 goto out;
752 }
753
754 set_region(bitarray, offset, num_bits, to_set, NULL);
755 ret = 0;
756
757 out:
758 k_spin_unlock(&bitarray->lock, key);
759 return ret;
760 }
761
sys_bitarray_test_and_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)762 int sys_bitarray_test_and_set_region(sys_bitarray_t *bitarray, size_t num_bits,
763 size_t offset, bool to_set)
764 {
765 int ret;
766 bool region_clear;
767 struct bundle_data bd;
768
769 __ASSERT_NO_MSG(bitarray != NULL);
770 __ASSERT_NO_MSG(bitarray->num_bits > 0);
771
772 size_t off_end = offset + num_bits - 1;
773 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
774
775
776 if ((num_bits == 0)
777 || (num_bits > bitarray->num_bits)
778 || (offset >= bitarray->num_bits)
779 || (off_end >= bitarray->num_bits)) {
780 ret = -EINVAL;
781 goto out;
782 }
783
784 region_clear = match_region(bitarray, offset, num_bits, !to_set, &bd, NULL);
785 if (region_clear) {
786 set_region(bitarray, offset, num_bits, to_set, &bd);
787 ret = 0;
788 } else {
789 ret = -EEXIST;
790 }
791
792 out:
793 k_spin_unlock(&bitarray->lock, key);
794 return ret;
795 }
796
sys_bitarray_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)797 int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
798 size_t offset)
799 {
800 return set_clear_region(bitarray, num_bits, offset, true);
801 }
802
sys_bitarray_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)803 int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
804 size_t offset)
805 {
806 return set_clear_region(bitarray, num_bits, offset, false);
807 }
808
