Merge branch 'for-next/rpi3'

8 files changed, 163 insertions, 14 deletions

diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 874d723e2c..a35db435c1 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -4,6 +4,9 @@ obj-$(CONFIG_ARM_EXCEPTIONS) += exceptions$(S64).o interrupts$(S64).o
 obj-$(CONFIG_MMU) += mmu$(S64).o
 lwl-y += lowlevel$(S64).o
 obj-pbl-$(CONFIG_MMU) += mmu-early$(S64).o
+obj-pbl-$(CONFIG_CPU_32v7) += hyp.o
+AFLAGS_hyp.o :=-Wa,-march=armv7-a -Wa,-mcpu=all
+AFLAGS_pbl-hyp.o :=-Wa,-march=armv7-a -Wa,-mcpu=all
 
 obj-y += start.o entry.o
 
@@ -23,6 +26,7 @@ endif
 obj-$(CONFIG_ARM_PSCI) += psci.o
 obj-pbl-$(CONFIG_ARM_SMCCC) += smccc-call$(S64).o
 AFLAGS_smccc-call$(S64).o :=-Wa,-march=armv$(if $(S64),8,7)-a
+AFLAGS_pbl-smccc-call$(S64).o :=-Wa,-march=armv$(if $(S64),8,7)-a
 obj-$(CONFIG_ARM_SECURE_MONITOR) += sm.o sm_as.o
 AFLAGS_sm_as.o		:=-Wa,-march=armv7-a
 
