/* 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;
}