bitmap: add bitmap_find_free_region

Signed-off-by: lipengfei28 <[email protected]>

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

libs/libc/misc/lib_bitmap.c

@@ -22,17 +22,97 @@
  * Included Files
  ****************************************************************************/
 
-#include <assert.h>
 #include <errno.h>
-#include <debug.h>
-#include <string.h>
+#include <strings.h>
 
 #include <nuttx/bits.h>
 
 /****************************************************************************
  * 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 @@ unsigned long find_next_zero_bit(FAR const unsigned long *addr,
     }
 
 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;
 }