Support for running with M-Mode

12 files changed, 889 insertions, 238 deletions

diff --git a/libcpu/risc-v/virt64/context_gcc.S b/libcpu/risc-v/virt64/context_gcc.S
index 881bcd162c..620559a8d0 100644
--- a/libcpu/risc-v/virt64/context_gcc.S
+++ b/libcpu/risc-v/virt64/context_gcc.S
@@ -9,44 +9,300 @@
  * 2018/12/27     Jesven       Add SMP support
  * 2021/02/02     lizhirui     Add userspace support
  */
-
 #include "cpuport.h"
-#include "stackframe.h"
 
+#ifdef RT_USING_SMP
+#define rt_hw_interrupt_disable rt_hw_local_irq_disable
+#define rt_hw_interrupt_enable  rt_hw_local_irq_enable
+#endif
+
+/*
+ * rt_base_t rt_hw_interrupt_disable(void);
+ */
+    .globl rt_hw_interrupt_disable
+rt_hw_interrupt_disable:
+#ifdef RISCV_S_MODE
+    csrrci a0, sstatus, 2
+#else
+    csrrci a0, mstatus, 8
+#endif
+    ret
+
+
+/*
+ * void rt_hw_interrupt_enable(rt_base_t level);
+ */
+    .globl rt_hw_interrupt_enable
+rt_hw_interrupt_enable:
+    csrw SRC_XSTATUS, a0
+    ret
+
+/*
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch_to(rt_ubase_t to, stuct rt_thread *to_thread);
+ * #else
+ * void rt_hw_context_switch_to(rt_ubase_t to);
+ * #endif
+ * a0 --> to
+ * a1 --> to_thread
+ */
     .globl rt_hw_context_switch_to
 rt_hw_context_switch_to:
     LOAD sp, (a0)
 
-    la s0, rt_current_thread
-    LOAD s1, (s0)
-
-    RESTORE_ALL
-    sret
+#ifdef RT_USING_SMP
+    mv   a0,   a1
+    call  rt_cpus_lock_status_restore
+#endif
+    LOAD a0,   2 * REGBYTES(sp)
+    csrw SRC_XSTATUS, a0
+    j    rt_hw_context_switch_exit
 
 /*
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ * #else
  * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to);
+ * #endif
  *
  * a0 --> from
  * a1 --> to
+ * a2 --> to_thread
  */
     .globl rt_hw_context_switch
 rt_hw_context_switch:
-    mv t2, sp
-    li t0, 0x120//set SPIE and SPP = 1
-    csrs sstatus, t0//if enter here,caller must be in system thread
-    csrw sepc, ra//return address
-    //saved from thread context
-    SAVE_ALL
+    /* saved from thread context
+     *     x1/ra       -> sp(0)
+     *     x1/ra       -> sp(1)
+     *     mstatus.mie -> sp(2)
+     *     x(i)        -> sp(i-4)
+     */
+#ifdef ARCH_RISCV_FPU
+    addi    sp, sp, -32 * FREGBYTES
+
+    FSTORE  f0, 0 * FREGBYTES(sp)
+    FSTORE  f1, 1 * FREGBYTES(sp)
+    FSTORE  f2, 2 * FREGBYTES(sp)
+    FSTORE  f3, 3 * FREGBYTES(sp)
+    FSTORE  f4, 4 * FREGBYTES(sp)
+    FSTORE  f5, 5 * FREGBYTES(sp)
+    FSTORE  f6, 6 * FREGBYTES(sp)
+    FSTORE  f7, 7 * FREGBYTES(sp)
+    FSTORE  f8, 8 * FREGBYTES(sp)
+    FSTORE  f9, 9 * FREGBYTES(sp)
+    FSTORE  f10, 10 * FREGBYTES(sp)
+    FSTORE  f11, 11 * FREGBYTES(sp)
+    FSTORE  f12, 12 * FREGBYTES(sp)
+    FSTORE  f13, 13 * FREGBYTES(sp)
+    FSTORE  f14, 14 * FREGBYTES(sp)
+    FSTORE  f15, 15 * FREGBYTES(sp)
+    FSTORE  f16, 16 * FREGBYTES(sp)
+    FSTORE  f17, 17 * FREGBYTES(sp)
+    FSTORE  f18, 18 * FREGBYTES(sp)
+    FSTORE  f19, 19 * FREGBYTES(sp)
+    FSTORE  f20, 20 * FREGBYTES(sp)
+    FSTORE  f21, 21 * FREGBYTES(sp)
+    FSTORE  f22, 22 * FREGBYTES(sp)
+    FSTORE  f23, 23 * FREGBYTES(sp)
+    FSTORE  f24, 24 * FREGBYTES(sp)
+    FSTORE  f25, 25 * FREGBYTES(sp)
+    FSTORE  f26, 26 * FREGBYTES(sp)
+    FSTORE  f27, 27 * FREGBYTES(sp)
+    FSTORE  f28, 28 * FREGBYTES(sp)
+    FSTORE  f29, 29 * FREGBYTES(sp)
+    FSTORE  f30, 30 * FREGBYTES(sp)
+    FSTORE  f31, 31 * FREGBYTES(sp)
+
+#endif
+    addi  sp,  sp, -32 * REGBYTES
+    STORE sp,  (a0)
+
+    STORE x1,   0 * REGBYTES(sp)
+    STORE x1,   1 * REGBYTES(sp)
+
+
+    csrr a0, SRC_XSTATUS
+#ifdef RISCV_S_MODE
+    andi a0, a0, 2
+    beqz a0, save_spie
+    li   a0, 0x20
+save_spie:
+    STORE a0,   2 * REGBYTES(sp)
+#else
+    andi a0, a0, 8
+    beqz a0, save_mpie
+    li   a0, 0x80
+save_mpie:
+    STORE a0,   2 * REGBYTES(sp)
+#endif
+
+    STORE x4,   4 * REGBYTES(sp)
+    STORE x5,   5 * REGBYTES(sp)
+    STORE x6,   6 * REGBYTES(sp)
+    STORE x7,   7 * REGBYTES(sp)
+    STORE x8,   8 * REGBYTES(sp)
+    STORE x9,   9 * REGBYTES(sp)
+    STORE x10, 10 * REGBYTES(sp)
+    STORE x11, 11 * REGBYTES(sp)
+    STORE x12, 12 * REGBYTES(sp)
+    STORE x13, 13 * REGBYTES(sp)
+    STORE x14, 14 * REGBYTES(sp)
+    STORE x15, 15 * REGBYTES(sp)
+    STORE x16, 16 * REGBYTES(sp)
+    STORE x17, 17 * REGBYTES(sp)
+    STORE x18, 18 * REGBYTES(sp)
+    STORE x19, 19 * REGBYTES(sp)
+    STORE x20, 20 * REGBYTES(sp)
+    STORE x21, 21 * REGBYTES(sp)
+    STORE x22, 22 * REGBYTES(sp)
+    STORE x23, 23 * REGBYTES(sp)
+    STORE x24, 24 * REGBYTES(sp)
+    STORE x25, 25 * REGBYTES(sp)
+    STORE x26, 26 * REGBYTES(sp)
+    STORE x27, 27 * REGBYTES(sp)
+    STORE x28, 28 * REGBYTES(sp)
+    STORE x29, 29 * REGBYTES(sp)
+    STORE x30, 30 * REGBYTES(sp)
+    STORE x31, 31 * REGBYTES(sp)
+
+    /* restore to thread context
+     * sp(0) -> epc;
+     * sp(1) -> ra;
+     * sp(i) -> x(i+2)
+     */
+    LOAD sp,  (a1)
+
+#ifdef RT_USING_SMP
+    mv   a0,   a2
+    call  rt_cpus_lock_status_restore
+#endif /*RT_USING_SMP*/
+
+    j rt_hw_context_switch_exit
+
+#ifdef RT_USING_SMP
+/*
+ * void rt_hw_context_switch_interrupt(void *context, rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ *
+ * a0 --> context
+ * a1 --> from
+ * a2 --> to
+ * a3 --> to_thread
+ */
+    .globl rt_hw_context_switch_interrupt
+rt_hw_context_switch_interrupt:
+
+    STORE a0, 0(a1)
+
+    LOAD  sp, 0(a2)
+    move  a0, a3
+    call rt_cpus_lock_status_restore
+
+    j rt_hw_context_switch_exit
+
+#endif
+
+.global rt_hw_context_switch_exit
+rt_hw_context_switch_exit:
+#ifdef RT_USING_SMP
+#ifdef RT_USING_SIGNALS
+    mv a0, sp
+
+    csrr  t0, mhartid
+    /* switch interrupt stack of current cpu */
+    la    sp, __stack_start__
+    addi  t1, t0, 1
+    li    t2, __STACKSIZE__
+    mul   t1, t1, t2
+    add   sp, sp, t1 /* sp = (cpuid + 1) * __STACKSIZE__ + __stack_start__ */
+
+    call rt_signal_check
+    mv sp, a0
+#endif
+#endif
+    /* resw ra to mepc */
+    LOAD a0,   0 * REGBYTES(sp)
+    csrw SRC_XEPC, a0
+
+    LOAD x1,   1 * REGBYTES(sp)
+
+#ifdef RISCV_S_MODE
+    li    t0, 0x00000120
+    csrw  sstatus, t0
+    LOAD a0,   2 * REGBYTES(sp)
+    csrs sstatus, a0
+#else
+    li    t0, 0x00007800
+    csrw  mstatus, t0
+    LOAD a0,   2 * REGBYTES(sp)
+    csrs mstatus, a0
+#endif
 
