devicetree: Add overlay support

This patch adds support for merging a device tree overlay (as defined in
Documentation/dt-object-internal.txt in the dtc repository) into a base
device tree. The new functionality is not used (and therefore mostly
compile-time eliminated) for now, but we intend to augment our FIT
images with overlays in order to save binary size for small SKU
differences in the future.

BRANCH=None
BUG=None
TEST=Manually hacked up a framework to run all the Android libufdt
overlay test cases and manually compare them with the output produced by
libfdt (which showed no differences other than node/property ordering
which I think is irrelevant and one weird case of libfdt adjusting the
overlay phandles by an unexpectedly high offset, which suggests a bug in
their code but is ultimately inconsequential).

Change-Id: Icd3ed158f33e79e733617b74b1fc1edef762d69a
Signed-off-by: Julius Werner <jwerner@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1536387
Commit-Ready: ChromeOS CL Exonerator Bot <chromiumos-cl-exonerator@appspot.gserviceaccount.com>
Reviewed-by: Hung-Te Lin <hungte@chromium.org>
diff --git a/src/base/device_tree.c b/src/base/device_tree.c
index 6f5b25b..be1ffa1 100644
--- a/src/base/device_tree.c
+++ b/src/base/device_tree.c
@@ -1085,3 +1085,460 @@
 
 	return reserved;
 }
