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

[Michael Tretter]

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.tret...@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