-    STORE t2, 32 * REGBYTES(sp)//save user_sp
+    LOAD x4,   4 * REGBYTES(sp)
+    LOAD x5,   5 * REGBYTES(sp)
+    LOAD x6,   6 * REGBYTES(sp)
+    LOAD x7,   7 * REGBYTES(sp)
+    LOAD x8,   8 * REGBYTES(sp)
+    LOAD x9,   9 * REGBYTES(sp)
+    LOAD x10, 10 * REGBYTES(sp)
+    LOAD x11, 11 * REGBYTES(sp)
+    LOAD x12, 12 * REGBYTES(sp)
+    LOAD x13, 13 * REGBYTES(sp)
+    LOAD x14, 14 * REGBYTES(sp)
+    LOAD x15, 15 * REGBYTES(sp)
+    LOAD x16, 16 * REGBYTES(sp)
+    LOAD x17, 17 * REGBYTES(sp)
+    LOAD x18, 18 * REGBYTES(sp)
+    LOAD x19, 19 * REGBYTES(sp)
+    LOAD x20, 20 * REGBYTES(sp)
+    LOAD x21, 21 * REGBYTES(sp)
+    LOAD x22, 22 * REGBYTES(sp)
+    LOAD x23, 23 * REGBYTES(sp)
+    LOAD x24, 24 * REGBYTES(sp)
+    LOAD x25, 25 * REGBYTES(sp)
+    LOAD x26, 26 * REGBYTES(sp)
+    LOAD x27, 27 * REGBYTES(sp)
+    LOAD x28, 28 * REGBYTES(sp)
+    LOAD x29, 29 * REGBYTES(sp)
+    LOAD x30, 30 * REGBYTES(sp)
+    LOAD x31, 31 * REGBYTES(sp)
 
-    STORE sp, (a0)
+    addi sp,  sp, 32 * REGBYTES
 
-    //restore to thread context
-    LOAD sp, (a1)
+#ifdef ARCH_RISCV_FPU
+    FLOAD   f0, 0 * FREGBYTES(sp)
+    FLOAD   f1, 1 * FREGBYTES(sp)
+    FLOAD   f2, 2 * FREGBYTES(sp)
+    FLOAD   f3, 3 * FREGBYTES(sp)
+    FLOAD   f4, 4 * FREGBYTES(sp)
+    FLOAD   f5, 5 * FREGBYTES(sp)
+    FLOAD   f6, 6 * FREGBYTES(sp)
+    FLOAD   f7, 7 * FREGBYTES(sp)
+    FLOAD   f8, 8 * FREGBYTES(sp)
+    FLOAD   f9, 9 * FREGBYTES(sp)
+    FLOAD   f10, 10 * FREGBYTES(sp)
+    FLOAD   f11, 11 * FREGBYTES(sp)
+    FLOAD   f12, 12 * FREGBYTES(sp)
+    FLOAD   f13, 13 * FREGBYTES(sp)
+    FLOAD   f14, 14 * FREGBYTES(sp)
+    FLOAD   f15, 15 * FREGBYTES(sp)
+    FLOAD   f16, 16 * FREGBYTES(sp)
+    FLOAD   f17, 17 * FREGBYTES(sp)
+    FLOAD   f18, 18 * FREGBYTES(sp)
+    FLOAD   f19, 19 * FREGBYTES(sp)
+    FLOAD   f20, 20 * FREGBYTES(sp)
+    FLOAD   f21, 21 * FREGBYTES(sp)
+    FLOAD   f22, 22 * FREGBYTES(sp)
+    FLOAD   f23, 23 * FREGBYTES(sp)
+    FLOAD   f24, 24 * FREGBYTES(sp)
+    FLOAD   f25, 25 * FREGBYTES(sp)
+    FLOAD   f26, 26 * FREGBYTES(sp)
+    FLOAD   f27, 27 * FREGBYTES(sp)
+    FLOAD   f28, 28 * FREGBYTES(sp)
+    FLOAD   f29, 29 * FREGBYTES(sp)
+    FLOAD   f30, 30 * FREGBYTES(sp)
+    FLOAD   f31, 31 * FREGBYTES(sp)
 
-    la s0, rt_current_thread
-    LOAD s1, (s0)
+    addi    sp, sp, 32 * FREGBYTES
+#endif
 
