Support for running with M-Mode

26 files changed, 1160 insertions, 460 deletions

diff --git a/bsp/qemu-riscv-virt64/.config b/bsp/qemu-riscv-virt64/.config
index 8a61ab72eb..49c8fff6b0 100644
--- a/bsp/qemu-riscv-virt64/.config
+++ b/bsp/qemu-riscv-virt64/.config
@@ -213,6 +213,7 @@ CONFIG_RT_LIBC_FIXED_TIMEZONE=8
 # CONFIG_RT_USING_RYM is not set
 # CONFIG_RT_USING_ULOG is not set
 # CONFIG_RT_USING_UTEST is not set
+# CONFIG_RT_USING_RT_LINK is not set
 
 #
 # RT-Thread Utestcases
@@ -335,6 +336,7 @@ CONFIG_RT_LIBC_FIXED_TIMEZONE=8
 # CONFIG_PKG_USING_HELIX is not set
 # CONFIG_PKG_USING_AZUREGUIX is not set
 # CONFIG_PKG_USING_TOUCHGFX2RTT is not set
+# CONFIG_PKG_USING_NUEMWIN is not set
 
 #
 # tools packages
@@ -492,6 +494,7 @@ CONFIG_RT_LIBC_FIXED_TIMEZONE=8
 # CONFIG_PKG_USING_LIBNFC is not set
 # CONFIG_PKG_USING_MFOC is not set
 # CONFIG_PKG_USING_TMC51XX is not set
+# CONFIG_PKG_USING_TCA9534 is not set
 
 #
 # AI packages
@@ -519,6 +522,7 @@ CONFIG_RT_LIBC_FIXED_TIMEZONE=8
 # CONFIG_PKG_USING_FLEXIBLE_BUTTON is not set
 # CONFIG_PKG_USING_CANFESTIVAL is not set
 # CONFIG_PKG_USING_ZLIB is not set
+# CONFIG_PKG_USING_MINIZIP is not set
 # CONFIG_PKG_USING_DSTR is not set
 # CONFIG_PKG_USING_TINYFRAME is not set
 # CONFIG_PKG_USING_KENDRYTE_DEMO is not set
@@ -558,7 +562,8 @@ CONFIG_BOARD_virt=y
 CONFIG_RT_USING_USERSPACE=y
 
 #
-# General Purpose UARTs
+# RISCV qemu virt64 configs
 #
 # CONFIG_BSP_USING_UART1 is not set
+# CONFIG_RISCV_S_MODE is not set
 CONFIG___STACKSIZE__=16384
diff --git a/bsp/qemu-riscv-virt64/SConstruct b/bsp/qemu-riscv-virt64/SConstruct
index 422e592ccd..770a62ccc2 100644
--- a/bsp/qemu-riscv-virt64/SConstruct
+++ b/bsp/qemu-riscv-virt64/SConstruct
@@ -31,7 +31,11 @@ stack_size = 4096
 
 stack_lds = open('link_stacksize.lds', 'w')
 if GetDepend('__STACKSIZE__'): stack_size = GetDepend('__STACKSIZE__')
-stack_lds.write('__STACKSIZE__ = %d;' % stack_size)
+stack_lds.write('__STACKSIZE__ = %d;\r\n' % stack_size)
+if GetDepend('RISCV_S_MODE'):  start_addr = int(0x80200000) 
+else: start_addr = int(0x80000000)
+stack_lds.write('__START_ADDR__ = 0x%x;' % start_addr)
+
 stack_lds.close()
 
 # make a building
diff --git a/bsp/qemu-riscv-virt64/applications/main.c b/bsp/qemu-riscv-virt64/applications/main.c
index 288d4f08ba..50d2de7a22 100644
--- a/bsp/qemu-riscv-virt64/applications/main.c
+++ b/bsp/qemu-riscv-virt64/applications/main.c
@@ -1,10 +1,11 @@
 /*
- * Copyright (c) 2006-2018, RT-Thread Development Team
+ * Copyright (c) 2006-2021, RT-Thread Development Team
  *
  * SPDX-License-Identifier: Apache-2.0
  *
  * Change Logs:
  * Date           Author       Notes
+ * 2021-05-20     bigmagic     first version
  */
 
 #include <rtthread.h>
