.section .text.vectors, "ax"
.align 11
.global __vectors_el2
__vectors_el2:
    b vector_current_sync
    .org __vectors_el2 + 0x080
    b vector_current_irq
    .org __vectors_el2 + 0x100
    b vector_current_fiq
    .org __vectors_el2 + 0x180
    b vector_current_serror
    .org __vectors_el2 + 0x200
    b vector_current_sync
    .org __vectors_el2 + 0x280
    b vector_current_irq
    .org __vectors_el2 + 0x300
    b vector_current_fiq
    .org __vectors_el2 + 0x380
    b vector_current_serror
    .org __vectors_el2 + 0x400
    b vector_lower_sync
    .org __vectors_el2 + 0x480
    b vector_lower_irq
    .org __vectors_el2 + 0x500
    b vector_lower_fiq
    .org __vectors_el2 + 0x580
    b vector_lower_serror
    .org __vectors_el2 + 0x600
    b vector_lower_sync
    .org __vectors_el2 + 0x680
    b vector_lower_irq
    .org __vectors_el2 + 0x700
    b vector_lower_fiq
    .org __vectors_el2 + 0x780
    b vector_lower_serror

.macro SAVE_FRAME
    sub sp, sp, #304
    stp x0, x1, [sp, #0]
    stp x2, x3, [sp, #16]
    stp x4, x5, [sp, #32]
    stp x6, x7, [sp, #48]
    stp x8, x9, [sp, #64]
    stp x10, x11, [sp, #80]
    stp x12, x13, [sp, #96]
    stp x14, x15, [sp, #112]
    stp x16, x17, [sp, #128]
    stp x18, x19, [sp, #144]
    stp x20, x21, [sp, #160]
    stp x22, x23, [sp, #176]
    stp x24, x25, [sp, #192]
    stp x26, x27, [sp, #208]
    stp x28, x29, [sp, #224]
    str x30, [sp, #240]
    mrs x1, sp_el0
    str x1, [sp, #248]
    mrs x1, sp_el1
    str x1, [sp, #256]
    mrs x1, elr_el2
    str x1, [sp, #264]
    mrs x1, spsr_el2
    str x1, [sp, #272]
    mrs x1, esr_el2
    str x1, [sp, #280]
    mrs x1, far_el2
    str x1, [sp, #288]
    mrs x1, hpfar_el2
    str x1, [sp, #296]
.endm

.macro RESTORE_FRAME
    ldr x1, [sp, #248]
    msr sp_el0, x1
    ldr x1, [sp, #256]
    msr sp_el1, x1
    ldr x1, [sp, #264]
    msr elr_el2, x1
    ldr x1, [sp, #272]
    msr spsr_el2, x1
    ldp x0, x1, [sp, #0]
    ldp x2, x3, [sp, #16]
    ldp x4, x5, [sp, #32]
    ldp x6, x7, [sp, #48]
    ldp x8, x9, [sp, #64]
    ldp x10, x11, [sp, #80]
    ldp x12, x13, [sp, #96]
    ldp x14, x15, [sp, #112]
    ldp x16, x17, [sp, #128]
    ldp x18, x19, [sp, #144]
    ldp x20, x21, [sp, #160]
    ldp x22, x23, [sp, #176]
    ldp x24, x25, [sp, #192]
    ldp x26, x27, [sp, #208]
    ldp x28, x29, [sp, #224]
    ldr x30, [sp, #240]
    add sp, sp, #304
.endm

vector_lower_sync:
    SAVE_FRAME
    mov x0, sp
    bl trap_handle_lower_sync
    RESTORE_FRAME
    eret

vector_lower_irq:
vector_lower_fiq:
vector_lower_serror:
vector_current_irq:
vector_current_fiq:
vector_current_serror:
    SAVE_FRAME
    mov x0, sp
    bl trap_handle_irq
    RESTORE_FRAME
    eret

vector_current_sync:
    SAVE_FRAME
    mov x0, sp
    bl trap_handle_current_sync
    RESTORE_FRAME
    eret