[U-Boot] [PATCH 4/6] fdtdec: Implement fdtdec_add_reserved_memory()

[Thierry Reding]

From: Thierry Reding <treding at nvidia.com>

This function can be used to add subnodes in the /reserved-memory node.

Signed-off-by: Thierry Reding <treding at nvidia.com>
---
 include/fdtdec.h |  17 +++++
 lib/fdtdec.c     | 158 +++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 175 insertions(+)

diff --git a/include/fdtdec.h b/include/fdtdec.h
index 997103a87cdf..5c9108ced571 100644
--- a/include/fdtdec.h
+++ b/include/fdtdec.h
@@ -979,6 +979,23 @@ int fdtdec_get_max_phandle(const void *blob, uint32_t *maxp);
  */
 int fdtdec_set_phandle(void *blob, int node, uint32_t phandle);
 
+/**
+ * fdtdec_add_reserved_memory() - add or find a reserved-memory node
+ *
+ * If a reserved-memory node already exists for the given carveout, a phandle
+ * for that node will be returned. Otherwise a new node will be created and a
+ * phandle corresponding to it will be returned.
+ *
+ * @param blob		FDT blob
+ * @param basename	base name of the node to create
+ * @param carveout	information about the carveout region
+ * @param phandlep	return location for the phandle of the carveout region
+ * @return 0 on success or a negative error code on failure
+ */
+int fdtdec_add_reserved_memory(void *blob, const char *basename,
+			       const struct fdt_memory *carveout,
+			       uint32_t *phandlep);
+
 /**
  * Set up the device tree ready for use
  */
diff --git a/lib/fdtdec.c b/lib/fdtdec.c
index 9195a05d1129..a8b35c144ae0 100644
--- a/lib/fdtdec.c
+++ b/lib/fdtdec.c
@@ -1287,6 +1287,164 @@ int fdtdec_set_phandle(void *blob, int node, uint32_t phandle)
 	return 0;
 }
 
+static int fdtdec_init_reserved_memory(void *blob)
+{
+	int na, ns, node, err;
+	fdt32_t value;
+
+	/* inherit #address-cells and #size-cells from the root node */
+	na = fdt_address_cells(blob, 0);
+	ns = fdt_size_cells(blob, 0);
+
+	node = fdt_add_subnode(blob, 0, "reserved-memory");
+	if (node < 0)
+		return node;
+
+	err = fdt_setprop(blob, node, "ranges", NULL, 0);
+	if (err < 0)
+		return err;
+
+	value = cpu_to_fdt32(na);
+
+	err = fdt_setprop(blob, node, "#address-cells", &value, sizeof(value));
+	if (err < 0)
+		return err;
+
+	value = cpu_to_fdt32(ns);
+
+	err = fdt_setprop(blob, node, "#size-cells", &value, sizeof(value));
+	if (err < 0)
+		return err;
+
+	return node;
+}
+
+static void fdt_addr_unpack(fdt_addr_t addr, fdt32_t *upper, fdt32_t *lower)
+{
+#ifdef CONFIG_PHYS_64BIT
+	*upper = addr >> 32;
+#else
+	*upper = 0;
+#endif
+
+	*lower = addr;
+}
+
+static void fdt_size_unpack(fdt_size_t size, fdt32_t *upper, fdt32_t *lower)
+{
+#ifdef CONFIG_PHYS_64BIT
+	*upper = size >> 32;
+#else
+	*upper = 0;
+#endif
+
+	*lower = size;
+}
+
+int fdtdec_add_reserved_memory(void *blob, const char *basename,
+			       const struct fdt_memory *carveout,
+			       uint32_t *phandlep)
+{
+	fdt32_t cells[4] = {}, *ptr = cells;
+	uint32_t upper, lower, phandle;
+	int parent, node, na, ns, err;
+	char name[64];
+
+	/* create an empty /reserved-memory node if one doesn't exist */
+	parent = fdt_path_offset(blob, "/reserved-memory");
+	if (parent < 0) {
+		parent = fdtdec_init_reserved_memory(blob);
+		if (parent < 0)
+			return parent;
+	}
+
+	/* only 1 or 2 #address-cells and #size-cells are supported */
+	na = fdt_address_cells(blob, parent);
+	if (na < 1 || na > 2)
+		return -FDT_ERR_BADNCELLS;
+
+	ns = fdt_address_cells(blob, parent);
+	if (ns < 1 || ns > 2)
+		return -FDT_ERR_BADNCELLS;
+
+	/* find a matching node and return the phandle to that */
+	fdt_for_each_subnode(node, blob, parent) {
+		const char *name = fdt_get_name(blob, node, NULL);
+		phys_addr_t addr, size;
+
+		addr = fdtdec_get_addr_size(blob, node, "reg", &size);
+		if (addr == FDT_ADDR_T_NONE) {
+			printf("failed to read address/size for %s\n", name);
+			continue;
+		}
+
+		if (addr == carveout->start && (addr + size) == carveout->end) {
+			*phandlep = fdt_get_phandle(blob, node);
+			return 0;
+		}
+	}
+
+	/*
+	 * Unpack the start address and generate the name of the new node
+	 * base on the basename and the unit-address.
+	 */
+	fdt_addr_unpack(carveout->start, &upper, &lower);
+
+	if (na > 1)
+		snprintf(name, sizeof(name), "%s@%x,%x", basename, upper,
+			 lower);
+	else {
+		if (upper) {
+			printf("address %08x:%08x exceeds addressable space\n",
+			       upper, lower);
+			return -FDT_ERR_BADVALUE;
+		}
+
+		snprintf(name, sizeof(name), "%s@%x", basename, lower);
+	}
+
+	node = fdt_add_subnode(blob, parent, name);
+	if (node < 0)
+		return node;
+
+	/*
+	 * Generate a new phandle for the reserved-memory node. Look up the
+	 * highest phandle number currently in used and use the next higher
+	 * one.
+	 */
+	err = fdtdec_get_max_phandle(blob, &phandle);
+	if (err < 0)
+		return err;
+
+	err = fdtdec_set_phandle(blob, node, phandle + 1);
+	if (err < 0)
+		return err;
+
+	/* store one or two address cells */
+	if (na > 1)
+		*ptr++ = cpu_to_fdt32(upper);
+
+	*ptr++ = cpu_to_fdt32(lower);
+
+	/* store one or two size cells */
+	fdt_size_unpack(carveout->end - carveout->start, &upper, &lower);
+
+	if (ns > 1)
+		*ptr++ = cpu_to_fdt32(upper);
+
+	*ptr++ = cpu_to_fdt32(lower);
+
+	err = fdt_setprop(blob, node, "reg", cells, ptr - cells);
+	if (err < 0)
+		return err;
+
+	/* return the phandle for the new node for the caller to use */
+	if (phandlep)
+		*phandlep = phandle + 1;
+
+	return 0;
+}
+
 int fdtdec_setup(void)
 {
 #if CONFIG_IS_ENABLED(OF_CONTROL)
-- 
2.20.1