[PATCH 6/9] PBL: fdt: add fdt_fixup_mem to fixup memory nodes

[Michael Tretter <m.tretter@pengutronix.de>]

Board code in the PBL may use fdt_fixup_mem() to write the base
addresses and sizes of detected SDRAM to the fdt before passing the fdt
to other software like the TF-A and OP-TEE.

This is implemented on the fdt to avoid that the PBL needs to unpack the
device tree, use the of function for manipulation and repack the device
tree.

Signed-off-by: Michael Tretter <m.tretter@pengutronix.de>
---
 include/pbl.h |  1 +
 pbl/fdt.c     | 71 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)

diff --git a/include/pbl.h b/include/pbl.h
index abac3458593af2cec972a54ce9fb69e344179670..b330010562c4aba5fccbb4c421bb95291fa0bea1 100644
--- a/include/pbl.h
+++ b/include/pbl.h
@@ -17,6 +17,7 @@ extern unsigned long free_mem_end_ptr;
 void pbl_barebox_uncompress(void *dest, void *compressed_start, unsigned int len);
 
 void fdt_find_mem(const void *fdt, unsigned long *membase, unsigned long *memsize);
+int fdt_fixup_mem(void *fdt, unsigned long membase[], unsigned long memsize[], size_t num);
 
 struct fdt_device_id {
 	const char *compatible;
diff --git a/pbl/fdt.c b/pbl/fdt.c
index 3b1783152a8cb81f3eda1187b2f7bf998c4addf4..40564952acfcd88bef050966720e76b7378bc720 100644
--- a/pbl/fdt.c
+++ b/pbl/fdt.c
@@ -2,6 +2,7 @@
 #include <linux/libfdt.h>
 #include <pbl.h>
 #include <linux/printk.h>
+#include <stdio.h>
 
 static const __be32 *fdt_parse_reg(const __be32 *reg, uint32_t n,
 				   uint64_t *val)
@@ -73,6 +74,76 @@ void fdt_find_mem(const void *fdt, unsigned long *membase, unsigned long *memsiz
 	while (1);
 }
 
+int fdt_fixup_mem(void *fdt, unsigned long membase[], unsigned long memsize[], size_t num)
+{
+	int na, ns;
+	int node, root;
+	int err;
+	int i;
+
+	err = fdt_check_header(fdt);
+	if (err != 0) {
+		pr_err("Invalid device tree blob\n");
+		return err;
+	}
+
+	root = fdt_path_offset(fdt, "/");
+	if (root < 0) {
+		pr_err("Cannot find root node: %s\n", fdt_strerror(root));
+		return root;
+	}
+
+	na = fdt_address_cells(fdt, root);
+	if (na < 0) {
+		pr_err("Cannot find #address-cells property: %s\n",
+		       fdt_strerror(na));
+		return na;
+	}
+
+	ns = fdt_size_cells(fdt, root);
+	if (ns < 0) {
+		pr_err("Cannot find #size-cells property: %s\n",
+				fdt_strerror(ns));
+		return ns;
+	}
+
+	for (i = 0; i < num; i++) {
+		char name[32];
+
+		if (membase[i] == 0 || memsize[i] == 0)
+			continue;
+
+		snprintf(name, sizeof(name), "memory@%lx", membase[i]);
+		node = fdt_add_subnode(fdt, root, name);
+		if (node < 0) {
+			pr_err("Cannot to add node %s: %s\n",
+			       name, fdt_strerror(node));
+			err = node;
+			break;
+		}
+
+		pr_debug("Add memory node %s (0x%lx, size 0x%lx)\n",
+			 name, membase[i], memsize[i]);
+
+		err = fdt_setprop(fdt, node,
+				  "device_type", "memory", sizeof("memory"));
+		if (err < 0) {
+			pr_err("%s: Cannot set device_type: %s\n",
+			       name, fdt_strerror(err));
+			return err;
+		}
+
+		err = fdt_appendprop_addrrange(fdt, root, node, "reg", membase[i], memsize[i]);
+		if (err < 0) {
+			pr_err("Failed to fixup memory %lx %lx: %s\n",
+			       membase[i], memsize[i], fdt_strerror(err));
+			continue;
+		}
+	}
+
+	return err;
+}
+
 const void *fdt_device_get_match_data(const void *fdt, const char *nodepath,
 				      const struct fdt_device_id ids[])
 {

-- 
2.39.5