[PATCH 6/8] pbl: fdt: add support to parse reserved mem

[Rouven Czerwinski]

This allows the PBL to fill a reserved memory map which a subsequent
commit will use modify the early MMU mapping.

Signed-off-by: Rouven Czerwinski <r.czerwinski at pengutronix.de>
---
 include/pbl.h | 15 ++++++++++
 pbl/fdt.c     | 80 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 95 insertions(+)

diff --git a/include/pbl.h b/include/pbl.h
index 194d5e7508..df0f41ac5b 100644
--- a/include/pbl.h
+++ b/include/pbl.h
@@ -34,4 +34,19 @@ ssize_t pbl_fat_load(struct pbl_bio *, const char *filename, void *dest, size_t
 
 void fdt_find_mem(const void *fdt, unsigned long *membase, unsigned long *memsize);
 
+struct pbl_reserved_memory {
+	phys_addr_t			base;
+	phys_addr_t			size;
+	unsigned int			flag;
+};
+
+#define MAX_RESERVED_REGIONS	8
+
+#define FDT_RES_MEM_FLAG_NOMAP		BIT(0)
+
+void fdt_fill_reserve_mem(const void *fdt);
+
+struct pbl_reserved_memory* get_pbl_reserved_memory(void);
+int get_pbl_reserved_memory_num(void);
+
 #endif /* __PBL_H__ */
diff --git a/pbl/fdt.c b/pbl/fdt.c
index b4a40a514b..f8e301843f 100644
--- a/pbl/fdt.c
+++ b/pbl/fdt.c
@@ -68,3 +68,83 @@ err:
 	pr_err("No memory, cannot continue\n");
 	while (1);
 }
+
+struct pbl_reserved_memory reserved_mem[MAX_RESERVED_REGIONS];
+int reserved_mem_count;
+
+void fdt_fill_reserve_mem(const void *fdt)
+{
+	const __be32 *nap, *nsp, *reg;
+	uint32_t na, ns;
+	int node, size, i, parent;
+	const void * prop;
+	uint64_t memsize64, membase64;
+	bool nomap;
+
+	/* Make sure FDT blob is sane */
+	if (fdt_check_header(fdt) != 0) {
+		pr_err("Invalid device tree blob\n");
+		return;
+	}
+
+	parent = fdt_path_offset(fdt, "/reserved-memory");
+	if (parent < 0) {
+		pr_info("Cannot find reserved-memory node\n");
+		return;
+	}
+
+	nap = fdt_getprop(fdt, parent, "#address-cells", &size);
+	if (!nap || (size != 4)) {
+		pr_err("Cannot find #address-cells property");
+		return;
+	}
+	na = fdt32_to_cpu(*nap);
+
+	nsp = fdt_getprop(fdt, parent, "#size-cells", &size);
+	if (!nsp || (size != 4)) {
+		pr_err("Cannot find #size-cells property");
+		return;
+	}
+	ns = fdt32_to_cpu(*nap);
+
+	fdt_for_each_subnode(node, fdt, parent) {
+		nomap = true;
+		reg = fdt_getprop(fdt, node, "reg", &size);
+		if (size < (na + ns) * sizeof(u32)) {
+			pr_err("cannot get memory range\n");
+			return;
+		}
+
+		membase64 = 0;
+		for (i = 0; i < na; i++)
+			membase64 = (membase64 << 32) | fdt32_to_cpu(*reg++);
+
+		/* get the memsize and truncate it to under 4G on 32 bit machines */
+		memsize64 = 0;
+		for (i = 0; i < ns; i++)
+			memsize64 = (memsize64 << 32) | fdt32_to_cpu(*reg++);
+
+		prop = fdt_getprop(fdt, node, "nomap", NULL);
+		if (!prop)
+			nomap = false;
+
+		reserved_mem[reserved_mem_count].base = membase64;
+		reserved_mem[reserved_mem_count].size = memsize64;
+		if (nomap)
+			reserved_mem[reserved_mem_count].flag |= FDT_RES_MEM_FLAG_NOMAP;
+		reserved_mem_count++;
+	}
+
+	if ((node < 0) && (node != -FDT_ERR_NOTFOUND)) {
+		pr_err("Error while parsing reserved-memory nodes: %d\n", node);
+		return;
+	}
+}
+
+struct pbl_reserved_memory* get_pbl_reserved_memory(void) {
+	return (struct pbl_reserved_memory*)&reserved_mem;
+}
+
+int get_pbl_reserved_memory_num(void) {
+	return reserved_mem_count;
+}
-- 
2.31.1