-    RESTORE_ALL
-    sret
+    XRET
diff --git a/libcpu/risc-v/virt64/cpuport.c b/libcpu/risc-v/virt64/cpuport.c
index 50929125ac..df98bfe1eb 100644
--- a/libcpu/risc-v/virt64/cpuport.c
+++ b/libcpu/risc-v/virt64/cpuport.c
@@ -70,8 +70,11 @@ rt_uint8_t *rt_hw_stack_init(void       *tentry,
     frame->epc     = (rt_ubase_t)tentry;
     frame->user_sp_exc_stack = (rt_ubase_t)(((rt_ubase_t)stk) + sizeof(struct rt_hw_stack_frame));
 
-    /* force to supervisor mode(SPP=1) and set SPIE and SUM to 1 */
-    frame->sstatus = 0x00040120;
+#ifndef RISCV_S_MODE
+    frame->xstatus = 0x00007880;
+#else
+    frame->xstatus = 0x00040120;
+#endif
 
     return stk;
 }
diff --git a/libcpu/risc-v/virt64/cpuport.h b/libcpu/risc-v/virt64/cpuport.h
index bb838def4a..378d8085bd 100644
--- a/libcpu/risc-v/virt64/cpuport.h
+++ b/libcpu/risc-v/virt64/cpuport.h
@@ -22,7 +22,34 @@
 // error here, not portable
 #endif
 
-#endif
 #ifdef RISCV_U_MODE
 #define RISCV_USER_ENTRY 0xFFFFFFE000000000ULL
