diff --git a/include/nuttx/bits.h b/include/nuttx/bits.h index 9b0e133b11..7d31e2e491 100644 --- a/include/nuttx/bits.h +++ b/include/nuttx/bits.h @@ -74,13 +74,16 @@ #define GENMASK_ULL(h, l) \ (BUILD_BUG_ON_ZERO((l) > (h)) + __GENMASK_ULL(h, l)) -#define BMVAL(val, lsb, msb) (((val) & GENMASK(msb, lsb)) >> (lsb)) +#define BMVAL(val, lsb, msb) (((val) & GENMASK(msb, lsb)) >> (lsb)) /* Bitmap operations */ #define DECLARE_BITMAP(name, bits) \ unsigned long name[BITS_TO_LONGS(bits)] +#define BITMAP_FIRST_WORD_MASK(start) (~0ul << ((start) & (BITS_PER_LONG - 1))) +#define BITMAP_LAST_WORD_MASK(len) (~0ul >> (-(len) & (BITS_PER_LONG - 1))) + #define set_bit(nr, addr) \ (*(((FAR unsigned long *)(addr)) + BIT_WORD(nr)) |= \ BIT_WORD_MASK(nr)) @@ -93,8 +96,8 @@ (*(((FAR unsigned long *)(addr)) + BIT_WORD(nr)) & \ BIT_WORD_MASK(nr)) -#define find_first_zero_bit(addr, size) \ - find_next_zero_bit((addr), (size), 0) +#define find_first_bit(addr, size) find_next_bit(addr, size, 0) +#define find_first_zero_bit(addr, size) find_next_zero_bit(addr, size, 0) /**************************************************************************** * Type Definitions @@ -115,10 +118,51 @@ extern "C" /**************************************************************************** * Public Function Prototypes ****************************************************************************/ + #ifndef __ASSEMBLER__ +unsigned long find_next_bit(FAR const unsigned long *addr, + unsigned long size, + unsigned long offset); unsigned long find_next_zero_bit(FAR const unsigned long *addr, unsigned long size, unsigned long offset); + +void bitmap_set(FAR unsigned long *bitmap, + unsigned long satrt, + unsigned long len); +void bitmap_clear(FAR unsigned long *bitmap, + unsigned long start, + unsigned long len); + +int bitmap_allocate_region(FAR unsigned long *bitmap, + unsigned long start, + unsigned long len); +#define bitmap_release_region(bitmap, start, len) \ + bitmap_clear(bitmap, start, len) + +/**************************************************************************** + * Name: bitmap_find_free_region + * + * Description: + * Find a contiguous memory region + * + * Find a region of free (zero) len in a bitmap of size and + * allocate them (set them to one). + * + * Input Parameters: + * bitmap - Array of unsigned longs corresponding to the bitmap + * size - Number of bits in the bitmap + * len - Region size to find + * + * Returned Value: + * Return the bit offset in bitmap of the allocated region, + * otherwise return size + ****************************************************************************/ + +unsigned long bitmap_find_free_region(FAR unsigned long *bitmap, + unsigned long size, + unsigned long len); + #endif #undef EXTERN diff --git a/libs/libc/misc/lib_bitmap.c b/libs/libc/misc/lib_bitmap.c index 276320b2a3..c0f0d6a3f3 100644 --- a/libs/libc/misc/lib_bitmap.c +++ b/libs/libc/misc/lib_bitmap.c @@ -22,10 +22,8 @@ * Included Files ****************************************************************************/ -#include #include -#include -#include +#include #include @@ -33,6 +31,88 @@ * Public Functions ****************************************************************************/ +/**************************************************************************** + * Name: find_next_bit + * + * Description: + * This function is used to find the first set bit in a memory region. + * + * Invoke to get the bit set in map, implement find_{first,next}_bit. + * + * Input Parameters: + * addr - Map start memory address + * size - Max size to find bit + * offset - Offset of start to finding + * + * Returned Value: + * Return index position（0 ~ size-1）if finded, otherwise return size + ****************************************************************************/ + +unsigned long find_next_bit(FAR const unsigned long *addr, + unsigned long size, + unsigned long offset) +{ + FAR const unsigned long *p = addr + BIT_WORD(offset); + unsigned long result = offset & ~(BITS_PER_LONG - 1); + unsigned long tmp; + + if (offset >= size) + { + return size; + } + + size -= result; + offset %= BITS_PER_LONG; + if (offset) + { + tmp = *(p++); + tmp &= ~0ul << offset; + if (size < BITS_PER_LONG) + { + goto found_first; + } + + if (tmp) + { + goto found_middle; + } + + size -= BITS_PER_LONG; + result += BITS_PER_LONG; + } + + while (size & ~(BITS_PER_LONG - 1)) + { + if ((tmp = *(p++))) + { + goto found_middle; + } + + result += BITS_PER_LONG; + size -= BITS_PER_LONG; + } + + if (size == 0) + { + return result; + } + + tmp = *p; + +found_first: + tmp &= ~0ul >> (BITS_PER_LONG - size); + + /* Are all bits zero? */ + + if (tmp == 0ul) + { + return result + size; + } + +found_middle: + return result + ffsl((long)tmp) - 1; +} + /**************************************************************************** * Name: find_next_zero_bit * @@ -94,7 +174,7 @@ unsigned long find_next_zero_bit(FAR const unsigned long *addr, size -= BITS_PER_LONG; } - if (!size) + if (size == 0) { return result; } @@ -112,5 +192,115 @@ found_first: } found_middle: - return result + ffsl(~tmp) - 1; + return result + ffsl((long)~tmp) - 1; +} + +/**************************************************************************** + * Name: bitmap_set + ****************************************************************************/ + +void bitmap_set(FAR unsigned long *bitmap, + unsigned long start, + unsigned long len) +{ + FAR unsigned long *p = bitmap + BIT_WORD(start); + unsigned long bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG); + unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start); + unsigned long size = start + len; + + while (bits_to_set < len) + { + *p++ |= mask_to_set; + len -= bits_to_set; + bits_to_set = BITS_PER_LONG; + mask_to_set = ~0ul; + } + + if (len) + { + mask_to_set &= BITMAP_LAST_WORD_MASK(size); + *p |= mask_to_set; + } +} + +/**************************************************************************** + * Name: bitmap_set + ****************************************************************************/ + +void bitmap_clear(FAR unsigned long *bitmap, + unsigned long start, + unsigned long len) +{ + FAR unsigned long *p = bitmap + BIT_WORD(start); + unsigned long bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG); + unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start); + unsigned long size = start + len; + + while (bits_to_clear < len) + { + *p++ &= ~mask_to_clear; + len -= bits_to_clear; + bits_to_clear = BITS_PER_LONG; + mask_to_clear = ~0ul; + } + + if (len) + { + mask_to_clear &= BITMAP_LAST_WORD_MASK(size); + *p &= ~mask_to_clear; + } +} + +/**************************************************************************** + * Name: bitmap_allocate_region + ****************************************************************************/ + +int bitmap_allocate_region(FAR unsigned long *bitmap, + unsigned long start, + unsigned long len) +{ + if (find_next_bit(bitmap, start + len, start) < start + len) + { + return -EBUSY; + } + + bitmap_set(bitmap, start, len); + return 0; +} + +/**************************************************************************** + * Name: bitmap_find_free_region + * + * Description: + * Find a contiguous memory region + * + * Find a region of free (zero) len in a bitmap of size and + * allocate them (set them to one). + * + * Input Parameters: + * bitmap - Array of unsigned longs corresponding to the bitmap + * size - Number of bits in the bitmap + * len - Region size to find + * + * Returned Value: + * Return the bit offset in bitmap of the allocated region, + * otherwise return size + ****************************************************************************/ + +unsigned long bitmap_find_free_region(FAR unsigned long *bitmap, + unsigned long size, + unsigned long len) +{ + unsigned long pos; + unsigned long end; + + for (pos = 0; (end = pos + len) <= size; pos = end) + { + if (!bitmap_allocate_region(bitmap, pos, len)) + { + return pos; + } + } + + return size; }