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, 294 insertions, 0 deletions

diff --git a/ufdt_convert.c b/ufdt_convert.c
new file mode 100644
index 0000000..caa3ce3
--- /dev/null
+++ b/ufdt_convert.c
@@ -0,0 +1,294 @@
+#include "libufdt.h"
+
+#include "fdt_internal.h"
+#include "ufdt_util.h"
+
+
+struct ufdt *ufdt_construct(void *fdtp) {
+  struct ufdt *res_ufdt = dto_malloc(sizeof(struct ufdt));
+  res_ufdt->fdtp = fdtp;
+  res_ufdt->root = NULL;
+
+  return res_ufdt;
+}
+
+void ufdt_destruct(struct ufdt *tree) {
+  ufdt_node_destruct(tree->root);
+  dto_free(tree->phandle_table.data);
+}
+
+static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset) {
+  if (fdtp == NULL) {
+    dto_error("Failed to get new_node because tree is NULL\n");
+    return NULL;
+  }
+
+  fdt32_t *fdt_tag_ptr =
+      (fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t));
+  struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr);
+  return res;
+}
+
+static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset,
+                                          int *next_fdt_tag_offset,
+                                          int cur_tag) {
+  if (fdtp == NULL) {
+    return NULL;
+  }
+  uint32_t tag;
+  struct ufdt_node *res, *child_node;
+
+  res = NULL;
+  child_node = NULL;
+  tag = cur_tag;
+
+  switch (tag) {
+    case FDT_END_NODE:
+    case FDT_NOP:
+    case FDT_END:
+      break;
+
+    case FDT_PROP:
+      res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
+      break;
+
+    case FDT_BEGIN_NODE:
+      res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
+
+      do {
+        cur_fdt_tag_offset = *next_fdt_tag_offset;
+        tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset);
+        child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset,
+                                      next_fdt_tag_offset, tag);
+        ufdt_node_add_child(res, child_node);
+      } while (tag != FDT_END_NODE);
+      break;
+
+    default:
+      break;
+  }
+
+  return res;
+}
+
+void ufdt_print(struct ufdt *tree) { ufdt_node_print(tree->root, 0); }
+
+struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path,
+                                            int len) {
+  /*
+   * RARE: aliases
+   * In device tree, we can assign some alias to specific nodes by defining
+   * these relation in "/aliases" node.
+   * The node has the form:
+   * {
+   *   a = "/a_for_apple";
+   *   b = "/b_for_banana";
+   * };
+   * So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1".
+   */
+  if (*path != '/') {
+    const char *end = path + len;
+
+    const char *next_slash;
+    next_slash = dto_memchr(path, '/', end - path);
+    if (!next_slash) next_slash = end;
+
+    struct ufdt_node *aliases_node =
+        ufdt_node_get_node_by_path(tree->root, "/aliases");
+    aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path,
+                                                      next_slash - path);
+
+    int path_len = 0;
+    const char *alias_path =
+        ufdt_node_get_fdt_prop_data(aliases_node, &path_len);
+
+    if (alias_path == NULL) {
+      dto_error("Failed to find alias %s\n", path);
+      return NULL;
+    }
+
+    struct ufdt_node *target_node =
+        ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len);
+
+    return ufdt_node_get_node_by_path_len(target_node, next_slash,
+                                          end - next_slash);
+  }
+  return ufdt_node_get_node_by_path_len(tree->root, path, len);
+}
+
+struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) {
+  return ufdt_get_node_by_path_len(tree, path, dto_strlen(path));
+}
+
+struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree,
+                                           uint32_t phandle) {
+  struct ufdt_node *res = NULL;
+  /*
+   * Do binary search in phandle_table.data.
+   * [s, e) means the possible range which contains target node.
+   */
+  int s = 0, e = tree->phandle_table.len;
+  while (e - s > 1) {
+    int mid = s + ((e - s) >> 1);
+    uint32_t mid_phandle = tree->phandle_table.data[mid].phandle;
+    if (phandle < mid_phandle)
+      e = mid;
+    else
+      s = mid;
+  }
+  if (e - s > 0) {
+    res = tree->phandle_table.data[s].node;
+  }
+  return res;
+}
+
+int merge_children(struct ufdt_node *node_a, struct ufdt_node *node_b) {
+  int err = 0;
+  struct ufdt_node *it;
+  for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) {
+    struct ufdt_node *cur_node = it;
+    it = it->sibling;
+    cur_node->sibling = NULL;
+    struct ufdt_node *target_node = NULL;
+    if (tag_of(cur_node) == FDT_BEGIN_NODE) {
+      target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node));
+    } else {
+      target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node));
+    }
+    if (target_node == NULL) {
+      err = ufdt_node_add_child(node_a, cur_node);
+    } else {
+      err = merge_ufdt_into(target_node, cur_node);
+    }
+    if (err < 0) return -1;
+  }
+  /*
+   * The ufdt_node* in node_b will be copied to node_a.
+   * To prevent the ufdt_node from being freed twice
+   * (main_tree and overlay_tree) at the end of function
+   * ufdt_apply_overlay(), set this node in node_b
+   * (overlay_tree) to NULL.
+   */
+  ((struct fdt_node_ufdt_node *)node_b)->child = NULL;
+
+  return 0;
+}
+
+int merge_ufdt_into(struct ufdt_node *node_a, struct ufdt_node *node_b) {
+  if (tag_of(node_a) == FDT_PROP) {
+    node_a->fdt_tag_ptr = node_b->fdt_tag_ptr;
+    return 0;
+  }
+
+  int err = 0;
+  err = merge_children(node_a, node_b);
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) {
+  ufdt_node_map(tree->root, closure);
+}
+
+static int count_phandle_node(struct ufdt_node *node) {
+  if (node == NULL) return 0;
+  if (tag_of(node) != FDT_BEGIN_NODE) return 0;
+  int res = 0;
+  if (ufdt_node_get_phandle(node) > 0) res++;
+  struct ufdt_node **it;
+  for_each_child(it, node) { res += count_phandle_node(*it); }
+  return res;
+}
+
+static void set_phandle_table_entry(struct ufdt_node *node,
+                                    struct phandle_table_entry *data,
+                                    int *cur) {
+  if (node == NULL || tag_of(node) != FDT_BEGIN_NODE) return;
+  int ph = ufdt_node_get_phandle(node);
+  if (ph > 0) {
+    data[*cur].phandle = ph;
+    data[*cur].node = node;
+    (*cur)++;
+  }
+  struct ufdt_node **it;
+  for_each_node(it, node) set_phandle_table_entry(*it, data, cur);
+  return;
+}
+
+int phandle_table_entry_cmp(const void *pa, const void *pb) {
+  uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle;
+  uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle;
+  if (ph_a < ph_b)
+    return -1;
+  else if (ph_a == ph_b)
+    return 0;
+  else
+    return 1;
+}
+
+struct static_phandle_table build_phandle_table(struct ufdt *tree) {
+  struct static_phandle_table res;
+  res.len = count_phandle_node(tree->root);
+  res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len);
+  int cur = 0;
+  set_phandle_table_entry(tree->root, res.data, &cur);
+  dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry),
+            phandle_table_entry_cmp);
+  return res;
+}
+
+struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size) {
+  (void)(fdt_size); // unused parameter
+
+  struct ufdt *res_tree = ufdt_construct(fdtp);
+
+  int start_offset = fdt_path_offset(fdtp, "/");
+  if (start_offset < 0) {
+    res_tree->fdtp = NULL;
+    return res_tree;
+  }
+
+  int end_offset;
+  int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset);
+  res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag);
+
+  res_tree->phandle_table = build_phandle_table(res_tree);
+
+  return res_tree;
+}
+
+int ufdt_to_fdt(struct ufdt *tree, void *buf, int buf_size) {
+  int err;
+  err = fdt_create(buf, buf_size);
+  if (err < 0) return -1;
+
+  int n_mem_rsv = fdt_num_mem_rsv(tree->fdtp);
+  for (int i = 0; i < n_mem_rsv; i++) {
+    uint64_t addr, size;
+    fdt_get_mem_rsv(tree->fdtp, i, &addr, &size);
+    fdt_add_reservemap_entry(buf, addr, size);
+  }
+
+  err = fdt_finish_reservemap(buf);
+  if (err < 0) return -1;
+
+  /*
+   * Obtains all props for later use because getting them from
+   * FDT requires complicated manipulation.
+  */
+  struct ufdt_node_dict all_props = ufdt_node_dict_construct();
+  err = output_ufdt_node_to_fdt(tree->root, buf, &all_props);
+  if (err < 0) return -1;
+
+  ufdt_node_dict_destruct(&all_props);
+
+  err = fdt_finish(buf);
+  if (err < 0) return -1;
+
+  /*
+   * IMPORTANT: fdt_totalsize(buf) might be less than buf_size
+   * so this is needed to make use of remain spaces.
+   */
+  return fdt_open_into(buf, buf, buf_size);
+}