Revert "bootwrapper: Drop now-obsolete SMC interface support"

This reverts commit 2a42e4cbc40484bb21303358e656c4c716148b10.

Change-Id: I4d49b31308aa58e4863287f7b2b27993b0591e55

4 files changed, 119 insertions, 41 deletions

diff --git a/Makefile b/Makefile
index 0997dcf..f8fc841 100644
--- a/Makefile
+++ b/Makefile
@@ -15,8 +15,9 @@ endif
 
 LIBFDTOBJS      = libfdt/fdt.o libfdt/fdt_ro.o libfdt/fdt_wip.o \
 		  libfdt/fdt_sw.o libfdt/fdt_rw.o libfdt/fdt_strerror.o
+MONITOR		= monitor.S
 BOOTLOADER	= boot.S
-OBJS 		= boot.o c_start.o semihosting.o string.o semi_loader.o $(LIBFDTOBJS)
+OBJS 		= boot.o c_start.o monitor.o semihosting.o string.o semi_loader.o $(LIBFDTOBJS)
 KERNEL		= uImage
 
 IMAGE		= linux-system.axf
@@ -52,6 +53,9 @@ $(SEMIIMG): $(OBJS) modelsemi.lds
 boot.o: $(BOOTLOADER)
 	$(CC) $(CPPFLAGS) -DKCMD='$(KCMD)' -c -o $@ $<
 
+monitor.o: $(MONITOR)
+	$(CC) $(CPPFLAGS) -c -o $@ $<
+
 %.o: %.c
 	$(CC) $(CPPFLAGS) -O2 -ffreestanding -I. -Ilibfdt -c -o $@ $<
 
diff --git a/boot.S b/boot.S
index 40ebd33..6496a94 100644
--- a/boot.S
+++ b/boot.S
@@ -13,30 +13,20 @@
 	.text
 
 .macro enter_hyp
-	@ We assume we're entered in Secure Supervisor mode. To
-	@ get to Hyp mode we have to pass through Monitor mode
-	@ and NS-Supervisor mode. Note that there is no way to
-	@ return to the Secure world once we've done this.
-	@
-	@ This will trash r10 and r11.
-	ldr	r10, =vectors
-	mcr	p15, 0, r10, c12, c0, 1		@ Monitor vector base address
-	@ Switch to monitor mode, which will set up the HVBAR and
-	@ then return to us in NS-SVC
-	smc	#0
-	@ Now we're in NS-SVC, make a Hyp call to get into Hyp mode
+	@ 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
-	@ We will end up here in NS-Hyp.
 .endm
 
 .align 5
-/* We use the same vector table for Hyp and Monitor mode, since
- * we will only use each once and they don't overlap.
- */
-vectors:
+/* Once we get rid of monitor.S, use these smc vectors too! */
+hyp_vectors:
 	.word 0	/* reset */
 	.word 0	/* undef */
-	b	2f /* smc */
+	.word 0 /* svc */
 	.word 0 /* pabt */
 	.word 0 /* dabt */
 	b	1f
@@ -47,28 +37,6 @@ vectors:
 1:	mrs	lr, elr_hyp
 	mov	pc, lr
 
-/* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */
-2:
-	@ Set up HVBAR
-	mrc	p15, 0, r10, c1, c1, 0		@ SCR
-	@ Set SCR.NS=1 (needed for setting HVBAR and also returning to NS state)
-	@        .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode)
-	@        .FW,AW=1 (CPSR.A,F modifiable in NS state)
-	@        .nET=0 (early termination OK)
-	@        .SCD=1 (SMC in NS mode is UNDEF, so accidental SMCs don't
-	@                cause us to leap back into this code confusingly)
-	@        .HCE=1 (HVC does Hyp call)
-	bic	r10, r10, #0x07f
-	ldr	r11, =0x1b1
-	orr	r10, r10, r11
-	mcr	p15, 0, r11, c1, c1, 0
-	isb
-	ldr	r11, =vectors
-	mcr	p15, 4, r11, c12, c0, 0		@ set HVBAR
-	@ ...and return to calling code in NS state
-	movs	pc, lr
-
-
 	.globl	start
 start:
 #ifdef SMP
@@ -119,6 +87,15 @@ start:
 	orr	r0, r0, r1
 	mcr	p15, 0, r0, c1, c1, 2
 
+	@ 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, =hyp_vectors
+	mov	r7, #0xfffffff0
+	smc	#0				@ Set HVBAR
+
 	@ Check CPU nr again
 	mrc	p15, 0, r0, c0, c0, 5		@ MPIDR (ARMv7 only)
 	bfc	r0, #24, #8			@ CPU number, taking multicluster into account
diff --git a/model.lds.S b/model.lds.S
index 793df89..07fae8c 100644
--- a/model.lds.S
+++ b/model.lds.S
@@ -47,6 +47,8 @@ SECTIONS
 
 
  . = MON_OFFSET;
+ .monitor : { monitor.o }
+
  /* Put most of the actual boot loader code up in high memory
   * where it won't get overwritten by kernel, initrd or atags.
   */
diff --git a/monitor.S b/monitor.S
new file mode 100644
index 0000000..dea8551
--- /dev/null
+++ b/monitor.S
@@ -0,0 +1,95 @@
+/*
+ * monitor.S - simple monitor code to switch to NS state before executing kernel
+ *
+ * Copyright (C) 2011 Columbia University. All rights reserved.
+ * 		      Christoffer Dall <cdall@cs.columbia.edu>
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+
+#.syntax	unified
+	.section monitor, "x"
+
+	.word 0
+	.word 0
+	b	1f
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+	.word 0
+
+	@
+	@ Secure Monitor Call
+	@
+1:
+	ldr	sp, =_monitor_stack
+	push	{r10-r12}
+
+	cmp	r7, #0xffffffff
+	beq	_non_sec
+
+	@ Check smc number and VMID
+	bic	r12, r7, #0xf
+	cmp	r12, #0xfffffff0
+	movnes	pc, lr
+	and	r12, r7, #0xf
+	cmp	r12, #0x0
+	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	{r10-r12}
+	movnes	pc, lr
+
+	@ Jump to the right function
+	and	r12, r7, #0xf
+	adr	r11, _hyp_funcs
+	add	r11, r11, r12, lsl #2
+	ldr	pc, [r11]
+
+	@
+	@ Jump table for the SMC hypervisor API calls
+	@
+_hyp_funcs:
+	.long	_write_hvbar
+
+	@
+	@ Switch to non-secure mode
+	@
+_non_sec:
+	mrc	p15, 0, r12, c1, c1, 0		@ Secure configuration register
+	bic	r12, r12, #0x07f
+	ldr	r11, =0x131
+	orr	r12, r12, r11
+	mcr	p15, 0, r12, c1, c1, 0
+	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
+	mcr	p15, 0, r10, c1, c1, 0		@ Restore SCR
+	pop	{r10-r12}
+	movs	pc, lr
+
+	.ltorg
+
+	/* A bit of stack space for monitor mode */
+	.align	12
+_monitor_stack: