libufdt: device tree overlay via unflattening FDT

The original version of libdtoverlay is slow in searching for
particular nodes and adding subnodes/properties due to the operations
on flattened device tree (FDT).

`libufdt` builds a real tree structure (named ufdt -- unflattned
device tree) from FDT. In the real tree, we can perform certain
operations (e.g., merge 2 subtrees, search for a node by path) in
almost optimal time complexity with acceptable additional memory usage.

With libufdt, we improve the merging of two dtb files from O(N^2) to
O(N), where N is the number of nodes in the tree.

Bug: 30800619
Test: run test scripts (see libufdt/tests/README)
Test: manually ported libufdt into a bootloader,
      checked it can merge a base dtb and a dtbo to generate a final dtb
      to boot the device

Change-Id: I1ff886bb8a62bad1451edcd7c4fe5cb48b7a034a

1 files changed, 302 insertions, 0 deletions

diff --git a/ufdt_node.c b/ufdt_node.c
new file mode 100644
index 0000000..2ffd6a0
--- /dev/null
+++ b/ufdt_node.c
@@ -0,0 +1,302 @@
+#include "libufdt.h"
+#include "ufdt_util.h"
+
+
+int node_cmp(const void *a, const void *b) {
+  const struct ufdt_node *na = *(struct ufdt_node **)a;
+  const struct ufdt_node *nb = *(struct ufdt_node **)b;
+  return dto_strcmp(name_of(na), name_of(nb));
+}
+
+bool node_name_eq(const struct ufdt_node *node, const char *name, int len) {
+  if (!node) return false;
+  if (!name) return false;
+  if (dto_strncmp(name_of(node), name, len) != 0) return false;
+  if (name_of(node)[len] != '\0') return false;
+  return true;
+}
+
+/*
+ * ufdt_node methods.
+ */
+
+struct ufdt_node *ufdt_node_construct(void *fdtp, fdt32_t *fdt_tag_ptr) {
+  uint32_t tag = fdt32_to_cpu(*fdt_tag_ptr);
+  if (tag == FDT_PROP) {
+    struct fdt_prop_ufdt_node *res = dto_malloc(sizeof(struct fdt_prop_ufdt_node));
+    if (res == NULL) return NULL;
+    res->parent.fdt_tag_ptr = fdt_tag_ptr;
+    res->parent.sibling = NULL;
+    res->name = get_name(fdtp, (struct ufdt_node *)res);
+    return (struct ufdt_node *)res;
+  } else {
+    struct fdt_node_ufdt_node *res = dto_malloc(sizeof(struct fdt_node_ufdt_node));
+    if (res == NULL) return NULL;
+    res->parent.fdt_tag_ptr = fdt_tag_ptr;
+    res->parent.sibling = NULL;
+    res->child = NULL;
+    return (struct ufdt_node *)res;
+  }
+}
+
+void ufdt_node_destruct(struct ufdt_node *node) {
+  if (node == NULL) return;
+
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    ufdt_node_destruct(((struct fdt_node_ufdt_node *)node)->child);
+  }
+
+  ufdt_node_destruct(node->sibling);
+  dto_free(node);
+
+  return;
+}
+
+int ufdt_node_add_child(struct ufdt_node *parent, struct ufdt_node *child) {
+  if (!parent || !child) return -1;
+  if (tag_of(parent) != FDT_BEGIN_NODE) return -1;
+
+  int err = 0;
+  uint32_t child_tag = tag_of(child);
+
+  switch (child_tag) {
+    case FDT_PROP:
+    case FDT_BEGIN_NODE:
+      child->sibling = ((struct fdt_node_ufdt_node *)parent)->child;
+      ((struct fdt_node_ufdt_node *)parent)->child = child;
+      break;
+
+    default:
+      err = -1;
+      dto_error("invalid children tag type\n");
+  }
+
+  return err;
+}
+
+/*
+ * BEGIN of FDT_PROP related methods.
+ */
+
+struct ufdt_node *ufdt_node_get_subnode_by_name_len(const struct ufdt_node *node,
+                                                  const char *name, int len) {
+  struct ufdt_node **it = NULL;
+  for_each_node(it, node) {
+    if (node_name_eq(*it, name, len)) return *it;
+  }
+  return NULL;
+}
+
+struct ufdt_node *ufdt_node_get_subnode_by_name(const struct ufdt_node *node,
+                                              const char *name) {
+  return ufdt_node_get_subnode_by_name_len(node, name, strlen(name));
+}
+
+struct ufdt_node *ufdt_node_get_property_by_name_len(
+    const struct ufdt_node *node, const char *name, int len) {
+  if (!node) return NULL;
+
+  struct ufdt_node **it = NULL;
+  for_each_prop(it, node) {
+    if (node_name_eq(*it, name, len)) return *it;
+  }
+  return NULL;
+}
+
+struct ufdt_node *ufdt_node_get_property_by_name(const struct ufdt_node *node,
+                                                 const char *name) {
+  return ufdt_node_get_property_by_name_len(node, name, dto_strlen(name));
+}
+
+char *ufdt_node_get_fdt_prop_data(const struct ufdt_node *node, int *out_len) {
+  if (!node || tag_of(node) != FDT_PROP) {
+    return NULL;
+  }
+  const struct fdt_property *prop = (struct fdt_property *)node->fdt_tag_ptr;
+  if (out_len != NULL) {
+    *out_len = fdt32_to_cpu(prop->len);
+  }
+  return (char *)prop->data;
+}
+
+char *ufdt_node_get_fdt_prop_data_by_name_len(const struct ufdt_node *node,
+                                              const char *name, int len,
+                                              int *out_len) {
+  return ufdt_node_get_fdt_prop_data(
+      ufdt_node_get_property_by_name_len(node, name, len), out_len);
+}
+
+char *ufdt_node_get_fdt_prop_data_by_name(const struct ufdt_node *node,
+                                          const char *name, int *out_len) {
+  return ufdt_node_get_fdt_prop_data(ufdt_node_get_property_by_name(node, name),
+                                     out_len);
+}
+
+/*
+ * END of FDT_PROP related methods.
+ */
+
+/*
+ * BEGIN of searching-in-ufdt_node methods.
+ */
+
+uint32_t ufdt_node_get_phandle(const struct ufdt_node *node) {
+  if (!node || tag_of(node) != FDT_BEGIN_NODE) {
+    return 0;
+  }
+  int len = 0;
+  void *ptr = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len);
+  if (!ptr || len != sizeof(fdt32_t)) {
+    ptr = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len);
+    if (!ptr || len != sizeof(fdt32_t)) {
+      return 0;
+    }
+  }
+  return fdt32_to_cpu(*((fdt32_t *)ptr));
+}
+
+struct ufdt_node *ufdt_node_get_node_by_path_len(const struct ufdt_node *node,
+                                                 const char *path, int len) {
+  const char *end = path + len;
+
+  struct ufdt_node *cur = (struct ufdt_node *)node;
+
+  while (path < end) {
+    while (path[0] == '/') path++;
+    if (path == end) return cur;
+
+    const char *next_slash;
+    next_slash = dto_memchr(path, '/', end - path);
+    if (!next_slash) next_slash = end;
+
+    struct ufdt_node *next = NULL;
+
+    next = ufdt_node_get_subnode_by_name_len(cur, path, next_slash - path);
+
+    cur = next;
+    path = next_slash;
+    if (!cur) return cur;
+  }
+
+  return cur;
+}
+
+struct ufdt_node *ufdt_node_get_node_by_path(const struct ufdt_node *node,
+                                             const char *path) {
+  return ufdt_node_get_node_by_path_len(node, path, dto_strlen(path));
+}
+
+/*
+ * END of searching-in-ufdt_node methods.
+ */
+
+int output_property_to_fdt(struct ufdt_node *prop_node, void *fdtp,
+                           struct ufdt_node_dict *props_dict) {
+  int err = 0;
+  int len = 0;
+  void *data = ufdt_node_get_fdt_prop_data(prop_node, &len);
+  int nameoff = 0;
+  struct ufdt_node *same_name_prop =
+      ufdt_node_dict_find_node(props_dict, name_of(prop_node));
+
+  /*
+   * There's already a property with same name, take its nameoff instead.
+   */
+  if (same_name_prop != NULL) {
+    const struct fdt_property *prop =
+        (const struct fdt_property *)same_name_prop->fdt_tag_ptr;
+    nameoff = fdt32_to_cpu(prop->nameoff);
+  }
+  /*
+   * Modifies prop_node->fdt_tag_ptr to point to the node in new fdtp.
+   */
+  err = fast_fdt_sw_property(fdtp, name_of(prop_node), data, len, &nameoff,
+                             (struct fdt_property **)&(prop_node->fdt_tag_ptr));
+
+  if (err < 0) {
+    dto_error("Not enough space for the string space: %d\n",
+              fdt_size_dt_strings(fdtp));
+  }
+  ufdt_node_dict_add(props_dict, prop_node);
+  return err;
+}
+
+int output_ufdt_node_to_fdt(struct ufdt_node *node, void *fdtp,
+                            struct ufdt_node_dict *props_dict) {
+  uint32_t tag = tag_of(node);
+
+  if (tag == FDT_PROP) {
+    int err = output_property_to_fdt(node, fdtp, props_dict);
+    return err;
+  }
+
+  int err = fdt_begin_node(fdtp, name_of(node));
+  if (err < 0) return -1;
+
+  struct ufdt_node **it;
+  for_each_prop(it, node) {
+    err = output_ufdt_node_to_fdt(*it, fdtp, props_dict);
+    if (err < 0) return -1;
+  }
+
+  for_each_node(it, node) {
+    err = output_ufdt_node_to_fdt(*it, fdtp, props_dict);
+    if (err < 0) return -1;
+  }
+
+  err = fdt_end_node(fdtp);
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+#define TAB_SIZE 2
+
+void ufdt_node_print(const struct ufdt_node *node, int depth) {
+  if (!node) return;
+
+  int i;
+  for (i = 0; i < depth * TAB_SIZE; i++) dto_print(" ");
+
+  uint32_t tag;
+  tag = tag_of(node);
+
+  switch (tag) {
+    case FDT_BEGIN_NODE:
+      dto_print("NODE ");
+      break;
+    case FDT_PROP:
+      dto_print("PROP ");
+      break;
+    default:
+      dto_print("UNKNOWN ");
+      break;
+  }
+
+  if (name_of(node)) {
+    dto_print(":%s:\n", name_of(node));
+  } else {
+    dto_print("node name is NULL.\n");
+  }
+
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    struct ufdt_node **it;
+
+    for_each_prop(it, node) ufdt_node_print(*it, depth + 1);
+
+    for_each_node(it, node) ufdt_node_print(*it, depth + 1);
+  }
+
+  return;
+}
+
+void ufdt_node_map(struct ufdt_node *node, struct ufdt_node_closure closure) {
+  if (node == NULL) return;
+  closure.func(node, closure.env);
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    struct ufdt_node **it;
+    for_each_prop(it, node) ufdt_node_map(*it, closure);
+    for_each_node(it, node) ufdt_node_map(*it, closure);
+  }
+  return;
+}