.section .text.boot
	.global __start
	.type __start, %function

// From https://developer.arm.com/docs/ddi0487/ca/arm-architecture-reference-manual-armv8-for-armv8-a-architecture-profile
// ***************************************
// SCTLR_EL1, System Control Register (EL1), Page 2654 of AArch64-Reference-Manual.
// ***************************************

#define SCTLR_RESERVED                  (3 << 28) | (3 << 22) | (1 << 20) | (1 << 11)
#define SCTLR_EE_LITTLE_ENDIAN          (0 << 25)
#define SCTLR_EOE_LITTLE_ENDIAN         (0 << 24)
#define SCTLR_I_CACHE_DISABLED          (0 << 12)
#define SCTLR_D_CACHE_DISABLED          (0 << 2)
#define SCTLR_MMU_DISABLED              (0 << 0)
#define SCTLR_MMU_ENABLED               (1 << 0)

#define SCTLR_VALUE_MMU_DISABLED	(SCTLR_RESERVED | SCTLR_EE_LITTLE_ENDIAN | SCTLR_I_CACHE_DISABLED | SCTLR_D_CACHE_DISABLED | SCTLR_MMU_DISABLED)

// ***************************************
// HCR_EL2, Hypervisor Configuration Register (EL2), Page 2487 of AArch64-Reference-Manual.
// ***************************************

#define HCR_RW	    			(1 << 31)
#define HCR_VALUE			HCR_RW

// ***************************************
// SCR_EL3, Secure Configuration Register (EL3), Page 2648 of AArch64-Reference-Manual.
// ***************************************

#define SCR_RESERVED	    		(3 << 4)
#define SCR_RW				(1 << 10)
#define SCR_NS				(1 << 0)
#define SCR_VALUE	    	    	(SCR_RESERVED | SCR_RW | SCR_NS)

// ***************************************
// SPSR_EL3, Saved Program Status Register (EL3) Page 389 of AArch64-Reference-Manual.
// ***************************************

#define SPSR_MASK_ALL 			(7 << 6)
#define SPSR_EL1h			(5 << 0)
#define SPSR_VALUE			(SPSR_MASK_ALL | SPSR_EL1h)

#define SIMD_ENABLE			(3 << 20)

__start:
	//Use core 0 only
	mrs	x7, mpidr_el1
	and		x7, x7, #3
	cbz		x7, __switch_level
0: wfe
	b			0b

__switch_level:
	mrs		x0, CurrentEL
        lsr		x0, x0, #2 // Shift Right by 2
	and		x0, x0, #3
	cmp		x0, #3
	beq		__switch_l3_l1
	b __switch_l2_l1

__switch_l3_l1:
    ldr    x0, =SCTLR_VALUE_MMU_DISABLED
    msr    sctlr_el1, x0

    ldr    x0, =HCR_VALUE
    msr    hcr_el2, x0

    ldr    x0, =SCR_VALUE
    msr    scr_el3, x0

    ldr    x0, =SPSR_VALUE
    msr    spsr_el3, x0

    mrs    x0, CPACR_EL1
    and	   x0, x0, #SIMD_ENABLE
    msr	   CPACR_EL1, x0

    adr    x0, __start_master
    msr    elr_el3, x0

    eret


__switch_l2_l1:
    ldr    x0, =SCTLR_VALUE_MMU_DISABLED
    msr    sctlr_el1, x0

    ldr    x0, =HCR_VALUE
    msr    hcr_el2, x0

    ldr    x0, =SPSR_VALUE
    msr    spsr_el2, x0

    mrs    x0, CPACR_EL1
    and	   x0, x0, #SIMD_ENABLE
    msr	   CPACR_EL1, x0


    adr    x0, __start_master
    msr    elr_el2, x0


    eret

__start_master:
	//Setup stack pointer
	ldr		x2, = __stack_start
	mov		sp, x2

	//Clear BSS
	ldr		w0, = __bss_start
	ldr		w1, = __bss_size
1: cbz	x1, 2f
	str		xzr, [x0], #8
	sub		x1, x1, #1
	cbnz	x1, 1b
2:
	bl		kernelmain
	b			0b