MLK-18036-1 Add dt_overwrite_reg_node() function

dt_overwrite() overwrites specified properties in the device tree.
Properties to overwrite and new properties are also specified
in the device tree.

The format is the following :

overw_str = <&node_1 prop_1 ... prop_n>,
		<&node_2 prop_1 ... prop_n>,
		...
		<&node_n prop_1 ... prop_n>;

prop_n variables are 32bit integers.

Signed-off-by: Clement Faure <clement.faure@nxp.com>

4 files changed, 163 insertions, 2 deletions

diff --git a/core/arch/arm/plat-imx/imx.h b/core/arch/arm/plat-imx/imx.h
index 8fd7a5b7..43538ace 100644
--- a/core/arch/arm/plat-imx/imx.h
+++ b/core/arch/arm/plat-imx/imx.h
@@ -24,6 +24,7 @@
 
 uint32_t imx_get_src_gpr(int cpu);
 void imx_set_src_gpr(int cpu, uint32_t val);
+paddr_t imx_get_ocram_tz_start_addr(void);
 
 bool soc_is_imx6(void);
 bool soc_is_imx6sll(void);
@@ -41,6 +42,12 @@ void imx_gpcv2_set_core1_pup_by_software(void);
 
 void plat_cpu_wakeup_late(void);
 
+#ifdef CFG_DT
+void dt_debug(const char *node_str, const char *prop);
+void dt_overwrite(const char *node_str, const char *prop_str,
+				const char *overw_str, uint32_t size_prop);
+#endif
+
 #ifdef CFG_TZC380
 TEE_Result tzasc_init(void);
 #endif
