[U-Boot] [PATCH 1/6] ARM: add secure monitor handler to switch to non-secure state

[Andre Przywara]

A prerequisite for using virtualization is to be in HYP mode, which
requires the CPU to be in non-secure state.
Introduce a monitor handler routine which switches the CPU to
non-secure state by setting the NS and associated bits.
According to the ARM ARM this should not be done in SVC mode, so we
have to setup a SMC handler for this. We reuse the current vector
table for this and make sure that we only access the MVBAR register
if the CPU supports the security extension and only if we
configured the board to use it, since boards entering u-boot already
in non-secure mode would crash on accessing MVBAR otherwise.

Signed-off-by: Andre Przywara <andre.przywara at linaro.org>
---
 arch/arm/cpu/armv7/start.S | 31 ++++++++++++++++++++++++++++---
 1 file changed, 28 insertions(+), 3 deletions(-)

diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index e9e57e6..da48b36 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -155,6 +155,13 @@ reset:
 	/* Set vector address in CP15 VBAR register */
 	ldr	r0, =_start
 	mcr	p15, 0, r0, c12, c0, 0	@Set VBAR
+
+#ifdef CONFIG_ARMV7_VIRT
+	mrc	p15, 0, r1, c0, c1, 1	@ check for security extension
+	ands	r1, r1, #0x30
+	mcrne	p15, 0, r0, c12, c0, 1	@ Set secure monitor MVBAR
+#endif
+
 #endif
 
 	/* the mask ROM code should have PLL and others stable */
@@ -257,6 +264,12 @@ ENTRY(c_runtime_cpu_setup)
 	ldr     r0, =_start
 	mcr     p15, 0, r0, c12, c0, 0  @Set VBAR
 
+#ifdef CONFIG_ARMV7_VIRT
+	mrc	p15, 0, r1, c0, c1, 1	@ check for security extension
+	ands	r1, r1, #0x30
+	mcrne	p15, 0, r0, c12, c0, 1  @ Set secure monitor MVBAR
+#endif
+
 	bx	lr
 
 ENDPROC(c_runtime_cpu_setup)
@@ -490,11 +503,23 @@ undefined_instruction:
 	bad_save_user_regs
 	bl	do_undefined_instruction
 
+/*
+ * software interrupt aka. secure monitor handler
+ * This is executed on a "smc" instruction, we use a "smc #0" to switch
+ * to non-secure state
+ */
 	.align	5
 software_interrupt:
-	get_bad_stack_swi
-	bad_save_user_regs
-	bl	do_software_interrupt
+	mrc	p15, 0, r1, c1, c1, 0		@ read SCR
+	bic	r1, r1, #0x07f
+	orr	r1, r1, #0x31			@ enable NS, AW, FW
+
+	mrc	p15, 0, r0, c12, c0, 0		@ save secure copy of VBAR
+	mcr	p15, 0, r1, c1, c1, 0		@ write SCR, switch to non-sec
+	isb
+	mcr	p15, 0, r0, c12, c0, 0		@ write non-secure copy of VBAR
+
+	movs	pc, lr
 
 	.align	5
 prefetch_abort:
-- 
1.7.12.1