// vi:ft=asm:

.section .text

// ra+gp, 7 tN, 12 sN, 8 aN
.set GP_REGS_SIZE,          (2 + 7 + 12 + 8) * 8
// U-mode sp, sstatus, sepc, stval, scause, sscratch
.set CTL_REGS_SIZE,         6 * 8
.set TRAP_CONTEXT_SIZE,     (GP_REGS_SIZE) + (CTL_REGS_SIZE)

.set REG_UMODE_SP,          (GP_REGS_SIZE + 0 * 8)
.set REG_SSTATUS,           (GP_REGS_SIZE + 1 * 8)
.set REG_SEPC,              (GP_REGS_SIZE + 2 * 8)
.set REG_STVAL,             (GP_REGS_SIZE + 3 * 8)
.set REG_SCAUSE,            (GP_REGS_SIZE + 4 * 8)
.set REG_SSCRATCH,          (GP_REGS_SIZE + 5 * 8)

.macro SAVE_GP_REGS
    // Save all general-purpose registers, except:
    // * sp (saved elsewhere)
    // * tp (saved elsewhere)
    sd ra,   0 * 8(sp)
    sd gp,   1 * 8(sp)

    sd t0,   2 * 8(sp)
    sd t1,   3 * 8(sp)
    sd t2,   4 * 8(sp)
    sd t3,   5 * 8(sp)
    sd t4,   6 * 8(sp)
    sd t5,   7 * 8(sp)
    sd t6,   8 * 8(sp)

    sd s0,   9 * 8(sp)
    sd s1,  10 * 8(sp)
    sd s2,  11 * 8(sp)
    sd s3,  12 * 8(sp)
    sd s4,  13 * 8(sp)
    sd s5,  14 * 8(sp)
    sd s6,  15 * 8(sp)
    sd s7,  16 * 8(sp)
    sd s8,  17 * 8(sp)
    sd s9,  18 * 8(sp)
    sd s10, 19 * 8(sp)
    sd s11, 20 * 8(sp)

    sd a0,  21 * 8(sp)
    sd a1,  22 * 8(sp)
    sd a2,  23 * 8(sp)
    sd a3,  24 * 8(sp)
    sd a4,  25 * 8(sp)
    sd a5,  26 * 8(sp)
    sd a6,  27 * 8(sp)
    sd a7,  28 * 8(sp)
.endm

.macro LOAD_GP_REGS
    ld ra,   0 * 8(sp)
    ld gp,   1 * 8(sp)

    ld t0,   2 * 8(sp)
    ld t1,   3 * 8(sp)
    ld t2,   4 * 8(sp)
    ld t3,   5 * 8(sp)
    ld t4,   6 * 8(sp)
    ld t5,   7 * 8(sp)
    ld t6,   8 * 8(sp)

    ld s0,   9 * 8(sp)
    ld s1,  10 * 8(sp)
    ld s2,  11 * 8(sp)
    ld s3,  12 * 8(sp)
    ld s4,  13 * 8(sp)
    ld s5,  14 * 8(sp)
    ld s6,  15 * 8(sp)
    ld s7,  16 * 8(sp)
    ld s8,  17 * 8(sp)
    ld s9,  18 * 8(sp)
    ld s10, 19 * 8(sp)
    ld s11, 20 * 8(sp)

    ld a0,  21 * 8(sp)
    ld a1,  22 * 8(sp)
    ld a2,  23 * 8(sp)
    ld a3,  24 * 8(sp)
    ld a4,  25 * 8(sp)
    ld a5,  26 * 8(sp)
    ld a6,  27 * 8(sp)
    ld a7,  28 * 8(sp)
.endm

.macro SMODE_TRAP n, handler
.type __rv64_smode_trap_\n, @function
__rv64_smode_trap_\n:
    // If coming from userspace, sscratch = kernel-mode tp
    // If coming from kernelspace, sscratch = 0
    csrrw tp, sscratch, tp
    bnez tp, 1f

    // Coming from S-mode
    // tp = 0, sscratch contains kernel tp
    csrr tp, sscratch
    sd sp, 16(tp)           // Set proper S-mode sp
1:
    sd sp, 8(tp)            // Store U-mode sp
    ld sp, 16(tp)           // Load S-mode sp

    // Store pre-trap context
    addi sp, sp, -(TRAP_CONTEXT_SIZE)

    SAVE_GP_REGS
    // Save special registers
    ld t0, 8(tp)
    csrr t1, sstatus
    csrr t2, sepc
    csrr t3, stval
    csrr t4, scause
    csrr t5, sscratch

    sd t0, REG_UMODE_SP (sp)
    sd t1, REG_SSTATUS  (sp)
    sd t2, REG_SEPC     (sp)
    sd t3, REG_STVAL    (sp)
    sd t4, REG_SCAUSE   (sp)
    sd t5, REG_SSCRATCH (sp)

    // Reset sscratch to zero to make sure a S-mode -> S-mode nested exception
    // happens properly
    csrw sscratch, zero

    mv a0, sp
    call \handler

    // Return from exception
    ld t0, REG_SSTATUS  (sp)
    andi t0, t0, (1 << 8)
    bnez t0, 2f

    // Return to U-mode
    // Restore SSCRATCH to a proper value
    csrw sscratch, tp
2:
    // Return to S-mode

    // Restore special registers
    ld t0, REG_SSTATUS  (sp)
    ld t1, REG_SEPC     (sp)
    csrw sstatus, t0
    csrw sepc, t1

    // Restore general-purpose registers
    LOAD_GP_REGS

    ld tp, REG_SSCRATCH (sp)
    ld sp, REG_UMODE_SP (sp)

    sret
.size __rv64_smode_trap_\n, . - __rv64_smode_trap_\n
.endm

.option push
.option norvc

.global __rv64_smode_trap_vectors

.type __rv64_smode_trap_vectors, @function
.p2align 4
__rv64_smode_trap_vectors:
    j __rv64_smode_trap_0
    j __rv64_smode_trap_1
    j __rv64_smode_trap_2
    j __rv64_smode_trap_3
    j __rv64_smode_trap_4
    j __rv64_smode_trap_5
    j __rv64_smode_trap_6
    j __rv64_smode_trap_7
    j __rv64_smode_trap_8
    j __rv64_smode_trap_9
    j __rv64_smode_trap_10
    j __rv64_smode_trap_11
    j __rv64_smode_trap_12
    j __rv64_smode_trap_13
    j __rv64_smode_trap_14
    j __rv64_smode_trap_15
.size __rv64_smode_trap_vectors, . - __rv64_smode_trap_vectors

SMODE_TRAP 0,  {smode_general_handler}
SMODE_TRAP 1,  {smode_interrupt_handler}
SMODE_TRAP 2,  {smode_interrupt_handler}
SMODE_TRAP 3,  {smode_interrupt_handler}
SMODE_TRAP 4,  {smode_interrupt_handler}
SMODE_TRAP 5,  {smode_interrupt_handler}
SMODE_TRAP 6,  {smode_interrupt_handler}
SMODE_TRAP 7,  {smode_interrupt_handler}
SMODE_TRAP 8,  {smode_interrupt_handler}
SMODE_TRAP 9,  {smode_interrupt_handler}
SMODE_TRAP 10, {smode_interrupt_handler}
SMODE_TRAP 11, {smode_interrupt_handler}
SMODE_TRAP 12, {smode_interrupt_handler}
SMODE_TRAP 13, {smode_interrupt_handler}
SMODE_TRAP 14, {smode_interrupt_handler}
SMODE_TRAP 15, {smode_interrupt_handler}

.option pop
