603 lines
13 KiB
C
603 lines
13 KiB
C
/*
|
|
* Copyright (c) 2021 Intel Corporation
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <errno.h>
|
|
#include <stddef.h>
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <zephyr/sys/bitarray.h>
|
|
#include <zephyr/sys/check.h>
|
|
#include <zephyr/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)
|
|
{
|
|
size_t idx;
|
|
uint32_t bundle;
|
|
uint32_t mismatch_bundle;
|
|
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;
|
|
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;
|
|
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;
|
|
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;
|
|
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)
|
|
{
|
|
size_t 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 != NULL);
|
|
__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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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 (size_t idx = 0; idx < bitarray->num_bundles; idx++) {
|
|
if (~bitarray->bundles[idx] == 0U) {
|
|
/* bundle is all 1s => all allocated, skip */
|
|
bit_idx += bundle_bitness(bitarray);
|
|
continue;
|
|
}
|
|
|
|
if (bitarray->bundles[idx] != 0U) {
|
|
/* Find the first free bit in bundle if not all free */
|
|
off_start = find_lsb_set(~bitarray->bundles[idx]) - 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)) {
|
|
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;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
key = k_spin_lock(&bitarray->lock);
|
|
|
|
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 != NULL);
|
|
__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 != NULL);
|
|
__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_test_and_set_region(sys_bitarray_t *bitarray, size_t num_bits,
|
|
size_t offset, bool to_set)
|
|
{
|
|
int ret;
|
|
bool region_clear;
|
|
struct bundle_data bd;
|
|
|
|
__ASSERT_NO_MSG(bitarray != NULL);
|
|
__ASSERT_NO_MSG(bitarray->num_bits > 0);
|
|
|
|
size_t off_end = offset + num_bits - 1;
|
|
k_spinlock_key_t key = k_spin_lock(&bitarray->lock);
|
|
|
|
|
|
if ((num_bits == 0)
|
|
|| (num_bits > bitarray->num_bits)
|
|
|| (offset >= bitarray->num_bits)
|
|
|| (off_end >= bitarray->num_bits)) {
|
|
ret = -EINVAL;
|
|
goto out;
|
|
}
|
|
|
|
region_clear = match_region(bitarray, offset, num_bits, !to_set, &bd, NULL);
|
|
if (region_clear) {
|
|
set_region(bitarray, offset, num_bits, to_set, &bd);
|
|
ret = 0;
|
|
} else {
|
|
ret = -EEXIST;
|
|
}
|
|
|
|
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);
|
|
}
|