arm: mach-k3: am62: Fixup TF-A/OP-TEE reserved-memory node in FDT

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>
Acked-by: Bryan Brattlof <bb@ti.com>
Reviewed-by: Neha Malcom Francis <n-francis@ti.com>

3 files changed, 56 insertions, 0 deletions

diff --git a/arch/arm/mach-k3/am625_fdt.c b/arch/arm/mach-k3/am625_fdt.c
index 970dd34..b261864 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 645c4de..3bdedd7 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 4d23ae6..52c0795 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 */