diff --git a/include/zephyr/sys/bitarray.h b/include/zephyr/sys/bitarray.h index 90d1f59345b..6ca166f6b21 100644 --- a/include/zephyr/sys/bitarray.h +++ b/include/zephyr/sys/bitarray.h @@ -168,6 +168,23 @@ int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *p int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits, size_t *offset); +/** + * Count bits set in a bit array region + * + * This counts the number of bits set (@p count) in a + * region (@p offset, @p num_bits). + * + * @param[in] bitarray Bitarray struct + * @param[in] num_bits Number of bits to check, must be larger than 0 + * @param[in] offset Starting bit position + * @param[out] count Number of bits set in the region if successful + * + * @retval 0 Operation successful + * @retval -EINVAL Invalid argument (e.g. out-of-bounds access, trying to count 0 bits, etc.) + */ +int sys_bitarray_popcount_region(sys_bitarray_t *bitarray, size_t num_bits, size_t offset, + size_t *count); + /** * Free bits in a bit array * diff --git a/lib/utils/bitarray.c b/lib/utils/bitarray.c index 7213ae201f5..6e8c92f118b 100644 --- a/lib/utils/bitarray.c +++ b/lib/utils/bitarray.c @@ -210,6 +210,55 @@ static void set_region(sys_bitarray_t *bitarray, size_t offset, } } +int sys_bitarray_popcount_region(sys_bitarray_t *bitarray, size_t num_bits, size_t offset, + size_t *count) +{ + k_spinlock_key_t key; + size_t idx; + struct bundle_data bd; + int ret; + + key = k_spin_lock(&bitarray->lock); + + __ASSERT_NO_MSG(bitarray != NULL); + __ASSERT_NO_MSG(bitarray->num_bits > 0); + + if (num_bits == 0 || offset + num_bits > bitarray->num_bits) { + ret = -EINVAL; + goto out; + } + + CHECKIF(count == NULL) { + ret = -EINVAL; + goto out; + } + + setup_bundle_data(bitarray, &bd, offset, num_bits); + + if (bd.sidx == bd.eidx) { + /* Start/end at same bundle */ + *count = POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask); + } else { + /* Start/end at different bundle. + * So count the bits in start and end bundles + * separately with correct mask applied. For in-between bundles, + * count all bits. + */ + *count = 0; + *count += POPCOUNT(bitarray->bundles[bd.sidx] & bd.smask); + *count += POPCOUNT(bitarray->bundles[bd.eidx] & bd.emask); + for (idx = bd.sidx + 1; idx < bd.eidx; idx++) { + *count += POPCOUNT(bitarray->bundles[idx]); + } + } + + ret = 0; + +out: + k_spin_unlock(&bitarray->lock, key); + return ret; +} + int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit) { k_spinlock_key_t key; diff --git a/tests/kernel/common/src/bitarray.c b/tests/kernel/common/src/bitarray.c index 05dd12c5343..dbd43ed201f 100644 --- a/tests/kernel/common/src/bitarray.c +++ b/tests/kernel/common/src/bitarray.c @@ -3,6 +3,7 @@ * * SPDX-License-Identifier: Apache-2.0 */ +#include #include #include @@ -519,6 +520,103 @@ ZTEST(bitarray, test_bitarray_alloc_free) alloc_and_free_interval(); } +ZTEST(bitarray, test_bitarray_popcount_region) +{ + int ret; + size_t count; + + /* Bitarrays have embedded spinlocks and can't on the stack. */ + if (IS_ENABLED(CONFIG_KERNEL_COHERENCE)) { + ztest_test_skip(); + } + + SYS_BITARRAY_DEFINE(ba, 128); + + printk("Testing bit array region popcount spanning single bundle\n"); + + /* Pre-populate the bits */ + ba.bundles[0] = 0x00000005; + ba.bundles[1] = 0x00000000; + ba.bundles[2] = 0x00000000; + ba.bundles[3] = 0x00000000; + + ret = sys_bitarray_popcount_region(&ba, 1, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 1, 1, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 0, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 2, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 3, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 2, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 3, 1, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + printk("Testing bit array region popcount spanning multiple bundles\n"); + + /* Pre-populate the bits. + * First and last bit of bitarray are set + */ + ba.bundles[0] = 0x00000001; + ba.bundles[1] = 0x00000000; + ba.bundles[2] = 0x00000000; + ba.bundles[3] = 0x80000000; + + ret = sys_bitarray_popcount_region(&ba, 126, 1, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 0, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 126, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 127, 1, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + ret = sys_bitarray_popcount_region(&ba, 1, 127, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 1, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + ret = sys_bitarray_popcount_region(&ba, 128, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + zassert_equal(count, 2, "sys_bitarray_popcount_region() returned unexpected count: %d", + count); + + printk("Testing edge/error cases\n"); + ret = sys_bitarray_popcount_region(&ba, 0, 0, &count); + zassert_equal(ret, -EINVAL, "sys_bitarray_popcount_region() returned unexpected value: %d", + ret); + ret = sys_bitarray_popcount_region(&ba, 0, 128, &count); + zassert_equal(ret, -EINVAL, "sys_bitarray_popcount_region() returned unexpected value: %d", + ret); + ret = sys_bitarray_popcount_region(&ba, 128, 0, &count); + zassert_equal(ret, 0, "sys_bitarray_popcount_region() returned unexpected value: %d", ret); + + ret = sys_bitarray_popcount_region(&ba, 128, 1, &count); + zassert_equal(ret, -EINVAL, "sys_bitarray_popcount_region() returned unexpected value: %d", + ret); + ret = sys_bitarray_popcount_region(&ba, 129, 0, &count); + zassert_equal(ret, -EINVAL, "sys_bitarray_popcount_region() returned unexpected value: %d", + ret); +} + ZTEST(bitarray, test_bitarray_region_set_clear) { int ret;