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@xxxxxxxxxxxxxx>
Tested-by: Roland Hieber <r.hieber@xxxxxxxxxxxxxx>
---
v3:
- fix whitespace
- use __BARE_INIT
---
arch/arm/cpu/Makefile | 3 +
arch/arm/cpu/hyp.S | 116 ++++++++++++++++++++++++++++++++++
arch/arm/cpu/sm_as.S | 11 ----
arch/arm/include/asm/secure.h | 3 +
4 files changed, 122 insertions(+), 11 deletions(-)
create mode 100644 arch/arm/cpu/hyp.S
diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 374b7cda4c77..45de2728d01a 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-y += 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 000000000000..1314b56eab25
--- /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 0d01e1bf2435..de6cd0406f4f 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 a4cb1f6c1c44..663d81ea27dd 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,
--
2.19.1
_______________________________________________
barebox mailing list
barebox@xxxxxxxxxxxxxxxxxxx
http://lists.infradead.org/mailman/listinfo/barebox
