// See LICENSE for license details

#ifndef KL3_ENTRY_S
#define KL3_ENTRY_S

#include "encoding.h"
#include "bits.h"

.section      .iram

#define MSTATUS_PRV1 0x1800

/* When trap is an interrupt, this function is called */
interrupt:
    .global pxCurrentTCB
    /* make room in stack */
#ifndef __riscv_float_abi_soft
    addi    sp, sp, -REGBYTES * 64
#else
    addi    sp, sp, -REGBYTES * 32
#endif
    /* Save Context */
    STORE   x1, 0x0(sp)
    STORE   x2, 1 * REGBYTES(sp)
    STORE   x3, 2 * REGBYTES(sp)
    STORE   x4, 3 * REGBYTES(sp)
    STORE   x6, 5 * REGBYTES(sp)
    STORE   x7, 6 * REGBYTES(sp)
    STORE   x8, 30 * REGBYTES(sp)
    STORE   x9, 8 * REGBYTES(sp)
    STORE   x10, 9 * REGBYTES(sp)
    STORE   x11, 10 * REGBYTES(sp)
    STORE   x12, 11 * REGBYTES(sp)
    STORE   x13, 12 * REGBYTES(sp)
    STORE   x14, 13 * REGBYTES(sp)
    STORE   x15, 14 * REGBYTES(sp)
    STORE   x16, 15 * REGBYTES(sp)
    STORE   x17, 16 * REGBYTES(sp)
    STORE   x18, 17 * REGBYTES(sp)
    STORE   x19, 18 * REGBYTES(sp)
    STORE   x20, 19 * REGBYTES(sp)
    STORE   x21, 20 * REGBYTES(sp)
    STORE   x22, 21 * REGBYTES(sp)
    STORE   x23, 22 * REGBYTES(sp)
    STORE   x24, 23 * REGBYTES(sp)
    STORE   x25, 24 * REGBYTES(sp)
    STORE   x26, 25 * REGBYTES(sp)
    STORE   x27, 26 * REGBYTES(sp)
    STORE   x28, 27 * REGBYTES(sp)
    STORE   x29, 28 * REGBYTES(sp)
    STORE   x30, 29 * REGBYTES(sp)
    STORE   x31, 7 * REGBYTES(sp)
#ifndef __riscv_float_abi_soft
    fsw   f0, 31 * REGBYTES(sp)
    fsw   f1, 32 * REGBYTES(sp)
    fsw   f2, 33 * REGBYTES(sp)
    fsw   f3, 34 * REGBYTES(sp)
    fsw   f4, 35 * REGBYTES(sp)
    fsw   f5, 36 * REGBYTES(sp)
    fsw   f6, 37 * REGBYTES(sp)
    fsw   f7, 38 * REGBYTES(sp)
    fsw   f8, 39 * REGBYTES(sp)
    fsw   f9, 40 * REGBYTES(sp)
    fsw   f10, 41 * REGBYTES(sp)
    fsw   f11, 42 * REGBYTES(sp)
    fsw   f12, 43 * REGBYTES(sp)
    fsw   f13, 44 * REGBYTES(sp)
    fsw   f14, 45 * REGBYTES(sp)
    fsw   f15, 46 * REGBYTES(sp)
    fsw   f16, 47 * REGBYTES(sp)
    fsw   f17, 48 * REGBYTES(sp)
    fsw   f18, 49 * REGBYTES(sp)
    fsw   f19, 50 * REGBYTES(sp)
    fsw   f20, 51 * REGBYTES(sp)
    fsw   f21, 52 * REGBYTES(sp)
    fsw   f22, 53 * REGBYTES(sp)
    fsw   f23, 54 * REGBYTES(sp)
    fsw   f24, 55 * REGBYTES(sp)
    fsw   f25, 56 * REGBYTES(sp)
    fsw   f26, 57 * REGBYTES(sp)
    fsw   f27, 58 * REGBYTES(sp)
    fsw   f28, 59 * REGBYTES(sp)
    fsw   f29, 60 * REGBYTES(sp)
    fsw   f30, 61 * REGBYTES(sp)
    fsw   f31, 62 * REGBYTES(sp)
#endif
    csrr    x5,  mscratch
    STORE   x5,  4 * REGBYTES(sp)
    /* Store current stackpointer in task control block (TCB) */
    LOAD    t0, pxCurrentTCB    //pointer

#if RISCV_SMP_ENABLE
    csrr    t1, mhartid
    li      t2, 1
    beq     t1, t2, 3f
    LOAD    t0, pxCurrentTCB+4
