.section	.text.SW_Handler,"ax",@progbits
    .global SW_Handler
    .p2align 2      /* 4 bytes align */
    .func
SW_Handler:
    /* save all from thread context */
    addi sp, sp, -32 * 4
    sw x5,   5 * 4(sp)      /* 保存 t0, 后面要使用这个寄存器 */

    csrr t0, mstatus        /* 读取 mstatus */
    sw t0, 2 * 4(sp)        /* 保存到 x2 位置 */

    li t0,0x1800            /* 关闭中断，并进入 M-Mode 模式 */
    csrw mstatus, t0

    csrr  t0, mepc          /* 额外保存 mepc 到 x0 位置 */
    sw t0, 0 * 4(sp)

    sw x1,   1 * 4(sp)      /* return address */
    sw x4,   4 * 4(sp)      /* tp */
    sw x6,   6 * 4(sp)
    sw x7,   7 * 4(sp)
    sw x8,   8 * 4(sp)
    sw x9,   9 * 4(sp)
    sw x10, 10 * 4(sp)
    sw x11, 11 * 4(sp)
    sw x12, 12 * 4(sp)
    sw x13, 13 * 4(sp)
    sw x14, 14 * 4(sp)
    sw x15, 15 * 4(sp)
    sw x16, 16 * 4(sp)
    sw x17, 17 * 4(sp)
    sw x18, 18 * 4(sp)
    sw x19, 19 * 4(sp)
    sw x20, 20 * 4(sp)
    sw x21, 21 * 4(sp)
    sw x22, 22 * 4(sp)
    sw x23, 23 * 4(sp)
    sw x24, 24 * 4(sp)
    sw x25, 25 * 4(sp)
    sw x26, 26 * 4(sp)
    sw x27, 27 * 4(sp)
    sw x28, 28 * 4(sp)
    sw x29, 29 * 4(sp)
    sw x30, 30 * 4(sp)
    sw x31, 31 * 4(sp)

    la  s0, g_os_port_switch_from_sp   /* 更新旧任务栈指针 */
    lw  s1, 0(s0)
    sw sp, 0(s1)

    /* ---------- 收尾工作 ---------- */
    jal   os_port_on_switch_success

    /* ---------- 切换到新任务 ---------- */

    la  s0, g_os_port_switch_to_sp     /* 加载新任务栈指针 */
    lw  s1, 0(s0)
    lw  sp, 0(s1)

    lw  t0,  0 * 4(sp)          /* 恢复 mepc 指针 */
    csrw  mepc, t0              /* 将 t0 值写入 mepc */

    lw  x1,   1 * 4(sp)         /* 加载 return address */

    lw t0, 2*4(sp)              /* 将栈里面的 mstatus 恢复 */
    csrs mstatus, t0            /* 设置 MPIE = 1 */

    lw  x4,   4 * 4(sp)         /* 恢复其它寄存器 */
    lw  x5,   5 * 4(sp)
    lw  x6,   6 * 4(sp)
    lw  x7,   7 * 4(sp)
    lw  x8,   8 * 4(sp)
    lw  x9,   9 * 4(sp)
    lw  x10, 10 * 4(sp)
    lw  x11, 11 * 4(sp)
    lw  x12, 12 * 4(sp)
    lw  x13, 13 * 4(sp)
    lw  x14, 14 * 4(sp)
    lw  x15, 15 * 4(sp)
    lw  x16, 16 * 4(sp)
    lw  x17, 17 * 4(sp)
    lw  x18, 18 * 4(sp)
    lw  x19, 19 * 4(sp)
    lw  x20, 20 * 4(sp)
    lw  x21, 21 * 4(sp)
    lw  x22, 22 * 4(sp)
    lw  x23, 23 * 4(sp)
    lw  x24, 24 * 4(sp)
    lw  x25, 25 * 4(sp)
    lw  x26, 26 * 4(sp)
    lw  x27, 27 * 4(sp)
    lw  x28, 28 * 4(sp)
    lw  x29, 29 * 4(sp)
    lw  x30, 30 * 4(sp)
    lw  x31, 31 * 4(sp)
    addi  sp, sp, 32 * 4            /* 出栈 */

    mret
    .endfunc


    /*
     * void os_port_context_restore(os_uintptr_t to);
     * a0 --> to
     * a1 --> to_thread
     */
    .section	.text.os_port_context_restore,"ax",@progbits
    .globl os_port_context_restore
    .func
    .p2align 2      /* 4 bytes align */
os_port_context_restore:
    /* first save interrupt stack */
    la t0, _eusrstack
    addi t0, t0, -512
    csrw mscratch,t0

    /* keep machine mode */
    li    t0,      0x1800
    csrw  mstatus, t0

    /* ---------- 收尾工作 ---------- */
    jal   os_port_on_switch_success


    /* ---- Load target sp ---- */

    lw sp, (a0)             /* 将 to 的指针加载到 sp */

    /* resw ra to mepc */
    lw a0,   0 * 4(sp)
    csrw mepc, a0

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

    lw  t0,  2*4(sp)
    csrs  mstatus, t0

    addi sp,  sp, 32 * 4

    mret
    .endfunc


    .end