From 4fd78583f7fdcaf13820ab1a4b1908d0c4f3cfeb Mon Sep 17 00:00:00 2001
From: lipengfei28 <lipengfei28@xiaomi.com>
Date: Tue, 18 Jun 2024 20:00:04 +0800
Subject: [PATCH] libc: add find_next_zero_bit

The pci ep framework use bitmap manage free bar

Signed-off-by: lipengfei28 <lipengfei28@xiaomi.com>
---
 include/nuttx/bits.h          |   8 +++
 libs/libc/misc/CMakeLists.txt |   1 +
 libs/libc/misc/Make.defs      |   2 +-
 libs/libc/misc/lib_bitmap.c   | 116 ++++++++++++++++++++++++++++++++++
 4 files changed, 126 insertions(+), 1 deletion(-)
 create mode 100644 libs/libc/misc/lib_bitmap.c

diff --git a/include/nuttx/bits.h b/include/nuttx/bits.h
index c39d75dff3..9b0e133b11 100644
--- a/include/nuttx/bits.h
+++ b/include/nuttx/bits.h
@@ -93,6 +93,9 @@
         (*(((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)
+
 /****************************************************************************
  * Type Definitions
  ****************************************************************************/
@@ -112,6 +115,11 @@ extern "C"
 /****************************************************************************
  * Public Function Prototypes
  ****************************************************************************/
+#ifndef __ASSEMBLER__
+unsigned long find_next_zero_bit(FAR const unsigned long *addr,
+                                 unsigned long size,
+                                 unsigned long offset);
+#endif
 
 #undef EXTERN
 #ifdef __cplusplus
diff --git a/libs/libc/misc/CMakeLists.txt b/libs/libc/misc/CMakeLists.txt
index 7ddb3f5979..61301e3d70 100644
--- a/libs/libc/misc/CMakeLists.txt
+++ b/libs/libc/misc/CMakeLists.txt
@@ -24,6 +24,7 @@
 list(
   APPEND
   SRCS
+  lib_bitmap.c
   lib_mknod.c
   lib_umask.c
   lib_utsname.c
diff --git a/libs/libc/misc/Make.defs b/libs/libc/misc/Make.defs
index c6d4c107a8..281c662f00 100644
--- a/libs/libc/misc/Make.defs
+++ b/libs/libc/misc/Make.defs
@@ -20,7 +20,7 @@
 
 # Add the internal C files to the build
 
-CSRCS += lib_mknod.c lib_umask.c lib_utsname.c lib_getrandom.c
+CSRCS += lib_bitmap.c lib_mknod.c lib_umask.c lib_utsname.c lib_getrandom.c
 CSRCS += lib_xorshift128.c lib_tea_encrypt.c lib_tea_decrypt.c
 CSRCS += lib_cxx_initialize.c lib_impure.c lib_memfd.c lib_mutex.c
 CSRCS += lib_fchmodat.c lib_fstatat.c lib_getfullpath.c lib_openat.c
diff --git a/libs/libc/misc/lib_bitmap.c b/libs/libc/misc/lib_bitmap.c
new file mode 100644
index 0000000000..276320b2a3
--- /dev/null
+++ b/libs/libc/misc/lib_bitmap.c
@@ -0,0 +1,116 @@
+/****************************************************************************
+ * libs/libc/misc/lib_bitmap.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <assert.h>
+#include <errno.h>
+#include <debug.h>
+#include <string.h>
+
+#include <nuttx/bits.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: find_next_zero_bit
+ *
+ * Description:
+ *   This function is used to find the first cleared bit in a memory region.
+ *
+ *   Invoke to get the bit set in map, implement find_{first,next}_zero_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_zero_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 >> (BITS_PER_LONG - 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)
+    {
+      return result;
+    }
+
+  tmp = *p;
+
+found_first:
+  tmp |= ~0ul << size;
+
+  /* Are any bits zero? */
+
+  if (tmp == ~0ul)
+    {
+      return result + size;
+    }
+
+found_middle:
+  return result + ffsl(~tmp) - 1;
+}