/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * tashaboot arm64 entry. handles whatever EL the firmware left us in,
 * EL3, EL2 or EL1, with the MMU either on or off, and arrives at a
 * clean EL1 with the MMU off before calling C.
 *
 * the secondary cores park, spin table bringup is a later problem.
 *
 * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
 */

#include <asm/macro.h>

.section .text.boot
.globl _start
_start:
	b	reset

	.balign 8
.globl _text_base
_text_base:
	.quad	0x40000000

reset:
	/* keep the dtb pointer before anything clobbers x0 */
	mov	x19, x0

	/* park secondary cores, they have nothing to do yet */
	mrs	x0, mpidr_el1
	and	x0, x0, #0xff
	cbnz	x0, park

	/* which EL are we in, 0x8 per level shifted into bits 3:2 */
	mrs	x0, CurrentEL
	lsr	x0, x0, #2
	cmp	x0, #3
	b.eq	from_el3
	cmp	x0, #2
	b.eq	from_el2
	cmp	x0, #1
	b.eq	mmu_check
	b	park

from_el3:
	/*
	 * EL3 holds the security state. the kernel runs non-secure, so
	 * set SCR_EL3.NS before dropping to EL2, which the kernel
	 * prefers (booting.rst, EL2 RECOMMENDED).
	 */
	mrs	x0, scr_el3
	orr	x0, x0, #1		/* SCR_EL3.NS = 1, non-secure */
	msr	scr_el3, x0
	isb

	mov	x0, #0x3c9		/* EL2h, DAIF masked */
	msr	spsr_el3, x0
	adr	x0, from_el2
	msr	elr_el3, x0
	eret

from_el2:
	/*
	 * stay at EL2: the kernel wants it for the virtualization
	 * extensions and hands off from there. everything below scrubs
	 * the EL2 state so the kernel starts clean.
	 */

	/* EL1 will be aarch64 when the kernel drops itself down */
	mov	x0, #(1 << 31)		/* HCR_EL2.RW = 1 */
	msr	hcr_el2, x0

	/* let EL1 reach the counter, booting.rst demands it */
	mrs	x0, cnthctl_el2
	orr	x0, x0, #(3 << 0)	/* EL1PCTEN | EL1PCEN */
	msr	cnthctl_el2, x0

	/* no traps to EL2 behind EL1's back */
	msr	cptr_el2, xzr
	msr	hstr_el2, xzr
	msr	vpidr_el2, xzr

	b	mmu_check

mmu_check:
	/*
	 * whether the firmware left an MMU on: M bit, bit 0, of sctlr at
	 * the current EL. writing the register off would not fault, but
	 * the page tables it built are in its own memory, better to kill
	 * it here than trip over a stale mapping.
	 */
	mrs	x0, CurrentEL
	lsr	x0, x0, #2
	cmp	x0, #2
	b.lt	mmu_el1
	mrs	x0, sctlr_el2
	tbz	x0, #0, c_entry

	mov	x0, xzr
	msr	sctlr_el2, x0
	isb
	tlbi	alle2
	dsb	sy
	isb
	b	c_entry

mmu_el1:
	mrs	x0, sctlr_el1
	tbz	x0, #0, c_entry

	mov	x0, xzr
	msr	sctlr_el1, x0
	isb
	ic	iallu
	dsb	sy
	tlbi	vmalle1
	dsb	sy
	isb

c_entry:
	/*
	 * program the counter frequency, the kernel reads CNTFRQ right
	 * away (booting.rst). qemu virt runs the system counter at
	 * 62.5 MHz. the register is RW only at the highest implemented EL.
	 */
	mrs	x0, CurrentEL
	lsr	x0, x0, #2
	cmp	x0, #2
	b.lt	1f
	ldr	x0, =62500000
	msr	cntfrq_el0, x0
	isb
1:
	/* our own vectors, so aborts print instead of vanishing */
	adr	x0, vectors
	mrs	x1, CurrentEL
	lsr	x1, x1, #2
	cmp	x1, #2
	b.lt	2f
	msr	vbar_el2, x0
	b	3f
2:
	msr	vbar_el1, x0
3:
	isb

	/* stack for the bootloader, its own region above the bss */
	ldr	x0, =__stack_top
	mov	sp, x0

	/* clear bss */
	ldr	x0, =__bss_start
	ldr	x1, =__bss_end
1:	cmp	x0, x1
	b.hs	2f
	str	xzr, [x0], #8
	b	1b
2:

	/* FP/SIMD access, some kernels assume it is on */
	mov	x0, #(3 << 20)
	msr	cpacr_el1, x0
	isb

	/* dtb pointer into C arg 0 */
	mov	x0, x19
	bl	tashaboot_main

	/* if main returns there is nothing sensible to do */
park:
	wfe
	b	park

/*
 * exception vectors, the armv8 layout: 16 slots, 128 bytes each, in
 * the order the manual fixes. taken from EL1h the interesting slots
 * are 0x200 sync and 0x380 SError, irq and fiq just park, the
 * bootloader never enables interrupts on purpose.
 */
	.balign	2048
vectors:
	/* 0x000: current EL, SP_EL0 */
	.align	7
	b	exc_sync
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_serr

	/* 0x200: current EL, SP_ELx */
	.align	7
	b	exc_sync
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_serr

	/* 0x400: lower EL, AArch64 */
	.align	7
	b	exc_sync
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_serr

	/* 0x600: lower EL, AArch32 */
	.align	7
	b	exc_sync
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_park_irq
	.align	7
	b	exc_serr

exc_sync:
	stp	x29, x30, [sp, #-16]!
	mov	x29, sp
	mrs	x3, CurrentEL
	lsr	x3, x3, #2
	cmp	x3, #2
	b.lt	1f
	mrs	x0, esr_el2
	mrs	x1, far_el2
	b	2f
1:
	mrs	x0, esr_el1
	mrs	x1, far_el1
2:
	mov	x2, lr
	bl	exc_report
	ldp	x29, x30, [sp], #16
	b	park

exc_serr:
	stp	x29, x30, [sp, #-16]!
	mov	x29, sp
	mrs	x3, CurrentEL
	lsr	x3, x3, #2
	cmp	x3, #2
	b.lt	1f
	mrs	x0, esr_el2
	b	2f
1:
	mrs	x0, esr_el1
2:
	mov	x1, #0
	mov	x2, lr
	bl	exc_report
	ldp	x29, x30, [sp], #16
	b	park

exc_park_irq:
	b	park