4 files changed, 285 insertions, 0 deletions

diff --git a/arch/arm64/lib/cache_va.c b/arch/arm64/lib/cache_va.c
new file mode 100644
index 0000000..1fd7804
--- /dev/null
+++ b/arch/arm64/lib/cache_va.c
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * cache_va.c - cache maintenance by virtual address, the operations
+ * the manual prescribes for boot handoff: clean to point of
+ * coherency (dc cvac), invalidate (dc ivac), and clean and
+ * invalidate (dc civac), plus icache invalidate by VA to the point
+ * of unification (ic ivau). by VA beats by set and way when the
+ * address range is known, the manual's own guidance, set and way
+ * only for the full flush cases in cache.S.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#define CACHE_LINE_SHIFT	6	/* 64 byte lines on cortex-a class */
+#define CACHE_LINE_SIZE		(1 << CACHE_LINE_SHIFT)
+
+void tb_clean_dcache_range(uintptr_t start, size_t len)
+{
+	uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1);
+	uintptr_t end = start + len;
+
+	while (line < end) {
+		asm volatile("dc cvac, %0" :: "r" (line) : "memory");
+		line += CACHE_LINE_SIZE;
+	}
+
+	asm volatile("dsb sy" ::: "memory");
+}
+
+void tb_inval_dcache_range(uintptr_t start, size_t len)
+{
+	uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1);
+	uintptr_t end = start + len;
+
+	while (line < end) {
+		asm volatile("dc ivac, %0" :: "r" (line) : "memory");
+		line += CACHE_LINE_SIZE;
+	}
+
+	asm volatile("dsb sy" ::: "memory");
+}
+
+void tb_clean_inval_dcache_range(uintptr_t start, size_t len)
+{
+	uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1);
+	uintptr_t end = start + len;
+
+	while (line < end) {
+		asm volatile("dc civac, %0" :: "r" (line) : "memory");
+		line += CACHE_LINE_SIZE;
+	}
+
+	asm volatile("dsb sy" ::: "memory");
+}
+
+void tb_inval_icache_range(uintptr_t start, size_t len)
+{
+	uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1);
+	uintptr_t end = start + len;
+
+	while (line < end) {
+		asm volatile("ic ivau, %0" :: "r" (line) : "memory");
+		line += CACHE_LINE_SIZE;
+	}
+
+	asm volatile(
+		"dsb ish\n"
+		"isb\n"
+		::: "memory");
+}
diff --git a/arch/arm64/lib/psci.c b/arch/arm64/lib/psci.c
new file mode 100644
index 0000000..ad5f461
--- /dev/null
+++ b/arch/arm64/lib/psci.c
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * psci.c - PSCI 0.2 at EL2, the calls a payload makes to control
+ * cores and the system, per DEN 0022. the call arrives as an HVC
+ * trap at EL2 (EC 0x16), x0 holds the function id, x1 to x3 the
+ * arguments, the return value goes back in x0 and eret resumes the
+ * caller at EL1.
+ *
+ * CPU_ON writes the spin gate of the target core and SEVs, the pen
+ * from start.S does the release. CPU_OFF parks the calling core.
+ * SYSTEM_OFF and SYSTEM_RESET drive the architecture reset domain.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+#include <asm/psci.h>
+#include <debug.h>
+
+extern void tb_system_reset(void);
+extern void tb_system_off(void);
+
+/* the gates and stamps from start.S, one per possible core */
+extern unsigned long tb_spin_gates[8];
+extern unsigned char tb_pen_stamps[8];
+
+static uint64_t psci_cpu_on(uint64_t target, uint64_t entry,
+			    uint64_t ctx)
+{
+	unsigned long mpidr;
+	int cpu;
+
+	/* affinity 0 only, our gate array indexes cores 0..7 */
+	if (target > 7)
+		return PSCI_RET_INVALID_PARAMS;
+
+	asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr));
+	if ((mpidr & 0xff) == target)
+		return PSCI_RET_ALREADY_ON;
+
+	cpu = (int)target;
+
+	/*
+	 * the pen saves no context, CPU_ON per DEN 0022 passes an
+	 * entry and a context id. the pen enters with x0 = ctx, the
+	 * kernel secondary entry takes x0 as its context pointer.
+	 * the gate holds the entry, the stamp array the ctx.
+	 */
+	tb_spin_gates[cpu] = entry;
+	tb_pen_stamps[cpu] = (unsigned char)(ctx & 0xff);
+
+	/* make the gate write visible before the wake, D1-2255 */
+	asm volatile("dsb sy");
+	asm volatile("sev");
+
+	dprintf(ALWAYS, "psci: cpu_on %llu -> %llx\n",
+		(unsigned long long)target,
+		(unsigned long long)entry);
+
+	return PSCI_RET_SUCCESS;
+}
+
+extern void park_ret(void);
+
+static uint64_t psci_cpu_off(void)
+{
+	unsigned long mpidr;
+	int cpu;
+
+	asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr));
+	cpu = (int)(mpidr & 0xff);
+
+	if (cpu > 7)
+		return PSCI_RET_NOT_SUPPORTED;
+
+	/* clear our own gate and go back to the pen */
+	tb_spin_gates[cpu] = 0;
+
+	dprintf(ALWAYS, "psci: cpu_off %d\n", cpu);
+
+	asm volatile(
+		"dsb	sy\n"
+		"b	park_ret\n"
+	);
+
+	return PSCI_RET_INTERNAL_FAIL;	/* not reached */
+}
+
+/*
+ * the asm vector calls this with the caller x0-x3 still in place,
+ * function id in x0, arguments in x1-x3, the return lands in x0.
+ */
+uint64_t tb_psci_dispatch(uint64_t fn, uint64_t x1, uint64_t x2,
+			   uint64_t x3)
+{
+	switch (fn) {
+	case PSCI_FN_VERSION:
+		return PSCI_VERSION_0_2;
+
+	case PSCI_FN_CPU_ON:
+		return psci_cpu_on(x1, x2, x3);
+
+	case PSCI_FN_CPU_OFF:
+		return psci_cpu_off();
+
+	case PSCI_FN_SYSTEM_OFF:
+		dprintf(ALWAYS, "psci: system off\n");
+			tb_system_off();
+		return PSCI_RET_SUCCESS;
+
+	case PSCI_FN_SYSTEM_RESET:
+		dprintf(ALWAYS, "psci: system reset\n");
+		tb_system_reset();
+		return PSCI_RET_INTERNAL_FAIL;	/* not reached */
+
+	default:
+		return PSCI_RET_NOT_SUPPORTED;
+	}
+}
diff --git a/arch/arm64/lib/system.c b/arch/arm64/lib/system.c
new file mode 100644
index 0000000..4e227f6
--- /dev/null
+++ b/arch/arm64/lib/system.c
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * system.c - system power control, the PSCI SYSTEM_OFF and
+ * SYSTEM_RESET backends. off parks the core in WFI forever, the
+ * manual's low power entry (D1-2255). reset drives the PE reset
+ * domain: RMR_EL2 reset request with the system reset bit, RR bit 1,
+ * followed by a barrier pair so the request retires before anything
+ * else observes the core.
+ *
+ * On real hardware a SoC also needs a watchdog or PMIC write for a
+ * full board reset, that is board territory, the arch part is this.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+#include <debug.h>
+#include <stdint.h>
+
+void tb_system_off(void)
+{
+	dprintf(ALWAYS, "system off\n");
+
+	for (;;) {
+		asm volatile("wfi");
+	}
+}
+
+void tb_system_reset(void)
+{
+	uint64_t rmr;
+
+	dprintf(ALWAYS, "system reset\n");
+
+	asm volatile("mrs %0, rmr_el2" : "=r" (rmr));
+	rmr |= (1 << 1);	/* RR, request reset */
+	asm volatile(
+		"msr	rmr_el2, %0\n"
+		"dsb	sy\n"
+		"isb\n"
+		:: "r" (rmr));
+
+	/* if the reset domain ignores us, park */
+	for (;;) {
+		asm volatile("wfi");
+	}
+}
diff --git a/arch/arm64/lib/timer.c b/arch/arm64/lib/timer.c
new file mode 100644
index 0000000..605cd18
--- /dev/null
+++ b/arch/arm64/lib/timer.c
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * timer.c - generic timer delays, the system counter from D10. the
+ * counter is a fixed frequency free running counter, CNTFRQ_EL0
+ * carries the frequency, CNTVCT_EL0 the 64 bit count. delays are a
+ * busy wait on the counter, no interrupts needed, microsecond and
+ * millisecond granularity.
+ *
+ * CNTVCT_EL0 is the virtual counter view; at EL2 with no offset
+ * configured it is the physical count. the read is not speculative
+ * and needs an isb to serialize against subsequent counter reads
+ * per the counter access rules.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+#include <stdint.h>
+
+static uint64_t read_cntfrq(void)
+{
+	uint64_t v;
+
+	asm volatile("mrs %0, cntfrq_el0" : "=r" (v));
+	return v;
+}
+
+static uint64_t read_counter(void)
+{
+	uint64_t v;
+
+	asm volatile("isb\nmrs %0, cntvct_el0" : "=r" (v));
+	return v;
+}
+
+void tb_udelay(uint32_t us)
+{
+	uint64_t freq = read_cntfrq();
+	uint64_t start = read_counter();
+	uint64_t ticks = (uint64_t)us * freq / 1000000ULL;
+
+	while (read_counter() - start < ticks)
+		;
+}
+
+void tb_mdelay(uint32_t ms)
+{
+	tb_udelay(ms * 1000);
+}