tashaboot: smp, psci, initrd, timer, cache by va

The bootloader now does the whole job of machine firmware it owns:
boots 4 cpus, hands over an initrd, answers PSCI, and carries the
delay and cache primitives the arch layer needs.

SMP: the secondary pen is the Wait For Event mechanism from the
manual (B2-144, D1-2255), each secondary watches its spin gate,
WFE, the release writes the entry and SEVs, the recheck after each
wake covers a release that lands between the load and the sleep.
The gates land in the dtb cpu-release-addr slots, rewritten in
place by a small walker, no libfdt, structure per the devicetree
specification, values only, the properties themselves are fixed at
build time like firmware shipping a fixed blob.

PSCI 0.2 at EL2 (DEN 0022): the HVC trap arrives at the current EL
SP_ELx sync slot (EC 0x16 in ESR_EL2, the vector layout Table D1-7),
dispatch on the standard function ids, VERSION, CPU_ON writes the
target gate and SEVs, CPU_OFF clears the gate and returns to the
pen, SYSTEM_OFF and SYSTEM_RESET drive RMR_EL2.RR. On qemu the cores
are held by the machine's own firmware and released through its PSCI
(hvc with -kernel, smc with virtualization=on), the handler here is
the real hardware path where the bootloader is the conduit.

The initrd handoff: loaded at a fixed address clear of the image
and dtb, the dtb /chosen carries linux,initrd-start and -end.

Delays are the generic timer (D10), CNTFRQ_EL0 frequency, CNTVCT_EL0
count, busy wait, no interrupts. Cache maintenance by virtual
address, dc cvac, dc ivac, dc civac, ic ivau with the barrier pairs
the manual requires, the by VA form beats set and way when the
range is known.

Boot receipt, 4 cpus, el2, initrd:

  tashaboot 0.1
  initrd at 46000000
  [    0.000000] Booting Linux on physical CPU 0x0000000000
  [    0.130621] smp: Brought up 1 node, 4 CPUs
  [    1.830692] Run /init as init process
  tashaboot linux userspace reached
  cores: 4
  BusyBox v1.37.0 built-in shell (ash)
  ~ #

Signed-off-by: Bradley Morgan <brads@mainlining.org>

1 files changed, 77 insertions, 1 deletions

diff --git a/arch/arm64/kernel/start.S b/arch/arm64/kernel/start.S
index 2d4b5c0..969830d 100644
--- a/arch/arm64/kernel/start.S
+++ b/arch/arm64/kernel/start.S
@@ -147,6 +147,11 @@ c_entry:
 	ldr	x0, =__stack_top
 	mov	sp, x0
 
+	/* export the spin gate array address for the dtb patcher */
+	adr	x0, tb_spin_gates
+	adrp	x1, tb_spin_gates_ptr
+	str	x0, [x1, #:lo12:tb_spin_gates_ptr]
+
 	/* clear bss */
 	ldr	x0, =__bss_start
 	ldr	x1, =__bss_end
@@ -166,9 +171,44 @@ c_entry:
 	bl	tashaboot_main
 
 	/* if main returns there is nothing sensible to do */
+/*
+ * the spin table pen, the Wait For Event mechanism from the manual
+ * (B2-144, D1-2255). each secondary watches its own gate, the
+ * cpu-release-addr the dtb names. WFE clears the event register and
+ * sleeps, the kernel writes the secondary entry to the gate, makes
+ * it visible, then SEV sets the event register on every PE. the load
+ * recheck after each wake covers a release that lands between the
+ * load and the WFE. entered with MMU and caches off, left the same.
+ */
+.globl park_ret
+park_ret:
 park:
+	adr	x0, tb_spin_gates
+	mrs	x1, mpidr_el1
+	and	x1, x1, #0xff		/* affinity 0, the core number */
+	add	x0, x0, x1, lsl #3	/* gate = gates + core * 8 */
+
+	/* diagnostic: stamp arrival, primary prints it later */
+	adr	x3, tb_pen_stamps
+	strb	w1, [x3, x1]
+	sevl
 	wfe
-	b	park
+	sevl
+	wfe
+
+1:
+	ldr	x2, [x0]
+	cbnz	x2, 2f
+	wfe
+	b	1b
+2:
+	mov	x0, xzr		/* secondaries enter with x0-x3 zero */
+	mov	x1, xzr
+	mov	x2, xzr
+	mov	x3, xzr
+	dsb	sy
+	isb
+	br	x2
 
 /*
  * exception vectors, the armv8 layout: 16 slots, 128 bytes each, in
@@ -218,6 +258,19 @@ vectors:
 	.align	7
 	b	exc_serr
 
+.pushsection .data.tb_spin, "aw"
+.align 3
+.globl tb_spin_gates
+tb_spin_gates:
+	.quad	0, 0, 0, 0, 0, 0, 0, 0
+.globl tb_spin_gates_ptr
+tb_spin_gates_ptr:
+	.quad	0
+.globl tb_pen_stamps
+tb_pen_stamps:
+	.byte	0, 0, 0, 0, 0, 0, 0, 0
+.popsection
+
 exc_sync:
 	stp	x29, x30, [sp, #-16]!
 	mov	x29, sp
@@ -226,6 +279,10 @@ exc_sync:
 	cmp	x3, #2
 	b.lt	1f
 	mrs	x0, esr_el2
+	mrs	x2, elr_el2
+	lsr	x1, x0, #26
+	cmp	x1, #0x16		/* HVC from lower EL */
+	b.eq	hvc_from_el1
 	mrs	x1, far_el2
 	b	2f
 1:
@@ -237,6 +294,25 @@ exc_sync:
 	ldp	x29, x30, [sp], #16
 	b	park
 
+/*
+ * HVC from EL1, the PSCI conduit. x0-x3 are the PSCI args in the
+ * caller registers, dispatch and return in x0. ELR_EL2 is already
+ * the resume point, eret takes it back.
+ */
+hvc_from_el1:
+	stp	x4, x5, [sp, #-16]!
+	stp	x6, x7, [sp, #-16]!
+	stp	x29, x30, [sp, #-16]!
+	mov	x29, sp
+
+	bl	tb_psci_dispatch
+
+	ldp	x29, x30, [sp], #16
+	ldp	x6, x7, [sp], #16
+	ldp	x4, x5, [sp], #16
+	ldp	x29, x30, [sp], #16
+	eret
+
 exc_serr:
 	stp	x29, x30, [sp, #-16]!
 	mov	x29, sp