.equ REGBYTES, 0x4


.macro PUSH_ALL_REG
    addi sp, sp, -33*REGBYTES
    sw x1, 0*REGBYTES(sp)     //ra

    sw x2, 1*REGBYTES(sp)     //sp
    sw x3, 2*REGBYTES(sp)     //gp
    sw x4, 3*REGBYTES(sp)     //tp

    sw x5, 4*REGBYTES(sp)     //t0
    sw x6, 5*REGBYTES(sp)     //t1
    sw x7, 6*REGBYTES(sp)     //t2
    sw x8, 30*REGBYTES(sp)
    sw x9, 8*REGBYTES(sp)
    sw x10, 9*REGBYTES(sp)    //a0
    sw x11, 10*REGBYTES(sp)   //a1
    sw x12, 11*REGBYTES(sp)   //a2
    sw x13, 12*REGBYTES(sp)   //a3
    sw x14, 13*REGBYTES(sp)   //a4
    sw x15, 14*REGBYTES(sp)   //a5
    sw x16, 15*REGBYTES(sp)   //a6
    sw x17, 16*REGBYTES(sp)   //a7
    sw x18, 17*REGBYTES(sp)
    sw x19, 18*REGBYTES(sp)
    sw x20, 19*REGBYTES(sp)
    sw x21, 20*REGBYTES(sp)
    sw x22, 21*REGBYTES(sp)
    sw x23, 22*REGBYTES(sp)
    sw x24, 23*REGBYTES(sp)
    sw x25, 24*REGBYTES(sp)
    sw x26, 25*REGBYTES(sp)
    sw x27, 26*REGBYTES(sp)
    sw x28, 27*REGBYTES(sp)   //t3
    sw x29, 28*REGBYTES(sp)   //t4
    sw x30, 29*REGBYTES(sp)   //t5
    sw x31, 7*REGBYTES(sp)    //t6
    csrr  t0, mstatus
    sw  t0, 31*REGBYTES(sp)
    csrr  t0, mepc
    sw  t0, 32*REGBYTES(sp)
.endm

.macro POP_ALL_REG
    lw  t0, 31* REGBYTES(sp)
    csrw  mstatus, t0
    lw  t0, 32 * REGBYTES(sp)
    csrw  mepc, t0
    lw x1, 0*REGBYTES(sp)    //ra

    lw x5, 4*REGBYTES(sp)
    lw x6, 5*REGBYTES(sp)
    lw x7, 6*REGBYTES(sp)
    lw x8, 30*REGBYTES(sp)
    lw x9, 8*REGBYTES(sp)
    lw x10, 9*REGBYTES(sp)
    lw x11, 10*REGBYTES(sp)
    lw x12, 11*REGBYTES(sp)
    lw x13, 12*REGBYTES(sp)
    lw x14, 13*REGBYTES(sp)
    lw x15, 14*REGBYTES(sp)
    lw x16, 15*REGBYTES(sp)
    lw x17, 16*REGBYTES(sp)
    lw x18, 17*REGBYTES(sp)
    lw x19, 18*REGBYTES(sp)
    lw x20, 19*REGBYTES(sp)
    lw x21, 20*REGBYTES(sp)
    lw x22, 21*REGBYTES(sp)
    lw x23, 22*REGBYTES(sp)
    lw x24, 23*REGBYTES(sp)
    lw x25, 24*REGBYTES(sp)
    lw x26, 25*REGBYTES(sp)
    lw x27, 26*REGBYTES(sp)
    lw x28, 27*REGBYTES(sp)
    lw x29, 28*REGBYTES(sp)
    lw x30, 29*REGBYTES(sp)
    lw x31, 7*REGBYTES(sp)
    addi sp, sp, 33*REGBYTES
.endm

# .section .interrupt.text
.extern trap_entry
.global _trap_entry
.align 4
_trap_entry:
    csrw  mscratch, sp
    mv    a1, sp
    la    sp, _exc_sp
    call  trap_entry
    csrr  sp, mscratch
    POP_ALL_REG
    mret


# .section .interrupt.text
.extern trap_entry
.extern interrupt_entry
.global trap_handler
.equ TRAP_INTERRUPT_MODE_MASK, 0x80000000
.align 4
trap_handler:
    PUSH_ALL_REG
    csrr  a0, mcause
    li    a1, TRAP_INTERRUPT_MODE_MASK
    li    a2, 0x000000ff
    and   a1, a0, a1
    and   a0, a2, a0
    beqz  a1, _trap_entry
    csrw  mscratch, sp
    la    sp, _irq_sp
    call   interrupt_entry
    csrr  sp, mscratch

    POP_ALL_REG
    mret