[U-Boot] [RFC 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.
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 virtualization extensions.

Introduce a monitor handler routine which switches the CPU to
non-secure state by setting the NS bit (and associated bits).

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

diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index e9e57e6..7bfb19d 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -155,6 +155,10 @@ reset:
 	/* Set vector address in CP15 VBAR register */
 	ldr	r0, =_start
 	mcr	p15, 0, r0, c12, c0, 0	@Set VBAR
+
+	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
 
 	/* the mask ROM code should have PLL and others stable */
@@ -256,6 +260,9 @@ ENTRY(c_runtime_cpu_setup)
 	/* Set vector address in CP15 VBAR register */
 	ldr     r0, =_start
 	mcr     p15, 0, r0, c12, c0, 0  @Set VBAR
+	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
 
 	bx	lr
 
@@ -492,9 +499,16 @@ undefined_instruction:
 
 	.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