From 39e6e25565c89c656be9605a0dee27c5a440a3b0 Mon Sep 17 00:00:00 2001
From: Stuart Ianna <stuart.ianna@motec.com.au>
Date: Wed, 15 May 2024 15:38:41 +1000
Subject: [PATCH] drivers/devicetree/fdt: add additional fdt parsing utilities.

These utilities remove some of the boilerplate needed for FDT parsing and device initialization.
---
 drivers/devicetree/fdt.c | 93 ++++++++++++++++++++++++++++++++++++++++
 include/nuttx/fdt.h      | 59 +++++++++++++++++++++++++
 2 files changed, 152 insertions(+)

diff --git a/drivers/devicetree/fdt.c b/drivers/devicetree/fdt.c
index 6aa4c8a0fb..4bef5f6f06 100644
--- a/drivers/devicetree/fdt.c
+++ b/drivers/devicetree/fdt.c
@@ -26,6 +26,7 @@
 #include <endian.h>
 #include <errno.h>
 #include <assert.h>
+#include <ctype.h>
 #include <nuttx/compiler.h>
 #include <nuttx/fdt.h>
 #include <libfdt.h>
@@ -290,3 +291,95 @@ uintptr_t fdt_get_clock_frequency_from_clocks(FAR const void *fdt,
 
   return fdt_get_clock_frequency(fdt, pv_offset);
 }
+
+int fdt_node_index_from_label(FAR const char *node_label, int count)
+{
+  int dev_number = 0;
+  size_t label_length;
+
+  if (!node_label)
+    {
+      return -ENOENT;
+    }
+
+  label_length = strnlen(node_label, CONFIG_PATH_MAX);
+
+  if (count > label_length || count <= 0)
+    {
+      return -EINVAL;
+    }
+
+  for (int i = 0; i < count; i++)
+    {
+      int number = atoi(&node_label[label_length - i]);
+      if (number)
+        {
+          dev_number = number;
+        }
+    }
+
+  /* atoi returns 0 on failure, so check that the number isn't actually 0 */
+
+  if (!dev_number && !isdigit(node_label[label_length - 1]))
+    {
+      return -ENOENT;
+    }
+
+  return dev_number;
+}
+
+void fdt_node_from_compat(FAR const void *fdt,
+                          FAR const char **compatible_ids,
+                          FAR void (*driver_callback)(FAR const void *fdt,
+                                                      int offset))
+{
+  int offset = 0;
+
+  DEBUGASSERT(compatible_ids);
+  DEBUGASSERT(driver_callback);
+
+  while (*compatible_ids)
+    {
+      while (true)
+        {
+          offset
+              = fdt_node_offset_by_compatible(fdt, offset, *compatible_ids);
+          if (offset == -FDT_ERR_NOTFOUND)
+            {
+              break;
+            }
+
+          if (!fdt_device_is_available(fdt, offset))
+            {
+              continue;
+            }
+
+          driver_callback(fdt, offset);
+        }
+
+      compatible_ids++;
+    }
+}
+
+int fdt_load_prop_u32(FAR const void *fdt, int offset,
+                      FAR const char *property, int index,
+                      FAR uint32_t *value)
+{
+  DEBUGASSERT(property);
+  DEBUGASSERT(value);
+
+  int length;
+  const fdt32_t *pv = fdt_getprop(fdt, offset, property, &length);
+  if (!pv)
+    {
+      return -ENOENT;
+    }
+
+  if (index >= length)
+    {
+      return -EINVAL;
+    }
+
+  *value = fdt32_ld(pv + index);
+  return OK;
+}
diff --git a/include/nuttx/fdt.h b/include/nuttx/fdt.h
index 004a810cec..6ec27800f0 100644
--- a/include/nuttx/fdt.h
+++ b/include/nuttx/fdt.h
@@ -350,4 +350,63 @@ uintptr_t fdt_get_clock_frequency_from_clocks(FAR const void *fdt,
                                               int offset,
                                               int index);
 
+/****************************************************************************
+ * Name: fdt_node_index_from_label
+ *
+ * Description:
+ *  Get the node index from a device tree label.
+ *
+ * Input Parameters:
+ *  label - The device tree node_label
+ *  count - The number of characters from the end of the label to search
+ *
+ * Returns
+ *  The integer number found at the end of the label. e.g returns 4 for a
+ *  label called (i2c_4). Returns -ENOENT if an integer cannot be found.
+ *
+ ****************************************************************************/
+
+int fdt_node_index_from_label(FAR const char *node_label, int count);
+
+/****************************************************************************
+ * Name: fdt_node_from_compat
+ *
+ * Description:
+ *   Find all devices with a matching compatibility string and call a device
+ *   specific callback.
+ *
+ * Input Parameters:
+ *   fdt - The pointer to the raw FDT.
+ *   compatible_ids - NULL terminated list of compatible ids.
+ *   driver_callback - Function called on every found instance of the ID.
+ *
+ ****************************************************************************/
+
+void fdt_node_from_compat(FAR const void *fdt,
+                          FAR const char **compatible_ids,
+                          FAR void (*driver_callback)(FAR const void *fdt,
+                                                      int offset));
+
+/****************************************************************************
+ * Name: fdt_load_prop_u32
+ *
+ * Description:
+ *   Load an uint32_t type from a property at a given index
+ *
+ * Input Parameters:
+ *   fdt - The pointer to the raw FDT.
+ *   offset - The node offset
+ *   property - The property to load from.
+ *   index - The value index inside the property
+ *   value - Output parameter for found value.
+ *
+ * Returns:
+ *   OK on success, errno of failure.
+ *
+ ****************************************************************************/
+
+int fdt_load_prop_u32(FAR const void *fdt, int offset,
+                      FAR const char *property, int index,
+                      FAR uint32_t *value);
+
 #endif /* __INCLUDE_NUTTX_FDT_H */