/*
 * Copyright (C) 2018, bzt (bztsrc@github), https://github.com/bztsrc/raspi3-tutorial
 * Copyright (c) 2018, Sergey Matyukevich <https://github.com/s-matyukevich/raspberry-pi-os>
 *           (c) 2020, Santiago Pagani <santiagopagani@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 */

#include <raspi/sysregs.h>
#include <raspi/mm.h>
#include <raspi/mmu.h>
#include <uk/config.h>
#include <uk/arch/lcpu.h>

#include <arm/arm64/pagetable.S>

.section ".text.boot"

.global _start
_start:
	mov x25, x0 /* preserve dtb addr */

	// 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
	ldr	x0, =SCTLR_EL1_VALUE_MMU_DISABLED
	msr	sctlr_el1, x0

	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, =VA_START
	ldr 	x2, =_end
	sub	x1, x2, x0
	bl 	clean_and_invalidate_dcache_range
	dsb 	sy

	ldr 	x0, =vectors_el1
	msr 	vbar_el1, x0

	/* Enable the mmu */
	bl start_mmu

#if CONFIG_RASPI_WATERMARK_STACK
watermark_stack_start:
	ldr     x1, =VA_START
	ldr     w2, =0x10000
watermark_stack_loop:
	cbz     w2, watermark_stack_done
	str     x2, [x1], #8
	sub     w2, w2, #1
	cbnz    w2, watermark_stack_loop
watermark_stack_done:
#endif


clear_bss_start:
	// Clear bss
	ldr     x1, =__bss_start
	ldr     w2, =__bss_size
clear_bss_loop:
	cbz     w2, clear_bss_done
	str     xzr, [x1], #8
	sub     w2, w2, #1
	cbnz    w2, clear_bss_loop
clear_bss_done:
	// Set the stack before our code, use SP_ELx for exception level ELx
	msr	SPSel, #1
    	ldr     x1, =_start
	// stack space: _start ~ 0x0
	mov	sp, x1


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

	bl _libraspiplat_entry

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