Branch data Line data Source code
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 : :
29 : 0 : static void setup_bundle_data(sys_bitarray_t *bitarray,
30 : : struct bundle_data *bd,
31 : : size_t offset, size_t num_bits)
32 : : {
33 : 0 : bd->sidx = offset / bundle_bitness(bitarray);
34 : 0 : bd->soff = offset % bundle_bitness(bitarray);
35 : :
36 : 0 : bd->eidx = (offset + num_bits - 1) / bundle_bitness(bitarray);
37 : 0 : bd->eoff = (offset + num_bits - 1) % bundle_bitness(bitarray);
38 : :
39 : 0 : bd->smask = ~(BIT(bd->soff) - 1);
40 : 0 : bd->emask = (BIT(bd->eoff) - 1) | BIT(bd->eoff);
41 : :
42 [ # # ]: 0 : if (bd->sidx == bd->eidx) {
43 : : /* The region lies within the same bundle. So combine the masks. */
44 : 0 : bd->smask &= bd->emask;
45 : : }
46 : 0 : }
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 : : */
65 : 0 : 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 : 0 : size_t idx;
71 : 0 : uint32_t bundle;
72 : 0 : uint32_t mismatch_bundle;
73 : 0 : size_t mismatch_bundle_idx;
74 : 0 : size_t mismatch_bit_off;
75 : :
76 : 0 : setup_bundle_data(bitarray, bd, offset, num_bits);
77 : :
78 [ # # ]: 0 : if (bd->sidx == bd->eidx) {
79 : 0 : bundle = bitarray->bundles[bd->sidx];
80 [ # # ]: 0 : if (!match_set) {
81 : 0 : bundle = ~bundle;
82 : : }
83 : :
84 [ # # ]: 0 : if ((bundle & bd->smask) != bd->smask) {
85 : : /* Not matching to mask. */
86 : 0 : mismatch_bundle = ~bundle & bd->smask;
87 : 0 : mismatch_bundle_idx = bd->sidx;
88 : 0 : goto mismatch;
89 : : } else {
90 : : /* Matching to mask. */
91 : 0 : goto out;
92 : : }
93 : : }
94 : :
95 : : /* Region lies in a number of bundles. Need to loop through them. */
96 : :
97 : : /* Start of bundles */
98 : 0 : bundle = bitarray->bundles[bd->sidx];
99 [ # # ]: 0 : if (!match_set) {
100 : 0 : bundle = ~bundle;
101 : : }
102 : :
103 [ # # ]: 0 : if ((bundle & bd->smask) != bd->smask) {
104 : : /* Start bundle not matching to mask. */
105 : 0 : mismatch_bundle = ~bundle & bd->smask;
106 : 0 : mismatch_bundle_idx = bd->sidx;
107 : 0 : goto mismatch;
108 : : }
109 : :
110 : : /* End of bundles */
111 : 0 : bundle = bitarray->bundles[bd->eidx];
112 [ # # ]: 0 : if (!match_set) {
113 : 0 : bundle = ~bundle;
114 : : }
115 : :
116 [ # # ]: 0 : if ((bundle & bd->emask) != bd->emask) {
117 : : /* End bundle not matching to mask. */
118 : 0 : mismatch_bundle = ~bundle & bd->emask;
119 : 0 : mismatch_bundle_idx = bd->eidx;
120 : 0 : goto mismatch;
121 : : }
122 : :
123 : : /* In-between bundles */
124 [ # # ]: 0 : 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 : 0 : bundle = bitarray->bundles[idx];
129 [ # # ]: 0 : if (match_set) {
130 : 0 : bundle = ~bundle;
131 : : }
132 : :
133 [ # # ]: 0 : if (bundle != 0U) {
134 : : /* Bits in "between bundles" do not match */
135 : 0 : mismatch_bundle = bundle;
136 : 0 : mismatch_bundle_idx = idx;
137 : 0 : goto mismatch;
138 : : }
139 : : }
140 : :
141 : 0 : out:
142 : : /* All bits in region matched. */
143 : : return true;
144 : :
145 : 0 : mismatch:
146 [ # # ]: 0 : if (mismatch != NULL) {
147 : : /* Must have at least 1 bit set to indicate
148 : : * where the mismatch is.
149 : : */
150 [ # # ]: 0 : __ASSERT_NO_MSG(mismatch_bundle != 0);
151 : :
152 : 0 : mismatch_bit_off = find_lsb_set(mismatch_bundle) - 1;
153 : 0 : mismatch_bit_off += mismatch_bundle_idx *
154 : : bundle_bitness(bitarray);
155 : 0 : *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 : : */
172 : 0 : 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 : 0 : size_t idx;
177 : 0 : struct bundle_data bdata;
178 : :
179 [ # # ]: 0 : if (bd == NULL) {
180 : 0 : bd = &bdata;
181 : 0 : setup_bundle_data(bitarray, bd, offset, num_bits);
182 : : }
183 : :
184 [ # # ]: 0 : if (bd->sidx == bd->eidx) {
185 : : /* Start/end at same bundle */
186 [ # # ]: 0 : if (to_set) {
187 : 0 : bitarray->bundles[bd->sidx] |= bd->smask;
188 : : } else {
189 : 0 : 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 [ # # ]: 0 : if (to_set) {
198 : 0 : bitarray->bundles[bd->sidx] |= bd->smask;
199 : 0 : bitarray->bundles[bd->eidx] |= bd->emask;
200 [ # # ]: 0 : for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
201 : 0 : bitarray->bundles[idx] = ~0U;
202 : : }
203 : : } else {
204 : 0 : bitarray->bundles[bd->sidx] &= ~bd->smask;
205 : 0 : bitarray->bundles[bd->eidx] &= ~bd->emask;
206 [ # # ]: 0 : for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
207 : 0 : bitarray->bundles[idx] = 0U;
208 : : }
209 : : }
210 : : }
211 : 0 : }
212 : :
213 : 0 : int sys_bitarray_popcount_region(sys_bitarray_t *bitarray, size_t num_bits, size_t offset,
214 : : size_t *count)
215 : : {
216 : 0 : k_spinlock_key_t key;
217 : 0 : size_t idx;
218 : 0 : struct bundle_data bd;
219 : 0 : int ret;
220 : :
221 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
222 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
223 : :
224 : 0 : key = k_spin_lock(&bitarray->lock);
225 : :
226 [ # # # # ]: 0 : if (num_bits == 0 || offset + num_bits > bitarray->num_bits) {
227 : 0 : ret = -EINVAL;
228 : 0 : goto out;
229 : : }
230 : :
231 [ # # ]: 0 : CHECKIF(count == NULL) {
232 : 0 : ret = -EINVAL;
233 : 0 : goto out;
234 : : }
235 : :
236 : 0 : setup_bundle_data(bitarray, &bd, offset, num_bits);
237 : :
238 [ # # ]: 0 : if (bd.sidx == bd.eidx) {
239 : : /* Start/end at same bundle */
240 : 0 : *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 : 0 : *count = 0;
248 : 0 : *count += POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
249 : 0 : *count += POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask);
250 [ # # ]: 0 : for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
251 : 0 : *count += POPCOUNT(bitarray->bundles[idx]);
252 : : }
253 : : }
254 : :
255 : : ret = 0;
256 : :
257 : 0 : out:
258 : 0 : k_spin_unlock(&bitarray->lock, key);
259 : 0 : return ret;
260 : : }
261 : :
262 : 0 : int sys_bitarray_xor(sys_bitarray_t *dst, sys_bitarray_t *other, size_t num_bits, size_t offset)
263 : : {
264 : 0 : k_spinlock_key_t key_dst, key_other;
265 : 0 : int ret;
266 : 0 : size_t idx;
267 : 0 : struct bundle_data bd;
268 : :
269 [ # # ]: 0 : __ASSERT_NO_MSG(dst != NULL);
270 [ # # ]: 0 : __ASSERT_NO_MSG(dst->num_bits > 0);
271 [ # # ]: 0 : __ASSERT_NO_MSG(other != NULL);
272 [ # # ]: 0 : __ASSERT_NO_MSG(other->num_bits > 0);
273 : :
274 : 0 : key_dst = k_spin_lock(&dst->lock);
275 : 0 : key_other = k_spin_lock(&other->lock);
276 : :
277 : :
278 [ # # ]: 0 : if (dst->num_bits != other->num_bits) {
279 : 0 : ret = -EINVAL;
280 : 0 : goto out;
281 : : }
282 : :
283 [ # # # # ]: 0 : if (num_bits == 0 || offset + num_bits > dst->num_bits) {
284 : 0 : ret = -EINVAL;
285 : 0 : goto out;
286 : : }
287 : :
288 : 0 : setup_bundle_data(other, &bd, offset, num_bits);
289 : :
290 [ # # ]: 0 : if (bd.sidx == bd.eidx) {
291 : : /* Start/end at same bundle */
292 : 0 : dst->bundles[bd.sidx] =
293 : 0 : ((other->bundles[bd.sidx] ^ dst->bundles[bd.sidx]) & bd.smask) |
294 : 0 : (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 : 0 : dst->bundles[bd.sidx] =
302 : 0 : ((other->bundles[bd.sidx] ^ dst->bundles[bd.sidx]) & bd.smask) |
303 : 0 : (dst->bundles[bd.sidx] & ~bd.smask);
304 : 0 : dst->bundles[bd.eidx] =
305 : 0 : ((other->bundles[bd.eidx] ^ dst->bundles[bd.eidx]) & bd.emask) |
306 : 0 : (dst->bundles[bd.eidx] & ~bd.emask);
307 [ # # ]: 0 : for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
308 : 0 : dst->bundles[idx] ^= other->bundles[idx];
309 : : }
310 : : }
311 : :
312 : : ret = 0;
313 : :
314 : 0 : out:
315 : 0 : k_spin_unlock(&other->lock, key_other);
316 : 0 : k_spin_unlock(&dst->lock, key_dst);
317 : 0 : return ret;
318 : : }
319 : :
320 : 0 : int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit)
321 : : {
322 : 0 : k_spinlock_key_t key;
323 : 0 : int ret;
324 : 0 : size_t idx, off;
325 : :
326 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
327 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
328 : :
329 : 0 : key = k_spin_lock(&bitarray->lock);
330 : :
331 [ # # ]: 0 : if (bit >= bitarray->num_bits) {
332 : 0 : ret = -EINVAL;
333 : 0 : goto out;
334 : : }
335 : :
336 : 0 : idx = bit / bundle_bitness(bitarray);
337 : 0 : off = bit % bundle_bitness(bitarray);
338 : :
339 : 0 : bitarray->bundles[idx] |= BIT(off);
340 : :
341 : 0 : ret = 0;
342 : :
343 : 0 : out:
344 : 0 : k_spin_unlock(&bitarray->lock, key);
345 : 0 : return ret;
346 : : }
347 : :
348 : 0 : int sys_bitarray_clear_bit(sys_bitarray_t *bitarray, size_t bit)
349 : : {
350 : 0 : k_spinlock_key_t key;
351 : 0 : int ret;
352 : 0 : size_t idx, off;
353 : :
354 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
355 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
356 : :
357 : 0 : key = k_spin_lock(&bitarray->lock);
358 : :
359 [ # # ]: 0 : if (bit >= bitarray->num_bits) {
360 : 0 : ret = -EINVAL;
361 : 0 : goto out;
362 : : }
363 : :
364 : 0 : idx = bit / bundle_bitness(bitarray);
365 : 0 : off = bit % bundle_bitness(bitarray);
366 : :
367 : 0 : bitarray->bundles[idx] &= ~BIT(off);
368 : :
369 : 0 : ret = 0;
370 : :
371 : 0 : out:
372 : 0 : k_spin_unlock(&bitarray->lock, key);
373 : 0 : return ret;
374 : : }
375 : :
376 : 0 : int sys_bitarray_test_bit(sys_bitarray_t *bitarray, size_t bit, int *val)
377 : : {
378 : 0 : k_spinlock_key_t key;
379 : 0 : int ret;
380 : 0 : size_t idx, off;
381 : :
382 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
383 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
384 : :
385 : 0 : key = k_spin_lock(&bitarray->lock);
386 : :
387 [ # # ]: 0 : CHECKIF(val == NULL) {
388 : 0 : ret = -EINVAL;
389 : 0 : goto out;
390 : : }
391 : :
392 [ # # ]: 0 : if (bit >= bitarray->num_bits) {
393 : 0 : ret = -EINVAL;
394 : 0 : goto out;
395 : : }
396 : :
397 : 0 : idx = bit / bundle_bitness(bitarray);
398 : 0 : off = bit % bundle_bitness(bitarray);
399 : :
400 [ # # ]: 0 : if ((bitarray->bundles[idx] & BIT(off)) != 0) {
401 : 0 : *val = 1;
402 : : } else {
403 : 0 : *val = 0;
404 : : }
405 : :
406 : : ret = 0;
407 : :
408 : 0 : out:
409 : 0 : k_spin_unlock(&bitarray->lock, key);
410 : 0 : return ret;
411 : : }
412 : :
413 : 0 : int sys_bitarray_test_and_set_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
414 : : {
415 : 0 : k_spinlock_key_t key;
416 : 0 : int ret;
417 : 0 : size_t idx, off;
418 : :
419 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
420 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
421 : :
422 : 0 : key = k_spin_lock(&bitarray->lock);
423 : :
424 [ # # ]: 0 : CHECKIF(prev_val == NULL) {
425 : 0 : ret = -EINVAL;
426 : 0 : goto out;
427 : : }
428 : :
429 [ # # ]: 0 : if (bit >= bitarray->num_bits) {
430 : 0 : ret = -EINVAL;
431 : 0 : goto out;
432 : : }
433 : :
434 : 0 : idx = bit / bundle_bitness(bitarray);
435 : 0 : off = bit % bundle_bitness(bitarray);
436 : :
437 [ # # ]: 0 : if ((bitarray->bundles[idx] & BIT(off)) != 0) {
438 : 0 : *prev_val = 1;
439 : : } else {
440 : 0 : *prev_val = 0;
441 : : }
442 : :
443 : 0 : bitarray->bundles[idx] |= BIT(off);
444 : :
445 : 0 : ret = 0;
446 : :
447 : 0 : out:
448 : 0 : k_spin_unlock(&bitarray->lock, key);
449 : 0 : return ret;
450 : : }
451 : :
452 : 0 : int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
453 : : {
454 : 0 : k_spinlock_key_t key;
455 : 0 : int ret;
456 : 0 : size_t idx, off;
457 : :
458 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
459 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
460 : :
461 : 0 : key = k_spin_lock(&bitarray->lock);
462 : :
463 [ # # ]: 0 : CHECKIF(prev_val == NULL) {
464 : 0 : ret = -EINVAL;
465 : 0 : goto out;
466 : : }
467 : :
468 [ # # ]: 0 : if (bit >= bitarray->num_bits) {
469 : 0 : ret = -EINVAL;
470 : 0 : goto out;
471 : : }
472 : :
473 : 0 : idx = bit / bundle_bitness(bitarray);
474 : 0 : off = bit % bundle_bitness(bitarray);
475 : :
476 [ # # ]: 0 : if ((bitarray->bundles[idx] & BIT(off)) != 0) {
477 : 0 : *prev_val = 1;
478 : : } else {
479 : 0 : *prev_val = 0;
480 : : }
481 : :
482 : 0 : bitarray->bundles[idx] &= ~BIT(off);
483 : :
484 : 0 : ret = 0;
485 : :
486 : 0 : out:
487 : 0 : k_spin_unlock(&bitarray->lock, key);
488 : 0 : return ret;
489 : : }
490 : :
491 : 0 : int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits,
492 : : size_t *offset)
493 : : {
494 : 0 : k_spinlock_key_t key;
495 : 0 : uint32_t bit_idx;
496 : 0 : int ret;
497 : 0 : struct bundle_data bd;
498 : 0 : size_t off_start, off_end;
499 : 0 : size_t mismatch;
500 : :
501 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
502 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
503 : :
504 : 0 : key = k_spin_lock(&bitarray->lock);
505 : :
506 [ # # ]: 0 : CHECKIF(offset == NULL) {
507 : 0 : ret = -EINVAL;
508 : 0 : goto out;
509 : : }
510 : :
511 [ # # # # ]: 0 : if ((num_bits == 0) || (num_bits > bitarray->num_bits)) {
512 : 0 : ret = -EINVAL;
513 : 0 : 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 [ # # ]: 0 : for (size_t idx = 0; idx < bitarray->num_bundles; idx++) {
525 [ # # ]: 0 : if (~bitarray->bundles[idx] == 0U) {
526 : : /* bundle is all 1s => all allocated, skip */
527 : 0 : bit_idx += bundle_bitness(bitarray);
528 : 0 : continue;
529 : : }
530 : :
531 [ # # ]: 0 : if (bitarray->bundles[idx] != 0U) {
532 : : /* Find the first free bit in bundle if not all free */
533 : 0 : off_start = find_lsb_set(~bitarray->bundles[idx]) - 1;
534 : 0 : bit_idx += off_start;
535 : : }
536 : :
537 : : break;
538 : : }
539 : :
540 : 0 : off_end = bitarray->num_bits - num_bits;
541 : 0 : ret = -ENOSPC;
542 [ # # ]: 0 : while (bit_idx <= off_end) {
543 [ # # ]: 0 : if (match_region(bitarray, bit_idx, num_bits, false,
544 : : &bd, &mismatch)) {
545 : 0 : set_region(bitarray, bit_idx, num_bits, true, &bd);
546 : :
547 : 0 : *offset = bit_idx;
548 : 0 : ret = 0;
549 : 0 : break;
550 : : }
551 : :
552 : : /* Fast-forward to the bit just after
553 : : * the mismatched bit.
554 : : */
555 : 0 : bit_idx = mismatch + 1;
556 : : }
557 : :
558 : 0 : out:
559 : 0 : k_spin_unlock(&bitarray->lock, key);
560 : 0 : return ret;
561 : : }
562 : :
563 : 0 : 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 : 0 : k_spinlock_key_t key;
567 : 0 : size_t count, idx;
568 : 0 : uint32_t mask;
569 : 0 : struct bundle_data bd;
570 : 0 : int ret;
571 : :
572 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
573 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
574 : :
575 : 0 : key = k_spin_lock(&bitarray->lock);
576 : :
577 [ # # # # ]: 0 : if (n == 0 || num_bits == 0 || offset + num_bits > bitarray->num_bits) {
578 : 0 : ret = -EINVAL;
579 : 0 : goto out;
580 : : }
581 : :
582 : 0 : ret = 1;
583 : 0 : mask = 0;
584 : 0 : setup_bundle_data(bitarray, &bd, offset, num_bits);
585 : :
586 : 0 : count = POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask);
587 : : /* If we already found more bits set than n, we found the target bundle */
588 [ # # ]: 0 : if (count >= n) {
589 : 0 : idx = bd.sidx;
590 : 0 : mask = bd.smask;
591 : 0 : goto found;
592 : : }
593 : : /* Keep looking if there are more bundles */
594 [ # # ]: 0 : if (bd.sidx != bd.eidx) {
595 : : /* We are now only looking for the remaining bits */
596 : 0 : n -= count;
597 : : /* First bundle was already checked, keep looking in middle (complete)
598 : : * bundles.
599 : : */
600 [ # # ]: 0 : for (idx = bd.sidx + 1; idx < bd.eidx; idx++) {
601 : 0 : count = POPCOUNT(bitarray->bundles[idx]);
602 [ # # ]: 0 : if (count >= n) {
603 : 0 : mask = ~(mask & 0);
604 : 0 : goto found;
605 : : }
606 : 0 : n -= count;
607 : : }
608 : : /* Continue searching in last bundle */
609 : 0 : count = POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask);
610 [ # # ]: 0 : if (count >= n) {
611 : 0 : idx = bd.eidx;
612 : 0 : mask = bd.emask;
613 : 0 : goto found;
614 : : }
615 : : }
616 : :
617 : 0 : goto out;
618 : :
619 : 0 : 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 [ # # ]: 0 : for (int j = 0; j <= bundle_bitness(bitarray) - 1; j++) {
624 [ # # ]: 0 : if (bitarray->bundles[idx] & mask & BIT(j)) {
625 [ # # ]: 0 : if (--n <= 0) {
626 : 0 : *found_at = idx * bundle_bitness(bitarray) + j;
627 : 0 : ret = 0;
628 : 0 : break;
629 : : }
630 : : }
631 : : }
632 : :
633 : 0 : out:
634 : 0 : k_spin_unlock(&bitarray->lock, key);
635 : 0 : return ret;
636 : : }
637 : :
638 : 0 : int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
639 : : size_t offset)
640 : : {
641 : 0 : k_spinlock_key_t key;
642 : 0 : int ret;
643 : 0 : size_t off_end = offset + num_bits - 1;
644 : 0 : struct bundle_data bd;
645 : :
646 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
647 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
648 : :
649 : 0 : key = k_spin_lock(&bitarray->lock);
650 : :
651 [ # # ]: 0 : if ((num_bits == 0)
652 [ # # ]: 0 : || (num_bits > bitarray->num_bits)
653 [ # # ]: 0 : || (offset >= bitarray->num_bits)
654 [ # # ]: 0 : || (off_end >= bitarray->num_bits)) {
655 : 0 : ret = -EINVAL;
656 : 0 : 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 [ # # ]: 0 : if (match_region(bitarray, offset, num_bits, true, &bd, NULL)) {
664 : 0 : set_region(bitarray, offset, num_bits, false, &bd);
665 : 0 : ret = 0;
666 : : } else {
667 : : ret = -EFAULT;
668 : : }
669 : :
670 : 0 : out:
671 : 0 : k_spin_unlock(&bitarray->lock, key);
672 : 0 : return ret;
673 : : }
674 : :
675 : 0 : static bool is_region_set_clear(sys_bitarray_t *bitarray, size_t num_bits,
676 : : size_t offset, bool to_set)
677 : : {
678 : 0 : bool ret;
679 : 0 : struct bundle_data bd;
680 : 0 : size_t off_end = offset + num_bits - 1;
681 : 0 : k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
682 : :
683 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
684 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
685 : :
686 [ # # ]: 0 : if ((num_bits == 0)
687 [ # # ]: 0 : || (num_bits > bitarray->num_bits)
688 [ # # ]: 0 : || (offset >= bitarray->num_bits)
689 [ # # ]: 0 : || (off_end >= bitarray->num_bits)) {
690 : 0 : ret = false;
691 : 0 : goto out;
692 : : }
693 : :
694 : 0 : ret = match_region(bitarray, offset, num_bits, to_set, &bd, NULL);
695 : :
696 : 0 : out:
697 : 0 : k_spin_unlock(&bitarray->lock, key);
698 : 0 : return ret;
699 : : }
700 : :
701 : 0 : bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
702 : : size_t offset)
703 : : {
704 : 0 : return is_region_set_clear(bitarray, num_bits, offset, true);
705 : : }
706 : :
707 : 0 : bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
708 : : size_t offset)
709 : : {
710 : 0 : return is_region_set_clear(bitarray, num_bits, offset, false);
711 : : }
712 : :
713 : 0 : static int set_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
714 : : size_t offset, bool to_set)
715 : : {
716 : 0 : int ret;
717 : 0 : size_t off_end = offset + num_bits - 1;
718 : 0 : k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
719 : :
720 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
721 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
722 : :
723 [ # # ]: 0 : if ((num_bits == 0)
724 [ # # ]: 0 : || (num_bits > bitarray->num_bits)
725 [ # # ]: 0 : || (offset >= bitarray->num_bits)
726 [ # # ]: 0 : || (off_end >= bitarray->num_bits)) {
727 : 0 : ret = -EINVAL;
728 : 0 : goto out;
729 : : }
730 : :
731 : 0 : set_region(bitarray, offset, num_bits, to_set, NULL);
732 : 0 : ret = 0;
733 : :
734 : 0 : out:
735 : 0 : k_spin_unlock(&bitarray->lock, key);
736 : 0 : return ret;
737 : : }
738 : :
739 : 0 : int sys_bitarray_test_and_set_region(sys_bitarray_t *bitarray, size_t num_bits,
740 : : size_t offset, bool to_set)
741 : : {
742 : 0 : int ret;
743 : 0 : bool region_clear;
744 : 0 : struct bundle_data bd;
745 : :
746 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray != NULL);
747 [ # # ]: 0 : __ASSERT_NO_MSG(bitarray->num_bits > 0);
748 : :
749 : 0 : size_t off_end = offset + num_bits - 1;
750 : 0 : k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
751 : :
752 : :
753 [ # # ]: 0 : if ((num_bits == 0)
754 [ # # ]: 0 : || (num_bits > bitarray->num_bits)
755 [ # # ]: 0 : || (offset >= bitarray->num_bits)
756 [ # # ]: 0 : || (off_end >= bitarray->num_bits)) {
757 : 0 : ret = -EINVAL;
758 : 0 : goto out;
759 : : }
760 : :
761 : 0 : region_clear = match_region(bitarray, offset, num_bits, !to_set, &bd, NULL);
762 [ # # ]: 0 : if (region_clear) {
763 : 0 : set_region(bitarray, offset, num_bits, to_set, &bd);
764 : 0 : ret = 0;
765 : : } else {
766 : : ret = -EEXIST;
767 : : }
768 : :
769 : 0 : out:
770 : 0 : k_spin_unlock(&bitarray->lock, key);
771 : 0 : return ret;
772 : : }
773 : :
774 : 0 : int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
775 : : size_t offset)
776 : : {
777 : 0 : return set_clear_region(bitarray, num_bits, offset, true);
778 : : }
779 : :
780 : 0 : int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
781 : : size_t offset)
782 : : {
783 : 0 : return set_clear_region(bitarray, num_bits, offset, false);
784 : : }
|
