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 bit_idx = 0;
517
518 /* Find the first non-allocated bit by looking at bundles
519 * instead of individual bits.
520 *
521 * On RISC-V 64-bit, it complains about undefined reference to `ffs`.
522 * So don't use this on RISCV64.
523 */
524 for (size_t idx = 0; idx < bitarray->num_bundles; idx++) {
525 if (~bitarray->bundles[idx] == 0U) {
526 /* bundle is all 1s => all allocated, skip */
527 bit_idx += bundle_bitness(bitarray);
528 continue;
529 }
530
531 if (bitarray->bundles[idx] != 0U) {
532 /* Find the first free bit in bundle if not all free */
533 off_start = find_lsb_set(~bitarray->bundles[idx]) - 1;
534 bit_idx += off_start;
535 }
536
537 break;
538 }
539
540 off_end = bitarray->num_bits - num_bits;
541 ret = -ENOSPC;
542 while (bit_idx <= off_end) {
543 if (match_region(bitarray, bit_idx, num_bits, false,
544 &bd, &mismatch)) {
545 set_region(bitarray, bit_idx, num_bits, true, &bd);
546
547 *offset = bit_idx;
548 ret = 0;
549 break;
550 }
551
552 /* Fast-forward to the bit just after
553 * the mismatched bit.
554 */
555 bit_idx = mismatch + 1;
556 }
557
558 out:
559 k_spin_unlock(&bitarray->lock, key);
560 return ret;
561 }
562
sys_bitarray_find_nth_set(sys_bitarray_t * bitarray,size_t n,size_t num_bits,size_t offset,size_t * found_at)563 int sys_bitarray_find_nth_set(sys_bitarray_t *bitarray, size_t n, size_t num_bits, size_t offset,
564 size_t *found_at)
565 {
566 k_spinlock_key_t key;
567 size_t count, idx;
568 uint32_t mask;
569 struct bundle_data bd;
570 int ret;
571
572 __ASSERT_NO_MSG(bitarray != NULL);
573 __ASSERT_NO_MSG(bitarray->num_bits > 0);
574
575 key = k_spin_lock(&bitarray->lock);
576
577 if (n == 0 || num_bits == 0 || offset + num_bits > bitarray->num_bits) {
578 ret = -EINVAL;
579 goto out;
580 }
581
582 ret = 1;
583 mask = 0;
584 setup_bundle_data(bitarray, &bd, offset, num_bits);
585
586 count = POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
587 /* If we already found more bits set than n, we found the target bundle */
588 if (count >= n) {
589 idx = bd.sidx;
590 mask = bd.smask;
591 goto found;
592 }
593 /* Keep looking if there are more bundles */
594 if (bd.sidx != bd.eidx) {
595 /* We are now only looking for the remaining bits */
596 n -= count;
597 /* First bundle was already checked, keep looking in middle (complete)
598 * bundles.
599 */
600 for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
601 count = POPCOUNT(bitarray->bundles[idx]);
602 if (count >= n) {
603 mask = ~(mask & 0);
604 goto found;
605 }
606 n -= count;
607 }
608 /* Continue searching in last bundle */
609 count = POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask);
610 if (count >= n) {
611 idx = bd.eidx;
612 mask = bd.emask;
613 goto found;
614 }
615 }
616
617 goto out;
618
619 found:
620 /* The bit we are looking for must be in the current bundle idx.
621 * Find out the exact index of the bit.
622 */
623 for (int j = 0; j <= bundle_bitness(bitarray) - 1; j++) {
624 if (bitarray->bundles[idx] & mask & BIT(j)) {
625 if (--n <= 0) {
626 *found_at = idx * bundle_bitness(bitarray) + j;
627 ret = 0;
628 break;
629 }
630 }
631 }
632
633 out:
634 k_spin_unlock(&bitarray->lock, key);
635 return ret;
636 }
637
sys_bitarray_free(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)638 int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
639 size_t offset)
640 {
641 k_spinlock_key_t key;
642 int ret;
643 size_t off_end = offset + num_bits - 1;
644 struct bundle_data bd;
645
646 __ASSERT_NO_MSG(bitarray != NULL);
647 __ASSERT_NO_MSG(bitarray->num_bits > 0);
648
649 key = k_spin_lock(&bitarray->lock);
650
651 if ((num_bits == 0)
652 || (num_bits > bitarray->num_bits)
653 || (offset >= bitarray->num_bits)
654 || (off_end >= bitarray->num_bits)) {
655 ret = -EINVAL;
656 goto out;
657 }
658
659 /* Note that we need to make sure the bits in specified region
660 * (offset to offset + num_bits) are all allocated before we clear
661 * them.
662 */
663 if (match_region(bitarray, offset, num_bits, true, &bd, NULL)) {
664 set_region(bitarray, offset, num_bits, false, &bd);
665 ret = 0;
666 } else {
667 ret = -EFAULT;
668 }
669
670 out:
671 k_spin_unlock(&bitarray->lock, key);
672 return ret;
673 }
674
is_region_set_clear(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)675 static bool is_region_set_clear(sys_bitarray_t *bitarray, size_t num_bits,
676 size_t offset, bool to_set)
677 {
678 bool ret;
679 struct bundle_data bd;
680 size_t off_end = offset + num_bits - 1;
681 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
682
683 __ASSERT_NO_MSG(bitarray != NULL);
684 __ASSERT_NO_MSG(bitarray->num_bits > 0);
685
686 if ((num_bits == 0)
687 || (num_bits > bitarray->num_bits)
688 || (offset >= bitarray->num_bits)
689 || (off_end >= bitarray->num_bits)) {
690 ret = false;
691 goto out;
692 }
693
694 ret = match_region(bitarray, offset, num_bits, to_set, &bd, NULL);
695
696 out:
697 k_spin_unlock(&bitarray->lock, key);
698 return ret;
699 }
700
sys_bitarray_is_region_set(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)701 bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
702 size_t offset)
703 {
704 return is_region_set_clear(bitarray, num_bits, offset, true);
705 }
706
sys_bitarray_is_region_cleared(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)707 bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
708 size_t offset)
709 {
710 return is_region_set_clear(bitarray, num_bits, offset, false);
711 }
712
set_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)713 static int set_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
714 size_t offset, bool to_set)
715 {
716 int ret;
717 size_t off_end = offset + num_bits - 1;
718 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
719
720 __ASSERT_NO_MSG(bitarray != NULL);
721 __ASSERT_NO_MSG(bitarray->num_bits > 0);
722
723 if ((num_bits == 0)
724 || (num_bits > bitarray->num_bits)
725 || (offset >= bitarray->num_bits)
726 || (off_end >= bitarray->num_bits)) {
727 ret = -EINVAL;
728 goto out;
729 }
730
731 set_region(bitarray, offset, num_bits, to_set, NULL);
732 ret = 0;
733
734 out:
735 k_spin_unlock(&bitarray->lock, key);
736 return ret;
737 }
738
sys_bitarray_test_and_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset,bool to_set)739 int sys_bitarray_test_and_set_region(sys_bitarray_t *bitarray, size_t num_bits,
740 size_t offset, bool to_set)
741 {
742 int ret;
743 bool region_clear;
744 struct bundle_data bd;
745
746 __ASSERT_NO_MSG(bitarray != NULL);
747 __ASSERT_NO_MSG(bitarray->num_bits > 0);
748
749 size_t off_end = offset + num_bits - 1;
750 k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
751
752
753 if ((num_bits == 0)
754 || (num_bits > bitarray->num_bits)
755 || (offset >= bitarray->num_bits)
756 || (off_end >= bitarray->num_bits)) {
757 ret = -EINVAL;
758 goto out;
759 }
760
761 region_clear = match_region(bitarray, offset, num_bits, !to_set, &bd, NULL);
762 if (region_clear) {
763 set_region(bitarray, offset, num_bits, to_set, &bd);
764 ret = 0;
765 } else {
766 ret = -EEXIST;
767 }
768
769 out:
770 k_spin_unlock(&bitarray->lock, key);
771 return ret;
772 }
773
sys_bitarray_set_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)774 int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
775 size_t offset)
776 {
777 return set_clear_region(bitarray, num_bits, offset, true);
778 }
779
sys_bitarray_clear_region(sys_bitarray_t * bitarray,size_t num_bits,size_t offset)780 int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
781 size_t offset)
782 {
783 return set_clear_region(bitarray, num_bits, offset, false);
784 }
785