-#endif
\ No newline at end of file
+#endif
+
+#ifdef RISCV_S_MODE
+//csr in s-mode
+// M/U/S Interrupt Registers
+#define SRC_XIE         sie
+#define SRC_XIP         sip
+#define SRC_XTVEC       stvec
+#define SRC_XSTATUS     sstatus
+#define SRC_XSCRATCH    sscratch
+#define SRC_XEPC        sepc
+#define SRC_XCAUSE      scause
+#define SRC_XTVAL       stval
+#define XRET            sret
+#else
+//csr in m-mode
+// M/U/S Interrupt Registers
+#define SRC_XIE         mie
+#define SRC_XIP         mip
+#define SRC_XTVEC       mtvec
+#define SRC_XSTATUS     mstatus
+#define SRC_XSCRATCH    mscratch
+#define SRC_XEPC        mepc
+#define SRC_XCAUSE      mcause
+#define SRC_XTVAL       mtval
+#define XRET            mret
+#endif
+
+#endif
diff --git a/libcpu/risc-v/virt64/interrupt.c b/libcpu/risc-v/virt64/interrupt.c
new file mode 100644
index 0000000000..4280446198
--- /dev/null
+++ b/libcpu/risc-v/virt64/interrupt.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2018/10/01     Bernard      The first version
+ * 2018/12/27     Jesven       Change irq enable/disable to cpu0
+ */
+#include "tick.h"
+#include <plic.h>
+#include "encoding.h"
+#include "riscv.h"
+#include "interrupt.h"
+
+#define CPU_NUM         2
+#define MAX_HANDLERS    128
+
+static struct rt_irq_desc irq_desc[MAX_HANDLERS];
+
+static rt_isr_handler_t rt_hw_interrupt_handle(rt_uint32_t vector, void *param)
+{
+    rt_kprintf("UN-handled interrupt %d occurred!!!\n", vector);
+    return RT_NULL;
+}
+
+int rt_hw_clint_ipi_enable(void)
+{
+    /* Set the Machine-Software bit in MIE */
+    set_csr(mie, MIP_MSIP);
+    return 0;
+}
+
+int rt_hw_clint_ipi_disable(void)
+{
+    /* Clear the Machine-Software bit in MIE */
+    clear_csr(mie, MIP_MSIP);
+    return 0;
+}
+
+int rt_hw_plic_irq_enable(int irq_number)
+{
+    plic_irq_enable(irq_number);
+    return 0;
+}
+
+int rt_hw_plic_irq_disable(int irq_number)
+{
+    plic_irq_disable(irq_number);
+    return 0;
+}
+
+/**
+ * This function will initialize hardware interrupt
+ */
+void rt_hw_interrupt_init(void)
+{
+    int idx = 0;
+    /* init exceptions table */
+    for (idx = 0; idx < MAX_HANDLERS; idx++)
+    {
+        //rt_hw_interrupt_mask(idx);
+        irq_desc[idx].handler = (rt_isr_handler_t)rt_hw_interrupt_handle;
+        irq_desc[idx].param = RT_NULL;
+#ifdef RT_USING_INTERRUPT_INFO
+        rt_snprintf(irq_desc[idx].name, RT_NAME_MAX - 1, "default");
+        irq_desc[idx].counter = 0;
+#endif
+    }
+}
+
+/**
+ * This function will mask a interrupt.
+ * @param vector the interrupt number
+ */
+void rt_hw_interrupt_mask(int vector)
+{
+    rt_hw_plic_irq_disable(vector);
+}
+
+/**
+ * This function will un-mask a interrupt.
+ * @param vector the interrupt number
+ */
+void rt_hw_interrupt_umask(int vector)
+{
+    plic_set_priority(vector, 1);
+    plic_set_threshold(0);
+    rt_hw_plic_irq_enable(vector);
+}
+
+/**
+ * This function will install a interrupt service routine to a interrupt.
+ * @param vector the interrupt number
+ * @param new_handler the interrupt service routine to be installed
+ * @param old_handler the old interrupt service routine
+ */
+rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler,
+        void *param, const char *name)
+{
+    rt_isr_handler_t old_handler = RT_NULL;
+
+    if(vector < MAX_HANDLERS)
+    {
+        old_handler = irq_desc[vector].handler;
+        if (handler != RT_NULL)
+        {
+            irq_desc[vector].handler = (rt_isr_handler_t)handler;
+            irq_desc[vector].param = param;
+#ifdef RT_USING_INTERRUPT_INFO
+            rt_snprintf(irq_desc[vector].name, RT_NAME_MAX - 1, "%s", name);
+            irq_desc[vector].counter = 0;
+#endif
+        }
+    }
+
+    return old_handler;
+}
+
+RT_WEAK
+void plic_irq_handle(int irq)
+{
+    rt_kprintf("UN-handled interrupt %d occurred!!!\n", irq);
+    return ;
+}
+
+void dump_regs(struct rt_hw_stack_frame *regs)
+{
+    rt_kprintf("--------------Dump Registers-----------------\n");
+
+    rt_kprintf("Function Registers:\n");
+    rt_kprintf("\tra(x1) = 0x%p\tuser_sp = 0x%p\n",regs -> ra,regs -> user_sp_exc_stack);
+    rt_kprintf("\tgp(x3) = 0x%p\ttp(x4) = 0x%p\n",regs -> gp,regs -> tp);
+    rt_kprintf("Temporary Registers:\n");
+    rt_kprintf("\tt0(x5) = 0x%p\tt1(x6) = 0x%p\n",regs -> t0,regs -> t1);
+    rt_kprintf("\tt2(x7) = 0x%p\n",regs -> t2);
+    rt_kprintf("\tt3(x28) = 0x%p\tt4(x29) = 0x%p\n",regs -> t3,regs -> t4);
+    rt_kprintf("\tt5(x30) = 0x%p\tt6(x31) = 0x%p\n",regs -> t5,regs -> t6);
+    rt_kprintf("Saved Registers:\n");
+    rt_kprintf("\ts0/fp(x8) = 0x%p\ts1(x9) = 0x%p\n",regs -> s0_fp,regs -> s1);
+    rt_kprintf("\ts2(x18) = 0x%p\ts3(x19) = 0x%p\n",regs -> s2,regs -> s3);
+    rt_kprintf("\ts4(x20) = 0x%p\ts5(x21) = 0x%p\n",regs -> s4,regs -> s5);
+    rt_kprintf("\ts6(x22) = 0x%p\ts7(x23) = 0x%p\n",regs -> s6,regs -> s7);
+    rt_kprintf("\ts8(x24) = 0x%p\ts9(x25) = 0x%p\n",regs -> s8,regs -> s9);
+    rt_kprintf("\ts10(x26) = 0x%p\ts11(x27) = 0x%p\n",regs -> s10,regs -> s11);
+    rt_kprintf("Function Arguments Registers:\n");
+    rt_kprintf("\ta0(x10) = 0x%p\ta1(x11) = 0x%p\n",regs -> a0,regs -> a1);
+    rt_kprintf("\ta2(x12) = 0x%p\ta3(x13) = 0x%p\n",regs -> a2,regs -> a3);
+    rt_kprintf("\ta4(x14) = 0x%p\ta5(x15) = 0x%p\n",regs -> a4,regs -> a5);
+    rt_kprintf("\ta6(x16) = 0x%p\ta7(x17) = 0x%p\n",regs -> a6,regs -> a7);
+    rt_kprintf("xstatus = 0x%p\n",regs -> xstatus);
+    rt_kprintf("\t%s\n",(regs -> xstatus & SSTATUS_SIE) ? "Supervisor Interrupt Enabled" : "Supervisor Interrupt Disabled");
+    rt_kprintf("\t%s\n",(regs -> xstatus & SSTATUS_SPIE) ? "Last Time Supervisor Interrupt Enabled" : "Last Time Supervisor Interrupt Disabled");
+    rt_kprintf("\t%s\n",(regs -> xstatus & SSTATUS_SPP) ? "Last Privilege is Supervisor Mode" : "Last Privilege is User Mode");
+    rt_kprintf("\t%s\n",(regs -> xstatus & SSTATUS_PUM) ? "Permit to Access User Page" : "Not Permit to Access User Page");
+    rt_kprintf("\t%s\n",(regs -> xstatus & (1 << 19)) ? "Permit to Read Executable-only Page" : "Not Permit to Read Executable-only Page");
+    rt_size_t satp_v = read_csr(satp);
+    rt_kprintf("satp = 0x%p\n",satp_v);
+    const char *mode_str = "Unknown Address Translation/Protection Mode";
+    
+    switch(__MASKVALUE(satp_v >> 60,__MASK(4)))
+    {
+        case 0:
+            mode_str = "No Address Translation/Protection Mode";
+            break;
+
+        case 8:
+            mode_str = "Page-based 39-bit Virtual Addressing Mode";
+            break;
+
+        case 9:
+            mode_str = "Page-based 48-bit Virtual Addressing Mode";
+            break;
+    }
+
+    rt_kprintf("\tMode = %s\n",mode_str);
+    rt_kprintf("-----------------Dump OK---------------------\n");
+}
+
+void handle_trap(rt_size_t xcause,rt_size_t xtval,rt_size_t xepc,struct rt_hw_stack_frame *sp)
+{
+    int cause = (xcause & 0xFFFFFFFF);
+    int plic_irq = 0;
+    if (xcause & (1UL << 63))
+    {
+        switch (cause)
+        {
+            case IRQ_M_SOFT:
+                {
+                    
+                }
+                break;
+            case IRQ_M_TIMER:
+                tick_isr();
+                break;
+            case IRQ_S_TIMER:
+                tick_isr();
+                break;
+            case IRQ_S_EXT:
+                plic_irq = plic_claim();
+                plic_complete(plic_irq);
+                irq_desc[plic_irq].handler(plic_irq, irq_desc[plic_irq].param);
+                break;
+            case IRQ_M_EXT:
+                plic_irq = plic_claim();
+                plic_complete(plic_irq);
+                irq_desc[plic_irq].handler(plic_irq, irq_desc[plic_irq].param);
+                break;
+        }
+    }
+    else
+    {
+        rt_thread_t tid;
+        extern long list_thread();
+
+        rt_hw_interrupt_disable();
+
+        rt_kprintf("xcause = %08x,xtval = %08x,xepc = %08x\n", xcause, xtval, xepc);
+        tid = rt_thread_self();
+        rt_kprintf("\nException:\n");
+        switch (cause)
+        {
+            case CAUSE_MISALIGNED_FETCH:
+                rt_kprintf("Instruction address misaligned");
+                break;
+            case CAUSE_FAULT_FETCH:
+                rt_kprintf("Instruction access fault");
+                break;
+            case CAUSE_ILLEGAL_INSTRUCTION:
+                rt_kprintf("Illegal instruction");
+                break;
+            case CAUSE_BREAKPOINT:
+                rt_kprintf("Breakpoint");
+                break;
+            case CAUSE_MISALIGNED_LOAD:
+                rt_kprintf("Load address misaligned");
+                break;
+            case CAUSE_FAULT_LOAD:
+                rt_kprintf("Load access fault");
+                break;
+            case CAUSE_MISALIGNED_STORE:
+                rt_kprintf("Store address misaligned");
+                break;
+            case CAUSE_FAULT_STORE:
+                rt_kprintf("Store access fault");
+                break;
+            case CAUSE_USER_ECALL:
+                rt_kprintf("Environment call from U-mode");
+                break;
+            case CAUSE_SUPERVISOR_ECALL:
+                rt_kprintf("Environment call from S-mode");
+                break;
+            case CAUSE_HYPERVISOR_ECALL:
+                rt_kprintf("Environment call from H-mode");
+                break;
+            case CAUSE_MACHINE_ECALL:
+                rt_kprintf("Environment call from M-mode");
+                break;
+            default:
+                rt_kprintf("Uknown exception : %08lX", cause);
+                break;
+        }
+        rt_kprintf("\n");
+        dump_regs(sp);
+        rt_kprintf("exception pc => 0x%08x\n", xepc);
+        rt_kprintf("current thread: %.*s\n", RT_NAME_MAX, tid->name);
+#ifdef RT_USING_FINSH
+        list_thread();
+#endif
+        while(1);
+    }
+    rt_hw_interrupt_enable(0);
+}
diff --git a/libcpu/risc-v/virt64/interrupt.h b/libcpu/risc-v/virt64/interrupt.h
new file mode 100644
index 0000000000..01b26c78c8
--- /dev/null
+++ b/libcpu/risc-v/virt64/interrupt.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-20     bigmagic      The first version
+ */
+
+#ifndef INTERRUPT_H__
+#define INTERRUPT_H__
+
+#include <rthw.h>
+#include "stack.h"
+
+int rt_hw_clint_ipi_enable(void);
+int rt_hw_clint_ipi_disable(void);
+int rt_hw_plic_irq_enable(int irq_number);
+int rt_hw_plic_irq_disable(int irq_number);
+void rt_hw_interrupt_init(void);
+void rt_hw_interrupt_mask(int vector);
+rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler,
+        void *param, const char *name);
+void handle_trap(rt_size_t xcause,rt_size_t xtval,rt_size_t xepc,struct rt_hw_stack_frame *sp);
+
+#endif
\ No newline at end of file
diff --git a/libcpu/risc-v/virt64/interrupt_gcc.S b/libcpu/risc-v/virt64/interrupt_gcc.S
index 338413866e..730fa10d61 100644
--- a/libcpu/risc-v/virt64/interrupt_gcc.S
+++ b/libcpu/risc-v/virt64/interrupt_gcc.S
@@ -11,72 +11,131 @@
  */
 
 #include "cpuport.h"
