[PATCH v2 5/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>
---
Changes in v2:
- drop unused na and ns
- fixup existing memory nodes if possible
- remove existing memory nodes without @address postfix
- print error reason for invalid device tree blob
---
 include/pbl.h |  1 +
 pbl/fdt.c     | 78 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 79 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..c9820112c6442b65d4f462c93b6847442097c5ab 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,83 @@ void fdt_find_mem(const void *fdt, unsigned long *membase, unsigned long *memsiz
 	while (1);
 }
 
+static int fdt_find_or_add_memory(void *fdt, int parentoffset, const char *name)
+{
+	int err;
+	int node;
+
+	node = fdt_subnode_offset(fdt, parentoffset, name);
+	if (node != -FDT_ERR_NOTFOUND)
+		return node;
+
+	/* Create new memory node */
+	node = fdt_add_subnode(fdt, parentoffset, name);
+	if (node < 0)
+		return node;
+	err = fdt_setprop(fdt, node, "device_type", "memory", sizeof("memory"));
+	if (err < 0)
+		return err;
+
+	return node;
+}
+
+int fdt_fixup_mem(void *fdt, unsigned long membase[], unsigned long memsize[],
+		  size_t num)
+{
+	int node, root;
+	int err;
+	int i;
+
+	err = fdt_check_header(fdt);
+	if (err != 0) {
+		pr_err("Invalid device tree blob: %s\n", fdt_strerror(err));
+		return err;
+	}
+
+	root = fdt_path_offset(fdt, "/");
+	if (root < 0) {
+		pr_err("Cannot find root node: %s\n", fdt_strerror(root));
+		return root;
+	}
+
+	/* Delete memory node without @address postfix */
+	node = fdt_subnode_offset(fdt, root, "memory");
+	if (node >= 0)
+		fdt_del_node(fdt, node);
+
+	for (i = 0; i < num; i++) {
+		unsigned long base = membase[i];
+		unsigned long size = memsize[i];
+		char name[32];
+
+		if (size == 0)
+			continue;
+
+		snprintf(name, sizeof(name), "memory@%lx", base);
+		node = fdt_find_or_add_memory(fdt, root, name);
+		if (!node) {
+			pr_warn("%s: Failed to get node: %s\n",
+				name, fdt_strerror(err));
+			continue;
+		}
+
+		/* Add or rewrite the reg property */
+		fdt_delprop(fdt, node, "reg");
+		err = fdt_appendprop_addrrange(fdt, root, node, "reg",
+					       base, size);
+		if (err < 0) {
+			pr_warn("%s: Failed to set reg property %lx %lx: %s\n",
+				name, base, size, fdt_strerror(err));
+			continue;
+		}
+
+		/* Remove status property to ensure the node is enabled */
+		fdt_delprop(fdt, node, "status");
+	}
+
+	return err;
+}
+
 const void *fdt_device_get_match_data(const void *fdt, const char *nodepath,
 				      const struct fdt_device_id ids[])
 {

-- 
2.39.5