@@ -14,9 +15,7 @@
 
 int main(void)
 {
-    void rt_hw_uart_start_rx_thread();
-    rt_hw_uart_start_rx_thread();
-    printf("Hello RISC-V\n");
+    printf("Hello RISC-V!\n");
 
     return 0;
 }
diff --git a/bsp/qemu-riscv-virt64/driver/Kconfig b/bsp/qemu-riscv-virt64/driver/Kconfig
index 5744c89702..d6fca695ef 100644
--- a/bsp/qemu-riscv-virt64/driver/Kconfig
+++ b/bsp/qemu-riscv-virt64/driver/Kconfig
@@ -1,6 +1,6 @@
 
 
-menu "General Purpose UARTs"
+menu "RISCV qemu virt64 configs"
 
 menuconfig BSP_USING_UART1
     bool "Enable UART1"
@@ -14,5 +14,8 @@ menuconfig BSP_USING_UART1
             default 21
     endif
 
-endmenu
+config RISCV_S_MODE
+    bool "RT-Thread run in riscv smode"
+    default y
 
+endmenu
diff --git a/bsp/qemu-riscv-virt64/driver/board.c b/bsp/qemu-riscv-virt64/driver/board.c
index e723f49aca..1e567ea151 100644
--- a/bsp/qemu-riscv-virt64/driver/board.c
+++ b/bsp/qemu-riscv-virt64/driver/board.c
@@ -21,6 +21,9 @@
 #include "sbi.h"
 #include "riscv.h"
 #include "stack.h"
+#include "riscv_io.h"
+#include "plic.h"
+#include "interrupt.h"
 
 void primary_cpu_entry(void)
 {
@@ -33,11 +36,6 @@ void primary_cpu_entry(void)
     entry();
 }
 
-void rt_hw_interrupt_init()
-{
-    /* Enable machine external interrupts. */
-    set_csr(sie, SIP_SEIP);
-}
 
 void rt_hw_board_init(void)
 {
@@ -46,10 +44,7 @@ void rt_hw_board_init(void)
     /* initialize hardware interrupt */
     rt_hw_uart_init();
 
-    rt_hw_tick_init();
-
     #ifdef RT_USING_HEAP
-        rt_kprintf("heap: [0x%08x - 0x%08x]\n", (rt_ubase_t) RT_HW_HEAP_BEGIN, (rt_ubase_t) RT_HW_HEAP_END);
         /* initialize memory system */
         rt_system_heap_init(RT_HW_HEAP_BEGIN, RT_HW_HEAP_END);
     #endif
@@ -58,7 +53,9 @@ void rt_hw_board_init(void)
         /* set console device */
         rt_console_set_device("uart");
     #endif /* RT_USING_CONSOLE */
-
+    rt_hw_tick_init();
+    rt_kprintf("heap: [0x%08x - 0x%08x]\n", (rt_ubase_t) RT_HW_HEAP_BEGIN, (rt_ubase_t) RT_HW_HEAP_END);
+    
     #ifdef RT_USING_COMPONENTS_INIT
         rt_components_board_init();
     #endif
@@ -71,137 +68,3 @@ void rt_hw_cpu_reset(void)
 }
 MSH_CMD_EXPORT_ALIAS(rt_hw_cpu_reset, reboot, reset machine);
 