3:
#endif

    STORE   sp, 0x0(t0)
    /* Saves current error program counter (EPC) as task program counter */
    csrr    t0, mepc
#ifndef __riscv_float_abi_soft
    STORE   t0, 63 * REGBYTES(sp)
#else
    STORE   t0, 31 * REGBYTES(sp)
#endif
    /* Interrupt handling*/
    csrr    a0, mcause
    csrr    a1, mepc
    mv      a2, sp
    call    handle_interrupt
    NOP
    /* Load stack pointer from the current TCB */
    LOAD    sp, pxCurrentTCB

#if RISCV_SMP_ENABLE
    csrr    t1, mhartid
    li      t0, 1
    beq     t1, t0, 3f
    LOAD    sp, pxCurrentTCB+4
3:
#endif
    LOAD    sp, 0x0(sp)
    /* Load task program counter */
#ifndef __riscv_float_abi_soft
    LOAD    t0, 63 * REGBYTES(sp)
#else
    LOAD    t0, 31 * REGBYTES(sp)
#endif
    csrw    mepc, t0
    /* Run in machine mode */
    li      t0, MSTATUS_PRV1
    csrs    mstatus, t0
    /* Restore registers, Skip global pointer because that does not change */
    LOAD    x1, 0x0(sp)
    LOAD    x4, 3 * REGBYTES(sp)
    LOAD    x5, 4 * REGBYTES(sp)
    LOAD    x6, 5 * REGBYTES(sp)
    LOAD    x7, 6 * REGBYTES(sp)
    LOAD    x8, 30 * REGBYTES(sp)
    LOAD    x9, 8 * REGBYTES(sp)
    LOAD    x10, 9 * REGBYTES(sp)
    LOAD    x11, 10 * REGBYTES(sp)
    LOAD    x12, 11 * REGBYTES(sp)
    LOAD    x13, 12 * REGBYTES(sp)
    LOAD    x14, 13 * REGBYTES(sp)
    LOAD    x15, 14 * REGBYTES(sp)
    LOAD    x16, 15 * REGBYTES(sp)
    LOAD    x17, 16 * REGBYTES(sp)
    LOAD    x18, 17 * REGBYTES(sp)
    LOAD    x19, 18 * REGBYTES(sp)
    LOAD    x20, 19 * REGBYTES(sp)
    LOAD    x21, 20 * REGBYTES(sp)
    LOAD    x22, 21 * REGBYTES(sp)
    LOAD    x23, 22 * REGBYTES(sp)
    LOAD    x24, 23 * REGBYTES(sp)
    LOAD    x25, 24 * REGBYTES(sp)
    LOAD    x26, 25 * REGBYTES(sp)
    LOAD    x27, 26 * REGBYTES(sp)
    LOAD    x28, 27 * REGBYTES(sp)
    LOAD    x29, 28 * REGBYTES(sp)
    LOAD    x30, 29 * REGBYTES(sp)
    LOAD    x31, 7 * REGBYTES(sp)
#ifndef __riscv_float_abi_soft
    flw   f0, 31 * REGBYTES(sp)
    flw   f1, 32 * REGBYTES(sp)
    flw   f2, 33 * REGBYTES(sp)
    flw   f3, 34 * REGBYTES(sp)
    flw   f4, 35 * REGBYTES(sp)
    flw   f5, 36 * REGBYTES(sp)
    flw   f6, 37 * REGBYTES(sp)
    flw   f7, 38 * REGBYTES(sp)
    flw   f8, 39 * REGBYTES(sp)
    flw   f9, 40 * REGBYTES(sp)
    flw   f10, 41 * REGBYTES(sp)
    flw   f11, 42 * REGBYTES(sp)
    flw   f12, 43 * REGBYTES(sp)
    flw   f13, 44 * REGBYTES(sp)
    flw   f14, 45 * REGBYTES(sp)
    flw   f15, 46 * REGBYTES(sp)
    flw   f16, 47 * REGBYTES(sp)
    flw   f17, 48 * REGBYTES(sp)
    flw   f18, 49 * REGBYTES(sp)
    flw   f19, 50 * REGBYTES(sp)
    flw   f20, 51 * REGBYTES(sp)
    flw   f21, 52 * REGBYTES(sp)
    flw   f22, 53 * REGBYTES(sp)
    flw   f23, 54 * REGBYTES(sp)
    flw   f24, 55 * REGBYTES(sp)
    flw   f25, 56 * REGBYTES(sp)
    flw   f26, 57 * REGBYTES(sp)
    flw   f27, 58 * REGBYTES(sp)
    flw   f28, 59 * REGBYTES(sp)
    flw   f29, 60 * REGBYTES(sp)
    flw   f30, 61 * REGBYTES(sp)
    flw   f31, 62 * REGBYTES(sp)
