ARM: add file for HYP mode related setup

This adds routines to add hyp mode vectors and switch back to HYP
mode from SVC. This is needed in both the PBL and Barebox proper.

Signed-off-by: Lucas Stach <l.stach@pengutronix.de>
Tested-by: Roland Hieber <r.hieber@pengutronix.de>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

4 files changed, 122 insertions, 11 deletions

diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 374b7cda4c..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
 
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/sm_as.S b/arch/arm/cpu/sm_as.S
index 0d01e1bf24..de6cd0406f 100644
--- a/arch/arm/cpu/sm_as.S
+++ b/arch/arm/cpu/sm_as.S
@@ -148,17 +148,6 @@ 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
diff --git a/arch/arm/include/asm/secure.h b/arch/arm/include/asm/secure.h
index a4cb1f6c1c..663d81ea27 100644
--- a/arch/arm/include/asm/secure.h
+++ b/arch/arm/include/asm/secure.h
@@ -5,9 +5,12 @@
 
 int armv7_secure_monitor_install(void);
 int __armv7_secure_monitor_install(void);
+
 void armv7_switch_to_hyp(void);
+void armv7_hyp_install(void);
 
 extern unsigned char secure_monitor_init_vectors[];
+extern int __boot_cpu_mode;
 
 enum arm_security_state {
 	ARM_STATE_SECURE,