[PATCH 1/2] fsl-layerscape: add dtb overlay feature

[Sahil Malhotra <sahil.malhotra@nxp.com>]

This patch enables the DTB overlay feature for LS platforms.

Signed-off-by: Sahil Malhotra <sahil.malhotra@nxp.com>
---
 arch/arm/cpu/armv8/fsl-layerscape/Makefile    |  1 +
 arch/arm/cpu/armv8/fsl-layerscape/dt_optee.c  | 39 +++++++++++++++++++
 arch/arm/cpu/armv8/fsl-layerscape/dt_optee.h  | 10 +++++
 arch/arm/cpu/armv8/fsl-layerscape/fdt.c       | 12 ++++++
 .../cpu/armv8/fsl-layerscape/lowlevel_init.S  | 25 ++++++++++++
 5 files changed, 87 insertions(+)
 create mode 100644 arch/arm/cpu/armv8/fsl-layerscape/dt_optee.c
 create mode 100644 arch/arm/cpu/armv8/fsl-layerscape/dt_optee.h
 create mode 100644 arch/arm/cpu/armv8/fsl-layerscape/lowlevel_init.S

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Makefile b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
index 598c36ee66..97f1f291dd 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Makefile
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
@@ -7,6 +7,7 @@ obj-y += lowlevel.o
 obj-y += soc.o
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_MP) += mp.o spintable.o
+obj-$(CONFIG_OF_LIBFDT_OVERLAY) += lowlevel_init.o dt_optee.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 endif
 obj-$(CONFIG_SPL) += spl.o
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.c b/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.c
new file mode 100644
index 0000000000..2418ad09c7
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.c
@@ -0,0 +1,39 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2021 NXP
+ */
+#include <common.h>
+#include <errno.h>
+#include <fdt_support.h>
+#include <linux/sizes.h>
+#include "dt_optee.h"
+
+int ft_add_optee_overlay(void *fdt, struct bd_info *bd)
+{
+	int ret = 0;
+
+	/*
+	 * No BL32_BASE passed means no TEE running, so no
+	 * need to add optee node in dts
+	 */
+	if (!rom_pointer[0]) {
+		debug("No BL32_BASE passed means no TEE running\n");
+		return ret;
+	}
+
+	if (rom_pointer[2]) {
+		debug("OP-TEE: applying overlay on 0x%lx\n", rom_pointer[2]);
+		ret = fdt_check_header((void *)rom_pointer[2]);
+		if (ret == 0) {
+			/* Copy the fdt overlay to next 1M and use copied overlay */
+			memcpy((void *)(rom_pointer[2] + SZ_1M), (void *)rom_pointer[2],
+			       fdt_totalsize((void *)rom_pointer[2]));
+			ret = fdt_overlay_apply_verbose(fdt, (void *)(rom_pointer[2] + SZ_1M));
+			if (ret == 0) {
+				debug("Overlay applied with success");
+				fdt_pack(fdt);
+			}
+		}
+	}
+	return ret;
+}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.h b/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.h
new file mode 100644
index 0000000000..d1ff25d531
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/dt_optee.h
@@ -0,0 +1,10 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2021 NXP
+ */
+#ifndef __DT_OPTEE_H__
+#define __DT_OPTEE_H__
+
+extern unsigned long rom_pointer[];
+int ft_add_optee_overlay(void *fdt, struct bd_info *bd);
+#endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
index f1624ff30a..0824c62264 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
@@ -31,6 +31,7 @@
 #endif
 #include <asm/arch/speed.h>
 #include <fsl_qbman.h>
+#include "dt_optee.h"
 
 int fdt_fixup_phy_connection(void *blob, int offset, phy_interface_t phyc)
 {
@@ -698,3 +699,14 @@ void ft_cpu_setup(void *blob, struct bd_info *bd)
 	fdt_fixup_ecam(blob);
 #endif
 }
+
+#ifdef CONFIG_OF_SYSTEM_SETUP
+int ft_system_setup(void *blob, struct bd_info *bd)
+{
+#ifdef CONFIG_OF_LIBFDT_OVERLAY
+	return ft_add_optee_overlay(blob, bd);
+#else
+	return 0;
+#endif
+}
+#endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel_init.S b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel_init.S
new file mode 100644
index 0000000000..1d6a2d85fa
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel_init.S
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2021 NXP
+ */
+
+#include <config.h>
+
+.align 8
+.global rom_pointer
+rom_pointer:
+	.space 32
+
+/*
+ * Routine: save_boot_params (called after reset from start.S)
+ */
+
+.global save_boot_params
+save_boot_params:
+	/* The firmware provided FDT address can be found in r2/x0 */
+	adr	x0, rom_pointer
+	stp	x1, x2, [x0], #16
+	stp	x3, x4, [x0], #16
+
+	ldr	x1, =save_boot_params_ret
+	br	x1
-- 
2.17.1