+
+/*
+ * Increment a single phandle in prop at a given offset by a given adjustment.
+ *
+ * @param prop		Property whose phandle should be adjusted.
+ * @param adjustment	Value that should be added to the existing phandle.
+ * @param offset	Byte offset of the phandle in the property data.
+ *
+ * @return		New phandle value, or 0 on error.
+ */
+static uint32_t dt_adjust_phandle(DeviceTreeProperty *prop,
+				  uint32_t adjustment, uint32_t offset)
+{
+	if (offset + 4 > prop->prop.size)
+		return 0;
+
+	uint32_t phandle = be32dec(prop->prop.data + offset);
+	if (phandle == 0 ||
+	    phandle == PhandleIllegal ||
+	    phandle == 0xffffffff)
+		return 0;
+
+	phandle += adjustment;
+	if(phandle >= PhandleIllegal)
+		return 0;
+
+	be32enc(prop->prop.data + offset, phandle);
+	return phandle;
+}
+
+/*
+ * Adjust all phandles in subtree by adding a new base offset.
+ *
+ * @param node		Root node of the subtree to work on.
+ * @param base		New phandle base to be added to all phandles.
+ *
+ * @return		New highest phandle in the subtree, or 0 on error.
+ */
+static uint32_t dt_adjust_all_phandles(DeviceTreeNode *node, uint32_t base)
+{
+	uint32_t new_max = MAX(base, 1);  // make sure we don't return 0
+	DeviceTreeProperty *prop;
+	DeviceTreeNode *child;
+
+	if (!node)
+		return new_max;
+
+	list_for_each(prop, node->properties, list_node)
+		if (dt_prop_is_phandle(prop)) {
+			node->phandle = dt_adjust_phandle(prop, base, 0);
+			if (!node->phandle)
+				return 0;
+			new_max = MAX(new_max, node->phandle);
+		}  // no break -- can have more than one phandle prop
+
+	list_for_each(child, node->children, list_node)
+		new_max = MAX(new_max, dt_adjust_all_phandles(child, base));
+
+	return new_max;
+}
+
+/*
+ * Apply a /__local_fixup__ subtree to the corresponding overlay subtree.
+ *
+ * @param node		Root node of the overlay subtree to fix up.
+ * @param node		Root node of the /__local_fixup__ subtree.
+ * @param base		Adjustment that was added to phandles in the overlay.
+ *
+ * @return		0 on success, -1 on error.
+ */
+int dt_fixup_locals(DeviceTreeNode *node, DeviceTreeNode *fixup, uint32_t base)
+{
+	DeviceTreeProperty *prop;
+	DeviceTreeProperty *fixup_prop;
+	DeviceTreeNode *child;
+	DeviceTreeNode *fixup_child;
+	int i;
+
+	// For local fixups the /__local_fixup__ subtree contains the same node
+	// hierarchy as the main tree we're fixing up. Each property contains
+	// the fixup offsets for the respective property in the main tree. For
+	// each property in the fixup node, find the corresponding property in
+	// the base node and apply fixups to all offsets it specifies.
+	list_for_each(fixup_prop, fixup->properties, list_node) {
+		DeviceTreeProperty *base_prop = NULL;
+		list_for_each(prop, node->properties, list_node)
+			if (!strcmp(prop->prop.name, fixup_prop->prop.name)) {
+				base_prop = prop;
+				break;
+			}
+
+		// We should always find a corresponding base prop for a fixup,
+		// and fixup props contain a list of 32-bit fixup offsets.
+		if (!base_prop || fixup_prop->prop.size % sizeof(uint32_t))
+			return -1;
+
+		for (i = 0; i < fixup_prop->prop.size; i += sizeof(uint32_t))
+			if (!dt_adjust_phandle(base_prop, base,
+					be32dec(fixup_prop->prop.data + i)))
+				return -1;
+	}
+
+	// Now recursively descend both the base tree and the /__local_fixups__
+	// subtree in sync to apply all fixups.
+	list_for_each(fixup_child, fixup->children, list_node) {
+		DeviceTreeNode *base_child = NULL;
+		list_for_each(child, node->children, list_node)
+			if (!strcmp(child->name, fixup_child->name)) {
+				base_child = child;
+				break;
+			}
+
+		// All fixup nodes should have a corresponding base node.
+		if (!base_child)
+			return -1;
+
+		if (dt_fixup_locals(base_child, fixup_child, base) < 0)
+			return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Update all /__symbols__ properties in an overlay that start with
+ * "/fragment@X/__overlay__" with corresponding path prefix in the base tree.
+ *
+ * @param symbols	/__symbols__ done to update.
+ * @param fragment	/fragment@X node that references to should be updated.
+ * @param base_path	Path of base tree node that the fragment overlaid.
+ */
+static void dt_fix_symbols(DeviceTreeNode *symbols, DeviceTreeNode *fragment,
+			   const char *base_path)
+{
+	DeviceTreeProperty *prop;
+	char buf[512];		// Should be enough for maximum DT path length?
+	char node_path[64];	// easily enough for /fragment@XXXX/__overlay__
+
+	if (!symbols)	// If the overlay has no /__symbols__ node, we're done!
+		return;
+
+	int len = snprintf(node_path, sizeof(node_path), "/%s/__overlay__",
+			   fragment->name);
+
+	list_for_each(prop, symbols->properties, list_node)
+		if (!strncmp(prop->prop.data, node_path, len)) {
+			prop->prop.size = snprintf(buf, sizeof(buf), "%s%s",
+				base_path, (char *)prop->prop.data + len) + 1;
+			free(prop->prop.data);
+			prop->prop.data = strdup(buf);
+		}
+}
+
+/*
+ * Fix up overlay according to a property in /__fixup__. If the fixed property
+ * is a /fragment@X:target, also update /__symbols__ references to fragment.
+ *
+ * @params overlay	Overlay to fix up.
+ * @params fixup	/__fixup__ property.
+ * @params phandle	phandle value to insert where the fixup points to.
+ * @params base_path	Path to the base DT node that the fixup points to.
+ * @params overlay_symbols /__symbols__ node of the overlay.
+ *
+ * @return		0 on success, -1 on error.
+ */
+static int dt_fixup_external(DeviceTree *overlay, DeviceTreeProperty *fixup,
+			     uint32_t phandle, const char *base_path,
+			     DeviceTreeNode *overlay_symbols)
+{
+	DeviceTreeProperty *prop;
+
+	// External fixup properties are encoded as "<path>:<prop>:<offset>".
+	char *entry = fixup->prop.data;
+	while ((void *)entry < fixup->prop.data + fixup->prop.size) {
+		// okay to destroy fixup property value, won't be needed again
+		char *node_path = strsep(&entry, ":");
+		char *prop_name = strsep(&entry, ":");
+		char *offset_string = entry;
+		if (!node_path || !prop_name || !offset_string)
+			return -1;
+
+		DeviceTreeNode *ovl_node = dt_find_node_by_path(overlay,
+						node_path, NULL, NULL, 0);
+		if (!ovl_node)
+			return -1;
+
+		DeviceTreeProperty *ovl_prop = NULL;
+		list_for_each(prop, ovl_node->properties, list_node)
+			if (!strcmp(prop->prop.name, prop_name)) {
+				ovl_prop = prop;
+				break;
+			}
+
+		// Move entry to first char after number, must be a '\0'.
+		uint32_t offset = strtoul(offset_string, &entry, 0);
+		if (!ovl_prop || offset + 4 > ovl_prop->prop.size || entry[0])
+			return -1;
+		entry++;  // jump over '\0' to potential next fixup
+
+		be32enc(ovl_prop->prop.data + offset, phandle);
+
+		// If this is a /fragment@X:target property, update
+		// references to this fragment in the overlay __symbols__ now.
+		if (offset == 0 && !strcmp(prop_name, "target") &&
+		    !strchr(node_path + 1, '/')) // only toplevel nodes
+			dt_fix_symbols(overlay_symbols, ovl_node, base_path);
+	}
+
+	return 0;
+}
+
+/*
+ * Apply all /__fixup__ properties in the overlay. This will destroy the
+ * property data in /__fixup__ and it should not be accessed again.
+ *
+ * @params tree		Base device tree that the overlay updates.
+ * @params symbols	/__symbols__ node of the base device tree.
+ * @params overlay	Overlay to fix up.
+ * @params fixups	/__fixup__ node in the overlay.
+ * @params overlay_symbols /__symbols__ node of the overlay.
+ *
+ * @return		0 on success, -1 on error.
+ */
+static int dt_fixup_all_externals(DeviceTree *tree, DeviceTreeNode *symbols,
+				  DeviceTree *overlay, DeviceTreeNode *fixups,
+				  DeviceTreeNode *overlay_symbols)
+{
+	DeviceTreeProperty *fix;
+
+	// If we have any external fixups, the base tree must have /__symbols__.
+	if (!symbols)
+		return -1;
+
+	// Unlike /__local_fixups__, /__fixups__ is not a whole subtree that
+	// mirrors the node hierarchy. It's just a directory of fixup properties
+	// that each directly contain all information necessary to apply them.
+	list_for_each(fix, fixups->properties, list_node) {
+		// The name of a fixup property is the label of the node we want
+		// a property to phandle-reference. Look it up in /__symbols__.
+		const char *path = dt_find_string_prop(symbols, fix->prop.name);
+		if (!path)
+			return -1;
+
+		// Find the node the label pointed to to figure out its phandle.
+		DeviceTreeNode *node = dt_find_node_by_path(tree, path,
+							    NULL, NULL, 0);
+		if (!node)
+			return -1;
+
+		// Write it into the overlay property(s) pointing to that node.
+		if (dt_fixup_external(overlay, fix, node->phandle,
+				      path, overlay_symbols) < 0)
+			return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * Copy all nodes and properties from one DT subtree into another. This is a
+ * shallow copy so both trees will point to the same property data afterwards.
+ *
+ * @params dst		Destination subtree to copy into.
+ * @params src		Source subtree to copy from.
+ * @params upd		1 to overwrite same-name properties, 0 to discard them.
+ */
+static void dt_copy_subtree(DeviceTreeNode *dst, DeviceTreeNode *src, int upd)
+{
+	DeviceTreeProperty *prop;
+	DeviceTreeProperty *src_prop;
+	list_for_each (src_prop, src->properties, list_node) {
+		if (dt_prop_is_phandle(src_prop) ||
+		    !strcmp(src_prop->prop.name, "name")) {
+			printf("WARNING: ignoring illegal overlay prop '%s'\n",
+			       src_prop->prop.name);
+			continue;
+		}
+
+		DeviceTreeProperty *dst_prop = NULL;
+		list_for_each(prop, dst->properties, list_node)
+			if (!strcmp(prop->prop.name, src_prop->prop.name)) {
+				dst_prop = prop;
+				break;
+			}
+
+		if (dst_prop) {
+			if (!upd) {
+				printf("WARNING: ignoring prop update '%s'\n",
+				       src_prop->prop.name);
+				continue;
+			}
+		} else {
+			dst_prop = alloc_prop();
+			list_insert_after(&dst_prop->list_node,
+					  &dst->properties);
+		}
+
+		dst_prop->prop = src_prop->prop;
+	}
+
+	DeviceTreeNode *node;
+	DeviceTreeNode *src_node;
+	list_for_each (src_node, src->children, list_node) {
+		DeviceTreeNode *dst_node = NULL;
+		list_for_each (node, dst->children, list_node)
+			if (!strcmp(node->name, src_node->name)) {
+				dst_node = node;
+				break;
+			}
+
+		if (!dst_node) {
+			dst_node = alloc_node();
+			*dst_node = *src_node;
+			list_insert_after(&dst_node->list_node, &dst->children);
+		} else {
+			dt_copy_subtree(dst_node, src_node, upd);
+		}
+	}
+}
+
+/*
+ * Apply an overlay /fragment@X node to a base device tree.
+ *
+ * @param tree		Base device tree.
+ * @param fragment	/fragment@X node.
+ * @params overlay_symbols /__symbols__ node of the overlay.
+ *
+ * @return		0 on success, -1 on error.
+ */
+static int dt_import_fragment(DeviceTree *tree, DeviceTreeNode *fragment,
+			      DeviceTreeNode *overlay_symbols)
+{
+	// The actually overlaid nodes/props are in an __overlay__ child node.
+	static const char *overlay_path[] = { "__overlay__", NULL };
+	DeviceTreeNode *overlay = dt_find_node(fragment, overlay_path,
+					       NULL, NULL, 0);
+
+	// If it doesn't have an __overlay__ child, it's not a fragment.
+	if (!overlay)
+		return 0;
+
+	// The target node of the fragment can be given by path or by phandle.
+	DeviceTreeProperty *prop;
+	DeviceTreeProperty *phandle = NULL;
+	DeviceTreeProperty *path = NULL;
+	list_for_each(prop, fragment->properties, list_node) {
+		if (!strcmp(prop->prop.name, "target")) {
+			phandle = prop;
+			break; // phandle target has priority, stop looking here
+		}
+		if (!strcmp(prop->prop.name, "target-path"))
+			path = prop;
+	}
+
+	DeviceTreeNode *target = NULL;
+	if (phandle) {
+		if (phandle->prop.size != sizeof(uint32_t))
+			return -1;
+		target = dt_find_node_by_phandle(tree->root,
+						 be32dec(phandle->prop.data));
+		// Symbols already updated as part of dt_fixup_external(target).
+	} else if (path) {
+		target = dt_find_node_by_path(tree, path->prop.data,
+					      NULL, NULL, 0);
+		dt_fix_symbols(overlay_symbols, fragment, path->prop.data);
+	}
+	if (!target)
+		return -1;
+
+	dt_copy_subtree(target, overlay, 1);
+	return 0;
+}
+
+/*
+ * Apply a device tree overlay (flattened) to a base device tree (unflattened).
+ * This will destroy/incorporate the overlay blob, so it should not be reused.
+ * See dtc.git/Documentation/dt-object-internal.txt for overlay format details.
+ *
+ * @param tree		Base device tree to add the overlay into.
+ * @param overlay_blob	Overlay in raw, unparsed FDT blob form.
+ *
+ * @return		0 on success, -1 on error.
+ */
+int dt_apply_overlay(DeviceTree *tree, void *overlay_blob)
+{
+	DeviceTree *overlay = fdt_unflatten(overlay_blob);
+	if (!overlay)
+		return -1;
+
+	// First, we need to make sure phandles inside the overlay don't clash
+	// with those in the base tree. We just define the highest phandle value
+	// in the base tree as the "phandle offset" for this overlay and
+	// increment all phandles in it by that value.
+	uint32_t phandle_base = tree->max_phandle;
+	uint32_t new_max = dt_adjust_all_phandles(overlay->root, phandle_base);
+	if (!new_max) {
+		printf("ERROR: invalid phandles in overlay\n");
+		return -1;
+	}
+	tree->max_phandle = new_max;
+
+	// Now that we changed phandles in the overlay, we need to update any
+	// nodes referring to them. Those are listed in /__local_fixups__.
+	DeviceTreeNode *local_fixups = dt_find_node_by_path(overlay,
+					"/__local_fixups__", NULL, NULL, 0);
+	if (local_fixups && dt_fixup_locals(overlay->root, local_fixups,
+					    phandle_base) < 0) {
+		printf("ERROR: invalid local fixups in overlay\n");
+		return -1;
+	}
+
+	// Besides local phandle references (from nodes within the overlay to
+	// other nodes within the overlay), the overlay may also contain phandle
+	// references to the base tree. These are stored with invalid values and
+	// must be updated now. /__symbols__ contains a list of all labels in
+	// the base tree, and /__fixups__ describes all nodes in the overlay
+	// that contain external phandle references.
+	// We also take this opportunity to update all /fragment@X/__overlay__/
+	// prefixes in the overlay's /__symbols__ node to the correct path that
+	// the fragment will be placed in later, since this is the only step
+	// where we have all necessary information for that easily available.
+	DeviceTreeNode *symbols = dt_find_node_by_path(tree, "/__symbols__",
+						       NULL, NULL, 0);
+	DeviceTreeNode *fixups = dt_find_node_by_path(overlay, "/__fixups__",
+						      NULL, NULL, 0);
+	DeviceTreeNode *overlay_symbols = dt_find_node_by_path(overlay,
+						"/__symbols__", NULL, NULL, 0);
+	if (fixups && dt_fixup_all_externals(tree, symbols, overlay,
+					     fixups, overlay_symbols) < 0) {
+		printf("ERROR: cannot match external fixups from overlay\n");
+		return -1;
+	}
+
+	// After all this fixing up, we can finally merge the overlay into the
+	// tree (one fragment at a time, because for some reason it's split up).
+	DeviceTreeNode *fragment;
+	list_for_each(fragment, overlay->root->children, list_node)
+		if (dt_import_fragment(tree, fragment, overlay_symbols) < 0) {
+			printf("ERROR: bad DT fragment '%s'\n", fragment->name);
+			return -1;
+		}
+
+	// We need to also update /__symbols__ to include labels from this
+	// overlay, in case we want to load further overlays with external
+	// phandle references to it. If the base tree already has a /__symbols__
+	// we merge them together, otherwise we just insert the overlay's
+	// /__symbols__ node into the base tree root.
+	if (overlay_symbols) {
+		if (symbols)
+			dt_copy_subtree(symbols, overlay_symbols, 0);
+		else
+			list_insert_after(&overlay_symbols->list_node,
+					  &tree->root->children);
+	}
+
+	return 0;
+}
diff --git a/src/base/device_tree.h b/src/base/device_tree.h
index af73a11..7eee3c5 100644
--- a/src/base/device_tree.h
+++ b/src/base/device_tree.h
@@ -176,6 +176,10 @@
 		      size_t *size);
 const char *dt_find_string_prop(DeviceTreeNode *node, const char *name);
 
+// Apply an overlay FDT blob to an already unpacked device tree. Ownership of
+// overlay_blob passes to the tree -- do not free() or access it afterwards!
+int dt_apply_overlay(DeviceTree *tree, void *overlay_blob);
+
 /*
  * Fixups to apply to a kernel's device tree before booting it.
  */