-#include "encoding.h"
-#include "stackframe.h"
 
   .section      .text.entry
   .align 2
   .global trap_entry
-  .extern __stack_cpu0
-  .extern get_current_thread_kernel_stack_top
 trap_entry:
-    //backup sp
-    csrrw sp, sscratch, sp
-    //load interrupt stack
-    la sp, __stack_cpu0
-    //backup context
-    SAVE_ALL
-    
-    RESTORE_SYS_GP
+#ifdef ARCH_RISCV_FPU
+    addi    sp, sp, -32 * FREGBYTES
 
-    csrr a0, scause
-    csrrc a1, stval, zero
-    csrr  a2, sepc
-    mv    a3, sp
+    FSTORE  f0, 0 * FREGBYTES(sp)
+    FSTORE  f1, 1 * FREGBYTES(sp)
+    FSTORE  f2, 2 * FREGBYTES(sp)
+    FSTORE  f3, 3 * FREGBYTES(sp)
+    FSTORE  f4, 4 * FREGBYTES(sp)
+    FSTORE  f5, 5 * FREGBYTES(sp)
+    FSTORE  f6, 6 * FREGBYTES(sp)
+    FSTORE  f7, 7 * FREGBYTES(sp)
+    FSTORE  f8, 8 * FREGBYTES(sp)
+    FSTORE  f9, 9 * FREGBYTES(sp)
+    FSTORE  f10, 10 * FREGBYTES(sp)
+    FSTORE  f11, 11 * FREGBYTES(sp)
+    FSTORE  f12, 12 * FREGBYTES(sp)
+    FSTORE  f13, 13 * FREGBYTES(sp)
+    FSTORE  f14, 14 * FREGBYTES(sp)
+    FSTORE  f15, 15 * FREGBYTES(sp)
+    FSTORE  f16, 16 * FREGBYTES(sp)
+    FSTORE  f17, 17 * FREGBYTES(sp)
+    FSTORE  f18, 18 * FREGBYTES(sp)
+    FSTORE  f19, 19 * FREGBYTES(sp)
+    FSTORE  f20, 20 * FREGBYTES(sp)
+    FSTORE  f21, 21 * FREGBYTES(sp)
+    FSTORE  f22, 22 * FREGBYTES(sp)
+    FSTORE  f23, 23 * FREGBYTES(sp)
+    FSTORE  f24, 24 * FREGBYTES(sp)
+    FSTORE  f25, 25 * FREGBYTES(sp)
+    FSTORE  f26, 26 * FREGBYTES(sp)
+    FSTORE  f27, 27 * FREGBYTES(sp)
+    FSTORE  f28, 28 * FREGBYTES(sp)
+    FSTORE  f29, 29 * FREGBYTES(sp)
+    FSTORE  f30, 30 * FREGBYTES(sp)
+    FSTORE  f31, 31 * FREGBYTES(sp)
 
-    /* scause, stval, sepc, sp */
-    call  handle_trap
+#endif
 
-    /* need to switch new thread */
-    la    s0, rt_thread_switch_interrupt_flag
-    lw    s2, 0(s0)
-    beqz  s2, spurious_interrupt
-    sw    zero, 0(s0)
+    /* save thread context to thread stack */
+    addi sp, sp, -32 * REGBYTES
 
-.global rt_hw_context_switch_interrupt_do
-rt_hw_context_switch_interrupt_do:
+    STORE x1,   1 * REGBYTES(sp)
 
-//switch to thread kernel stack
-    csrr t0, sstatus
-    andi t0, t0, 0x100
-    beqz t0, __restore_sp_from_tcb_interrupt
+    csrr  x1, SRC_XSTATUS
+    STORE x1,   2 * REGBYTES(sp)
 
-__restore_sp_from_sscratch_interrupt:
-    csrr t0, sscratch
-    j __move_stack_context_interrupt
+    csrr  x1, SRC_XEPC
+    STORE x1, 0 * REGBYTES(sp)
 
-__restore_sp_from_tcb_interrupt:
-    la    s0, rt_interrupt_from_thread
-    LOAD  a0, 0(s0)
-    jal rt_thread_sp_to_thread
-    jal get_thread_kernel_stack_top
-    mv t0, a0
+    STORE x4,   4 * REGBYTES(sp)
+    STORE x5,   5 * REGBYTES(sp)
+    STORE x6,   6 * REGBYTES(sp)
+    STORE x7,   7 * REGBYTES(sp)
+    STORE x8,   8 * REGBYTES(sp)
+    STORE x9,   9 * REGBYTES(sp)
+    STORE x10, 10 * REGBYTES(sp)
+    STORE x11, 11 * REGBYTES(sp)
+    STORE x12, 12 * REGBYTES(sp)
+    STORE x13, 13 * REGBYTES(sp)
+    STORE x14, 14 * REGBYTES(sp)
+    STORE x15, 15 * REGBYTES(sp)
+    STORE x16, 16 * REGBYTES(sp)
+    STORE x17, 17 * REGBYTES(sp)
+    STORE x18, 18 * REGBYTES(sp)
+    STORE x19, 19 * REGBYTES(sp)
+    STORE x20, 20 * REGBYTES(sp)
+    STORE x21, 21 * REGBYTES(sp)
+    STORE x22, 22 * REGBYTES(sp)
+    STORE x23, 23 * REGBYTES(sp)
+    STORE x24, 24 * REGBYTES(sp)
+    STORE x25, 25 * REGBYTES(sp)
+    STORE x26, 26 * REGBYTES(sp)
+    STORE x27, 27 * REGBYTES(sp)
+    STORE x28, 28 * REGBYTES(sp)
+    STORE x29, 29 * REGBYTES(sp)
+    STORE x30, 30 * REGBYTES(sp)
+    STORE x31, 31 * REGBYTES(sp)
+
+    /* switch to interrupt stack */
+    move  s0, sp
+
+#ifndef RISCV_S_MODE
+    /* get cpu id */
+    csrr  t0, mhartid
+#else
+    li    t0, 0
+#endif
+
+    /* switch interrupt stack of current cpu */
+    la    sp, __stack_start__
+    addi  t1, t0, 1
+    li    t2, __STACKSIZE__
+    mul   t1, t1, t2
+    add   sp, sp, t1 /* sp = (cpuid + 1) * __STACKSIZE__ + __stack_start__ */
+
+    /* handle interrupt */
+    call  rt_interrupt_enter
+    csrr  a0, SRC_XCAUSE
+    csrr  a1, SRC_XEPC
+    mv    a2, s0
+    call  handle_trap
+    call  rt_interrupt_leave
+
+#ifdef RT_USING_SMP
+    /* s0 --> sp */
+    mv  sp, s0
+    mv  a0, s0
+    call rt_scheduler_do_irq_switch
+    tail   rt_hw_context_switch_exit
 
