sys: introduce bit arrays
This introduces bit arrays as a new data type. This is different than sys_bitfield as it is working on raw arrays of 32-bit data. The bit arrays encode additional data inside the struct to avoid going beyond the declared number of bits, and also provides locking. Signed-off-by: Daniel Leung <daniel.leung@intel.com>
This commit is contained in:
parent
783b20712e
commit
ff407fb922
3 changed files with 794 additions and 0 deletions
228
include/sys/bitarray.h
Normal file
228
include/sys/bitarray.h
Normal file
|
@ -0,0 +1,228 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Intel Corporation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef ZEPHYR_INCLUDE_SYS_BITARRAY_H_
|
||||
#define ZEPHYR_INCLUDE_SYS_BITARRAY_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <kernel.h>
|
||||
|
||||
struct sys_bitarray {
|
||||
/* Number of bits */
|
||||
uint32_t num_bits;
|
||||
|
||||
/* Number of bundles */
|
||||
uint32_t num_bundles;
|
||||
|
||||
/* Bundle of bits */
|
||||
uint32_t *bundles;
|
||||
|
||||
/* Spinlock guarding access to this bit array */
|
||||
struct k_spinlock lock;
|
||||
};
|
||||
|
||||
typedef struct sys_bitarray sys_bitarray_t;
|
||||
|
||||
/**
|
||||
* @def SYS_BITARRAY_DEFINE
|
||||
*
|
||||
* @brief Create a bitarray object.
|
||||
*
|
||||
* @param name Name of the bitarray object.
|
||||
* @param total_bits Total number of bits in this bitarray object.
|
||||
*/
|
||||
#define SYS_BITARRAY_DEFINE(name, total_bits) \
|
||||
uint32_t _sys_bitarray_bundles_##name \
|
||||
[(((total_bits + 8 - 1) / 8) + sizeof(uint32_t) - 1) \
|
||||
/ sizeof(uint32_t)] = {0U}; \
|
||||
sys_bitarray_t name = { \
|
||||
.num_bits = total_bits, \
|
||||
.num_bundles = (((total_bits + 8 - 1) / 8) \
|
||||
+ sizeof(uint32_t) - 1) \
|
||||
/ sizeof(uint32_t), \
|
||||
.bundles = _sys_bitarray_bundles_##name, \
|
||||
}
|
||||
|
||||
/**
|
||||
* Set a bit in a bit array
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] bit The bit to be set
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to set exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit);
|
||||
|
||||
/**
|
||||
* Clear a bit in a bit array
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] bit The bit to be cleared
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to clear exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_clear_bit(sys_bitarray_t *bitarray, size_t bit);
|
||||
|
||||
/**
|
||||
* Test whether a bit is set or not
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] bit The bit to be tested
|
||||
* @param[out] val The value of the bit (0 or 1)
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to test exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_test_bit(sys_bitarray_t *bitarray, size_t bit, int *val);
|
||||
|
||||
/**
|
||||
* Test the bit and set it
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] bit The bit to be tested and set
|
||||
* @param[out] prev_val Previous value of the bit (0 or 1)
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to test exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_test_and_set_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val);
|
||||
|
||||
/**
|
||||
* Test the bit and clear it
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] bit The bit to be tested and cleared
|
||||
* @param[out] prev_val Previous value of the bit (0 or 1)
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to test exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val);
|
||||
|
||||
/**
|
||||
* Allocate bits in a bit array
|
||||
*
|
||||
* This finds a number of bits (@p num_bits) in a contiguous of
|
||||
* previosly unallocated region. If such a region exists, the bits are
|
||||
* marked as allocated and the offset to the start of this region is
|
||||
* returned via @p offset.
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] num_bits Number of bits to allocate
|
||||
* @param[out] offset Offset to the start of allocated region if
|
||||
* successful
|
||||
*
|
||||
* @retval 0 Allocation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. allocating more bits than
|
||||
* the bitarray has, trying to allocate 0 bits, etc.)
|
||||
* @retval -ENOSPC No contiguous region big enough to accommodate
|
||||
* the allocation
|
||||
*/
|
||||
int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t *offset);
|
||||
|
||||
/**
|
||||
* Free bits in a bit array
|
||||
*
|
||||
* This marks the number of bits (@p num_bits) starting from @p offset
|
||||
* as no longer allocated.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param num_bits Number of bits to free
|
||||
* @param offset Starting bit position to free
|
||||
*
|
||||
* @retval 0 Free is successful
|
||||
* @retval -EINVAL Invalid argument (e.g. try to free more bits than
|
||||
* the bitarray has, trying to free 0 bits, etc.)
|
||||
* @retval -EFAULT The bits in the indicated region are not all allocated.
|
||||
*/
|
||||
int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset);
|
||||
|
||||
/**
|
||||
* Test if bits in a region is all set.
|
||||
*
|
||||
* This tests if the number of bits (@p num_bits) in region starting
|
||||
* from @p offset are all set.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param num_bits Number of bits to test
|
||||
* @param offset Starting bit position to test
|
||||
*
|
||||
* @retval true All bits are set.
|
||||
* @retval false Not all bits are set.
|
||||
*/
|
||||
bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset);
|
||||
|
||||
/**
|
||||
* Test if bits in a region is all cleared.
|
||||
*
|
||||
* This tests if the number of bits (@p num_bits) in region starting
|
||||
* from @p offset are all cleared.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param num_bits Number of bits to test
|
||||
* @param offset Starting bit position to test
|
||||
*
|
||||
* @retval true All bits are cleared.
|
||||
* @retval false Not all bits are cleared.
|
||||
*/
|
||||
bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset);
|
||||
|
||||
/**
|
||||
* Set all bits in a region.
|
||||
*
|
||||
* This sets the number of bits (@p num_bits) in region starting
|
||||
* from @p offset.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param num_bits Number of bits to test
|
||||
* @param offset Starting bit position to test
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to set exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset);
|
||||
|
||||
/**
|
||||
* Clear all bits in a region.
|
||||
*
|
||||
* This clears the number of bits (@p num_bits) in region starting
|
||||
* from @p offset.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param num_bits Number of bits to test
|
||||
* @param offset Starting bit position to test
|
||||
*
|
||||
* @retval 0 Operation successful
|
||||
* @retval -EINVAL Invalid argument (e.g. bit to set exceeds
|
||||
* the number of bits in bit array, etc.)
|
||||
*/
|
||||
int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* ZEPHYR_INCLUDE_SYS_BITARRAY_H_ */
|
|
@ -22,6 +22,7 @@ zephyr_sources(
|
|||
timeutil.c
|
||||
heap.c
|
||||
heap-validate.c
|
||||
bitarray.c
|
||||
)
|
||||
|
||||
zephyr_sources_ifdef(CONFIG_CBPRINTF_COMPLETE cbprintf_complete.c)
|
||||
|
|
565
lib/os/bitarray.c
Normal file
565
lib/os/bitarray.c
Normal file
|
@ -0,0 +1,565 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Intel Corporation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <errno.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/bitarray.h>
|
||||
#include <sys/check.h>
|
||||
#include <sys/sys_io.h>
|
||||
|
||||
/* Number of bits represented by one bundle */
|
||||
#define bundle_bitness(ba) (sizeof(ba->bundles[0]) * 8)
|
||||
|
||||
struct bundle_data {
|
||||
/* Start and end index of bundles */
|
||||
size_t sidx, eidx;
|
||||
|
||||
/* Offset inside start and end bundles */
|
||||
size_t soff, eoff;
|
||||
|
||||
/* Masks for start/end bundles */
|
||||
uint32_t smask, emask;
|
||||
};
|
||||
|
||||
static void setup_bundle_data(sys_bitarray_t *bitarray,
|
||||
struct bundle_data *bd,
|
||||
size_t offset, size_t num_bits)
|
||||
{
|
||||
bd->sidx = offset / bundle_bitness(bitarray);
|
||||
bd->soff = offset % bundle_bitness(bitarray);
|
||||
|
||||
bd->eidx = (offset + num_bits - 1) / bundle_bitness(bitarray);
|
||||
bd->eoff = (offset + num_bits - 1) % bundle_bitness(bitarray);
|
||||
|
||||
bd->smask = ~(BIT(bd->soff) - 1);
|
||||
bd->emask = (BIT(bd->eoff) - 1) | BIT(bd->eoff);
|
||||
|
||||
if (bd->sidx == bd->eidx) {
|
||||
/* The region lies within the same bundle. So combine the masks. */
|
||||
bd->smask &= bd->emask;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Find out if the bits in a region is all set or all clear.
|
||||
*
|
||||
* @param[in] bitarray Bitarray struct
|
||||
* @param[in] offset Starting bit location
|
||||
* @param[in] num_bits Number of bits in the region
|
||||
* @param[in] match_set True if matching all set bits,
|
||||
* False if matching all cleared bits
|
||||
* @param[out] bd Data related to matching which can be
|
||||
* used later to find out where the region
|
||||
* lies in the bitarray bundles.
|
||||
* @param[out] mismatch Offset to the mismatched bit.
|
||||
* Can be NULL.
|
||||
*
|
||||
* @retval true If all bits are set or cleared
|
||||
* @retval false Not all bits are set or cleared
|
||||
*/
|
||||
static bool match_region(sys_bitarray_t *bitarray, size_t offset,
|
||||
size_t num_bits, bool match_set,
|
||||
struct bundle_data *bd,
|
||||
size_t *mismatch)
|
||||
{
|
||||
int idx;
|
||||
uint32_t bundle;
|
||||
uint32_t mismatch_bundle;
|
||||
uint32_t mismatch_mask;
|
||||
size_t mismatch_bundle_idx;
|
||||
size_t mismatch_bit_off;
|
||||
|
||||
setup_bundle_data(bitarray, bd, offset, num_bits);
|
||||
|
||||
if (bd->sidx == bd->eidx) {
|
||||
bundle = bitarray->bundles[bd->sidx];
|
||||
if (!match_set) {
|
||||
bundle = ~bundle;
|
||||
}
|
||||
|
||||
if ((bundle & bd->smask) != bd->smask) {
|
||||
/* Not matching to mask. */
|
||||
mismatch_bundle = ~bundle & bd->smask;
|
||||
mismatch_bundle_idx = bd->sidx;
|
||||
mismatch_mask = bd->smask;
|
||||
goto mismatch;
|
||||
} else {
|
||||
/* Matching to mask. */
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
/* Region lies in a number of bundles. Need to loop through them. */
|
||||
|
||||
/* Start of bundles */
|
||||
bundle = bitarray->bundles[bd->sidx];
|
||||
if (!match_set) {
|
||||
bundle = ~bundle;
|
||||
}
|
||||
|
||||
if ((bundle & bd->smask) != bd->smask) {
|
||||
/* Start bundle not matching to mask. */
|
||||
mismatch_bundle = ~bundle & bd->smask;
|
||||
mismatch_bundle_idx = bd->sidx;
|
||||
mismatch_mask = bd->smask;
|
||||
goto mismatch;
|
||||
}
|
||||
|
||||
/* End of bundles */
|
||||
bundle = bitarray->bundles[bd->eidx];
|
||||
if (!match_set) {
|
||||
bundle = ~bundle;
|
||||
}
|
||||
|
||||
if ((bundle & bd->emask) != bd->emask) {
|
||||
/* End bundle not matching to mask. */
|
||||
mismatch_bundle = ~bundle & bd->emask;
|
||||
mismatch_bundle_idx = bd->eidx;
|
||||
mismatch_mask = bd->emask;
|
||||
goto mismatch;
|
||||
}
|
||||
|
||||
/* In-between bundles */
|
||||
for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
|
||||
/* Note that this is opposite from above so that
|
||||
* we are simply checking if bundle == 0.
|
||||
*/
|
||||
bundle = bitarray->bundles[idx];
|
||||
if (match_set) {
|
||||
bundle = ~bundle;
|
||||
}
|
||||
|
||||
if (bundle != 0U) {
|
||||
/* Bits in "between bundles" do not match */
|
||||
mismatch_bundle = ~bundle;
|
||||
mismatch_bundle_idx = idx;
|
||||
mismatch_mask = ~0U;
|
||||
goto mismatch;
|
||||
}
|
||||
}
|
||||
|
||||
out:
|
||||
/* All bits in region matched. */
|
||||
return true;
|
||||
|
||||
mismatch:
|
||||
if (mismatch != NULL) {
|
||||
/* Must have at least 1 bit set to indicate
|
||||
* where the mismatch is.
|
||||
*/
|
||||
__ASSERT_NO_MSG(mismatch_bundle != 0);
|
||||
|
||||
mismatch_bit_off = find_lsb_set(mismatch_bundle) - 1;
|
||||
mismatch_bit_off += mismatch_bundle_idx *
|
||||
bundle_bitness(bitarray);
|
||||
*mismatch = (uint32_t)mismatch_bit_off;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set or clear a region of bits.
|
||||
*
|
||||
* @param bitarray Bitarray struct
|
||||
* @param offset Starting bit location
|
||||
* @param num_bits Number of bits in the region
|
||||
* @param to_set True if to set all bits.
|
||||
* False if to clear all bits.
|
||||
* @param bd Bundle data. Can reuse the output from
|
||||
* match_region(). NULL if there is no
|
||||
* prior call to match_region().
|
||||
*/
|
||||
static void set_region(sys_bitarray_t *bitarray, size_t offset,
|
||||
size_t num_bits, bool to_set,
|
||||
struct bundle_data *bd)
|
||||
{
|
||||
int idx;
|
||||
struct bundle_data bdata;
|
||||
|
||||
if (bd == NULL) {
|
||||
bd = &bdata;
|
||||
setup_bundle_data(bitarray, bd, offset, num_bits);
|
||||
}
|
||||
|
||||
if (bd->sidx == bd->eidx) {
|
||||
/* Start/end at same bundle */
|
||||
if (to_set) {
|
||||
bitarray->bundles[bd->sidx] |= bd->smask;
|
||||
} else {
|
||||
bitarray->bundles[bd->sidx] &= ~bd->smask;
|
||||
}
|
||||
} else {
|
||||
/* Start/end at different bundle.
|
||||
* So set/clear the bits in start and end bundles
|
||||
* separately. For in-between bundles,
|
||||
* set/clear all bits.
|
||||
*/
|
||||
if (to_set) {
|
||||
bitarray->bundles[bd->sidx] |= bd->smask;
|
||||
bitarray->bundles[bd->eidx] |= bd->emask;
|
||||
for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
|
||||
bitarray->bundles[idx] = ~0U;
|
||||
}
|
||||
} else {
|
||||
bitarray->bundles[bd->sidx] &= ~bd->smask;
|
||||
bitarray->bundles[bd->eidx] &= ~bd->emask;
|
||||
for (idx = bd->sidx + 1; idx < bd->eidx; idx++) {
|
||||
bitarray->bundles[idx] = 0U;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int sys_bitarray_set_bit(sys_bitarray_t *bitarray, size_t bit)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t idx, off;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
if (bit >= bitarray->num_bits) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
idx = bit / bundle_bitness(bitarray);
|
||||
off = bit % bundle_bitness(bitarray);
|
||||
|
||||
bitarray->bundles[idx] |= BIT(off);
|
||||
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_clear_bit(sys_bitarray_t *bitarray, size_t bit)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t idx, off;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
if (bit >= bitarray->num_bits) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
idx = bit / bundle_bitness(bitarray);
|
||||
off = bit % bundle_bitness(bitarray);
|
||||
|
||||
bitarray->bundles[idx] &= ~BIT(off);
|
||||
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_test_bit(sys_bitarray_t *bitarray, size_t bit, int *val)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t idx, off;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
CHECKIF(val == NULL) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (bit >= bitarray->num_bits) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
idx = bit / bundle_bitness(bitarray);
|
||||
off = bit % bundle_bitness(bitarray);
|
||||
|
||||
if ((bitarray->bundles[idx] & BIT(off)) != 0) {
|
||||
*val = 1;
|
||||
} else {
|
||||
*val = 0;
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_test_and_set_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t idx, off;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
CHECKIF(prev_val == NULL) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (bit >= bitarray->num_bits) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
idx = bit / bundle_bitness(bitarray);
|
||||
off = bit % bundle_bitness(bitarray);
|
||||
|
||||
if ((bitarray->bundles[idx] & BIT(off)) != 0) {
|
||||
*prev_val = 1;
|
||||
} else {
|
||||
*prev_val = 0;
|
||||
}
|
||||
|
||||
bitarray->bundles[idx] |= BIT(off);
|
||||
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_test_and_clear_bit(sys_bitarray_t *bitarray, size_t bit, int *prev_val)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t idx, off;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
CHECKIF(prev_val == NULL) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (bit >= bitarray->num_bits) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
idx = bit / bundle_bitness(bitarray);
|
||||
off = bit % bundle_bitness(bitarray);
|
||||
|
||||
if ((bitarray->bundles[idx] & BIT(off)) != 0) {
|
||||
*prev_val = 1;
|
||||
} else {
|
||||
*prev_val = 0;
|
||||
}
|
||||
|
||||
bitarray->bundles[idx] &= ~BIT(off);
|
||||
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_alloc(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t *offset)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
uint32_t bit_idx;
|
||||
int ret;
|
||||
struct bundle_data bd;
|
||||
size_t off_start, off_end;
|
||||
size_t mismatch;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
CHECKIF(offset == NULL) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if ((num_bits == 0) || (num_bits > bitarray->num_bits)) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
bit_idx = 0;
|
||||
|
||||
/* Find the first non-allocated bit by looking at bundles
|
||||
* instead of individual bits.
|
||||
*
|
||||
* On RISC-V 64-bit, it complains about undefined reference to `ffs`.
|
||||
* So don't use this on RISCV64.
|
||||
*/
|
||||
for (ret = 0; ret < bitarray->num_bundles; ret++) {
|
||||
if (~bitarray->bundles[ret] == 0U) {
|
||||
/* bundle is all 1s => all allocated, skip */
|
||||
bit_idx += bundle_bitness(bitarray);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (bitarray->bundles[ret] != 0U) {
|
||||
/* Find the first free bit in bundle if not all free */
|
||||
off_start = find_lsb_set(~bitarray->bundles[ret]) - 1;
|
||||
bit_idx += off_start;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
off_end = bitarray->num_bits - num_bits;
|
||||
ret = -ENOSPC;
|
||||
while (bit_idx <= off_end) {
|
||||
if (match_region(bitarray, bit_idx, num_bits, false,
|
||||
&bd, &mismatch)) {
|
||||
off_end = bit_idx + num_bits - 1;
|
||||
|
||||
set_region(bitarray, bit_idx, num_bits, true, &bd);
|
||||
|
||||
*offset = bit_idx;
|
||||
ret = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Fast-forward to the bit just after
|
||||
* the mismatched bit.
|
||||
*/
|
||||
bit_idx = mismatch + 1;
|
||||
}
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_free(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset)
|
||||
{
|
||||
k_spinlock_key_t key;
|
||||
int ret;
|
||||
size_t off_end = offset + num_bits - 1;
|
||||
struct bundle_data bd;
|
||||
|
||||
key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
if ((num_bits == 0)
|
||||
|| (num_bits > bitarray->num_bits)
|
||||
|| (offset >= bitarray->num_bits)
|
||||
|| (off_end >= bitarray->num_bits)) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* Note that we need to make sure the bits in specified region
|
||||
* (offset to offset + num_bits) are all allocated before we clear
|
||||
* them.
|
||||
*/
|
||||
if (match_region(bitarray, offset, num_bits, true, &bd, NULL)) {
|
||||
set_region(bitarray, offset, num_bits, false, &bd);
|
||||
ret = 0;
|
||||
} else {
|
||||
ret = -EFAULT;
|
||||
}
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool is_region_set_clear(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset, bool to_set)
|
||||
{
|
||||
bool ret;
|
||||
struct bundle_data bd;
|
||||
size_t off_end = offset + num_bits - 1;
|
||||
k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
if ((num_bits == 0)
|
||||
|| (num_bits > bitarray->num_bits)
|
||||
|| (offset >= bitarray->num_bits)
|
||||
|| (off_end >= bitarray->num_bits)) {
|
||||
ret = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
ret = match_region(bitarray, offset, num_bits, to_set, &bd, NULL);
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool sys_bitarray_is_region_set(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset)
|
||||
{
|
||||
return is_region_set_clear(bitarray, num_bits, offset, true);
|
||||
}
|
||||
|
||||
bool sys_bitarray_is_region_cleared(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset)
|
||||
{
|
||||
return is_region_set_clear(bitarray, num_bits, offset, false);
|
||||
}
|
||||
|
||||
static int set_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset, bool to_set)
|
||||
{
|
||||
int ret;
|
||||
size_t off_end = offset + num_bits - 1;
|
||||
k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
|
||||
|
||||
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
||||
|
||||
if ((num_bits == 0)
|
||||
|| (num_bits > bitarray->num_bits)
|
||||
|| (offset >= bitarray->num_bits)
|
||||
|| (off_end >= bitarray->num_bits)) {
|
||||
ret = -EINVAL;
|
||||
goto out;
|
||||
}
|
||||
|
||||
set_region(bitarray, offset, num_bits, to_set, NULL);
|
||||
ret = 0;
|
||||
|
||||
out:
|
||||
k_spin_unlock(&bitarray->lock, key);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int sys_bitarray_set_region(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset)
|
||||
{
|
||||
return set_clear_region(bitarray, num_bits, offset, true);
|
||||
}
|
||||
|
||||
int sys_bitarray_clear_region(sys_bitarray_t *bitarray, size_t num_bits,
|
||||
size_t offset)
|
||||
{
|
||||
return set_clear_region(bitarray, num_bits, offset, false);
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue