/* SPDX-License-Identifier: Apache-2.0
 * Copyright 2024 The TenonOS Authors
 */

#include <uk/config.h>
#include <uk/arch/lcpu.h>
#include <uk/arch/limits.h>
#include <arm/switch_el.h>
#include <d9/sysregs.h>

#define BOOTSTACK_SIZE	4096

.section .bss
.align	16
.space BOOTSTACK_SIZE
lcpu_bootstack:

.section ".text.boot"
ENTRY(_start)
	/* preserve dtb addr */
	mov 	x25, x0

	/* detect EL level */
	switch_el x0, el2_entry, el1_entry

el2_entry:
	ldr	x0, =SCTLR_EL2_VALUE
	msr	sctlr_el2, x0
	isb

	// define register width, el1: aarch64, el0: by code. other bits are 0.
	ldr	x0, =HCR_EL2_VALUE
	msr	hcr_el2, x0

	// Set the SPSR state to restore when returning from EL2 to EL1
	ldr	x0, =SPSR_EL2_VALUE
	msr	spsr_el2, x0

	// set return address when returning from EL2 to EL1
	adr	x0, el1_entry
	msr	elr_el2, x0

	eret

el1_entry:
	// disable mmu and cache
	mrs	x2, sctlr_el1
	mov	x3, #SCTLR_EL1_M_BIT|SCTLR_EL1_C_BIT
	bic	x2, x2, x3
	msr	sctlr_el1, x2

	// Disable coprocessor traps
	ldr	x0, =CPACR_EL1_VALUE
	msr	cpacr_el1, x0

	/*
	 * We will disable MMU and cache before the pagetables are ready.
	 * This means we will change memory with cache disabled, so we need to
	 * invalidate the cache to ensure there is no stale data in it.
	 * But it would be expensive to invalidate the whole cache.
	 * In this case, just need to invalidate what we are going to use:
	 * DTB, TEXT, DATA, BSS, and bootstack.
	 */
	ldr	x0, =_start_ram_addr
	ldr	x1, =_end
	sub	x1, x1, x0
	bl	clean_and_invalidate_dcache_range

	/* Enable the mmu */
	bl	start_mmu

	bl	clear_bss

	ldr	x0, =lcpu_bootstack
	and	x0, x0, ~(__STACK_ALIGN_SIZE - 1)
	mov	sp, x0

	/* Set exception vector table*/
	ldr	x0, =vectors_el1
	msr	vbar_el1, x0

	/* Set the context id */
	msr	contextidr_el1, xzr

	bl	set_bootstrap_cpu

	ldr	x0, =lcpus
	msr	tpidr_el1, x0

	/* Load dtb address to x0 as a parameter */
	mov	x0, x25

	bl	_libd9plat_entry
END(_start)