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: I6a282cc99129d5280ecbf40852723f83735fa523

1 files changed, 648 insertions, 0 deletions

diff --git a/ufdt_overlay.c b/ufdt_overlay.c
new file mode 100644
index 0000000..d581ca7
--- /dev/null
+++ b/ufdt_overlay.c
@@ -0,0 +1,648 @@
+/*-
+ * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@FreeBSD.org>
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under sponsorship from
+ * the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "ufdt_overlay.h"
+
+#include "libufdt.h"
+
+
+/*
+ * The original version of fdt_overlay.c is slow in searching for particular
+ * nodes and adding subnodes/properties due to the operations on flattened
+ * device tree (FDT).
+ *
+ * Here we introduce `libufdt` which 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.
+ *
+ * This file is the improved version of fdt_overlay.c by using the real tree
+ * structure defined in libufdt.
+ *
+ * How the device tree overlay works and some
+ * special terms (e.g., fixups, local fixups, fragment, etc)
+ * are described in the document
+ * external/dtc/Documentation/dt-object-internal.txt.
+ */
+
+/* BEGIN of operations about phandles in ufdt. */
+
+/*
+ * Increases u32 value at pos by offset.
+ */
+static void fdt_increase_u32(void *pos, uint32_t offset) {
+  uint32_t val;
+
+  dto_memcpy(&val, pos, sizeof(val));
+  val = cpu_to_fdt32(fdt32_to_cpu(val) + offset);
+  dto_memcpy(pos, &val, sizeof(val));
+}
+
+/*
+ * Gets the max phandle of a given ufdt.
+ */
+static uint32_t ufdt_get_max_phandle(struct ufdt *tree) {
+  struct static_phandle_table sorted_table = tree->phandle_table;
+  if (sorted_table.len > 0)
+    return sorted_table.data[sorted_table.len - 1].phandle;
+  else
+    return 0;
+}
+
+/*
+ * Tries to increase the phandle value of a node
+ * if the phandle exists.
+ */
+static void ufdt_node_try_increase_phandle(struct ufdt_node *node,
+                                           uint32_t offset) {
+  int len = 0;
+  char *prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len);
+  if (prop_data != NULL && len == sizeof(fdt32_t)) {
+    fdt_increase_u32(prop_data, offset);
+  }
+  prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len);
+  if (prop_data != NULL && len == sizeof(fdt32_t)) {
+    fdt_increase_u32(prop_data, offset);
+  }
+}
+
+/*
+ * Increases all phandles by offset in a ufdt
+ * in O(n) time.
+ */
+static void ufdt_try_increase_phandle(struct ufdt *tree, uint32_t offset) {
+  struct static_phandle_table sorted_table = tree->phandle_table;
+  int i;
+
+  for (i = 0; i < sorted_table.len; i++) {
+    struct ufdt_node *target_node = sorted_table.data[i].node;
+
+    ufdt_node_try_increase_phandle(target_node, offset);
+  }
+}
+
+/* END of operations about phandles in ufdt. */
+
+/*
+ * In the overlay_tree, there are some references (phandle)
+ * pointing to somewhere in the main_tree.
+ * Fix-up operations is to resolve the right address
+ * in the overlay_tree.
+ */
+
+/* BEGIN of doing fixup in the overlay ufdt. */
+
+/*
+ * Returns exact memory location specified by fixup in format
+ * /path/to/node:property:offset.
+ * A property might contain multiple values and the offset is used to locate a
+ * reference inside the property.
+ * e.g.,
+ * "property"=<1, 2, &ref, 4>, we can use /path/to/node:property:8 to get ref,
+ * where 8 is sizeof(uint32) + sizeof(unit32).
+ */
+static void *ufdt_get_fixup_location(struct ufdt *tree, const char *fixup) {
+  char *path, *prop_ptr, *offset_ptr, *end_ptr;
+  int prop_offset, prop_len;
+  const char *prop_data;
+
+  /*
+   * TODO(akaineko): Keep track of substring lengths so we don't have to
+   * dto_malloc a copy and split it up.
+   */
+  path = dto_strdup(fixup);
+  prop_ptr = dto_strchr(path, ':');
+  if (prop_ptr == NULL) {
+    dto_error("Missing property part in '%s'\n", path);
+    goto fail;
+  }
+
+  *prop_ptr = '\0';
+  prop_ptr++;
+
+  offset_ptr = dto_strchr(prop_ptr, ':');
+  if (offset_ptr == NULL) {
+    dto_error("Missing offset part in '%s'\n", path);
+    goto fail;
+  }
+
+  *offset_ptr = '\0';
+  offset_ptr++;
+
+  prop_offset = dto_strtoul(offset_ptr, &end_ptr, 10 /* base */);
+  if (*end_ptr != '\0') {
+    dto_error("'%s' is not valid number\n", offset_ptr);
+    goto fail;
+  }
+
+  struct ufdt_node *target_node;
+  target_node = ufdt_get_node_by_path(tree, path);
+  if (target_node == NULL) {
+    dto_error("Path '%s' not found\n", path);
+    goto fail;
+  }
+
+  prop_data =
+      ufdt_node_get_fdt_prop_data_by_name(target_node, prop_ptr, &prop_len);
+  if (prop_data == NULL) {
+    dto_error("Property '%s' not found in  '%s' node\n", prop_ptr, path);
+    goto fail;
+  }
+  /*
+   * Note that prop_offset is the offset inside the property data.
+   */
+  if (prop_len < prop_offset + (int)sizeof(uint32_t)) {
+    dto_error("%s: property length is too small for fixup\n", path);
+    goto fail;
+  }
+
+  dto_free(path);
+  return (char *)prop_data + prop_offset;
+
+fail:
+  dto_free(path);
+  return NULL;
+}
+
+/*
+ * Process one entry in __fixups__ { } node.
+ * @fixups is property value, array of NUL-terminated strings
+ *   with fixup locations.
+ * @fixups_len length of the fixups array in bytes.
+ * @phandle is value for these locations.
+ */
+static int ufdt_do_one_fixup(struct ufdt *tree, const char *fixups,
+                             int fixups_len, int phandle) {
+  void *fixup_pos;
+  uint32_t val;
+
+  val = cpu_to_fdt32(phandle);
+
+  while (fixups_len > 0) {
+    fixup_pos = ufdt_get_fixup_location(tree, fixups);
+    if (fixup_pos != NULL) {
+      dto_memcpy(fixup_pos, &val, sizeof(val));
+    } else {
+      return -1;
+    }
+
+    fixups_len -= dto_strlen(fixups) + 1;
+    fixups += dto_strlen(fixups) + 1;
+  }
+
+  return 0;
+}
+
+/*
+ * Handle __fixups__ node in overlay tree.
+ */
+
+static int ufdt_overlay_do_fixups(struct ufdt *main_tree,
+                                  struct ufdt *overlay_tree) {
+  int len = 0;
+  struct ufdt_node *main_symbols_node, *overlay_fixups_node;
+
+  main_symbols_node = ufdt_get_node_by_path(main_tree, "/__symbols__");
+  overlay_fixups_node = ufdt_get_node_by_path(overlay_tree, "/__fixups__");
+
+  if (!main_symbols_node) {
+    dto_error("Bad main_symbols in ufdt_overlay_do_fixups\n");
+    return -1;
+  }
+
+  if (!overlay_fixups_node) {
+    dto_error("Bad overlay_fixups in ufdt_overlay_do_fixups\n");
+    return -1;
+  }
+
+  struct ufdt_node **it;
+  for_each_prop(it, overlay_fixups_node) {
+    /*
+     * A property in __fixups__ looks like:
+     * symbol_name =
+     * "/path/to/node:prop:offset0\x00/path/to/node:prop:offset1..."
+     * So we firstly find the node "symbol_name" and obtain its phandle in
+     * __symbols__ of the main_tree.
+     */
+
+    struct ufdt_node *fixups = *it;
+    char *symbol_path = ufdt_node_get_fdt_prop_data_by_name(
+        main_symbols_node, name_of(fixups), &len);
+
+    if (!symbol_path) {
+      dto_error("Couldn't find '%s' symbol in main dtb\n", name_of(fixups));
+      return -1;
+    }
+
+    struct ufdt_node *symbol_node;
+    symbol_node = ufdt_get_node_by_path(main_tree, symbol_path);
+
+    if (!symbol_node) {
+      dto_error("Couldn't find '%s' path in main dtb\n", symbol_path);
+      return -1;
+    }
+
+    uint32_t phandle = ufdt_node_get_phandle(symbol_node);
+
+    const char *fixups_paths = ufdt_node_get_fdt_prop_data(fixups, &len);
+
+    if (ufdt_do_one_fixup(overlay_tree, fixups_paths, len, phandle) < 0) {
+      dto_error("Failed one fixup in ufdt_do_one_fixup\n");
+      return -1;
+    }
+  }
+
+  return 0;
+}
+
+/* END of doing fixup in the overlay ufdt. */
+
+/*
+ * Here is to overlay all fragments in the overlay_tree to the main_tree.
+ * What is "overlay fragment"? The main purpose is to add some subtrees to the
+ * main_tree in order to complete the entire device tree.
+ *
+ * A frgament consists of two parts: 1. the subtree to be added 2. where it
+ * should be added.
+ *
+ * Overlaying a fragment requires: 1. find the node in the main_tree 2. merge
+ * the subtree into that node in the main_tree.
+ */
+
+/* BEGIN of applying fragments. */
+
+/*
+ * Overlay the overlay_node over target_node.
+ */
+static int ufdt_overlay_node(struct ufdt_node *target_node,
+                             struct ufdt_node *overlay_node) {
+  return merge_ufdt_into(target_node, overlay_node);
+}
+
+/*
+ * Return value of ufdt_apply_fragment().
+ */
+
+enum overlay_result {
+  OVERLAY_RESULT_OK,
+  OVERLAY_RESULT_MISSING_TARGET,
+  OVERLAY_RESULT_MISSING_OVERLAY,
+  OVERLAY_RESULT_TARGET_PATH_INVALID,
+  OVERLAY_RESULT_TARGET_INVALID,
+  OVERLAY_RESULT_MERGE_FAIL,
+};
+
+/*
+ * Apply one overlay fragment (subtree).
+ */
+static enum overlay_result ufdt_apply_fragment(struct ufdt *tree,
+                                               struct ufdt_node *frag_node) {
+  uint32_t target;
+  const char *target_path;
+  const void *val;
+  struct ufdt_node *target_node = NULL;
+  struct ufdt_node *overlay_node = NULL;
+
+  val = ufdt_node_get_fdt_prop_data_by_name(frag_node, "target", NULL);
+  if (val) {
+    dto_memcpy(&target, val, sizeof(target));
+    target = fdt32_to_cpu(target);
+    target_node = ufdt_get_node_by_phandle(tree, target);
+    if (target_node == NULL) {
+      dto_error("failed to find target %04x\n", target);
+      return OVERLAY_RESULT_TARGET_INVALID;
+    }
+  }
+
+  if (target_node == NULL) {
+    target_path =
+        ufdt_node_get_fdt_prop_data_by_name(frag_node, "target-path", NULL);
+    if (target_path == NULL) {
+      return OVERLAY_RESULT_MISSING_TARGET;
+    }
+
+    target_node = ufdt_get_node_by_path(tree, target_path);
+    if (target_node == NULL) {
+      dto_error("failed to find target-path %s\n", target_path);
+      return OVERLAY_RESULT_TARGET_PATH_INVALID;
+    }
+  }
+
+  overlay_node = ufdt_node_get_node_by_path(frag_node, "__overlay__");
+  if (overlay_node == NULL) {
+    dto_error("missing __overlay__ sub-node\n");
+    return OVERLAY_RESULT_MISSING_OVERLAY;
+  }
+
+  int err = ufdt_overlay_node(target_node, overlay_node);
+
+  if (err < 0) {
+    dto_error("failed to overlay node %s to target %s\n", name_of(overlay_node),
+              name_of(target_node));
+    return OVERLAY_RESULT_MERGE_FAIL;
+  }
+
+  return OVERLAY_RESULT_OK;
+}
+
+/*
+ * Applies all fragments to the main_tree.
+ */
+static int ufdt_overlay_apply_fragments(struct ufdt *main_tree,
+                                        struct ufdt *overlay_tree) {
+  enum overlay_result err;
+  struct ufdt_node **it;
+  /*
+   * This loop may iterate to subnodes that's not a fragment node.
+   * In such case, ufdt_apply_fragment would fail with return value = -1.
+   */
+  for_each_node(it, overlay_tree->root) {
+    err = ufdt_apply_fragment(main_tree, *it);
+    if (err == OVERLAY_RESULT_MERGE_FAIL) {
+      return -1;
+    }
+  }
+  return 0;
+}
+
+/* END of applying fragments. */
+
+/*
+ * Since the overlay_tree will be "merged" into the main_tree, some
+ * references (e.g., phandle values that acts as an unique ID) need to be
+ * updated so it won't lead to collision that different nodes have the same
+ * phandle value.
+ *
+ * Two things need to be done:
+ *
+ * 1. ufdt_try_increase_phandle()
+ * Update phandle (an unique integer ID of a node in the device tree) of each
+ * node in the overlay_tree. To achieve this, we simply increase each phandle
+ * values in the overlay_tree by the max phandle value of the main_tree.
+ *
+ * 2. ufdt_overlay_do_local_fixups()
+ * If there are some reference in the overlay_tree that references nodes
+ * inside the overlay_tree, we have to modify the reference value (address of
+ * the referenced node: phandle) so that it corresponds to the right node inside
+ * the overlay_tree. Where the reference exists is kept in __local_fixups__ node
+ * in the overlay_tree.
+ */
+
+/* BEGIN of updating local references (phandle values) in the overlay ufdt. */
+
+/*
+ * local fixups
+ */
+static int ufdt_local_fixup_prop(struct ufdt_node *target_prop_node,
+                                 struct ufdt_node *local_fixup_prop_node,
+                                 uint32_t phandle_offset) {
+  /*
+   * prop_offsets_ptr should be a list of fdt32_t.
+   * <offset0 offset1 offset2 ...>
+   */
+  char *prop_offsets_ptr;
+  int len = 0;
+  prop_offsets_ptr = ufdt_node_get_fdt_prop_data(local_fixup_prop_node, &len);
+
+  char *prop_data;
+  int target_length = 0;
+
+  prop_data = ufdt_node_get_fdt_prop_data(target_prop_node, &target_length);
+
+  if (prop_offsets_ptr == NULL || prop_data == NULL) return -1;
+
+  int i;
+  for (i = 0; i < len; i += sizeof(fdt32_t)) {
+    int offset = fdt32_to_cpu(*(fdt32_t *)(prop_offsets_ptr + i));
+    if (offset + sizeof(fdt32_t) > (size_t)target_length) return -1;
+    fdt_increase_u32((prop_data + offset), phandle_offset);
+  }
+  return 0;
+}
+
+static int ufdt_local_fixup_node(struct ufdt_node *target_node,
+                                 struct ufdt_node *local_fixups_node,
+                                 uint32_t phandle_offset) {
+  if (local_fixups_node == NULL) return 0;
+
+  struct ufdt_node **it_local_fixups;
+  struct ufdt_node *sub_target_node;
+
+  for_each_prop(it_local_fixups, local_fixups_node) {
+    sub_target_node =
+        ufdt_node_get_property_by_name(target_node, name_of(*it_local_fixups));
+
+    if (sub_target_node != NULL) {
+      int err = ufdt_local_fixup_prop(sub_target_node, *it_local_fixups,
+                                      phandle_offset);
+      if (err < 0) return -1;
+    } else {
+      return -1;
+    }
+  }
+
+  for_each_node(it_local_fixups, local_fixups_node) {
+    sub_target_node =
+        ufdt_node_get_node_by_path(target_node, name_of(*it_local_fixups));
+    if (sub_target_node != NULL) {
+      int err = ufdt_local_fixup_node(sub_target_node, *it_local_fixups,
+                                      phandle_offset);
+      if (err < 0) return -1;
+    } else {
+      return -1;
+    }
+  }
+
+  return 0;
+}
+
+/*
+ * Handle __local_fixups__ node in overlay DTB
+ * The __local_fixups__ format we expect is
+ * __local_fixups__ {
+ *   path {
+ *    to {
+ *      local_ref1 = <offset>;
+ *    };
+ *   };
+ *   path2 {
+ *    to2 {
+ *      local_ref2 = <offset1 offset2 ...>;
+ *    };
+ *   };
+ * };
+ *
+ * which follows the dtc patch from:
+ * https://marc.info/?l=devicetree&m=144061468601974&w=4
+ */
+static int ufdt_overlay_do_local_fixups(struct ufdt *tree,
+                                        uint32_t phandle_offset) {
+  struct ufdt_node *overlay_node = ufdt_get_node_by_path(tree, "/");
+  struct ufdt_node *local_fixups_node =
+      ufdt_get_node_by_path(tree, "/__local_fixups__");
+
+  int err =
+      ufdt_local_fixup_node(overlay_node, local_fixups_node, phandle_offset);
+
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+static int ufdt_overlay_local_ref_update(struct ufdt *main_tree,
+                                         struct ufdt *overlay_tree) {
+  uint32_t phandle_offset = 0;
+
+  phandle_offset = ufdt_get_max_phandle(main_tree);
+  if (phandle_offset > 0) {
+    ufdt_try_increase_phandle(overlay_tree, phandle_offset);
+  }
+
+  int err = ufdt_overlay_do_local_fixups(overlay_tree, phandle_offset);
+  if (err < 0) {
+    dto_error("failed to perform local fixups in overlay\n");
+    return -1;
+  }
+  return 0;
+}
+
+/* END of updating local references (phandle values) in the overlay ufdt. */
+
+static int ufdt_overlay_apply(struct ufdt *main_tree, struct ufdt *overlay_tree,
+                              size_t overlay_length) {
+  if (overlay_length < sizeof(struct fdt_header)) {
+    dto_error("Overlay_length %zu smaller than header size %zu\n",
+              overlay_length, sizeof(struct fdt_header));
+    return -1;
+  }
+
+  if (ufdt_overlay_local_ref_update(main_tree, overlay_tree) < 0) {
+    dto_error("failed to perform local fixups in overlay\n");
+    return -1;
+  }
+
+  if (ufdt_overlay_do_fixups(main_tree, overlay_tree) < 0) {
+    dto_error("failed to perform fixups in overlay\n");
+    return -1;
+  }
+  if (ufdt_overlay_apply_fragments(main_tree, overlay_tree) < 0) {
+    dto_error("failed to apply fragments\n");
+    return -1;
+  }
+
+  return 0;
+}
+
+struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size) {
+  struct fdt_header *pHeader;
+  int err;
+
+  dto_debug("ufdt_install_blob (0x%08jx)\n", (uintmax_t)blob);
+
+  if (blob_size < sizeof(struct fdt_header)) {
+    dto_error("Blob_size %zu smaller than the header size %zu\n", blob_size,
+              sizeof(struct fdt_header));
+    return NULL;
+  }
+
+  pHeader = (struct fdt_header *)blob;
+  err = fdt_check_header(pHeader);
+  if (err < 0) {
+    if (err == -FDT_ERR_BADVERSION) {
+      dto_error("incompatible blob version: %d, should be: %d",
+                fdt_version(pHeader), FDT_LAST_SUPPORTED_VERSION);
+
+    } else {
+      dto_error("error validating blob: %s", fdt_strerror(err));
+    }
+    return NULL;
+  }
+
+  return pHeader;
+}
+
+/*
+* From Google, based on dt_overlay_apply() logic
+* Will dto_malloc a new fdt blob and return it. Will not dto_free parameters.
+*/
+struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header,
+                                 size_t main_fdt_size,
+                                 void *overlay_fdtp,
+                                 size_t overlay_size) {
+  size_t out_fdt_size;
+
+  if (main_fdt_header == NULL) {
+    return NULL;
+  }
+
+  if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay_fdtp)) {
+    dto_error("Bad overlay size!\n");
+    return NULL;
+  }
+  if (main_fdt_size < 8 || main_fdt_size != fdt_totalsize(main_fdt_header)) {
+    dto_error("Bad fdt size!\n");
+    return NULL;
+  }
+
+  out_fdt_size = fdt_totalsize(main_fdt_header) + overlay_size;
+  /* It's actually more than enough */
+  struct fdt_header *out_fdt_header = dto_malloc(out_fdt_size);
+
+  if (out_fdt_header == NULL) {
+    dto_error("failed to allocate memory for DTB blob with overlays\n");
+    return NULL;
+  }
+
+  struct ufdt *main_tree, *overlay_tree;
+
+  main_tree = fdt_to_ufdt(main_fdt_header, main_fdt_size);
+
+  overlay_tree = fdt_to_ufdt(overlay_fdtp, overlay_size);
+
+  int err = ufdt_overlay_apply(main_tree, overlay_tree, overlay_size);
+  if (err < 0) {
+    goto fail;
+  }
+
+  err = ufdt_to_fdt(main_tree, out_fdt_header, out_fdt_size);
+  if (err < 0) {
+    dto_error("Failed to dump the device tree to out_fdt_header\n");
+    goto fail;
+  }
+
+  ufdt_destruct(main_tree);
+  ufdt_destruct(overlay_tree);
+  return out_fdt_header;
+
+fail:
+  ufdt_destruct(main_tree);
+  ufdt_destruct(overlay_tree);
+  dto_free(out_fdt_header);
+  return NULL;
+}