-__move_stack_context_interrupt:
-    mv t1, sp//src
-    mv sp, t0//switch stack
-    addi sp, sp, -33 * REGBYTES
-    //copy context
-    li s0, 33//cnt
-    mv t2, sp//dst
+#else
 
-copy_context_loop_interrupt:
-    LOAD t0, 0(t1)
-    STORE t0, 0(t2)
-    addi s0, s0, -1
-    addi t1, t1, 8
-    addi t2, t2, 8
-    bnez s0, copy_context_loop_interrupt
+    /* switch to from_thread stack */
+    move  sp, s0
+
+    /* need to switch new thread */
+    la    s0, rt_thread_switch_interrupt_flag
+    lw    s2, 0(s0)
+    beqz  s2, spurious_interrupt
+    sw    zero, 0(s0)
 
     la    s0, rt_interrupt_from_thread
     LOAD  s1, 0(s0)
@@ -86,16 +145,7 @@ copy_context_loop_interrupt:
     LOAD  s1, 0(s0)
     LOAD  sp, 0(s1)
 
-spurious_interrupt:
-    RESTORE_ALL
-    sret
+#endif
 
-.global rt_hw_interrupt_enable
-rt_hw_interrupt_enable:
-    csrs sstatus, a0
-    jr ra
-
-.global rt_hw_interrupt_disable
-rt_hw_interrupt_disable:
-    csrrci a0, sstatus, 2
-    jr ra
+spurious_interrupt:
+    tail rt_hw_context_switch_exit
diff --git a/libcpu/risc-v/virt64/riscv_io.h b/libcpu/risc-v/virt64/riscv_io.h
index 8363ff23f8..109f7297e5 100644
--- a/libcpu/risc-v/virt64/riscv_io.h
+++ b/libcpu/risc-v/virt64/riscv_io.h
@@ -10,100 +10,128 @@
 #ifndef __RISCV_IO_H__
 #define __RISCV_IO_H__
 
-    static inline void __raw_writeb(rt_uint8_t val, volatile void *addr)
-    {
-        asm volatile("sb %0, 0(%1)" : : "r"(val), "r"(addr));
-    }
+// which hart (core) is this?
+static inline uint32_t  r_mhartid()
+{
+#ifndef RISCV_S_MODE
+    uint32_t x;
+    asm volatile("csrr %0, mhartid" : "=r" (x) );
+    return x;
+#else
+    return 0;
+#endif
+}
 
-    static inline void __raw_writew(rt_uint16_t val, volatile void *addr)
-    {
-        asm volatile("sh %0, 0(%1)" : : "r"(val), "r"(addr));
-    }
+static inline void __raw_writeb(rt_uint8_t val, volatile void *addr)
+{
+    asm volatile("sb %0, 0(%1)"
+                 :
+                 : "r"(val), "r"(addr));
+}
 
-    static inline void __raw_writel(rt_uint32_t val, volatile void *addr)
-    {
-        asm volatile("sw %0, 0(%1)" : : "r"(val), "r"(addr));
-    }
+static inline void __raw_writew(rt_uint16_t val, volatile void *addr)
+{
+    asm volatile("sh %0, 0(%1)"
+                 :
+                 : "r"(val), "r"(addr));
+}
 
-    #if __riscv_xlen != 32
-    static inline void __raw_writeq(rt_uint64_t val, volatile void *addr)
-    {
-        asm volatile("sd %0, 0(%1)" : : "r"(val), "r"(addr));
-    }
-    #endif
+static inline void __raw_writel(rt_uint32_t val, volatile void *addr)
+{
+    asm volatile("sw %0, 0(%1)"
+                 :
+                 : "r"(val), "r"(addr));
+}
 
-    static inline rt_uint8_t __raw_readb(const volatile void *addr)
-    {
-        rt_uint8_t val;
+#if __riscv_xlen != 32
+static inline void __raw_writeq(rt_uint64_t val, volatile void *addr)
+{
+    asm volatile("sd %0, 0(%1)"
+                 :
+                 : "r"(val), "r"(addr));
+}
+#endif
 
-        asm volatile("lb %0, 0(%1)" : "=r"(val) : "r"(addr));
-        return val;
-    }
+static inline rt_uint8_t __raw_readb(const volatile void *addr)
+{
+    rt_uint8_t val;
 
-    static inline rt_uint16_t __raw_readw(const volatile void *addr)
-    {
-        rt_uint16_t val;
+    asm volatile("lb %0, 0(%1)"
+                 : "=r"(val)
+                 : "r"(addr));
+    return val;
+}
 
-        asm volatile("lh %0, 0(%1)" : "=r"(val) : "r"(addr));
-        return val;
-    }
+static inline rt_uint16_t __raw_readw(const volatile void *addr)
+{
+    rt_uint16_t val;
 
-    static inline rt_uint32_t __raw_readl(const volatile void *addr)
-    {
-        rt_uint32_t val;
+    asm volatile("lh %0, 0(%1)"
+                 : "=r"(val)
+                 : "r"(addr));
+    return val;
+}
 
-        asm volatile("lw %0, 0(%1)" : "=r"(val) : "r"(addr));
-        return val;
-    }
+static inline rt_uint32_t __raw_readl(const volatile void *addr)
+{
+    rt_uint32_t val;
 
-    #if __riscv_xlen != 32
-    static inline rt_uint64_t __raw_readq(const volatile void *addr)
-    {
-        rt_uint64_t val;
+    asm volatile("lw %0, 0(%1)"
+                 : "=r"(val)
+                 : "r"(addr));
+    return val;
+}
 
-        asm volatile("ld %0, 0(%1)" : "=r"(val) : "r"(addr));
-        return val;
-    }
-    #endif
+#if __riscv_xlen != 32
+static inline rt_uint64_t __raw_readq(const volatile void *addr)
+{
+    rt_uint64_t val;
 
-    /* FIXME: These are now the same as asm-generic */
+    asm volatile("ld %0, 0(%1)"
+                 : "=r"(val)
+                 : "r"(addr));
+    return val;
+}
+#endif
 
-    /* clang-format off */
+/* FIXME: These are now the same as asm-generic */
 
-    #define __io_rbr()		do {} while (0)
-    #define __io_rar()		do {} while (0)
-    #define __io_rbw()		do {} while (0)
-    #define __io_raw()		do {} while (0)
+/* clang-format off */
 
-    #define readb_relaxed(c)	({ rt_uint8_t  __v; __io_rbr(); __v = __raw_readb(c); __io_rar(); __v; })
-    #define readw_relaxed(c)	({ rt_uint16_t __v; __io_rbr(); __v = __raw_readw(c); __io_rar(); __v; })
-    #define readl_relaxed(c)	({ rt_uint32_t __v; __io_rbr(); __v = __raw_readl(c); __io_rar(); __v; })
+#define __io_rbr()		do {} while (0)
+#define __io_rar()		do {} while (0)
+#define __io_rbw()		do {} while (0)
+#define __io_raw()		do {} while (0)
 
