/*
 * Copyright 2024 Hangzhou Yingyi Technology Co., Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <rk3568/sysregs.h>
#include <uk/config.h>
#include <uk/arch/lcpu.h>
#include <uk/arch/limits.h>
#include <arm/arm64/pagetable.S>

#define BOOTSTACK_SIZE	4096

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

.section ".text.boot"

.global _start
_start:

	/* preserve dtb addr */
	mov 	x25, x0

	// read cpu id, stop slave cores
	mrs     x1, mpidr_el1
	and     x1, x1, #3
	cbz     x1, master

// If the cpu id is > 0, hang here
hang:  wfe
    	b       hang

// Continue if cpu id == 0
master:
	// 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

	ldr	x0, =SCTLR_EL2_VALUE
	msr	sctlr_el2, x0
	isb

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

	// 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:
	/*
	 * 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

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

	bl	_librk3568plat_entry

	/* As a failsafe, we also hang the main core */
	b	hang