MLK-16753-1 imx8m: add optee node according to runtime status

If TEE is enabled according to rom_pointer[0,1] passed to BL33
from ATF, uboot need to add the optee node in dts to let
Kernel could probe the TEE driver.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
(cherry picked from commit 4341d35cc99a6226f2671e15154d3148ebf49c47)
(cherry picked from commit 609e6607572225bfff9dcd84928fd061c1682669)

1 files changed, 49 insertions, 1 deletions

diff --git a/arch/arm/mach-imx/imx8m/soc.c b/arch/arm/mach-imx/imx8m/soc.c
index 5d1ead9a9c..ed0c24ec58 100644
--- a/arch/arm/mach-imx/imx8m/soc.c
+++ b/arch/arm/mach-imx/imx8m/soc.c
@@ -345,6 +345,54 @@ bool is_usb_boot(void)
 }
 
 #ifdef CONFIG_OF_SYSTEM_SETUP
+static int ft_add_optee_node(void *fdt, bd_t *bd)
+{
+	const char *path, *subpath;
+	int offs;
+
+	/*
+	 * No TEE space allocated indicating no TEE running, so no
+	 * need to add optee node in dts
+	 */
+	if (!rom_pointer[1])
+		return 0;
+
+	offs = fdt_increase_size(fdt, 512);
+	if (offs) {
+		printf("No Space for dtb\n");
+		return 1;
+	}
+
+	path = "/firmware";
+	offs = fdt_path_offset(fdt, path);
+	if (offs < 0) {
+		path = "/";
+		offs = fdt_path_offset(fdt, path);
+
+		if (offs < 0) {
+			printf("Could not find root node.\n");
+			return 1;
+		}
+
+		subpath = "firmware";
+		offs = fdt_add_subnode(fdt, offs, subpath);
+		if (offs < 0) {
+			printf("Could not create %s node.\n", subpath);
+		}
+	}
+
+	subpath = "optee";
+	offs = fdt_add_subnode(fdt, offs, subpath);
+	if (offs < 0) {
+		printf("Could not create %s node.\n", subpath);
+	}
+
+	fdt_setprop_string(fdt, offs, "compatible", "linaro,optee-tz");
+	fdt_setprop_string(fdt, offs, "method", "smc");
+
+	return 0;
+}
+
 int ft_system_setup(void *blob, bd_t *bd)
 {
 	int i = 0;
@@ -439,7 +487,7 @@ usb_modify_speed:
 		}
 	}
 
-	return 0;
+	return ft_add_optee_node(blob, bd);
 }
 #endif