#endif
#ifndef __riscv_float_abi_soft
    addi    sp, sp, REGBYTES * 64
#else
    addi    sp, sp, REGBYTES * 32
#endif
    mret

.section      .iram.entry
.align 4
.global kl3_trap_entry

kl3_trap_entry:
    /* Interrupt trap */
    csrw   mscratch, t0
    csrr   t0, mcause
    blt    t0, x0, interrupt

    /* System call and other traps */
    csrrw   t0, mscratch, sp
    la      sp, _trap_sp
#ifndef __riscv_float_abi_soft
    addi    sp, sp, -REGBYTES*64
#else
    addi    sp, sp, -REGBYTES*32
#endif
    STORE   x1, 0*REGBYTES(sp)
    STORE   x2, 1*REGBYTES(sp)
    STORE   x3, 2*REGBYTES(sp)
    STORE   x4, 3*REGBYTES(sp)
    STORE   x5, 4*REGBYTES(sp)
    STORE   x6, 5*REGBYTES(sp)
    STORE   x7, 6*REGBYTES(sp)
    STORE   x8, 30*REGBYTES(sp)
    STORE   x9, 8*REGBYTES(sp)
    STORE   x10, 9*REGBYTES(sp)
    STORE   x11, 10*REGBYTES(sp)
    STORE   x12, 11*REGBYTES(sp)
    STORE   x13, 12*REGBYTES(sp)
    STORE   x14, 13*REGBYTES(sp)
    STORE   x15, 14*REGBYTES(sp)
    STORE   x16, 15*REGBYTES(sp)
    STORE   x17, 16*REGBYTES(sp)
    STORE   x18, 17*REGBYTES(sp)
    STORE   x19, 18*REGBYTES(sp)
    STORE   x20, 19*REGBYTES(sp)
    STORE   x21, 20*REGBYTES(sp)
    STORE   x22, 21*REGBYTES(sp)
    STORE   x23, 22*REGBYTES(sp)
    STORE   x24, 23*REGBYTES(sp)
    STORE   x25, 24*REGBYTES(sp)
    STORE   x26, 25*REGBYTES(sp)
    STORE   x27, 26*REGBYTES(sp)
    STORE   x28, 27*REGBYTES(sp)
    STORE   x29, 28*REGBYTES(sp)
    STORE   x30, 29*REGBYTES(sp)
    STORE   x31, 7*REGBYTES(sp)
#ifndef __riscv_float_abi_soft
    fsw   f0, 31 * REGBYTES(sp)
    fsw   f1, 32 * REGBYTES(sp)
    fsw   f2, 33 * REGBYTES(sp)
    fsw   f3, 34 * REGBYTES(sp)
    fsw   f4, 35 * REGBYTES(sp)
    fsw   f5, 36 * REGBYTES(sp)
    fsw   f6, 37 * REGBYTES(sp)
    fsw   f7, 38 * REGBYTES(sp)
    fsw   f8, 39 * REGBYTES(sp)
    fsw   f9, 40 * REGBYTES(sp)
    fsw   f10, 41 * REGBYTES(sp)
    fsw   f11, 42 * REGBYTES(sp)
    fsw   f12, 43 * REGBYTES(sp)
    fsw   f13, 44 * REGBYTES(sp)
    fsw   f14, 45 * REGBYTES(sp)
    fsw   f15, 46 * REGBYTES(sp)
    fsw   f16, 47 * REGBYTES(sp)
    fsw   f17, 48 * REGBYTES(sp)
    fsw   f18, 49 * REGBYTES(sp)
    fsw   f19, 50 * REGBYTES(sp)
    fsw   f20, 51 * REGBYTES(sp)
    fsw   f21, 52 * REGBYTES(sp)
    fsw   f22, 53 * REGBYTES(sp)
    fsw   f23, 54 * REGBYTES(sp)
    fsw   f24, 55 * REGBYTES(sp)
    fsw   f25, 56 * REGBYTES(sp)
    fsw   f26, 57 * REGBYTES(sp)
    fsw   f27, 58 * REGBYTES(sp)
    fsw   f28, 59 * REGBYTES(sp)
    fsw   f29, 60 * REGBYTES(sp)
    fsw   f30, 61 * REGBYTES(sp)
    fsw   f31, 62 * REGBYTES(sp)
