imx: mx6ulevk: support optee

Use this configuration file, mx6ul_14x14_evk_optee_defconfig

Steps:
make ARCH=arm mx6ul_14x14_evk_optee_defconfig
make ARCH=arm

Flash u-boot.imx to the sd card.

Signed-off-by: Peng Fan <van.freenix@gmail.com>

arch/arm/cpu/armv7/Kconfig

@@ -6,6 +6,13 @@ config CPU_V7_HAS_NONSEC
 config CPU_V7_HAS_VIRT
         bool
 
+config ARMV7_TEE
+	boolean "Enable tee support for booting in non-secure mode" if EXPERT
+	depends on CPU_V7_HAS_NONSEC
+	default y
+	---help---
+	Say Y here to enable support for booting in non-secure / SVC mode.
+
 config ARMV7_NONSEC
 	boolean "Enable support for booting in non-secure mode" if EXPERT
 	depends on CPU_V7_HAS_NONSEC

arch/arm/cpu/armv7/mx6/soc.c

@@ -170,6 +170,18 @@ void init_aips(void)
 #endif
 }
 
+static void init_csu(void)
+{
+#ifdef CONFIG_ARMV7_NONSEC
+	int i;
+	u32 csu = 0x21c0000;
+	/* This is to allow device can be accessed in non-secure world */
+	for (i = 0; i < 40; i ++) {
+		*((u32 *)csu + i) = 0xffffffff;
+	}
+#endif
+}
+
 static void clear_ldo_ramp(void)
 {
 	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
@@ -450,6 +462,8 @@ int arch_cpu_init(void)
 
 	init_aips();
 
+	init_csu();
+
 	/* Need to clear MMDC_CHx_MASK to make warm reset work. */
 	clear_mmdc_ch_mask();
 
@@ -1128,3 +1142,21 @@ void reset_usb_phy1(void)
 	writel(0, USB_PHY0_BASE_ADDR + HW_USBPHY_PWD);
 }
 #endif
+
+#ifdef CONFIG_ARMV7_TEE
+#ifdef CONFIG_MX6UL
+void smp_kick_all_cpus(void)
+{
+        return;
+}
+void smp_set_core_boot_addr(unsigned long addr, int corenr)
+{
+	return;
+}
+
+void smp_waitloop(unsigned previous_address)
+{
+	return;
+}
+#endif
+#endif

arch/arm/cpu/armv7/nonsec_virt.S

@@ -11,13 +11,39 @@
 #include <asm/gic.h>
 #include <asm/armv7.h>
 #include <asm/proc-armv/ptrace.h>
+#ifdef CONFIG_ARMV7_TEE
+#ifndef CONFIG_OPTEE_ENTRY
+#error "error optee entry"
+#endif
+#define OPTEE_ENTRY CONFIG_OPTEE_ENTRY
+#endif
 
 .arch_extension sec
 .arch_extension virt
 
 	.pushsection ._secure.text, "ax"
 
 	.align	5
+/* Ugly code which can only boot smp 2 cores, enough for 7d */
+_regs_save:
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+/* Maybe we can use cpu sync in optee os, but not in uboot */
+_cpu_sync:
+	.word 0
 /* the vector table for secure state and HYP mode */
 _monitor_vectors:
 	.word 0	/* reset */
@@ -113,6 +139,16 @@ ENDPROC(_do_nonsec_entry)
 	add	\addr, \addr, \tmp
 .endm
 
+.macro get_core_pos reg, tmp
+#define MPIDR_CPU_MASK          0xff
+#define MPIDR_CLUSTER_SHIFT     8
+#define MPIDR_CLUSTER_MASK      (0xff << MPIDR_CLUSTER_SHIFT)
+	mrc     p15, 0, \reg, c0, c0, 5
+	and     \tmp, \reg, #MPIDR_CPU_MASK
+	and     \reg, \reg, #MPIDR_CLUSTER_MASK
+	add     \reg, \tmp, \reg, LSR #6
+.endm
+
 #ifndef CONFIG_ARMV7_PSCI
 /*
  * Secondary CPUs start here and call the code for the core specific parts
@@ -121,9 +157,26 @@ ENDPROC(_do_nonsec_entry)
  * Then they go back to wfi and wait to be woken up by the kernel again.
  */
 ENTRY(_smp_pen)
+	/* Remove this in future */
+	ldr sp, =0x80001000
 	cpsid	i
 	cpsid	f
 
+	/* Ugly sync code, use cpu sync in optee os in future */
+wait_cpu0:
+	dsb
+	adr	r0, _cpu_sync
+	ldr	r1, [r0]
+	ldr	r2, =0x55555555
+	cmp	r1, r2
+	bne	wait_cpu0
+
+	adr	r0, _cpu_sync
+	ldr	r1, =0xaaaabbbb
+	str	r1, [r0]
+
+	dsb
+
 	bl	_nonsec_init
 
 	adr	r0, _smp_pen			@ do not use this address again
