Merge branch 'for-next/optee'

9 files changed, 172 insertions, 10 deletions

diff --git a/arch/arm/include/asm/barebox-arm.h b/arch/arm/include/asm/barebox-arm.h
index 8b2ecd9ab2..599852d644 100644
--- a/arch/arm/include/asm/barebox-arm.h
+++ b/arch/arm/include/asm/barebox-arm.h
@@ -119,7 +119,7 @@ void *barebox_arm_boot_dtb(void);
 static inline unsigned long arm_mem_stack_top(unsigned long membase,
 					      unsigned long endmem)
 {
-	if (IS_ENABLED(CONFIG_BOOTM_OPTEE))
+	if (IS_ENABLED(CONFIG_BOOTM_OPTEE) || IS_ENABLED(CONFIG_PBL_OPTEE))
 		endmem -= OPTEE_SIZE;
 
 	return endmem - SZ_64K;
diff --git a/arch/arm/include/asm/setjmp.h b/arch/arm/include/asm/setjmp.h
new file mode 100644
index 0000000000..62bac613d6
--- /dev/null
+++ b/arch/arm/include/asm/setjmp.h
@@ -0,0 +1,29 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * (C) Copyright 2017 Theobroma Systems Design und Consulting GmbH
+ * (C) Copyright 2016 Alexander Graf <agraf@suse.de>
+ */
+
+#ifndef _SETJMP_H_
+#define _SETJMP_H_	1
+
+#include <asm/types.h>
+
+/*
+ * This really should be opaque, but the EFI implementation wrongly
+ * assumes that a 'struct jmp_buf_data' is defined.
+ */
+struct jmp_buf_data {
+#if defined(__aarch64__)
+	u64  regs[13];
+#else
+	u32  regs[10];  /* r4-r9, sl, fp, sp, lr */
+#endif
+};
+
+typedef struct jmp_buf_data jmp_buf[1];
+
+int setjmp(jmp_buf jmp);
+void longjmp(jmp_buf jmp, int ret);
+
+#endif /* _SETJMP_H_ */
diff --git a/arch/arm/lib32/Makefile b/arch/arm/lib32/Makefile
index cd43147e66..597bc07905 100644
--- a/arch/arm/lib32/Makefile
+++ b/arch/arm/lib32/Makefile
@@ -28,3 +28,7 @@ extra-y += barebox.lds
 pbl-y	+= lib1funcs.o
 pbl-y	+= ashldi3.o
 pbl-y	+= div0.o
+
+obj-pbl-y	+= setjmp.o
+
+pbl-$(CONFIG_PBL_OPTEE)	+= optee-early.o
diff --git a/arch/arm/lib32/bootm.c b/arch/arm/lib32/bootm.c
index 180624445d..d64e705c40 100644
--- a/arch/arm/lib32/bootm.c
+++ b/arch/arm/lib32/bootm.c
@@ -137,16 +137,10 @@ static int get_kernel_addresses(size_t image_size,
 static int optee_verify_header_request_region(struct image_data *data, struct optee_header *hdr)
 {
 	int ret = 0;
-	if (hdr->magic != OPTEE_MAGIC) {
-		pr_err("Invalid header magic 0x%08x, expected 0x%08x\n",
-		       hdr->magic, OPTEE_MAGIC);
-		return -EINVAL;
-	}
 
-	if (hdr->arch != OPTEE_ARCH_ARM32 || hdr->init_load_addr_hi) {
-		pr_err("Only 32bit supported\n");
-		return -EINVAL;
-	}
+	ret = optee_verify_header(hdr);
+	if (ret < 0)
+		return ret;
 
 	data->tee_res = request_sdram_region("TEE", hdr->init_load_addr_lo, hdr->init_size);
 	if (!data->tee_res) {
diff --git a/arch/arm/lib32/optee-early.c b/arch/arm/lib32/optee-early.c
new file mode 100644
index 0000000000..197325b8a0
--- /dev/null
+++ b/arch/arm/lib32/optee-early.c
@@ -0,0 +1,38 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * optee-early.c - start OP-TEE during PBL
+ *
+ * Copyright (c) 2019 Rouven Czerwinski <r.czerwinski@pengutronix.de>, Pengutronix
+ *
+ */
+#include <asm/cache.h>
+#include <asm/setjmp.h>
+#include <tee/optee.h>
+#include <debug_ll.h>
+
+static jmp_buf tee_buf;
+
+int start_optee_early(void *fdt, void *tee)
+{
+	void (*tee_start)(void *r0, void *r1, void *r2);
+	struct optee_header *hdr;
+	int ret;
+
+	hdr = tee;
+	ret = optee_verify_header(hdr);
+	if (ret < 0)
+		return ret;
+
+	memcpy((void *)hdr->init_load_addr_lo, tee + sizeof(*hdr), hdr->init_size);
+	tee_start = (void *) hdr->init_load_addr_lo;
+
+	/* We use setjmp/longjmp here because OP-TEE clobbers most registers */
+	ret = setjmp(tee_buf);
+	if (ret == 0) {
+		sync_caches_for_execution();
+		tee_start(0, 0, fdt);
+		longjmp(tee_buf, 1);
+	}
+
+	return 0;
+}
diff --git a/arch/arm/lib32/setjmp.S b/arch/arm/lib32/setjmp.S
new file mode 100644
index 0000000000..f0606a7f66
--- /dev/null
+++ b/arch/arm/lib32/setjmp.S
@@ -0,0 +1,36 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * (C) 2017 Theobroma Systems Design und Consulting GmbH
+ */
+
+#include <config.h>
+#include <asm/assembler.h>
+#include <linux/linkage.h>
+
+.pushsection .text.setjmp, "ax"
+ENTRY(setjmp)
+	/*
+	 * A subroutine must preserve the contents of the registers
+	 * r4-r8, r10, r11 (v1-v5, v7 and v8) and SP (and r9 in PCS
+	 * variants that designate r9 as v6).
+	 */
+	mov  ip, sp
+	stm  a1, {v1-v8, ip, lr}
+	mov  a1, #0
+	bx   lr
+ENDPROC(setjmp)
+.popsection
+
+.pushsection .text.longjmp, "ax"
+ENTRY(longjmp)
+	ldm  a1, {v1-v8, ip, lr}
+	mov  sp, ip
+	mov  a1, a2
+	/* If we were passed a return value of zero, return one instead */
+	cmp  a1, #0
+	bne  1f
+	mov  a1, #1
+1:
+	bx   lr
+ENDPROC(longjmp)
+.popsection
diff --git a/arch/arm/lib64/Makefile b/arch/arm/lib64/Makefile
index 5068431342..69cb3d8ea1 100644
--- a/arch/arm/lib64/Makefile
+++ b/arch/arm/lib64/Makefile
@@ -5,5 +5,6 @@ obj-$(CONFIG_ARM_OPTIMZED_STRING_FUNCTIONS)	+= memcpy.o
 obj-$(CONFIG_ARM_OPTIMZED_STRING_FUNCTIONS)	+= memset.o string.o
 extra-y += barebox.lds
 obj-pbl-y   += runtime-offset.o
+obj-pbl-y   += setjmp.o
 
 pbl-y	+= div0.o pbl.o
diff --git a/arch/arm/lib64/setjmp.S b/arch/arm/lib64/setjmp.S
new file mode 100644
index 0000000000..a7f6d05417
--- /dev/null
+++ b/arch/arm/lib64/setjmp.S
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * (C) 2017 Theobroma Systems Design und Consulting GmbH
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+.pushsection .text.setjmp, "ax"
+ENTRY(setjmp)
+	/* Preserve all callee-saved registers and the SP */
+	stp  x19, x20, [x0,#0]
+	stp  x21, x22, [x0,#16]
+	stp  x23, x24, [x0,#32]
+	stp  x25, x26, [x0,#48]
+	stp  x27, x28, [x0,#64]
+	stp  x29, x30, [x0,#80]
+	mov  x2, sp
+	str  x2, [x0, #96]
+	mov  x0, #0
+	ret
+ENDPROC(setjmp)
+.popsection
+
+.pushsection .text.longjmp, "ax"
+ENTRY(longjmp)
+	ldp  x19, x20, [x0,#0]
+	ldp  x21, x22, [x0,#16]
+	ldp  x23, x24, [x0,#32]
+	ldp  x25, x26, [x0,#48]
+	ldp  x27, x28, [x0,#64]
+	ldp  x29, x30, [x0,#80]
+	ldr  x2, [x0,#96]
+	mov  sp, x2
+	/* Move the return value in place, but return 1 if passed 0. */
+	adds x0, xzr, x1
+	csinc x0, x0, xzr, ne
+	ret
+ENDPROC(longjmp)
+.popsection
diff --git a/arch/arm/mach-imx/imx6.c b/arch/arm/mach-imx/imx6.c
index 8f949534b8..6a9ea23c71 100644
--- a/arch/arm/mach-imx/imx6.c
+++ b/arch/arm/mach-imx/imx6.c
@@ -11,6 +11,7 @@
  *
  */
 
+#include <abort.h>
 #include <init.h>
 #include <common.h>
 #include <io.h>
@@ -288,6 +289,22 @@ int imx6_devices_init(void)
 	return 0;
 }
 
+static bool imx6_cannot_write_l2x0(void)
+{
+	void __iomem *l2x0_base = IOMEM(0x00a02000);
+	u32 val;
+	/*
+	 * Mask data aborts and try to access the PL210. If OP-TEE is running we
+	 * will receive a data-abort and assume barebox is running in the normal
+	 * world.
+	 */
+	val = readl(l2x0_base + L2X0_PREFETCH_CTRL);
+
+	data_abort_mask();
+	writel(val, l2x0_base + L2X0_PREFETCH_CTRL);
+	return data_abort_unmask();
+}
+
 static int imx6_mmu_init(void)
 {
 	void __iomem *l2x0_base = IOMEM(0x00a02000);
@@ -296,6 +313,9 @@ static int imx6_mmu_init(void)
 	if (!cpu_is_mx6())
 		return 0;
 
+	if (imx6_cannot_write_l2x0())
+		return 0;
+
 	val = readl(l2x0_base + L2X0_CACHE_ID);
 	cache_part = val & L2X0_CACHE_ID_PART_MASK;
 	cache_rtl  = val & L2X0_CACHE_ID_RTL_MASK;