1 files changed, 59 insertions, 6 deletions

diff --git a/arch/arm64/kernel/start.S b/arch/arm64/kernel/start.S
index 6ee9941..2a5e2ae 100644
--- a/arch/arm64/kernel/start.S
+++ b/arch/arm64/kernel/start.S
@@ -38,12 +38,16 @@ reset:
 	/* keep the dtb pointer before anything clobbers x0 */
 	mov	x19, x0
 
-	/* park secondary cores, they have nothing to do yet */
+	/*
+	 * park secondary cores, they have nothing to do yet. at
+	 * EL3 they still get the monitor: a firmware call on any
+	 * PE must land in a handler, a secondary with no EL3
+	 * vectors traps into nothing.
+	 */
 	mrs	x0, mpidr_el1
 	and	x0, x0, #0xff
-	cbnz	x0, park
+	cbnz	x0, secondary_boot
 
-	/* which EL are we in, 0x8 per level shifted into bits 3:2 */
 	mrs	x0, CurrentEL
 	lsr	x0, x0, #2
 	cmp	x0, #3
@@ -54,12 +58,34 @@ reset:
 	b.eq	mmu_check
 	b	park
 
+secondary_boot:
+	mrs	x0, CurrentEL
+	lsr	x0, x0, #2
+	cmp	x0, #3
+	b.ne	park
+	/*
+	 * the same security state as the primary: SCR_EL3.NS
+	 * clear leaves a PE secure, and a secondary released
+	 * into the kernel secure is the inconsistent mode boot
+	 * the kernel warns about, its calls trap to EL3 as if
+	 * they were firmware's own.
+	 */
+	mrs	x0, scr_el3
+	orr	x0, x0, #1
+	msr	scr_el3, x0
+	isb
+	bl	tb_monitor_init
+	b	park
+
 from_el3:
 	/*
-	 * EL3 holds the security state. the kernel runs non-secure, so
-	 * set SCR_EL3.NS before dropping to EL2, which the kernel
-	 * prefers (booting.rst, EL2 RECOMMENDED).
+	 * EL3 holds the security state, so the monitor lives here:
+	 * vectors, its own stack, the SMC conduit. it is resident
+	 * after this, the kernel's firmware calls trap into it.
 	 */
+	bl	tb_monitor_init
+
+	/* the kernel runs non-secure, drop to the EL2 it prefers */
 	mrs	x0, scr_el3
 	orr	x0, x0, #1		/* SCR_EL3.NS = 1, non-secure */
 	msr	scr_el3, x0
@@ -222,6 +248,24 @@ park:
 	wfe
 	b	1b
 2:
+	/* interrupts masked at release, the manual's boot state */
+	msr	daifset, #0xf
+	/*
+	 * every PE must read the same virtual counter. whatever
+	 * ran before this loader could have left a per cpu offset
+	 * in the virtual counter view, the kernel has no way to
+	 * repair that itself. CNTVOFF_EL2 is writable at EL2 and
+	 * the write holds for the EL1 virtual timer the kernel
+	 * runs on. below EL2 it is out of reach, the reset value
+	 * is the best a lower EL can do.
+	 */
+	mrs	x4, CurrentEL
+	lsr	x4, x4, #2
+	cmp	x4, #2
+	b.lt	3f
+	msr	cntvoff_el2, xzr
+	isb
+3:
 	mov	x0, xzr		/* secondaries enter with x0-x3 zero */
 	mov	x1, xzr
 	mov	x2, xzr
@@ -396,6 +440,15 @@ exc_serr:
 	mov	x1, #0
 	mov	x2, lr
 	bl	exc_report
+	/*
+	 * an SError while this loader runs means the machine is
+	 * broken. handing the kernel a cpu that already lost is
+	 * worse than stopping: report, then drive the reset domain
+	 * the same way PSCI SYSTEM_RESET does. the reset call does
+	 * not return, the park below is the fallback if a reset
+	 * domain ignores the request.
+	 */
+	bl	tb_system_reset
 	ldp	x29, x30, [sp], #16
 	b	park