@@ -177,12 +230,54 @@ ENTRY(_nonsec_init)
 	mcreq	p15, 0, r1, c14, c0, 0		@ write CNTFRQ
 #endif
 
+#ifdef CONFIG_ARMV7_TEE
+	isb
+
+	mov	ip, r0
+	adr	r0, _regs_save
+	str	ip, [r0]
+	add	r0, r0, #4
+
+	str	sp, [r0]
+	add	r0, r0, #4
+
+	stmia	r0!, {r1-r12}
+
+	str	lr, [r0]
+	adr	lr, end_init_optee
+
+	ldr	r0, =OPTEE_ENTRY
+	movs	pc, r0
+	b	.
+end_init_optee:
+#if 0
+	b end_init_optee			@for test use
+#endif
+	adr	lr, _regs_save
+	ldr	r0, [lr]
+	add	lr, lr, #4
+
+	ldr	sp, [lr]
+	add	lr, lr, #4
+
+	ldmfd	lr!, {r1-r12}
+	ldr	lr, [lr]
+
+	adr	r0, _cpu_sync
+	ldr	r1, =0x55555555
+	str	r1, [r0]
+
+	dsb
+
+	bx	lr
+#else
 	adr	r1, _monitor_vectors
 	mcr	p15, 0, r1, c12, c0, 1		@ set MVBAR to secure vectors
 	isb
 
 	mov	r0, r3				@ return GICC address
 	bx	lr
+#endif
 ENDPROC(_nonsec_init)
 
 #ifdef CONFIG_SMP_PEN_ADDR
@@ -197,7 +292,13 @@ ENTRY(smp_waitloop)
 	cmp	r0, r1			@ make sure we dont execute this code
 	beq	smp_waitloop		@ again (due to a spurious wakeup)
 	mov	r0, r1
+#ifdef CONFIG_ARMV7_TEE
+	mrs	r1, cpsr
+	msr	spsr, r1
+	movs	pc, r0
+#else
 	b	_do_nonsec_entry
+#endif
 ENDPROC(smp_waitloop)
 .weak smp_waitloop
 #endif

arch/arm/lib/bootm.c

@@ -307,8 +307,17 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
 #if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
 		if (armv7_boot_nonsec()) {
 			armv7_init_nonsec();
+#if defined(CONFIG_ARMV7_TEE)
+			/*
+			 * Optee os will reopen cache, so cleanup again
+			 * before boot into linux
+			 */
+			cleanup_before_linux();
+			kernel_entry(0, machid, r2);
+#else
 			secure_ram_addr(_do_nonsec_entry)(kernel_entry,
 							  0, machid, r2);
+#endif
 		} else
 #endif
 			kernel_entry(0, machid, r2);

board/freescale/mx6ul_14x14_evk/mx6ul_14x14_evk.c

@@ -253,7 +253,11 @@ struct i2c_pads_info i2c_pad_info1 = {
 
 int dram_init(void)
 {
+#ifdef CONFIG_ARMV7_TEE
+	gd->ram_size = PHYS_SDRAM_SIZE - CONFIG_TEE_RAM_SIZE;
+#else
 	gd->ram_size = PHYS_SDRAM_SIZE;
+#endif
 
 	return 0;
 }

configs/mx6ul_14x14_evk_optee_defconfig

@@ -0,0 +1,5 @@
+CONFIG_SYS_EXTRA_OPTIONS="IMX_CONFIG=board/freescale/mx6ul_14x14_evk/imximage.cfg,MX6UL,TEE_RAM_SIZE=0x4000000,ARMV7_TEE,ARMV7_NONSEC,OPTEE_ENTRY=0x9c100000"
+CONFIG_ARM=y
+CONFIG_TARGET_MX6UL_14X14_EVK=y
+CONFIG_DM=y
+CONFIG_DM_THERMAL=y

include/configs/mx6ul_14x14_evk.h

@@ -20,6 +20,12 @@
 #define CONFIG_DISPLAY_CPUINFO
 #define CONFIG_DISPLAY_BOARDINFO
 
+#ifdef CONFIG_ARMV7_TEE
+#define CONFIG_SMP_PEN_ADDR 0
+#define CONFIG_TIMER_CLK_FREQ		CONFIG_SC_TIMER_CLK
+#define CONFIG_ARMV7_SECURE_BASE	(IRAM_BASE_ADDR + SZ_32K)
+#endif
+
 #if !defined(CONFIG_MX6UL_9X9_LPDDR2)
 /* DCDC used on 14x14 EVK, no PMIC */
 #undef CONFIG_LDO_BYPASS_CHECK