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