-    #define writeb_relaxed(v,c)	({ __io_rbw(); __raw_writeb((v),(c)); __io_raw(); })
-    #define writew_relaxed(v,c)	({ __io_rbw(); __raw_writew((v),(c)); __io_raw(); })
-    #define writel_relaxed(v,c)	({ __io_rbw(); __raw_writel((v),(c)); __io_raw(); })
+#define readb_relaxed(c)	({ rt_uint8_t  __v; __io_rbr(); __v = __raw_readb(c); __io_rar(); __v; })
+#define readw_relaxed(c)	({ rt_uint16_t __v; __io_rbr(); __v = __raw_readw(c); __io_rar(); __v; })
+#define readl_relaxed(c)	({ rt_uint32_t __v; __io_rbr(); __v = __raw_readl(c); __io_rar(); __v; })
 
-    #if __riscv_xlen != 32
-    #define readq_relaxed(c)	({ rt_uint64_t __v; __io_rbr(); __v = __raw_readq(c); __io_rar(); __v; })
-    #define writeq_relaxed(v,c)	({ __io_rbw(); __raw_writeq((v),(c)); __io_raw(); })
-    #endif
+#define writeb_relaxed(v,c)	({ __io_rbw(); __raw_writeb((v),(c)); __io_raw(); })
+#define writew_relaxed(v,c)	({ __io_rbw(); __raw_writew((v),(c)); __io_raw(); })
+#define writel_relaxed(v,c)	({ __io_rbw(); __raw_writel((v),(c)); __io_raw(); })
 
-    #define __io_br()	do {} while (0)
-    #define __io_ar()	__asm__ __volatile__ ("fence i,r" : : : "memory");
-    #define __io_bw()	__asm__ __volatile__ ("fence w,o" : : : "memory");
-    #define __io_aw()	do {} while (0)
+#if __riscv_xlen != 32
+#define readq_relaxed(c)	({ rt_uint64_t __v; __io_rbr(); __v = __raw_readq(c); __io_rar(); __v; })
+#define writeq_relaxed(v,c)	({ __io_rbw(); __raw_writeq((v),(c)); __io_raw(); })
+#endif
 
-    #define readb(c)	({ rt_uint8_t  __v; __io_br(); __v = __raw_readb(c); __io_ar(); __v; })
-    #define readw(c)	({ rt_uint16_t __v; __io_br(); __v = __raw_readw(c); __io_ar(); __v; })
-    #define readl(c)	({ rt_uint32_t __v; __io_br(); __v = __raw_readl(c); __io_ar(); __v; })
+#define __io_br()	do {} while (0)
+#define __io_ar()	__asm__ __volatile__ ("fence i,r" : : : "memory");
+#define __io_bw()	__asm__ __volatile__ ("fence w,o" : : : "memory");
+#define __io_aw()	do {} while (0)
 
-    #define writeb(v,c)	({ __io_bw(); __raw_writeb((v),(c)); __io_aw(); })
-    #define writew(v,c)	({ __io_bw(); __raw_writew((v),(c)); __io_aw(); })
-    #define writel(v,c)	({ __io_bw(); __raw_writel((v),(c)); __io_aw(); })
+#define readb(c)	({ rt_uint8_t  __v; __io_br(); __v = __raw_readb(c); __io_ar(); __v; })
+#define readw(c)	({ rt_uint16_t __v; __io_br(); __v = __raw_readw(c); __io_ar(); __v; })
+#define readl(c)	({ rt_uint32_t __v; __io_br(); __v = __raw_readl(c); __io_ar(); __v; })
 
-    #if __riscv_xlen != 32
-    #define readq(c)	({ rt_uint64_t __v; __io_br(); __v = __raw_readq(c); __io_ar(); __v; })
-    #define writeq(v,c)	({ __io_bw(); __raw_writeq((v),(c)); __io_aw(); })
-    #endif
+#define writeb(v,c)	({ __io_bw(); __raw_writeb((v),(c)); __io_aw(); })
+#define writew(v,c)	({ __io_bw(); __raw_writew((v),(c)); __io_aw(); })
+#define writel(v,c)	({ __io_bw(); __raw_writel((v),(c)); __io_aw(); })
+
+#if __riscv_xlen != 32
+#define readq(c)	({ rt_uint64_t __v; __io_br(); __v = __raw_readq(c); __io_ar(); __v; })
+#define writeq(v,c)	({ __io_bw(); __raw_writeq((v),(c)); __io_aw(); })
+#endif
 
 #endif
