OSDev.org

#include <asm.h>

IMPORT_ASM(_cpu_el3_vec_tbl_set)
IMPORT_ASM(_cpu_el2_vec_tbl_set)
IMPORT_ASM(_cpu_el1_vec_tbl_set)

IMPORT_C(init)
IMPORT_C(main)

.text

ENTRY(_start)
    mrs x0, MPIDR_EL1
    and x0, x0, #0x3
    cmp x0, #0
    beq __elx

__wfe_cpu1_3:
    wfe
    b __wfe_cpu1_3
   
__elx:
__el3:
    mrs x0, CurrentEL
    and x0, x0, #0xC
    asr x0, x0, #2
    cmp x0, #3
    bne __el2
__el3_stack:
    ldr x0, =_stack_el2_e
    mov sp, x0
__el3_vector:
    bl _cpu_el3_vec_tbl_set
    msr SCTLR_EL2, xzr
    msr HCR_EL2, xzr
    mrs x0, SCR_EL3
    orr x0, x0, #(1<<10)
    orr x0, x0, #(1<<0)
    msr SCR_EL3, x0
    mov x0, #0b01001
    msr SPSR_EL3, x0
    adr x0, __el2
    msr ELR_EL3, x0
    eret
   
__el2:
    mrs x0, CurrentEL
    and x0, x0, #0xC
    asr x0, x0, #2
    cmp x0, #2
    bne __el1
__el2_stack:
    ldr x0, =_stack_el2_e
    mov sp, x0
__el2_vector:
    bl _cpu_el2_vec_tbl_set
    msr SCTLR_EL1, xzr
    mrs x0, HCR_EL2
    orr x0, x0, #(1<<31)
    msr HCR_EL2, x0
    mov x0, xzr
    orr x0, x0, #(7 << 6)
    orr x0, x0, #(1 << 2)
    orr x0, x0, #(1 << 0)
    msr SPSR_EL2, x0
    ldr x0, =__el1
    msr ELR_EL2, x0
    eret

__el1:
__el1_stack:
    ldr x0, =_stack_el1_e
    mov sp, x0
__el1_vector:
    bl _cpu_el1_vec_tbl_set
   
    ldr x0, =_bss_s
    ldr x1, =_bss_e
    sub x1, x1, x0
    mov x2, #0x0
    cbz x1, __init
__bss:
    strb w2, [x0], #1
    sub x1, x1, #1
    cbnz x1, __bss
__init:
    bl init

__main:
    bl main
__main_wfe:
    wfe
    b __main_wfe

.end