1 files changed, 132 insertions, 0 deletions
diff --git a/arch/arm64/kernel/monitor.S b/arch/arm64/kernel/monitor.S
new file mode 100644
index 0000000..c6f5ec8
--- /dev/null
+++ b/arch/arm64/kernel/monitor.S
@@ -0,0 +1,132 @@
+/*
+ * monitor.S - the EL3 secure monitor, the resident layer real
+ * firmware ships. the loader drops to non-secure and never
+ * returns, but the kernel keeps calling into firmware: PSCI
+ * through the SMC conduit, and on hardware with the security
+ * extension the group routing of the interrupt controller is
+ * only writable from here.
+ *
+ * the entry path runs once per PE: configure EL3, install the
+ * monitor vectors, hand the next stage non-secure EL2 in the
+ * manual's boot state. SMCCC calls from the kernel trap into
+ * the SMC slot, the C dispatcher behind it is the same one the
+ * hvc path uses.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+/*
+ * the monitor stack. SP_EL3 needs memory no non-secure stage
+ * will touch, the region after the loader stack, sixteen
+ * bytes a call deep at most.
+ */
+.section .bss.el3stack, "aw", %nobits
+.align 4
+.globl __el3_stack_bottom
+__el3_stack_bottom:
+	.quad 0, 0, 0, 0
+	.quad 0, 0, 0, 0
+.globl __el3_stack_top
+__el3_stack_top:
+
+/*
+ * EL3 vectors, same sixteen slot layout every exception level
+ * uses. only the lower EL sync slot carries work, the SMC
+ * conduit, everything else parks.
+ */
+.balign 2048
+.globl tb_el3_vectors
+tb_el3_vectors:
+	/* 0x000: current EL, SP_EL0, unused */
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+
+	/* 0x200: current EL, SP_ELx, unused */
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+
+	/* 0x400: lower EL, AArch64, the SMC conduit lives here */
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_smc
+
+	/* 0x600: lower EL, AArch32, unused */
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+	.align 7
+	b	el3_park
+
+/*
+ * one time per PE, from the reset path. x30 = the next stage
+ * entry in non-secure EL2, x0 = the dtb pointer.
+ */
+.globl tb_monitor_init
+tb_monitor_init:
+	/* SP_EL3 on its own region */
+	adr	x1, __el3_stack_top
+	msr	spsel, #0
+	mov	sp, x1
+	msr	spsel, #1
+
+	/* the monitor vectors */
+	adr	x1, tb_el3_vectors
+	msr	vbar_el3, x1
+	isb
+
+	/*
+	 * SMC as the conduit, SVE traps off, no interrupt routing
+	 * into EL3: FIQ/IRQ stay whatever SCR_EL3.SCR left them,
+	 * the kernel owns the world below.
+	 */
+	mrs	x1, scr_el3
+	bic	x1, x1, #(1 << 2)	/* SMD, SMC enabled */
+	msr	scr_el3, x1
+	isb
+
+	ret
+
+el3_park:
+	b	el3_park
+
+/*
+ * the SMC trap from lower EL. the SMCCC calling convention is
+ * the SMC register set, function id in x0, arguments x1 to
+ * x3, results in x0 to x3. x17 and x18 are caller save in
+ * this convention, the dispatcher clobbers x0 to x18.
+ */
+el3_smc:
+	stp	x29, x30, [sp, #-16]!
+	mov	x29, sp
+	stp	x19, x20, [sp, #-16]!
+	stp	x21, x22, [sp, #-16]!
+	stp	x23, x24, [sp, #-16]!
+
+	bl	tb_psci_dispatch
+
+	ldp	x23, x24, [sp], #16
+	ldp	x21, x22, [sp], #16
+	ldp	x19, x20, [sp], #16
+	ldp	x29, x30, [sp], #16
+
+	eret