\ No newline at end of file
diff --git a/libcpu/risc-v/virt64/stack.h b/libcpu/risc-v/virt64/stack.h
index 27e9cdf9fa..09179238ea 100644
--- a/libcpu/risc-v/virt64/stack.h
+++ b/libcpu/risc-v/virt64/stack.h
@@ -16,7 +16,7 @@ struct rt_hw_stack_frame
 {
     rt_ubase_t epc;        /* epc - epc    - program counter                     */
     rt_ubase_t ra;         /* x1  - ra     - return address for jumps            */
-    rt_ubase_t sstatus;    /*              - supervisor status register          */
+    rt_ubase_t xstatus;    /*              - supervisor/machine/user status register          */
     rt_ubase_t gp;         /* x3  - gp     - global pointer                      */
     rt_ubase_t tp;         /* x4  - tp     - thread pointer                      */
     rt_ubase_t t0;         /* x5  - t0     - temporary register 0                */
diff --git a/libcpu/risc-v/virt64/stackframe.h b/libcpu/risc-v/virt64/stackframe.h
index e5a5271a7a..835f03ee3a 100644
--- a/libcpu/risc-v/virt64/stackframe.h
+++ b/libcpu/risc-v/virt64/stackframe.h
@@ -19,10 +19,10 @@
 
     STORE x1,   1 * REGBYTES(sp)
 
-    csrr  x1, sstatus
+    csrr  x1, SRC_XSTATUS
     STORE x1,   2 * REGBYTES(sp)
 
-    csrr  x1, sepc
+    csrr  x1, SRC_XEPC
     STORE x1, 0 * REGBYTES(sp)
 
     STORE x3,   3 * REGBYTES(sp)
@@ -54,17 +54,17 @@
     STORE x29, 29 * REGBYTES(sp)
     STORE x30, 30 * REGBYTES(sp)
     STORE x31, 31 * REGBYTES(sp)
-    csrr t0, sscratch
+    csrr t0, SRC_XSCRATCH
     STORE t0, 32 * REGBYTES(sp)
 .endm
 
 .macro RESTORE_ALL_ONLY
         /* resw ra to sepc */
     LOAD x1,   0 * REGBYTES(sp)
-    csrw sepc, x1
+    csrw SRC_XEPC, x1
 
     LOAD x1,   2 * REGBYTES(sp)
-    csrw sstatus, x1
+    csrw SRC_XSTATUS, x1
 
     LOAD x1,   1 * REGBYTES(sp)
 
@@ -104,10 +104,10 @@
 .macro RESTORE_ALL
         /* resw ra to sepc */
     LOAD x1,   0 * REGBYTES(sp)
-    csrw sepc, x1
+    csrw SRC_XEPC, x1
 
     LOAD x1,   2 * REGBYTES(sp)
-    csrw sstatus, x1
+    csrw SRC_XSTATUS, x1
 
     LOAD x1,   1 * REGBYTES(sp)
 
@@ -152,12 +152,4 @@
     .option pop
 .endm
 
-.macro OPEN_INTERRUPT
-    csrsi sstatus, 2
-.endm
-
-.macro CLOSE_INTERRUPT
-    csrci sstatus, 2
-.endm
-
 #endif
\ No newline at end of file
diff --git a/libcpu/risc-v/virt64/startup_gcc.S b/libcpu/risc-v/virt64/startup_gcc.S
index 9b1a27be9a..ac980f6ed2 100644
--- a/libcpu/risc-v/virt64/startup_gcc.S
+++ b/libcpu/risc-v/virt64/startup_gcc.S
@@ -10,71 +10,48 @@
  * 2020/6/12      Xim          Port to QEMU and remove SMP support
  */
 
-#define SSTATUS_FS      0x00006000U /* initial state of FPU, clear to disable */
+#define XSTATUS_FS         (3 << 13) /* initial state of FPU, clear to disable */
+#define XSTATUS_PUM        (1 << 18) 
 #include <cpuport.h>
 
   .global	_start
   .section ".start", "ax"
 _start:
-  j 1f
-  .word 0xdeadbeef
-  .align 3
-  .global g_wake_up
-  g_wake_up:
-      .dword 1
-      .dword 0
-1:
-  csrw sie, 0
-  csrw sip, 0
-  la t0, trap_entry
-  csrw stvec, t0
+#ifndef RISCV_S_MODE
+  # setup stacks per hart
+  csrr t0, mhartid                  # read current hart id
+  slli t0, t0, 10                   # shift left the hart id by 1024
+
+  # park harts with id != 0
+  csrr a0, mhartid                  # read current hart id
+  bnez a0, park                     # if we're not on the hart 0
+#endif
 
-  li x1, 0
-  li x2, 0
-  li x3, 0
-  li x4, 0
-  li x5, 0
-  li x6, 0
-  li x7, 0
-  li x8, 0
-  li x9, 0
-  li x10,0
-  li x11,0
-  li x12,0
-  li x13,0
-  li x14,0
-  li x15,0
-  li x16,0
-  li x17,0
-  li x18,0
-  li x19,0
-  li x20,0
-  li x21,0
-  li x22,0
-  li x23,0
-  li x24,0
-  li x25,0
-  li x26,0
-  li x27,0
-  li x28,0
-  li x29,0
-  li x30,0
-  li x31,0
+  csrw SRC_XIE, 0                   # clear Interrupt Registers
+  csrw SRC_XIP, 0
+
+  la t0, trap_entry
+  csrw SRC_XTVEC, t0                # set Trap Vector Base Address Register
 
   /* set to disable FPU */
-  li t0, SSTATUS_FS
-  csrc sstatus, t0
-  li t0, 0x40000 // SUM in sstatus
-  csrs sstatus, t0
+  li t0, XSTATUS_FS                 # close fpu
+  csrc SRC_XSTATUS, t0
+#ifdef RISCV_S_MODE
+  li t0, XSTATUS_PUM                # PUM has no effect
+  csrs SRC_XSTATUS, t0
+#endif
 
 .option push
 .option norelax
   la gp, __global_pointer$
 .option pop
 
-  // removed SMP support here
   la   sp, __stack_start__
   li   t0, __STACKSIZE__
   add  sp, sp, t0
-  csrw sscratch, sp
+  csrw SRC_XSCRATCH, sp
   j primary_cpu_entry
+
+park:
+    wfi
+    j park
\ No newline at end of file
diff --git a/libcpu/risc-v/virt64/tick.c b/libcpu/risc-v/virt64/tick.c
index 4f340c544f..9647751b46 100644
--- a/libcpu/risc-v/virt64/tick.c
+++ b/libcpu/risc-v/virt64/tick.c
@@ -11,8 +11,12 @@
 #include <rthw.h>
 #include <rtthread.h>
 
-#include <encoding.h>
 #include "sbi.h"
+#include "tick.h"
+#include <riscv_io.h>
+#include <encoding.h>
+
+#define VIRT_CLINT_TIMEBASE_FREQ    (10000000)
 
 static volatile uint64_t time_elapsed = 0;
 static volatile unsigned long tick_cycles = 0;
@@ -27,11 +31,14 @@ static uint64_t get_ticks()
 
 int tick_isr(void)
 {
-    // uint64_t core_id = current_coreid();
-    int tick_cycles = 40000;
-    // clint->mtimecmp[core_id] += tick_cycles;
+    int tick_cycles = VIRT_CLINT_TIMEBASE_FREQ / RT_TICK_PER_SECOND;
     rt_tick_increase();
+#ifdef RISCV_S_MODE
     sbi_set_timer(get_ticks() + tick_cycles);
+#else
+    int id = r_mhartid();
+    *(uint64_t*)CLINT_MTIMECMP(id) = *(uint64_t*)CLINT_MTIME + tick_cycles;
+#endif
 
     return 0;
 }
@@ -39,10 +46,9 @@ int tick_isr(void)
 /* Sets and enable the timer interrupt */
 int rt_hw_tick_init(void)
 {
-    /* Read core id */
-    // unsigned long core_id = current_coreid();
-    unsigned long interval = 1000/RT_TICK_PER_SECOND;
+    unsigned long interval = VIRT_CLINT_TIMEBASE_FREQ / RT_TICK_PER_SECOND;
 
+#ifdef RISCV_S_MODE
     /* Clear the Supervisor-Timer bit in SIE */
     clear_csr(sie, SIP_STIP);
 
@@ -51,9 +57,15 @@ int rt_hw_tick_init(void)
     tick_cycles = 40000;
     /* Set timer */
     sbi_set_timer(get_ticks() + tick_cycles);
-
+    
     /* Enable the Supervisor-Timer bit in SIE */
     set_csr(sie, SIP_STIP);
-
+#else
+    clear_csr(mie, MIP_MTIP);
+    clear_csr(mip, MIP_MTIP);
+    int id = r_mhartid();
+    *(uint64_t*)CLINT_MTIMECMP(id) = *(uint64_t*)CLINT_MTIME + interval;
+    set_csr(mie, MIP_MTIP);
+#endif
     return 0;
 }
diff --git a/libcpu/risc-v/virt64/tick.h b/libcpu/risc-v/virt64/tick.h
index 0821004263..1168e611e6 100644
--- a/libcpu/risc-v/virt64/tick.h
+++ b/libcpu/risc-v/virt64/tick.h
@@ -11,6 +11,11 @@
 #ifndef TICK_H__
 #define TICK_H__
 
+//ask the CLINT for a timer interrupt.
+#define CLINT                   (0x2000000L)
+#define CLINT_MTIMECMP(hartid)  (CLINT + 0x4000 + 4*(hartid))
+#define CLINT_MTIME             (CLINT + 0xBFF8)            // cycles since boot.
+
 int tick_isr(void);
 int rt_hw_tick_init(void);