#endif
    csrr    a0, mcause
    csrr    a1, mepc
    mv      a2, sp
#ifndef __riscv_float_abi_soft
    STORE   a1, 63*REGBYTES(sp)
#else
    STORE   a1, 31*REGBYTES(sp)
#endif
    call    kl3_handle_trap
    csrw    mepc, a0
    # Remain in M-mode after mret
    li      t0, MSTATUS_MPP
    csrs    mstatus, t0
    LOAD    x1, 0*REGBYTES(sp)
    LOAD    x2, 1*REGBYTES(sp)
    LOAD    x3, 2*REGBYTES(sp)
    LOAD    x4, 3*REGBYTES(sp)
    LOAD    x5, 4*REGBYTES(sp)
    LOAD    x6, 5*REGBYTES(sp)
    LOAD    x7, 6*REGBYTES(sp)
    LOAD    x8, 30*REGBYTES(sp)
    LOAD    x9, 8*REGBYTES(sp)
    LOAD    x10, 9*REGBYTES(sp)
    LOAD    x11, 10*REGBYTES(sp)
    LOAD    x12, 11*REGBYTES(sp)
    LOAD    x13, 12*REGBYTES(sp)
    LOAD    x14, 13*REGBYTES(sp)
    LOAD    x15, 14*REGBYTES(sp)
    LOAD    x16, 15*REGBYTES(sp)
    LOAD    x17, 16*REGBYTES(sp)
    LOAD    x18, 17*REGBYTES(sp)
    LOAD    x19, 18*REGBYTES(sp)
    LOAD    x20, 19*REGBYTES(sp)
    LOAD    x21, 20*REGBYTES(sp)
    LOAD    x22, 21*REGBYTES(sp)
    LOAD    x23, 22*REGBYTES(sp)
    LOAD    x24, 23*REGBYTES(sp)
    LOAD    x25, 24*REGBYTES(sp)
    LOAD    x26, 25*REGBYTES(sp)
    LOAD    x27, 26*REGBYTES(sp)
    LOAD    x28, 27*REGBYTES(sp)
    LOAD    x29, 28*REGBYTES(sp)
    LOAD    x30, 29*REGBYTES(sp)
    LOAD    x31, 7*REGBYTES(sp)
#ifndef __riscv_float_abi_soft
    flw   f0, 31 * REGBYTES(sp)
    flw   f1, 32 * REGBYTES(sp)
    flw   f2, 33 * REGBYTES(sp)
    flw   f3, 34 * REGBYTES(sp)
    flw   f4, 35 * REGBYTES(sp)
    flw   f5, 36 * REGBYTES(sp)
    flw   f6, 37 * REGBYTES(sp)
    flw   f7, 38 * REGBYTES(sp)
    flw   f8, 39 * REGBYTES(sp)
    flw   f9, 40 * REGBYTES(sp)
    flw   f10, 41 * REGBYTES(sp)
    flw   f11, 42 * REGBYTES(sp)
    flw   f12, 43 * REGBYTES(sp)
    flw   f13, 44 * REGBYTES(sp)
    flw   f14, 45 * REGBYTES(sp)
    flw   f15, 46 * REGBYTES(sp)
    flw   f16, 47 * REGBYTES(sp)
    flw   f17, 48 * REGBYTES(sp)
    flw   f18, 49 * REGBYTES(sp)
    flw   f19, 50 * REGBYTES(sp)
    flw   f20, 51 * REGBYTES(sp)
    flw   f21, 52 * REGBYTES(sp)
    flw   f22, 53 * REGBYTES(sp)
    flw   f23, 54 * REGBYTES(sp)
    flw   f24, 55 * REGBYTES(sp)
    flw   f25, 56 * REGBYTES(sp)
    flw   f26, 57 * REGBYTES(sp)
    flw   f27, 58 * REGBYTES(sp)
    flw   f28, 59 * REGBYTES(sp)
    flw   f29, 60 * REGBYTES(sp)
    flw   f30, 61 * REGBYTES(sp)
    flw   f31, 62 * REGBYTES(sp)
#endif
#ifndef __riscv_float_abi_soft
    addi    sp, sp, REGBYTES*64
#else
    addi    sp, sp, REGBYTES*32
#endif
    csrr    sp, mscratch
    mret

.weak kl3_handle_trap
kl3_handle_trap:
1:
    j   1b
#endif