diff --git a/core/arch/arm/plat-imx/imx_dt.c b/core/arch/arm/plat-imx/imx_dt.c
new file mode 100644
index 00000000..8e1c0241
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx_dt.c
@@ -0,0 +1,146 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018 NXP
+ *
+ */
+#include <trace.h>
+#include <libfdt.h>
+#include <kernel/generic_boot.h>
+#include <imx.h>
+
+#define DT_U32_PROP_SIZE	4
+#define DT_NUM_PROP_MAX		5
+
+/*
+ * dt_overwrite() overwrites specified properties in the device tree.
+ * Properties to overwrite and new properties are also specified in
+ * the device tree.
+ *
+ * The format is the following :
+ *
+ * overw_str = <&node_1 prop_1 ... prop_n>,
+ *		<&node_2 prop_1 ... prop_n>,
+ *		...
+ *		<&node_n prop_1 ... prop_n>;
+ *
+ * prop_n variables are 32bit integers.
+ *
+ * @node_str: Compatible string of optee node.
+ * @prop_str: Property to overwrite (reg, clocks, ...).
+ * @overw_str: Array in node_str containing new values to overwrite property
+ *		specified prop_str.
+ * @size_prop: Size of the property to overwrite.
+ */
+void dt_overwrite(const char *node_str, const char *prop_str,
+				const char *overw_str, uint32_t size_prop)
+{
+	void *fdt, *prop;
+	uint32_t *fdt_overw_node;
+	int offset, phandle, ret, len;
+	uint32_t i, j, sub_node, sub[DT_NUM_PROP_MAX];
+
+	if (size_prop > DT_NUM_PROP_MAX)
+		EMSG("Number of properties to overwrite is too high");
+
+	if (node_str == NULL) {
+		EMSG("Compatible string empty");
+		return;
+	}
+
+	/* Get dtb */
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		EMSG("No DTB found");
+		return;
+	}
+
+	/* Get offset of node_str */
+	offset = fdt_node_offset_by_compatible(fdt, 0, node_str);
+	if (offset < 0) {
+		EMSG("Cannot find %s node in the device tree", node_str);
+		return;
+	}
+
+	/* Get overw_node property */
+	fdt_overw_node = (uint32_t *)fdt_getprop(fdt, offset, overw_str, &len);
+	if (!fdt_overw_node) {
+		DMSG("%s not found in %s", overw_str, node_str);
+		return;
+	}
+	sub_node = len / (size_prop * DT_U32_PROP_SIZE);
+
+	/* For each node to substitute */
+	for (i = 0; i < sub_node; i++) {
+
+		/* fdt_overw_node[0] has to be the phandle */
+		phandle = fdt32_to_cpu(fdt_overw_node[i*size_prop]);
+
+		/* Save new properties starting from fdt_overw_node[1] */
+		for (j = 0; j < size_prop; j++)
+			sub[j] = fdt_overw_node[i*size_prop+j+1];
+
+		/* Get offset based on the phandle */
+		offset = fdt_node_offset_by_phandle(fdt, phandle);
+		if (offset < 0) {
+			EMSG("Cannot find offset based on phandle");
+			return;
+		}
+
+		/* Get size of prop_str */
+		prop = fdt_getprop_w(fdt, offset, prop_str, &len);
+		if (!prop) {
+			DMSG("There is no property %s in the phandle %d",
+							prop_str, phandle);
+			return;
+		}
+
+		/* Replace prop_str property */
+		ret = fdt_setprop_inplace(fdt, offset, prop_str, sub, len);
+		if (ret) {
+			EMSG("Error setprop inplace ret=%d", ret);
+			return;
+		}
+	}
+}
+
+void dt_debug(const char *node_str, const char *prop)
+{
+	void *fdt;
+	uint32_t *fdt_prop;
+	int offset;
+
+	if (node_str == NULL) {
+		EMSG("Compatible string empty");
+		return;
+	}
+
+	/* Get dtb */
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		EMSG("No DTB found");
+		return;
+	}
+
+	/* Get offset of node_str */
+	offset = fdt_node_offset_by_compatible(fdt, 0, node_str);
+	if (offset < 0) {
+		EMSG("Cannot find %s node in the device tree", node_str);
+		return;
+	}
+
+	/* Get overw_clock property */
+	fdt_prop = (uint32_t *)fdt_getprop(fdt, offset, prop, NULL);
+	if (!fdt_prop) {
+		EMSG("%s not found in %s", prop, node_str);
+		return;
+	}
+
+	DMSG(
+		"Node: %s prop %s = 0x%X, 0x%X, 0x%X",
+			node_str,
+			prop,
+			fdt32_to_cpu(fdt_prop[0]),
+			fdt32_to_cpu(fdt_prop[1]),
+			fdt32_to_cpu(fdt_prop[2])
+		);
+}
diff --git a/core/arch/arm/plat-imx/imx_ocram.c b/core/arch/arm/plat-imx/imx_ocram.c
index 1c07bae6..19e9b8f4 100644
--- a/core/arch/arm/plat-imx/imx_ocram.c
+++ b/core/arch/arm/plat-imx/imx_ocram.c
@@ -64,6 +64,12 @@ static TEE_Result init_ocram(void)
 	/* Initialize the Secure OCRAM */
 	init_tz_ocram();
 
+#ifdef CFG_DT
+	/* Move ocram nodes to the OCRAM for Linux */
+	dt_overwrite("fsl,optee-lpm-sram", "reg", "overw_reg", 3);
+	dt_overwrite("fsl,optee-lpm-sram", "clocks", "overw_clock", 3);
+#endif
+
 #ifdef CFG_MX7
 	iram_tlb_phys_addr = TRUSTZONE_OCRAM_START + IRAM_TBL_OFFSET;
 	phys_addr = phys_addr_imx7;
diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk
index 1be0685b..33a6af42 100644
--- a/core/arch/arm/plat-imx/sub.mk
+++ b/core/arch/arm/plat-imx/sub.mk
@@ -20,9 +20,11 @@ ifneq (,$(filter y, $(CFG_MX7) $(CFG_MX7ULP) $(CFG_MX6UL) $(CFG_MX6ULL)))
 srcs-y += a7_plat_init.S
 endif
 
-subdirs-$(CFG_PSCI_ARM32) += pm
 
 srcs-$(CFG_TZC380) += tzasc.c
+srcs-$(CFG_DT) += imx_dt.c
 srcs-$(CFG_CSU) += imx_csu.c
 srcs-$(CFG_SCU) += imx_scu.c
-srcs-$(CFG_IMX_OCRAM) += imx_ocram.c
\ No newline at end of file
+srcs-$(CFG_IMX_OCRAM) += imx_ocram.c
+
+subdirs-$(CFG_PSCI_ARM32) += pm