-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("sstatus = 0x%p\n",regs -> sstatus);
-    rt_kprintf("\t%s\n",(regs -> sstatus & SSTATUS_SIE) ? "Supervisor Interrupt Enabled" : "Supervisor Interrupt Disabled");
-    rt_kprintf("\t%s\n",(regs -> sstatus & SSTATUS_SPIE) ? "Last Time Supervisor Interrupt Enabled" : "Last Time Supervisor Interrupt Disabled");
-    rt_kprintf("\t%s\n",(regs -> sstatus & SSTATUS_SPP) ? "Last Privilege is Supervisor Mode" : "Last Privilege is User Mode");
-    rt_kprintf("\t%s\n",(regs -> sstatus & SSTATUS_PUM) ? "Permit to Access User Page" : "Not Permit to Access User Page");
-    rt_kprintf("\t%s\n",(regs -> sstatus & (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");
-}
-
-static const char *Exception_Name[] = 
-                                {
-                                    "Instruction Address Misaligned",
-                                    "Instruction Access Fault",
-                                    "Illegal Instruction",
-                                    "Breakpoint",
-                                    "Load Address Misaligned",
-                                    "Load Access Fault",
-                                    "Store/AMO Address Misaligned",
-                                    "Store/AMO Access Fault",
-                                    "Environment call from U-mode",
-                                    "Environment call from S-mode",
-                                    "Reserved-10",
-                                    "Reserved-11",
-                                    "Instruction Page Fault",
-                                    "Load Page Fault",
-                                    "Reserved-14",
-                                    "Store/AMO Page Fault"
-                                };
-
-static const char *Interrupt_Name[] = 
-                                {
-                                    "User Software Interrupt",
-                                    "Supervisor Software Interrupt",
-                                    "Reversed-2",
-                                    "Reversed-3",
-                                    "User Timer Interrupt",
-                                    "Supervisor Timer Interrupt",
-                                    "Reversed-6",
-                                    "Reversed-7",
-                                    "User External Interrupt",
-                                    "Supervisor External Interrupt",
-                                    "Reserved-10",
-                                    "Reserved-11",
-                                };
-
-void handle_trap(rt_size_t scause,rt_size_t stval,rt_size_t sepc,struct rt_hw_stack_frame *sp)
-{
-    if(scause == (uint64_t)(0x8000000000000005))
-    {
-        rt_interrupt_enter();
-        tick_isr();
-        rt_interrupt_leave();
-    }
-    else
-    {
-        rt_size_t id = __MASKVALUE(scause,__MASK(63UL));
-        const char *msg;
-
-        if(scause >> 63)
-        {
-            if(id < sizeof(Interrupt_Name) / sizeof(const char *))
-            {
-                msg = Interrupt_Name[id];
-            }
-            else
-            {
-                msg = "Unknown Interrupt";
-            }
-
-            rt_kprintf("Unhandled Interrupt %ld:%s\n",id,msg);
-        }
-        else
-        {
-            if(id < sizeof(Exception_Name) / sizeof(const char *))
-            {
-                msg = Exception_Name[id];
-            }
-            else
-            {
-                msg = "Unknown Exception";
-            }
-
-            rt_kprintf("Unhandled Exception %ld:%s\n",id,msg);
-        }
-
-        rt_kprintf("scause:0x%p,stval:0x%p,sepc:0x%p\n",scause,stval,sepc);
-        dump_regs(sp);
-        while(1);
-    }
-}
diff --git a/bsp/qemu-riscv-virt64/driver/drv_uart.c b/bsp/qemu-riscv-virt64/driver/drv_uart.c
index 754a1fbfd1..f6ee2e1c7f 100644
--- a/bsp/qemu-riscv-virt64/driver/drv_uart.c
+++ b/bsp/qemu-riscv-virt64/driver/drv_uart.c
@@ -1,10 +1,12 @@
 /*
- * Copyright (c) 2019-2020, Xim
+ * Copyright (c) 2006-2021, RT-Thread Development Team
  *
  * SPDX-License-Identifier: Apache-2.0
  *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-20     bigmagic     first version
  */
-
 #include <rthw.h>
 #include <rtdevice.h>
 
@@ -13,8 +15,7 @@
 
 #include <stdio.h>
 #include "sbi.h"
-
-#define UART_DEFAULT_BAUDRATE               115200
+#include "interrupt.h"
 
 struct device_uart
 {
@@ -27,23 +28,25 @@ static rt_err_t uart_control(struct rt_serial_device *serial, int cmd, void *arg
 static int       drv_uart_putc(struct rt_serial_device *serial, char c);
 static int       drv_uart_getc(struct rt_serial_device *serial);
 
-const struct rt_uart_ops _uart_ops =
+void virt_uart_init(void)
 {
-    rt_uart_configure,
-    uart_control,
-    drv_uart_putc,
-    drv_uart_getc,
-    //TODO: add DMA support
-    RT_NULL
-};
+    //http://byterunner.com/16550.html
+    uart_write_reg(IER, 0x00);
 
-void uart_init(void)
-{
-    return ;
-}
+    uint8_t lcr = uart_read_reg(LCR);
+    uart_write_reg(LCR, lcr | (1 << 7));
+    uart_write_reg(DLL, 0x03);
+    uart_write_reg(DLM, 0x00);
 
-struct rt_serial_device  serial1;
-struct device_uart       uart1;
+    lcr = 0;
+    uart_write_reg(LCR, lcr | (3 << 0));
+
+    /*
+     * enable receive interrupts.
+     */
+    uint8_t ier = uart_read_reg(IER);
+    uart_write_reg(IER, ier | (1 << 0));
+}
 
 /*
  * UART interface
@@ -58,22 +61,6 @@ static rt_err_t rt_uart_configure(struct rt_serial_device *serial, struct serial
     return (RT_EOK);
 }
 
-#define UART_LSR_DR     0x01            /* Data ready */
-#define UART_LSR_THRE   0x20            /* Xmit holding register empty */
-
-#define UART_RBR(hw)    HWREG32(hw + 0x00)
-#define UART_IER(hw)    HWREG32(hw + 0x04)
-#define UART_LSR(hw)    HWREG32(hw + 0x14)
-
-static volatile uint64_t uart_hwbase = 0x10000000;
-
-void uart_putc(char c)
-{
-    while ((UART_LSR(uart_hwbase) & UART_LSR_THRE) == 0);
-
-    UART_RBR(uart_hwbase) = c;
-}
-
 static rt_err_t uart_control(struct rt_serial_device *serial, int cmd, void *arg)
 {
     struct device_uart *uart;
@@ -97,38 +84,38 @@ static rt_err_t uart_control(struct rt_serial_device *serial, int cmd, void *arg
 }
 
 static int drv_uart_putc(struct rt_serial_device *serial, char c)
-{
-    sbi_console_putchar(c);
-    return (1);
+{   
+    while ((uart_read_reg(LSR) & LSR_TX_IDLE) == 0);
+    return uart_write_reg(THR, c);
 }
 
 static int drv_uart_getc(struct rt_serial_device *serial)
 {
-    return sbi_console_getchar();
+    if (uart_read_reg(LSR) & LSR_RX_READY){
+        return uart_read_reg(RHR);
+    } else {
+        return -1;
+    }
+    //return sbi_console_getchar();
 }
 
-char rt_hw_console_getchar(void)
+static void rt_hw_uart_isr(int irqno, void *param)
 {
-    return sbi_console_getchar();
+    struct rt_serial_device *serial = (struct rt_serial_device*)param;
+    rt_hw_serial_isr(serial, RT_SERIAL_EVENT_RX_IND);
 }
 
-static void uart_rx(void *param)
-{
-    struct rt_serial_device *serial = (struct rt_serial_device *)param;
-
-    while(1)
-    {
-        rt_hw_serial_isr((struct rt_serial_device *)serial,RT_SERIAL_EVENT_RX_IND);
-        rt_thread_mdelay(10);
-    }
-}
+struct rt_serial_device  serial1;
+struct device_uart       uart1;
 
-void rt_hw_uart_start_rx_thread()
+const struct rt_uart_ops _uart_ops =
 {
-    rt_thread_t th;
-    RT_ASSERT((th = rt_thread_create("uartrx",uart_rx,(void *)&serial1,8192,8,20)) != RT_NULL);
-    RT_ASSERT(rt_thread_startup(th) == RT_EOK);
-}
+    rt_uart_configure,
+    uart_control,
+    drv_uart_putc,
+    drv_uart_getc,
+    RT_NULL
+};
 
 /*
  * UART Initiation
@@ -138,7 +125,6 @@ int rt_hw_uart_init(void)
     struct rt_serial_device *serial;
     struct device_uart      *uart;
     struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
-
     {
         serial  = &serial1;
         uart    = &uart1;
@@ -147,19 +133,19 @@ int rt_hw_uart_init(void)
         serial->config           = config;
         serial->config.baud_rate = UART_DEFAULT_BAUDRATE;
 
-        uart->hw_base   = 0x10000000;
-        uart->irqno     = 0xa;
+        uart->hw_base   = UART_BASE;
+        uart->irqno     = UART0_IRQ;
+
+        virt_uart_init();
 
         rt_hw_serial_register(serial,
                               "uart",
                               RT_DEVICE_FLAG_STREAM | RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
                               uart);
+        rt_hw_interrupt_install(uart->irqno, rt_hw_uart_isr, serial, "uart");
+        
+        rt_hw_interrupt_umask(uart->irqno);
     }
     
     return 0;
 }
-
-/* WEAK for SDK 0.5.6 */
-RT_WEAK void uart_debug_init(int uart_channel)
-{
-}
diff --git a/bsp/qemu-riscv-virt64/driver/drv_uart.h b/bsp/qemu-riscv-virt64/driver/drv_uart.h
index a7a18d0dde..1a590b83df 100644
--- a/bsp/qemu-riscv-virt64/driver/drv_uart.h
+++ b/bsp/qemu-riscv-virt64/driver/drv_uart.h
@@ -1,15 +1,43 @@
 /*
- * Copyright (c) 2019-2020, Xim
+ * Copyright (c) 2006-2021, RT-Thread Development Team
  *
  * SPDX-License-Identifier: Apache-2.0
  *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-20     bigmagic     first version
  */
 
 #ifndef __DRV_UART_H__
 #define __DRV_UART_H__
 
-void rt_hw_uart_start_rx_thread();
+#define UART0_IRQ   (10)
+
+#define UART_DEFAULT_BAUDRATE               115200
+
+#define UART_BASE            (0x10000000L)
+
+#define RHR 0    // Receive Holding Register (read mode)
+#define THR 0    // Transmit Holding Register (write mode)
+#define DLL 0    // LSB of Divisor Latch (write mode)
+#define IER 1    // Interrupt Enable Register (write mode)
+#define DLM 1    // MSB of Divisor Latch (write mode)
+#define FCR 2    // FIFO Control Register (write mode)
+#define ISR 2    // Interrupt Status Register (read mode)
+#define LCR 3    // Line Control Register
+#define MCR 4    // Modem Control Register
+#define LSR 5    // Line Status Register
+#define MSR 6    // Modem Status Register
+#define SPR 7    // ScratchPad Register
+
+#define UART_REG(reg) ((volatile uint8_t *)(UART_BASE + reg))
+
+#define LSR_RX_READY (1 << 0)
+#define LSR_TX_IDLE  (1 << 5)
+
+#define uart_read_reg(reg) (*(UART_REG(reg)))
+#define uart_write_reg(reg, v) (*(UART_REG(reg)) = (v))
+
 int rt_hw_uart_init(void);
-void drv_uart_puts(char *str); // for syscall
 
 #endif /* __DRV_UART_H__ */
diff --git a/bsp/qemu-riscv-virt64/driver/plic.c b/bsp/qemu-riscv-virt64/driver/plic.c
new file mode 100644
index 0000000000..afd186933d
--- /dev/null
+++ b/bsp/qemu-riscv-virt64/driver/plic.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-20     bigmagic     first version
+ */
+#include "rtthread.h"
+#include "plic.h"
+#include <riscv_io.h>
+#include "encoding.h"
+
+/*
+* Each PLIC interrupt source can be assigned a priority by writing 
+* to its 32-bit memory-mapped priority register.
+* The QEMU-virt (the same as FU540-C000) supports 7 levels of priority. 
+* A priority value of 0 is reserved to mean "never interrupt" and 
+* effectively disables the interrupt. 
+* Priority 1 is the lowest active priority, and priority 7 is the highest. 
+* Ties between global interrupts of the same priority are broken by 
+* the Interrupt ID; interrupts with the lowest ID have the highest 
+* effective priority.
+*/
+void plic_set_priority(int irq, int priority)
+{
+    *(uint32_t*)PLIC_PRIORITY(irq) = priority;
+}
+
+/*
+* Each global interrupt can be enabled by setting the corresponding 
+* bit in the enables registers.
+*/
+void plic_irq_enable(int irq)
+{
+    int hart = r_mhartid();
+    *(uint32_t*)PLIC_ENABLE(hart) = ((*(uint32_t*)PLIC_ENABLE(hart)) | (1 << irq));
+#ifdef  RISCV_S_MODE
+    set_csr(sie, read_csr(sie) | MIP_SEIP);
+#else
+    set_csr(mie, read_csr(mie) | MIP_MEIP);
+#endif
+}
+
+void plic_irq_disable(int irq)
+{
+    int hart = r_mhartid();
+    *(uint32_t*)PLIC_ENABLE(hart) = (((*(uint32_t*)PLIC_ENABLE(hart)) & (~(1 << irq))));
+}
+
+/* 
+* PLIC will mask all interrupts of a priority less than or equal to threshold.
+* Maximum threshold is 7.
+* For example, a threshold value of zero permits all interrupts with
+* non-zero priority, whereas a value of 7 masks all interrupts.
+* Notice, the threshold is global for PLIC, not for each interrupt source.
+*/
+void plic_set_threshold(int threshold)
+{
+    int hart = r_mhartid();
+    *(uint32_t*)PLIC_THRESHOLD(hart) = threshold;
+}
+
+/* 
+ * DESCRIPTION:
+ *    Query the PLIC what interrupt we should serve.
+ *    Perform an interrupt claim by reading the claim register, which
+ *    returns the ID of the highest-priority pending interrupt or zero if there 
+ *    is no pending interrupt. 
+ *    A successful claim also atomically clears the corresponding pending bit
+ *    on the interrupt source.
+ * RETURN VALUE:
+ *    the ID of the highest-priority pending interrupt or zero if there 
+ *    is no pending interrupt.
+ */
+int plic_claim(void)
+{
+    int hart = r_mhartid();
+    int irq = *(uint32_t*)PLIC_CLAIM(hart);
+    return irq;
+}
+
+/* 
+ * DESCRIPTION:
+  *    Writing the interrupt ID it received from the claim (irq) to the 
+ *    complete register would signal the PLIC we've served this IRQ. 
+ *    The PLIC does not check whether the completion ID is the same as the 
+ *    last claim ID for that target. If the completion ID does not match an 
+ *    interrupt source that is currently enabled for the target, the completion
+ *    is silently ignored.
+ * RETURN VALUE: none
+ */
+void plic_complete(int irq)
+{
+    int hart = r_mhartid();
+    *(uint32_t*)PLIC_COMPLETE(hart) = irq;
+}
diff --git a/bsp/qemu-riscv-virt64/driver/plic.h b/bsp/qemu-riscv-virt64/driver/plic.h
new file mode 100644
index 0000000000..79af90cbfb
--- /dev/null
+++ b/bsp/qemu-riscv-virt64/driver/plic.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-05-20     bigmagic     first version
+ */
+
+#ifndef PLIC_H
+#define PLIC_H
+
+#include <rtconfig.h>
+/*
+ * This machine puts platform-level interrupt controller (PLIC) here.
+ * Here only list PLIC registers in Machine mode.
+ * 
+ */
+
+#define VIRT_PLIC_BASE 0x0c000000L
+
+#define PLIC_PRIORITY_OFFSET                (0x0)
+#define PLIC_PENDING_OFFSET                 (0x1000)
+
+#ifndef RISCV_S_MODE
+#define PLIC_MENABLE_OFFSET                 (0x2000)
+#define PLIC_MTHRESHOLD_OFFSET              (0x200000)
+#define PLIC_MCLAIM_OFFSET                  (0x200004)
+#define PLIC_MCOMPLETE_OFFSET               (0x200004)
+
+#define PLIC_ENABLE(hart)             (VIRT_PLIC_BASE + PLIC_MENABLE_OFFSET + (hart) * 0x80)
+#define PLIC_THRESHOLD(hart)          (VIRT_PLIC_BASE + PLIC_MTHRESHOLD_OFFSET + (hart) * 0x1000)
+#define PLIC_CLAIM(hart)              (VIRT_PLIC_BASE + PLIC_MCLAIM_OFFSET + (hart) * 0x1000)
+#define PLIC_COMPLETE(hart)           (VIRT_PLIC_BASE + PLIC_MCOMPLETE_OFFSET + (hart) * 0x1000)
+
+#else
+#define PLIC_SENABLE_OFFSET                 (0x2080)
+#define PLIC_STHRESHOLD_OFFSET              (0x201000)
+#define PLIC_SCLAIM_OFFSET                  (0x201004)
+#define PLIC_SCOMPLETE_OFFSET               (0x201004)
+
+#define PLIC_ENABLE(hart)             (VIRT_PLIC_BASE + PLIC_SENABLE_OFFSET + (hart) * 0x80)
+#define PLIC_THRESHOLD(hart)          (VIRT_PLIC_BASE + PLIC_STHRESHOLD_OFFSET + (hart) * 0x1000)
+#define PLIC_CLAIM(hart)              (VIRT_PLIC_BASE + PLIC_SCLAIM_OFFSET + (hart) * 0x1000)
+#define PLIC_COMPLETE(hart)           (VIRT_PLIC_BASE + PLIC_SCOMPLETE_OFFSET + (hart) * 0x1000)
+#endif
+
+#define PLIC_PRIORITY(id)              (VIRT_PLIC_BASE + PLIC_PRIORITY_OFFSET + (id) * 4)
+#define PLIC_PENDING(id)               (VIRT_PLIC_BASE + PLIC_PENDING_OFFSET + ((id) / 32))
+
+
+
+void plic_set_priority(int irq, int priority);
+void plic_irq_enable(int irq);
+void plic_irq_disable(int irq);
+void plic_set_threshold(int mthreshold);
+int  plic_claim(void);
+void plic_complete(int irq);
+
+#endif
diff --git a/bsp/qemu-riscv-virt64/link.lds b/bsp/qemu-riscv-virt64/link.lds
index e8c0db9c24..2614e0eb36 100644
--- a/bsp/qemu-riscv-virt64/link.lds
+++ b/bsp/qemu-riscv-virt64/link.lds
@@ -21,13 +21,13 @@ OUTPUT_ARCH( "riscv" )
 
 MEMORY
 {
-   SRAM : ORIGIN = 0x80200000, LENGTH = 0x7FF000
+   SRAM : ORIGIN = __START_ADDR__, LENGTH = 0x7FF000
 }
 
 ENTRY(_start)
 SECTIONS
 {
-    . = 0x80200000 ;
+    . = __START_ADDR__ ;
 
     /* __STACKSIZE__ = 4096; */
 
diff --git a/bsp/qemu-riscv-virt64/link_stacksize.lds b/bsp/qemu-riscv-virt64/link_stacksize.lds
index 8685bc0f1c..4a0d736ae8 100644
--- a/bsp/qemu-riscv-virt64/link_stacksize.lds
+++ b/bsp/qemu-riscv-virt64/link_stacksize.lds
@@ -1 +1,2 @@
-__STACKSIZE__ = 16384;
\ No newline at end of file
+__STACKSIZE__ = 16384;
+__START_ADDR__ = 0x80000000;
\ No newline at end of file
diff --git a/bsp/qemu-riscv-virt64/qemu-nographic-smode.sh b/bsp/qemu-riscv-virt64/qemu-nographic-smode.sh
new file mode 100755
index 0000000000..750f6f0fcd
--- /dev/null
+++ b/bsp/qemu-riscv-virt64/qemu-nographic-smode.sh
@@ -0,0 +1 @@
+qemu-system-riscv64 -nographic -machine virt -m 256M -kernel rtthread.bin
\ No newline at end of file
diff --git a/bsp/qemu-riscv-virt64/qemu-nographic.sh b/bsp/qemu-riscv-virt64/qemu-nographic.sh
index 934c4f935b..546e7a3d87 100644..100755
--- a/bsp/qemu-riscv-virt64/qemu-nographic.sh
+++ b/bsp/qemu-riscv-virt64/qemu-nographic.sh
@@ -1 +1 @@
-qemu-system-riscv64 -nographic -machine virt -m 256M -kernel rtthread.bin
+qemu-system-riscv64 -nographic -machine virt -m 256M -bios rtthread.bin
diff --git a/bsp/qemu-riscv-virt64/rtconfig.h b/bsp/qemu-riscv-virt64/rtconfig.h
index 38d19a2265..fab069701e 100644
--- a/bsp/qemu-riscv-virt64/rtconfig.h
+++ b/bsp/qemu-riscv-virt64/rtconfig.h
@@ -177,7 +177,7 @@
 #define BOARD_virt
 #define RT_USING_USERSPACE
 
-/* General Purpose UARTs */
+/* RISCV qemu virt64 configs */
 
 #define __STACKSIZE__ 16384
 
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);