diff --git a/arch/arm/cpu/armv7/Kconfig b/arch/arm/cpu/armv7/Kconfig index 61e7c82459..b8079dedec 100644 --- a/arch/arm/cpu/armv7/Kconfig +++ b/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 diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c index f2a91f2059..76601eebcb 100644 --- a/arch/arm/cpu/armv7/mx6/soc.c +++ b/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 diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S index 30d81db8b8..3a4ab1d305 100644 --- a/arch/arm/cpu/armv7/nonsec_virt.S +++ b/arch/arm/cpu/armv7/nonsec_virt.S @@ -11,6 +11,12 @@ #include #include #include +#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 @@ -18,6 +24,26 @@ .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 diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c index 2d6b676154..1a17d21fc1 100644 --- a/arch/arm/lib/bootm.c +++ b/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); diff --git a/board/freescale/mx6ul_14x14_evk/mx6ul_14x14_evk.c b/board/freescale/mx6ul_14x14_evk/mx6ul_14x14_evk.c index 149ac2a548..d9ed6f56b2 100644 --- a/board/freescale/mx6ul_14x14_evk/mx6ul_14x14_evk.c +++ b/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; } diff --git a/configs/mx6ul_14x14_evk_optee_defconfig b/configs/mx6ul_14x14_evk_optee_defconfig new file mode 100644 index 0000000000..5f579b028f --- /dev/null +++ b/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 diff --git a/include/configs/mx6ul_14x14_evk.h b/include/configs/mx6ul_14x14_evk.h index 833f2b0772..0272bd0c73 100644 --- a/include/configs/mx6ul_14x14_evk.h +++ b/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