[PATCH 4/6] arm: mach-k3: am62: Fixup TF-A/OP-TEE reserved-memory node in FDT

[Andrew Davis <afd@ti.com>]

The address we load TF-A and OP-TEE to is configurable by Kconfig
CONFIG_K3_{ATF,OPTEE}_LOAD_ADDR, but the DT nodes reserving this memory
are often statically defined. As these binaries are dynamically loadable,
and in the case of OP-TEE may not even be loaded at all, hard-coding these
addresses is not a hardware description, but rather a configuration.

If the address that U-Boot loaded TF-A or OP-TEE does not match the
address in hard-coded in DT, then fix that node address. This also handles
the case when no reserved memory for these is provided by DT, which is
more correct as explained above.

Add this fixup function, and enable it for AM62.

Signed-off-by: Andrew Davis <afd@ti.com>
---
 arch/arm/mach-k3/am625_fdt.c  |  2 ++
 arch/arm/mach-k3/common_fdt.c | 52 +++++++++++++++++++++++++++++++++++
 arch/arm/mach-k3/common_fdt.h |  2 ++
 3 files changed, 56 insertions(+)

diff --git a/arch/arm/mach-k3/am625_fdt.c b/arch/arm/mach-k3/am625_fdt.c
index 970dd3447de..b26186456f3 100644
--- a/arch/arm/mach-k3/am625_fdt.c
+++ b/arch/arm/mach-k3/am625_fdt.c
@@ -43,6 +43,8 @@ int ft_system_setup(void *blob, struct bd_info *bd)
 	fdt_fixup_cores_nodes_am625(blob, k3_get_core_nr());
 	fdt_fixup_gpu_nodes_am625(blob, k3_has_gpu());
 	fdt_fixup_pru_node_am625(blob, k3_has_pru());
+	fdt_fixup_reserved(blob, "tfa", CONFIG_K3_ATF_LOAD_ADDR, 0x80000);
+	fdt_fixup_reserved(blob, "optee", CONFIG_K3_OPTEE_LOAD_ADDR, 0x1800000);
 
 	return 0;
 }
diff --git a/arch/arm/mach-k3/common_fdt.c b/arch/arm/mach-k3/common_fdt.c
index 645c4de42f7..3bdedd7b509 100644
--- a/arch/arm/mach-k3/common_fdt.c
+++ b/arch/arm/mach-k3/common_fdt.c
@@ -112,3 +112,55 @@ int fdt_del_node_path(void *blob, const char *path)
 
 	return ret;
 }
+
+int fdt_fixup_reserved(void *blob, const char *name,
+		       unsigned int new_address, unsigned int new_size)
+{
+	int nodeoffset, subnode;
+	int ret;
+
+	/* Find reserved-memory */
+	nodeoffset = fdt_subnode_offset(blob, 0, "reserved-memory");
+	if (nodeoffset < 0) {
+		debug("Could not find reserved-memory node\n");
+		return 0;
+	}
+
+	/* Find existing matching subnode and remove it */
+	fdt_for_each_subnode(subnode, blob, nodeoffset) {
+		const char *node_name;
+		fdt_addr_t addr;
+		fdt_size_t size;
+
+		/* Name matching */
+		node_name = fdt_get_name(blob, subnode, NULL);
+		if (!name)
+			return -EINVAL;
+		if (!strncmp(node_name, name, strlen(name))) {
+			/* Read out old size first */
+			addr = fdtdec_get_addr_size(blob, subnode, "reg", &size);
+			if (addr == FDT_ADDR_T_NONE)
+				return -EINVAL;
+			new_size = size;
+
+			/* Delete node */
+			ret = fdt_del_node(blob, subnode);
+			if (ret < 0)
+				return ret;
+
+			/* Only one matching node */
+			break;
+		}
+	}
+
+	struct fdt_memory carveout = {
+		.start = new_address,
+		.end = new_address + new_size - 1,
+	};
+	ret = fdtdec_add_reserved_memory(blob, name, &carveout, NULL, 0, NULL,
+					 FDTDEC_RESERVED_MEMORY_NO_MAP);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
diff --git a/arch/arm/mach-k3/common_fdt.h b/arch/arm/mach-k3/common_fdt.h
index 4d23ae638ca..52c07957483 100644
--- a/arch/arm/mach-k3/common_fdt.h
+++ b/arch/arm/mach-k3/common_fdt.h
@@ -8,5 +8,7 @@
 
 int fdt_fixup_msmc_ram_k3(void *blob);
 int fdt_del_node_path(void *blob, const char *path);
+int fdt_fixup_reserved(void *blob, const char *name,
+		       unsigned int new_address, unsigned int new_size);
 
 #endif /* _COMMON_FDT_H */
-- 
2.39.2