2 files changed, 94 insertions, 0 deletions
diff --git a/common/main.c b/common/main.c
index fb388cb..8d5ee63 100644
--- a/common/main.c
+++ b/common/main.c
@@ -61,6 +61,23 @@ void tashaboot_main(uintptr_t fw_arg)
 
 	dprintf(ALWAYS, "tashaboot " TB_VERSION "\n");
 
+#ifdef TB_ENABLE_MMU
+	{
+		extern int tb_mmu_enable(void);
+		extern int tb_mmu_selftest(void);
+		extern void tb_mmu_disable(void);
+
+		if (tb_mmu_enable() == 0) {
+			if (tb_mmu_selftest() == 0)
+				dprintf(ALWAYS, "mmu: identity map on\n");
+			else
+				dprintf(ALWAYS, "mmu: self test failed, "
+					 "running unmapped\n");
+			tb_mmu_disable();
+		}
+	}
+#endif
+
 	ret = tb_load_semihosting(TB_BOOTFILE, TB_LOAD_ADDR, &img);
 	if (ret) {
 		dprintf(ALWAYS, "load failed (%d), halting\n", ret);
diff --git a/common/mmutest.c b/common/mmutest.c
new file mode 100644
index 0000000..1b60110
--- /dev/null
+++ b/common/mmutest.c
@@ -0,0 +1,77 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * mmutest.c - self test for the identity map, AT S1E2R translates a
+ * VA through the tables and PAR_EL1 returns the walk result. if the
+ * map is wrong the instruction faults to our vectors instead, so a
+ * clean return with a valid PA in PAR means the tables walk.
+ *
+ * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
+ */
+
+#include <asm/mmu.h>
+#include <debug.h>
+
+#define PAR_F		(1ULL << 0)	/* fault, no translation */
+#define PAR_PA_MASK	0x000ffffffffff000ULL
+
+static uint64_t translate(uint64_t va)
+{
+	uint64_t par, el;
+
+	asm volatile("mrs %0, CurrentEL" : "=r" (el));
+	el >>= 2;
+
+	if (el == 2)
+		asm volatile(
+			"at	s1e2r, %1\n"
+			"isb\n"
+			"mrs	%0, par_el1\n"
+			: "=r" (par)
+			: "r" (va)
+			: "memory");
+	else
+		asm volatile(
+			"at	s1e1r, %1\n"
+			"isb\n"
+			"mrs	%0, par_el1\n"
+			: "=r" (par)
+			: "r" (va)
+			: "memory");
+	return par;
+}
+
+static int check(const char *name, uint64_t va)
+{
+	uint64_t par = translate(va);
+
+	if (par & PAR_F) {
+		dprintf(ALWAYS, "mmu: %s faulted (par 0x%016llx)\n",
+			name, (unsigned long long)par);
+		return 1;
+	}
+
+	if ((par & PAR_PA_MASK) != (va & PAR_PA_MASK)) {
+		dprintf(ALWAYS, "mmu: %s pa %llx != va %llx\n",
+			name, (unsigned long long)(par & PAR_PA_MASK),
+			(unsigned long long)va);
+		return 1;
+	}
+
+	dprintf(ALWAYS, "mmu: %s ok, pa %llx\n",
+		name, (unsigned long long)(par & PAR_PA_MASK));
+	return 0;
+}
+
+int tb_mmu_selftest(void)
+{
+	int ret = 0;
+
+	ret |= check("mmio 0x09000000 (uart)", 0x09000000);
+	ret |= check("mmio 0x00000000", 0x00000000);
+	ret |= check("ram  0x40200000 (load)", 0x40200000);
+	ret |= check("ram  0x41000000", 0x41000000);
+	ret |= check("self 0x40080000 (stack guard region, no map)",
+		     0x40080000);
+
+	return ret;
+}