diff --git a/arch/arm/cpu/hyp.S b/arch/arm/cpu/hyp.S
new file mode 100644
index 0000000000..1314b56eab
--- /dev/null
+++ b/arch/arm/cpu/hyp.S
@@ -0,0 +1,116 @@
+#include <linux/linkage.h>
+#include <asm/system.h>
+#include <asm/opcodes-virt.h>
+#include <init.h>
+
+.arch_extension sec
+.arch_extension virt
+
+__BARE_INIT
+
+.data
+	.align  2
+ENTRY(__boot_cpu_mode)
+	.long   0
+.text
+
+ENTRY(__hyp_install)
+	mrs	r12, cpsr
+	and	r12, r12, #MODE_MASK
+
+	@ Save the initial CPU state
+	adr	r0, .L__boot_cpu_mode_offset
+	ldr	r1, [r0]
+	str	r12, [r0, r1]
+
+	cmp	r12, #HYP_MODE
+	movne	pc, lr			@ give up if the CPU is not in HYP mode
+
+	@ Now install the hypervisor stub:
+	adr	r12, __hyp_vectors
+	mcr	p15, 4, r12, c12, c0, 0	@ set hypervisor vector base (HVBAR)
+
+	@ Disable all traps, so we don't get any nasty surprise
+	mov	r12, #0
+	mcr	p15, 4, r12, c1, c1, 0	@ HCR
+	mcr	p15, 4, r12, c1, c1, 2	@ HCPTR
+	mcr	p15, 4, r12, c1, c1, 3	@ HSTR
+
+THUMB(	orr	r12, #(1 << 30)	)	@ HSCTLR.TE
+	mcr	p15, 4, r12, c1, c0, 0	@ HSCTLR
+
+	mrc	p15, 4, r12, c1, c1, 1	@ HDCR
+	and	r12, #0x1f		@ Preserve HPMN
+	mcr	p15, 4, r12, c1, c1, 1	@ HDCR
+
+	@ Make sure NS-SVC is initialised appropriately
+	mrc	p15, 0, r12, c1, c0, 0	@ SCTLR
+	orr	r12, #(1 << 5)		@ CP15 barriers enabled
+	bic	r12, #(3 << 7)		@ Clear SED/ITD for v8 (RES0 for v7)
+	bic	r12, #(3 << 19)		@ WXN and UWXN disabled
+	mcr	p15, 0, r12, c1, c0, 0	@ SCTLR
+
+	mrc	p15, 0, r12, c0, c0, 0	@ MIDR
+	mcr	p15, 4, r12, c0, c0, 0	@ VPIDR
+
+	mrc	p15, 0, r12, c0, c0, 5	@ MPIDR
+	mcr	p15, 4, r12, c0, c0, 5	@ VMPIDR
+	bx	lr
+ENDPROC(__hyp_install)
+
+ENTRY(armv7_hyp_install)
+	mov	r2, lr
+
+	bl	__hyp_install
+
+	/* set the cpu to SVC32 mode, mask irq and fiq */
+	mrs	r12, cpsr
+	eor	r12, r12, #HYP_MODE
+	tst	r12, #MODE_MASK
+	bic	r12, r12, #MODE_MASK
+	orr	r12, r12, #(PSR_I_BIT | PSR_F_BIT | SVC_MODE)
+THUMB(	orr	r12, r12, #PSR_T_BIT	)
+	bne	1f
+	orr	r12, r12, #PSR_A_BIT
+	adr	lr, 2f
+	msr	spsr_cxsf, r12
+	__MSR_ELR_HYP(14)
+	__ERET
+1:	msr	cpsr_c, r12
+2:
+	mov	pc, r2
+ENDPROC(armv7_hyp_install)
+
+ENTRY(armv7_switch_to_hyp)
+	mov	r0, lr
+	mov	r1, sp		@ save SVC copy of LR and SP
+	isb
+	hvc #0			@ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
+	mov	sp, r1
+	mov	lr, r0		@ restore SVC copy of LR and SP
+
+	bx	lr
+ENDPROC(armv7_switch_to_hyp)
+
+.align 2
+.L__boot_cpu_mode_offset:
+	.long	__boot_cpu_mode - .
+
+/* The HYP trap is crafted to match armv7_switch_to_hyp() */
+__hyp_do_trap:
+	mov lr, r0
+	mov sp, r1
+	bx  lr
+ENDPROC(__hyp_do_trap)
+
+.align 5
+__hyp_vectors:
+__hyp_reset:	W(b)	.
+__hyp_und:	W(b)	.
+__hyp_svc:	W(b)	.
+__hyp_pabort:	W(b)	.
+__hyp_dabort:	W(b)	.
+__hyp_trap:	W(b)	__hyp_do_trap
+__hyp_irq:	W(b)	.
+__hyp_fiq:	W(b)	.
+ENDPROC(__hyp_vectors)
diff --git a/arch/arm/cpu/lowlevel.S b/arch/arm/cpu/lowlevel.S
index 7696a198e7..3b52644e43 100644
--- a/arch/arm/cpu/lowlevel.S
+++ b/arch/arm/cpu/lowlevel.S
@@ -1,16 +1,33 @@
 #include <linux/linkage.h>
 #include <init.h>
 #include <asm/system.h>
+#include <asm/opcodes-virt.h>
 
 .section ".text_bare_init_","ax"
 ENTRY(arm_cpu_lowlevel_init)
 	/* save lr, since it may be banked away with a processor mode change */
 	mov	r2, lr
+
+#ifdef CONFIG_CPU_32v7
+	/* careful: the hyp install corrupts r0 and r1 */
+	bl	__hyp_install
+#endif
+
 	/* set the cpu to SVC32 mode, mask irq and fiq */
 	mrs	r12, cpsr
-	bic	r12, r12, #0x1f
-	orr	r12, r12, #0xd3
-	msr	cpsr, r12
+	eor	r12, r12, #HYP_MODE
+	tst	r12, #MODE_MASK
+	bic	r12, r12, #MODE_MASK
+	orr	r12, r12, #(PSR_I_BIT | PSR_F_BIT | SVC_MODE)
+THUMB(	orr	r12, r12, #PSR_T_BIT	)
+	bne	1f
+	orr	r12, r12, #PSR_A_BIT
+	adr	lr, 2f
+	msr	spsr_cxsf, r12
+	__MSR_ELR_HYP(14)
+	__ERET
+1:	msr	cpsr_c, r12
+2:
 
 #if __LINUX_ARM_ARCH__ >= 6
 	/*
diff --git a/arch/arm/cpu/sm.c b/arch/arm/cpu/sm.c
index cb0bac106b..6fad30adab 100644
--- a/arch/arm/cpu/sm.c
+++ b/arch/arm/cpu/sm.c
@@ -184,6 +184,9 @@ int armv7_secure_monitor_install(void)
 		return -EINVAL;
 	}
 
+	if (__boot_cpu_mode == HYP_MODE)
+		return 0;
+
 	mmuon = get_cr() & CR_M;
 
 	vbar = get_vbar();
@@ -259,6 +262,9 @@ static int sm_init(void)
 				  bootm_secure_state_names,
 				  ARRAY_SIZE(bootm_secure_state_names));
 
+	if (__boot_cpu_mode == HYP_MODE)
+		bootm_secure_state = ARM_STATE_HYP;
+
 	return 0;
 }
 device_initcall(sm_init);
\ No newline at end of file
diff --git a/arch/arm/cpu/sm_as.S b/arch/arm/cpu/sm_as.S
index 09580e75de..de6cd0406f 100644
--- a/arch/arm/cpu/sm_as.S
+++ b/arch/arm/cpu/sm_as.S
@@ -129,7 +129,9 @@ secure_monitor:
 	sub	sp, sp, #4*4			@ allocate result structure on stack
 	mov	r12, sp
 	push	{r4-r6, r12}
+#ifdef CONFIG_ARM_PSCI
 	bl	psci_entry
+#endif
 	pop	{r4-r6, r12}
 	ldm	r12, {r0-r3}
 	add	sp, sp, #4*4
@@ -146,23 +148,14 @@ hyp_trap:
         mov pc, lr                              @ do no switch modes, but
                                                 @ return to caller
 
-ENTRY(armv7_switch_to_hyp)
-	mov	r0, lr
-	mov	r1, sp		@ save SVC copy of LR and SP
-	isb
-	hvc #0			@ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
-	mov	sp, r1
-	mov	lr, r0		@ restore SVC copy of LR and SP
-
-	bx	lr
-ENDPROC(armv7_switch_to_hyp)
-
 ENTRY(psci_cpu_entry)
 	mrc	p15, 0, r0, c1, c0, 1	@ ACTLR
 	orr	r0, r0, #(1 << 6)	@ Set SMP bit
 	mcr	p15, 0, r0, c1, c0, 1	@ ACTLR
 
 	bl	secure_monitor_stack_setup
+#ifdef CONFIG_ARM_PSCI
 	bl	psci_cpu_entry_c
+#endif
 
 ENDPROC(psci_cpu_entry)
diff --git a/arch/arm/cpu/start-pbl.c b/arch/arm/cpu/start-pbl.c
index 16159d7f9d..48a8086456 100644
--- a/arch/arm/cpu/start-pbl.c
+++ b/arch/arm/cpu/start-pbl.c
@@ -26,6 +26,7 @@
 #include <asm/barebox-arm-head.h>
 #include <asm-generic/memory_layout.h>
 #include <asm/sections.h>
+#include <asm/secure.h>
 #include <asm/cache.h>
 #include <asm/mmu.h>
 #include <asm/unaligned.h>
@@ -98,5 +99,8 @@ __noreturn void barebox_single_pbl_start(unsigned long membase,
 	else
 		barebox = (void *)barebox_base;
 
+	if (IS_ENABLED(CONFIG_CPU_V7) && __boot_cpu_mode == HYP_MODE)
+		armv7_switch_to_hyp();
+
 	barebox(membase, memsize, boarddata);
 }
diff --git a/arch/arm/cpu/start.c b/arch/arm/cpu/start.c
index 64bd942ad7..a79d64eaa6 100644
--- a/arch/arm/cpu/start.c
+++ b/arch/arm/cpu/start.c
@@ -24,6 +24,7 @@
 #include <asm/barebox-arm-head.h>
 #include <asm-generic/memory_layout.h>
 #include <asm/sections.h>
+#include <asm/secure.h>
 #include <asm/unaligned.h>
 #include <asm/cache.h>
 #include <asm/mmu.h>
@@ -147,6 +148,10 @@ __noreturn void barebox_non_pbl_start(unsigned long membase,
 	unsigned long barebox_base = arm_mem_barebox_image(membase,
 							   endmem,
 							   barebox_size);
+
+	if (IS_ENABLED(CONFIG_CPU_V7))
+		armv7_hyp_install();
+
 	if (IS_ENABLED(CONFIG_RELOCATABLE))
 		relocate_to_adr(barebox_base);
 
diff --git a/arch/arm/cpu/uncompress.c b/arch/arm/cpu/uncompress.c
index 43b49b4212..048bca0c95 100644
--- a/arch/arm/cpu/uncompress.c
+++ b/arch/arm/cpu/uncompress.c
@@ -27,6 +27,7 @@
 #include <asm/barebox-arm-head.h>
 #include <asm-generic/memory_layout.h>
 #include <asm/sections.h>
+#include <asm/secure.h>
 #include <asm/cache.h>
 #include <asm/mmu.h>
 #include <asm/unaligned.h>
@@ -105,5 +106,8 @@ void __noreturn barebox_multi_pbl_start(unsigned long membase,
 
 	pr_debug("jumping to uncompressed image at 0x%p\n", barebox);
 
+	if (IS_ENABLED(CONFIG_CPU_V7) && __boot_cpu_mode == HYP_MODE)
+		armv7_switch_to_hyp();
+
 	barebox(membase, memsize, boarddata);
 }