From 3018791610e7272f2e35f7e1da61fd9d74cce2ee Mon Sep 17 00:00:00 2001
From: Rusty Russell <rusty@rustcorp.com.au>
Date: Thu, 23 Feb 2012 08:20:20 +1030
Subject: Boot kernel in hyp mode.

Leave the old monitor stub there for the moment, so we can boot current
Christoffer kernels.

Includes fixes by Dave Martin to make it actually work :)

Signed-off-by: Rusty Russell <rusty@rustcorp.com.au>
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 boot.S    | 27 +++++++++++++++++++++++++--
 monitor.S | 19 ++++++++++++++-----
 2 files changed, 39 insertions(+), 7 deletions(-)

diff --git a/boot.S b/boot.S
index cf8bdb0..823def8 100644
--- a/boot.S
+++ b/boot.S
@@ -9,6 +9,7 @@
 
 	.syntax	unified
 	.arch_extension sec
+	.arch_extension virt
 	.text
 
 #ifdef SEMIHOSTING
@@ -104,12 +105,34 @@ _start:
 	orr	r0, r0, r1
 	mcr	p15, 0, r0, c1, c1, 2
 
-	@ Change to NS-mode
+	@ Leave monitor.S trap in place for the transition...
 	mov	r0, #0xf0000000
 	mcr	p15, 0, r0, c12, c0, 1		@ Monitor vector base address
+
+	@ Set up hvbar so hvc comes back here.
+	ldr	r0, =vectors
+	mov	r7, #0xfffffff0
+	smc	#0				@ Set HVBAR
+
+	@ We can't call hvc from secure mode, so drop down first.
 	mov	r7, #0xffffffff
 	smc	#0				@ Change to NS-mode 
 
+	@ This is how we enter hyp mode, for booting the next stage.
+	hvc	#0
+
+/* Once we get rid of monitor.S, use these smc vectors too! */
+vectors:
+	.word 0	/* reset */
+	.word 0	/* undef */
+	.word 0 /* svc */
+	.word 0 /* pabt */
+	.word 0 /* dabt */
+	b	into_hyp_mode /* hvc */
+	.word 0 /* irq */
+	.word 0 /* fiq */
+
+into_hyp_mode:
 	@ Check CPU nr again
 	mrc	p15, 0, r0, c0, c0, 5		@ MPIDR (ARMv7 only)
 	and	r0, r0, #15			@ CPU number
@@ -485,7 +508,7 @@ sh_cmdline:
 	@ Semihosting command line will be written here
 
 #else /* not SEMIHOSTING */
-	.org	0x100
+	.org	0x200
 	@ Static ATAGS for when kernel/etc are compiled into the ELF file
 atags:
 	@ ATAG_CORE
diff --git a/monitor.S b/monitor.S
index 052ab1e..dea8551 100644
--- a/monitor.S
+++ b/monitor.S
@@ -25,7 +25,7 @@
 	@
 1:
 	ldr	sp, =_monitor_stack
-	push	{r11, r12}
+	push	{r10-r12}
 
 	cmp	r7, #0xffffffff
 	beq	_non_sec
@@ -36,15 +36,20 @@
 	movnes	pc, lr
 	and	r12, r7, #0xf
 	cmp	r12, #0x0
-	popgt	{r11, r12}
+	popgt   {r10-r12}
 	movgts	pc, lr
 
 	@ Check the VMID is 0
+	mrc	p15, 0, r10, c1, c1, 0		@ SCR
+	orr	r11, r10, #1			@ SCR.NS = 1
+	mcr	p15, 0, r11, c1, c1, 0
+	isb
 	mrrc	p15, 6, r12, r11, c2
+	mcr	p15, 0, r10, c1, c1, 0		@ Restore SCR
 	lsr	r11, r11, #16
 	and	r11, r11, #0xff
 	cmp	r11, #0
-	popne	{r11, r12}
+	popne	{r10-r12}
 	movnes	pc, lr
 
 	@ Jump to the right function
@@ -68,15 +73,19 @@ _non_sec:
 	ldr	r11, =0x131
 	orr	r12, r12, r11
 	mcr	p15, 0, r12, c1, c1, 0
-	pop	{r11, r12}
+	pop	{r10-r12}
 	movs	pc, lr
 
 	@
 	@ Read/Write HVBAR
 	@
 _write_hvbar:
+	orr	r11, r10, #1			@ SCR.NS = 1 (r10 already = SCR)
+	mcr	p15, 0, r11, c1, c1, 0
+	isb
 	mcr	p15, 4, r0, c12, c0, 0
-	pop	{r11, r12}
+	mcr	p15, 0, r10, c1, c1, 0		@ Restore SCR
+	pop	{r10-r12}
 	movs	pc, lr
 
 	.ltorg
-- 
cgit v1.2.3