diff --git a/Cargo.lock b/Cargo.lock
index cf8f35ff..fbc66c14 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -180,6 +180,7 @@ dependencies = [
  "psci",
  "qemu-exit",
  "raw-cpuid",
+ "riscv 0.13.0",
  "riscv 0.6.0",
  "riscv-decode",
  "riscv-pac",
diff --git a/Cargo.toml b/Cargo.toml
index e300318c..4920c3c3 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -32,7 +32,8 @@ psci = { version = "0.1.0", default-features = false, features = ["smc"]}
 [target.'cfg(target_arch = "riscv64")'.dependencies]
 sbi-rt = { version = "0.0.3", features = ["legacy"] }
 sbi-spec = "0.0.8"
-riscv = { git = "https://github.com/rcore-os/riscv", features = ["inline-asm"] }
+riscv = "0.13.0"
+riscv_h = { package = "riscv", git = "https://github.com/rcore-os/riscv", features = ["inline-asm"] }
 riscv-decode = "0.2.1"
 riscv-peripheral = "0.2.1"
 riscv-pac = "0.2.0"
diff --git a/src/arch/riscv64/csr.rs b/src/arch/riscv64/csr.rs
index aee4e94f..556d7899 100644
--- a/src/arch/riscv64/csr.rs
+++ b/src/arch/riscv64/csr.rs
@@ -57,6 +57,8 @@ pub const CSR_HENVCFGH: u64 = 0x61A;
 pub const CSR_STIMECMP: u64 = 0x14D;
 pub const CSR_STIMECMPH: u64 = 0x15D;
 
+pub const HSTATUS_SPV: u64 = 1 << 7;
+
 macro_rules! read_csr {
     ($csr_number:expr) => {
         {
@@ -73,6 +75,7 @@ macro_rules! read_csr {
     }
 }
 pub(crate) use read_csr;
+
 macro_rules! write_csr {
     ($csr_number:expr, $value: expr) => {
         unsafe{
diff --git a/src/arch/riscv64/ipi.rs b/src/arch/riscv64/ipi.rs
index 7a7d89eb..4f837263 100644
--- a/src/arch/riscv64/ipi.rs
+++ b/src/arch/riscv64/ipi.rs
@@ -23,3 +23,10 @@ pub fn arch_send_event(cpu_id: u64, _sgi_num: u64) {
     #[cfg(not(feature = "aclint"))]
     sbi_rt::send_ipi(HartMask::from_mask_base(1 << cpu_id, 0));
 }
+
+/// Handle send_ipi event.
+pub fn arch_ipi_handler() {
+    unsafe {
+        riscv_h::register::hvip::set_vssip();
+    }
+}
diff --git a/src/arch/riscv64/sbi.rs b/src/arch/riscv64/sbi.rs
index ba191e26..d3084750 100644
--- a/src/arch/riscv64/sbi.rs
+++ b/src/arch/riscv64/sbi.rs
@@ -15,43 +15,42 @@
 //
 //! SBI call wrappers
 
-#![allow(unused)]
+use super::cpu::ArchCpu;
+use crate::arch::csr::*;
+use crate::event::{send_event, IPI_EVENT_SEND_IPI, IPI_EVENT_WAKEUP};
 use crate::hypercall::HyperCall;
 use crate::percpu::{get_cpu_data, this_cpu_data};
+use core::sync::atomic::{self, Ordering};
+use riscv::register::sie;
+use riscv_h::register::hvip;
+use sbi_rt::{HartMask, SbiRet};
+use sbi_spec::binary::{
+    RET_ERR_ALREADY_AVAILABLE, RET_ERR_FAILED, RET_ERR_NOT_SUPPORTED, RET_SUCCESS,
+};
+use sbi_spec::{base, hsm, rfnc, spi, time};
 
-use super::cpu::ArchCpu;
-use crate::arch::csr::*;
-use crate::event::{send_event, IPI_EVENT_WAKEUP};
-use riscv::register::{hvip, sie};
-#[allow(non_snake_case)]
-pub mod SBI_EID {
-    pub const BASE_EXTID: usize = 0x10;
-    pub const SET_TIMER: usize = 0x54494D45;
-    pub const EXTID_HSM: usize = 0x48534D;
-    pub const SEND_IPI: usize = 0x735049;
-    pub const RFENCE: usize = 0x52464E43;
-    pub const PMU: usize = 0x504D55;
-    pub const HVISOR: usize = 0x114514;
-}
-pub const SBI_SUCCESS: i64 = 0;
-pub const SBI_ERR_FAILURE: i64 = -1;
-pub const SBI_ERR_NOT_SUPPORTED: i64 = -2;
-pub const SBI_ERR_INVALID_PARAM: i64 = -3;
-pub const SBI_ERR_DENIED: i64 = -4;
-pub const SBI_ERR_INVALID_ADDRESS: i64 = -5;
-pub const SBI_ERR_ALREADY_AVAILABLE: i64 = -6;
-pub struct SbiRet {
-    error: i64,
-    value: i64,
-}
-/// use sbi call to putchar in console (qemu uart handler)
-pub fn console_putchar(c: u8) {
+// Reserved for hvisor-tool.
+pub const EID_HVISOR: usize = 0x114514;
+
+// Hvisor supported SBI extensions.
+pub const NUM_EXT: usize = 6;
+pub const EXT_TABLE: [usize; NUM_EXT] = [
+    base::EID_BASE,
+    time::EID_TIME,
+    hsm::EID_HSM,
+    spi::EID_SPI,
+    rfnc::EID_RFNC,
+    EID_HVISOR,
+];
+
+/// Use sbi call to putchar in console (qemu uart handler)
+pub fn sbi_console_putchar(c: u8) {
     #[allow(deprecated)]
     sbi_rt::legacy::console_putchar(c as _);
 }
 
-/// use sbi call to getchar from console (qemu uart handler)
-pub fn console_getchar() -> Option<u8> {
+/// Use sbi call to getchar from console (qemu uart handler)
+pub fn sbi_console_getchar() -> Option<u8> {
     #[allow(deprecated)]
     match sbi_rt::legacy::console_getchar() {
         x if x <= 0xff => Some(x as _),
@@ -59,150 +58,112 @@ pub fn console_getchar() -> Option<u8> {
     }
 }
 
-/// use sbi call to set timer
-pub fn set_timer(timer: usize) {
-    sbi_rt::set_timer(timer as _);
-}
-
-/// use sbi call to shutdown the kernel
-pub fn shutdown(failure: bool) -> ! {
-    use sbi_rt::{system_reset, NoReason, Shutdown, SystemFailure};
-    if !failure {
-        system_reset(Shutdown, NoReason);
-    } else {
-        system_reset(Shutdown, SystemFailure);
-    }
-    unreachable!()
-}
-
+/// Handle SBI calls form VS mode, sscratch -> current_cpu
 pub fn sbi_vs_handler(current_cpu: &mut ArchCpu) {
+    // a7 encodes the SBI extension ID (EID)
+    // a6 encodes the SBI function ID (FID)
     let eid: usize = current_cpu.x[17];
     let fid: usize = current_cpu.x[16];
     let sbi_ret;
     match eid {
-        //SBI_EXTID_BASE => sbi_ret = sbi_base_handler(fid, current_cpu),
-        SBI_EID::BASE_EXTID => {
-            trace!("SBI_EID::BASE,fid:{:#x}", fid);
-            sbi_ret = sbi_call_5(
-                eid,
-                fid,
-                current_cpu.x[10],
-                current_cpu.x[11],
-                current_cpu.x[12],
-                current_cpu.x[13],
-                current_cpu.x[14],
-            );
+        // Base Extension (SBI Spec Chapter 4)
+        base::EID_BASE => {
+            sbi_ret = sbi_base_handler(fid, current_cpu);
         }
-        SBI_EID::SET_TIMER => {
-            //debug!("SBI_EID::SET_TIMER on CPU {}", current_cpu.cpuid);
+        // Timer Extension (SBI Spec Chapter 6)
+        time::EID_TIME => {
             sbi_ret = sbi_time_handler(fid, current_cpu);
         }
-        SBI_EID::EXTID_HSM => {
-            info!("SBI_EID::EXTID_HSM on CPU {}", current_cpu.cpuid);
+        // Hart State Management (SBI Spec Chapter 9)
+        hsm::EID_HSM => {
             sbi_ret = sbi_hsm_handler(fid, current_cpu);
         }
-        SBI_EID::SEND_IPI => {
-            trace!("SBI_EID::SEND_IPI on CPU {}", current_cpu.cpuid);
-            trace!(
-                "SBI_EID::SEND_IPI,cpuid:{:#x},mask:{:#x}",
-                current_cpu.x[10],
-                current_cpu.x[11]
-            );
-            sbi_ret = sbi_call_5(
-                eid,
-                fid,
-                current_cpu.x[10],
-                current_cpu.x[11],
-                current_cpu.x[12],
-                current_cpu.x[13],
-                current_cpu.x[14],
-            );
+        // IPI Extension (SBI Spec Chapter 7)
+        spi::EID_SPI => {
+            sbi_ret = sbi_ipi_handler(fid, current_cpu);
         }
-        SBI_EID::RFENCE => {
-            trace!("SBI_EID::RFENCE,mask:{:#x}", current_cpu.x[10]);
-            sbi_ret = sbi_call_5(
-                eid,
-                fid,
-                current_cpu.x[10],
-                current_cpu.x[11],
-                current_cpu.x[12],
-                current_cpu.x[13],
-                current_cpu.x[14],
-            );
+        // RFENCE Extension (SBI Spec Chapter 8)
+        rfnc::EID_RFNC => {
+            sbi_ret = sbi_rfence_handler(fid, current_cpu);
         }
-        SBI_EID::PMU => {
-            trace!("SBI_EID::PMU,fid:{:#x}", fid);
-            sbi_ret = sbi_call_5(
-                eid,
-                fid,
-                current_cpu.x[10],
-                current_cpu.x[11],
-                current_cpu.x[12],
-                current_cpu.x[13],
-                current_cpu.x[14],
-            );
-        }
-        SBI_EID::HVISOR => {
-            trace!("SBI_EID::HVISOR,fid:{:#x}", fid);
+        // Hvisor Extension (Hvisor Defined)
+        EID_HVISOR => {
             sbi_ret = sbi_hvisor_handler(current_cpu);
         }
-        //_ => sbi_ret = sbi_dummy_handler(),
         _ => {
-            warn!(
-                "Pass through SBI call eid {:#x} fid:{:#x} on CPU {}",
-                eid, fid, current_cpu.cpuid
-            );
-            sbi_ret = sbi_call_5(
-                eid,
-                fid,
-                current_cpu.x[10],
-                current_cpu.x[11],
-                current_cpu.x[12],
-                current_cpu.x[13],
-                current_cpu.x[14],
-            );
+            // Pass through SBI call
+            warn!("Unsupported SBI extension {:#x} function {:#x}", eid, fid);
+            sbi_ret = SbiRet {
+                error: RET_ERR_NOT_SUPPORTED,
+                value: 0,
+            };
         }
     }
+    // Write the return value back to the current_cpu
     current_cpu.x[10] = sbi_ret.error as usize;
     current_cpu.x[11] = sbi_ret.value as usize;
 }
 
-pub fn sbi_call_5(
-    eid: usize,
-    fid: usize,
-    arg0: usize,
-    arg1: usize,
-    arg2: usize,
-    arg3: usize,
-    arg4: usize,
-) -> SbiRet {
-    let (error, value);
-    unsafe {
-        core::arch::asm!(
-            "ecall",
-            in("a7") eid,
-            in("a6") fid,
-            inlateout("a0") arg0 => error,
-            inlateout("a1") arg1 => value,
-            in("a2") arg2,
-            in("a3") arg3,
-            in("a4") arg4,
-        );
+/// Handle SBI Base Extension calls.
+pub fn sbi_base_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
+    let mut sbi_ret = SbiRet {
+        error: RET_SUCCESS,
+        value: 0,
+    };
+    match fid {
+        base::GET_SBI_SPEC_VERSION => {
+            // The minor number of the SBI specification is encoded in the low 24 bits
+            // with the major number encoded in the next 7 bits.
+            // Bit 31 must be 0 and is reserved for future expansion.
+            sbi_ret.value = 0x0200_0000; // spec version 2.0
+        }
+        base::GET_SBI_IMPL_VERSION => {
+            // The implementation ID is a 32-bit value that identifies the SBI implementation.
+            // The implementation ID is defined by the SBI implementer.
+            sbi_ret.value = sbi_rt::get_sbi_impl_version();
+        }
+        base::GET_SBI_IMPL_ID => {
+            sbi_ret.value = sbi_rt::get_sbi_impl_id();
+        }
+        base::PROBE_EXTENSION => {
+            // This function needs one arg(extesion_id).
+            let ext_id = current_cpu.x[10];
+            // Returns 0 if the given SBI extension ID (EID) is not available or 1 if it is available
+            // unless defined as any other non-zero value by the implementation.
+            if EXT_TABLE.contains(&ext_id) {
+                sbi_ret.value = ext_id;
+            }
+        }
+        base::GET_MVENDORID | base::GET_MARCHID | base::GET_MIMPID => {
+            // Return a value that is legal for the mvendorid CSR and 0 is always a legal value for this CSR.
+            // Return a value that is legal for the marchid CSR and 0 is always a legal value for this CSR.
+            // Return a value that is legal for the mimpid CSR and 0 is always a legal value for this CSR.
+            sbi_ret.value = 0;
+        }
+        _ => {
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_base_extension fid {:#x}", fid);
+        }
     }
-    SbiRet { error, value }
+    sbi_ret
 }
 
+/// SBI Set Timer handler
 pub fn sbi_time_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
-    let mut sbi_ret = SbiRet {
-        error: SBI_SUCCESS,
-        value: 0,
-    };
-    let stime = current_cpu.x[10];
-    warn!("SBI_SET_TIMER stime: {:#x}", stime);
+    if fid != time::SET_TIMER {
+        warn!("SBI: unsupported sbi_time_extension fid {:#x}", fid);
+        return SbiRet {
+            error: RET_ERR_NOT_SUPPORTED,
+            value: 0,
+        };
+    }
+    let stime_value = current_cpu.x[10];
     if current_cpu.sstc {
-        write_csr!(CSR_VSTIMECMP, stime);
+        // Set the vstimecmp, and don't need to inject timer interrupt.
+        write_csr!(CSR_VSTIMECMP, stime_value);
     } else {
-        set_timer(stime);
+        // Hvisor should receive timer interrupt and inject it to guest.
+        sbi_rt::set_timer(stime_value as _);
         unsafe {
             // clear guest timer interrupt pending
             hvip::clear_vstip();
@@ -210,73 +171,178 @@ pub fn sbi_time_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
             sie::set_stimer();
         }
     }
-    //debug!("SBI_SET_TIMER stime: {:#x}", stime);
-    return sbi_ret;
+    SbiRet {
+        error: RET_SUCCESS,
+        value: 0,
+    }
 }
+
+#[allow(unused)]
+#[derive(Debug, PartialEq)]
+pub enum HSM_STATUS {
+    STARTED,
+    STOPPED,
+    START_PENDING,
+    STOP_PENDING,
+    SUSPENDED,
+    SUSPEND_PENDING,
+    RESUMING_PENDING,
+}
+
+/// SBI Hart State Management handler.
 pub fn sbi_hsm_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
     let mut sbi_ret = SbiRet {
-        error: SBI_SUCCESS,
+        error: RET_SUCCESS,
         value: 0,
     };
     match fid {
-        0 => {
+        hsm::HART_START => {
             // hsm start
             sbi_ret = sbi_hsm_start_handler(current_cpu);
         }
+        hsm::HART_STOP => {
+            // Todo: support hart stop.
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_hsm_hart_stop");
+        }
+        hsm::HART_GET_STATUS => {
+            // Todo: support hart get status.
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_hsm_hart_get_status");
+        }
+        hsm::HART_SUSPEND => {
+            // Todo: support hart suspend.
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_hsm_hart_suspend");
+        }
         _ => {
-            error!("Unsupported HSM function {:#x}", fid);
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_hsm_extension fid {:#x}", fid);
         }
     }
     sbi_ret
 }
+
+/// SBI Hart State Management start handler.
 pub fn sbi_hsm_start_handler(current_cpu: &mut ArchCpu) -> SbiRet {
     let mut sbi_ret = SbiRet {
-        error: SBI_SUCCESS,
+        error: RET_SUCCESS,
         value: 0,
     };
-    let cpuid = current_cpu.x[10];
-
-    if (cpuid == current_cpu.cpuid) {
-        sbi_ret.error = SBI_ERR_ALREADY_AVAILABLE;
+    let cpuid = current_cpu.x[10]; // In hvisor, it is physical cpu id.
+    let start_addr = current_cpu.x[11];
+    let opaque = current_cpu.x[12];
+    if cpuid == current_cpu.cpuid {
+        sbi_ret.error = RET_ERR_ALREADY_AVAILABLE;
     } else {
-        //TODO:add sbi conext in archcpu
-        let cpuid = current_cpu.x[10];
-        let start_addr = current_cpu.x[11];
-        let opaque = current_cpu.x[12];
-
-        info!("sbi: try to wake up cpu {} run@{:#x}", cpuid, start_addr);
+        info!(
+            "SBI: try to wake up cpu {} run@ {:#x}, opaque@ {:#x}",
+            cpuid, start_addr, opaque
+        );
         let target_cpu = get_cpu_data(cpuid);
-        //todo add power_on check
         let _lock = target_cpu.ctrl_lock.lock();
-        target_cpu.cpu_on_entry = start_addr;
-        target_cpu.dtb_ipa = opaque;
-        send_event(cpuid, 0, IPI_EVENT_WAKEUP);
-
+        // Todo: use hsm status, not just hvisor defined power_on.
+        if target_cpu.arch_cpu.power_on {
+            sbi_ret.error = RET_ERR_FAILED;
+        } else {
+            // Set the target cpu to start-pending.
+            target_cpu.cpu_on_entry = start_addr;
+            target_cpu.dtb_ipa = opaque;
+            // Insert memory fence.
+            atomic::fence(atomic::Ordering::SeqCst);
+            send_event(cpuid, 0, IPI_EVENT_WAKEUP);
+        }
         drop(_lock);
     }
     sbi_ret
 }
-pub fn sbi_hvisor_handler(current_cpu: &mut ArchCpu) -> SbiRet {
+
+/// SBI IPI handler.
+pub fn sbi_ipi_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
+    if fid != spi::SEND_IPI {
+        warn!("SBI: unsupported sbi_ipi_extension fid {:#x}", fid);
+        return SbiRet {
+            error: RET_ERR_NOT_SUPPORTED,
+            value: 0,
+        };
+    }
+    // unsigned long hart_mask is a scalar bit-vector containing hartids
+    // unsigned long hart_mask_base is the starting hartid from which the bit-vector must be computed.
+    let hart_mask = current_cpu.x[10];
+    let hart_mask_base = current_cpu.x[11];
+    let hart_mask_bits = HartMask::from_mask_base(hart_mask, hart_mask_base);
+    for cpu_id in 0..64 {
+        // hart_mask is 64 bits
+        if hart_mask_bits.has_bit(cpu_id) {
+            // the second parameter is ignored is riscv64.
+            send_event(cpu_id, 0, IPI_EVENT_SEND_IPI);
+        }
+    }
+    SbiRet {
+        error: RET_SUCCESS,
+        value: 0,
+    }
+}
+
+/// SBI RFENCE handler.
+pub fn sbi_rfence_handler(fid: usize, current_cpu: &mut ArchCpu) -> SbiRet {
     let mut sbi_ret = SbiRet {
-        error: SBI_SUCCESS,
+        error: RET_SUCCESS,
         value: 0,
     };
-    let (code, arg0, arg1) = (current_cpu.x[10], current_cpu.x[11], current_cpu.x[12]);
+    let hart_mask = current_cpu.x[10];
+    let hart_mask_base = current_cpu.x[11];
+    let start_addr = current_cpu.x[12];
+    let size = current_cpu.x[13];
+    let asid = current_cpu.x[14];
+    let hart_mask_bits = HartMask::from_mask_base(hart_mask, hart_mask_base);
+    match fid {
+        rfnc::REMOTE_FENCE_I => {
+            // Instructs remote harts to execute FENCE.I instruction.
+            sbi_ret = sbi_rt::remote_fence_i(hart_mask_bits);
+        }
+        rfnc::REMOTE_SFENCE_VMA => {
+            // Instruct the remote harts to execute one or more HFENCE.VVMA instructions, covering the range of
+            // guest virtual addresses between start_addr and start_addr + size for current VMID (in hgatp CSR) of
+            // calling hart.
+            sbi_ret = sbi_rt::remote_hfence_vvma(hart_mask_bits, start_addr, size);
+        }
+        rfnc::REMOTE_SFENCE_VMA_ASID => {
+            // Instruct the remote harts to execute one or more HFENCE.VVMA instructions, covering the range of
+            // guest virtual addresses between start_addr and start_addr + size for the given ASID and current
+            // VMID (in hgatp CSR) of calling hart.
+            sbi_ret = sbi_rt::remote_hfence_vvma_asid(hart_mask_bits, start_addr, size, asid);
+        }
+        _ => {
+            sbi_ret.error = RET_ERR_NOT_SUPPORTED;
+            warn!("SBI: unsupported sbi_rfence_extension fid {:#x}", fid);
+        }
+    }
+    sbi_ret
+}
 
+/// SBI hvisor handler.
+pub fn sbi_hvisor_handler(current_cpu: &mut ArchCpu) -> SbiRet {
+    // Todo: according SBI spec, use a6 instead of a0 to tranfer code is more reasonable.
+    let (code, arg0, arg1) = (current_cpu.x[10], current_cpu.x[11], current_cpu.x[12]);
     let cpu_data = this_cpu_data();
     debug!(
-        "HVC from CPU{},code:{:#x?},arg0:{:#x?},arg1:{:#x?}",
+        "Hvisor ecall from CPU{},code: {:#x?}, arg0: {:#x?}, arg1: {:#x?}",
         cpu_data.id, code, arg0, arg1
     );
+    // Handle hvisor related hypercall.
     let result = HyperCall::new(cpu_data).hypercall(code as _, arg0 as _, arg1 as _);
     match result {
-        Ok(ret) => {
-            sbi_ret.value = ret as _;
-        }
+        Ok(ret) => SbiRet {
+            error: RET_SUCCESS,
+            value: ret as _,
+        },
         Err(e) => {
-            sbi_ret.error = SBI_ERR_FAILURE;
-            sbi_ret.value = e.code() as _;
+            error!("Hvisor ecall error: {:#x?}", e);
+            SbiRet {
+                error: RET_ERR_FAILED,
+                value: e.code() as _,
+            }
         }
     }
-    sbi_ret
 }
diff --git a/src/arch/riscv64/trap.rs b/src/arch/riscv64/trap.rs
index a0c21873..17d9a59f 100644
--- a/src/arch/riscv64/trap.rs
+++ b/src/arch/riscv64/trap.rs
@@ -27,10 +27,10 @@ use crate::memory::{GuestPhysAddr, HostPhysAddr};
 use crate::percpu::this_cpu_data;
 use crate::platform::__board::*;
 use core::arch::{asm, global_asm};
-use riscv::register::mtvec::TrapMode;
-use riscv::register::stvec;
-use riscv::register::{hvip, sie};
+use riscv::register::stvec::TrapMode;
+use riscv::register::{sie, stvec};
 use riscv_decode::Instruction;
+use riscv_h::register::hvip;
 
 extern "C" {
     fn _hyp_trap_vector();
@@ -100,36 +100,41 @@ pub const INS_RS1_MASK: usize = 0x000f8000;
 pub const INS_RS2_MASK: usize = 0x01f00000;
 pub const INS_RD_MASK: usize = 0x00000f80;
 
+/// Set the trap vector.
 pub fn install_trap_vector() {
+    use riscv::register::stvec::Stvec;
+    let mut stvec = Stvec::from_bits(0);
+    stvec.set_address(_hyp_trap_vector as usize);
+    stvec.set_trap_mode(TrapMode::Direct);
     unsafe {
-        // Set the trap vector.
-        stvec::write(_hyp_trap_vector as usize, TrapMode::Direct);
+        stvec::write(stvec);
     }
 }
+
+/// Handle synchronous exceptions.
 pub fn sync_exception_handler(current_cpu: &mut ArchCpu) {
     trace!("current_cpu: stack{:#x}", current_cpu.stack_top);
-    let trap_code = read_csr!(CSR_SCAUSE);
+    let trap_code = riscv::register::scause::read().code();
     trace!("CSR_SCAUSE: {}", trap_code);
-    if (read_csr!(CSR_HSTATUS) & (1 << 7)) == 0 {
-        //HSTATUS_SPV
-        error!("exception from HS mode");
-        //unreachable!();
+
+    if !riscv_h::register::hstatus::read().spv() {
+        // Hvisor don't handle sync exception which occurs in hvisor self (HS-mode).
+        // If sync exception occurs, hvisor will panic!
+        panic!("exception from HS mode");
     }
-    let trap_value = read_csr!(CSR_HTVAL);
+
+    let trap_value = riscv_h::register::htval::read();
+    let trap_ins = riscv_h::register::htinst::read();
+    let trap_pc = riscv::register::sepc::read();
     trace!("CSR_HTVAL: {:#x}", trap_value);
-    let trap_ins = read_csr!(CSR_HTINST);
     trace!("CSR_HTINST: {:#x}", trap_ins);
-    let trap_pc = read_csr!(CSR_SEPC);
     trace!("CSR_SEPC: {:#x}", trap_pc);
-    trace!("PC{:#x}", current_cpu.sepc);
+
     match trap_code {
-        ExceptionType::ECALL_VU => {
-            error!("ECALL_VU");
-        }
         ExceptionType::ECALL_VS => {
             trace!("ECALL_VS");
             sbi_vs_handler(current_cpu);
-            current_cpu.sepc += 4;
+            current_cpu.sepc += 4; // For ecall, skip the ecall instruction.
         }
         ExceptionType::LOAD_GUEST_PAGE_FAULT => {
             trace!("LOAD_GUEST_PAGE_FAULT");
@@ -140,17 +145,16 @@ pub fn sync_exception_handler(current_cpu: &mut ArchCpu) {
             guest_page_fault_handler(current_cpu);
         }
         _ => {
-            warn!(
-                "CPU {} trap {},sepc: {:#x}",
-                current_cpu.cpuid, trap_code, current_cpu.sepc
-            );
-            warn!("trap info: {} {:#x} {:#x}", trap_code, trap_value, trap_ins);
             let raw_inst = read_inst(trap_pc);
             let inst = riscv_decode::decode(raw_inst);
-            warn!("trap ins: {:#x}  {:?}", raw_inst, inst);
-            // current_cpu.sepc += 4;
-            error!("unhandled trap");
-            current_cpu.idle();
+            warn!(
+                "CPU {} sync exception, sepc: {:#x}",
+                current_cpu.cpuid, current_cpu.sepc
+            );
+            warn!("Trap cause code: {}", trap_code);
+            warn!("htval: {:#x}, htinst: {:#x}", trap_value, trap_ins);
+            warn!("trap instruction: {:?}", inst);
+            panic!("Unhandled sync exception");
         }
     }
 }
@@ -162,6 +166,7 @@ pub fn ins_is_compressed(ins: usize) -> bool {
     (ins & 0x3) != 3
 }
 
+/// Check if the instruction in htinst is a pseudo instruction or not.
 #[inline(always)]
 pub fn ins_is_preudo(ins: usize) -> bool {
     /*
@@ -216,7 +221,7 @@ pub fn ins_ldst_decode(ins: usize) -> (usize, bool, bool) {
 pub fn guest_page_fault_handler(current_cpu: &mut ArchCpu) {
     #[cfg(feature = "plic")]
     {
-        use riscv::register::{htinst, htval, stval};
+        use riscv_h::register::{htinst, htval, stval};
         // htval: Hypervisor bad guest physical address.
         let addr: usize = (htval::read() << 2) | (stval::read() & 0x3);
         // htinst: Hypervisor trap instruction (transformed).
@@ -340,6 +345,7 @@ pub fn guest_page_fault_handler(current_cpu: &mut ArchCpu) {
     }
 }
 
+/// Read instruction from guest memory.
 fn read_inst(addr: GuestPhysAddr) -> u32 {
     let mut ins: u32;
     if addr & 0b1 != 0 {
@@ -356,6 +362,8 @@ fn read_inst(addr: GuestPhysAddr) -> u32 {
     ins
 }
 
+/// Hypervisor Virtual-Machine Load and Store Instruction.
+/// HLVX.HU emulate VS load instruction.
 fn hlvxhu(addr: GuestPhysAddr) -> u64 {
     let mut value: u64;
     unsafe {
@@ -368,7 +376,7 @@ fn hlvxhu(addr: GuestPhysAddr) -> u64 {
     value
 }
 
-/// decode risc-v instruction, return (inst len, inst)
+/// Decode risc-v instruction, return (inst len, inst).
 fn decode_inst(inst: u32) -> (usize, Option<Instruction>) {
     let i1 = inst as u16;
     let len = riscv_decode::instruction_length(i1);
@@ -380,93 +388,69 @@ fn decode_inst(inst: u32) -> (usize, Option<Instruction>) {
     (len, riscv_decode::decode(inst).ok())
 }
 
-/// handle external interrupt
+/// Handle interrupts which hvisor receives.
 pub fn interrupts_arch_handle(current_cpu: &mut ArchCpu) {
     trace!("interrupts_arch_handle @CPU{}", current_cpu.cpuid);
-    let trap_code: usize;
-    trap_code = read_csr!(CSR_SCAUSE);
-    trace!("CSR_SCAUSE: {:#x}", trap_code);
-    match trap_code & 0xfff {
+    let trap_code = unsafe { riscv::register::scause::read().code() };
+    match trap_code {
         InterruptType::STI => {
-            trace!("STI on CPU{}", current_cpu.cpuid);
-            unsafe {
-                hvip::set_vstip();
-                sie::clear_stimer();
-            }
-            trace!("sip{:#x}", read_csr!(CSR_SIP));
-            trace!("sie {:#x}", read_csr!(CSR_SIE));
+            // Inject timer interrupt to VS.
+            handle_timer_interrupt(current_cpu);
         }
         InterruptType::SSI => {
-            trace!("SSI on CPU {}", current_cpu.cpuid);
-            handle_ssi(current_cpu);
+            // Get event to handle and clear software interrupt pending bit.
+            handle_software_interrupt(current_cpu);
         }
         InterruptType::SEI => {
-            debug!("SEI on CPU {}", current_cpu.cpuid);
-            handle_eirq(current_cpu)
+            // Write external interrupt to vplic and then inject to VS.
+            handle_external_interrupt(current_cpu);
         }
         _ => {
-            error!(
+            panic!(
                 "unhandled trap {:#x},sepc: {:#x}",
                 trap_code, current_cpu.sepc
             );
-            unreachable!();
         }
     }
 }
 
-/// handle interrupt request(current only external interrupt)
-pub fn handle_eirq(current_cpu: &mut ArchCpu) {
+/// Handle supervisor timer interrupt.
+pub fn handle_timer_interrupt(current_cpu: &mut ArchCpu) {
+    unsafe {
+        hvip::set_vstip();
+        sie::clear_stimer();
+    }
+}
+
+/// Handle supervisor software interrupt.
+pub fn handle_software_interrupt(current_cpu: &mut ArchCpu) {
+    while check_events() {
+        // Get next event to handle, it is handled in check_events function.
+    }
+    unsafe {
+        riscv::register::sip::clear_ssoft();
+    }
+}
+
+/// Handle supervisor external interrupt.
+pub fn handle_external_interrupt(current_cpu: &mut ArchCpu) {
     #[cfg(feature = "plic")]
     {
+        // Note: in hvisor, all external interrupts are assigned to VS.
         // 1. claim hw irq.
-        let context_id = 2 * current_cpu.cpuid + 1;
-        let addr = PLIC_BASE + PLIC_CLAIM_OFFSET + context_id * 0x1000;
-        let irq_id = unsafe { core::ptr::read_volatile(addr as *const u32) };
-        // let irq_id = host_plic().claim(context_id);
+        let context_id = 2 * this_cpu_data().id + 1;
+        let irq_id = host_plic().claim(context_id);
 
+        // If this irq has been claimed, it will be 0.
         if irq_id == 0 {
             return;
         }
 
-        // 2. check if this zone belongs this irq.
-        if this_cpu_data()
-            .zone
-            .as_ref()
-            .unwrap()
-            .read()
-            .irq_in_zone(irq_id as u32)
-            == false
-        {
-            error!("irq {} is not belongs to this zone", irq_id);
-            return;
-        }
-
-        // 3. inject hw irq to zone.
-        this_cpu_data()
-            .zone
-            .as_ref()
-            .unwrap()
-            .read()
-            .vplic
-            .as_ref()
-            .unwrap()
-            .inject_irq(pcontext_to_vcontext(context_id), irq_id as usize, true);
+        // 2. inject hw irq to zone.
+        inject_irq(irq_id as usize, true);
     }
     #[cfg(feature = "aia")]
     {
         panic!("HS extensional interrupt")
     }
 }
-
-pub fn handle_ssi(current_cpu: &mut ArchCpu) {
-    trace!("handle_ssi");
-    let sip = read_csr!(CSR_SIP);
-    trace!("CPU{} sip: {:#x}", current_cpu.cpuid, sip);
-    clear_csr!(CSR_SIP, 1 << 1);
-    let sip2 = read_csr!(CSR_SIP);
-    trace!("CPU{} sip*: {:#x}", current_cpu.cpuid, sip2);
-
-    trace!("hvip: {:#x}", read_csr!(CSR_HVIP));
-    set_csr!(CSR_HVIP, 1 << 2);
-    check_events();
-}
diff --git a/src/device/irqchip/aia/aplic.rs b/src/device/irqchip/aia/aplic.rs
index 4495510e..953e62f2 100644
--- a/src/device/irqchip/aia/aplic.rs
+++ b/src/device/irqchip/aia/aplic.rs
@@ -13,14 +13,13 @@
 //
 // Authors:
 //
-use riscv::use_sv32;
+
 use spin::Once;
 use spin::RwLock;
 // use crate::device::irqchip::aia::imsic::imsic_trigger;
 use crate::config::root_zone_config;
 use crate::zone::Zone;
 use crate::{arch::cpu::ArchCpu, memory::GuestPhysAddr, percpu::this_cpu_data};
-use fdt::Fdt;
 use riscv_decode::Instruction;
 // S-mode interrupt delivery controller
 const APLIC_S_IDC: usize = 0xd00_4000;
diff --git a/src/device/irqchip/plic/mod.rs b/src/device/irqchip/plic/mod.rs
index 87576f24..68162508 100644
--- a/src/device/irqchip/plic/mod.rs
+++ b/src/device/irqchip/plic/mod.rs
@@ -30,8 +30,8 @@ use crate::platform::__board::*;
 use crate::zone::Zone;
 use crate::{arch::cpu::ArchCpu, percpu::this_cpu_data};
 use alloc::vec::Vec;
-use riscv::register::hvip;
 use riscv_decode::Instruction;
+use riscv_h::register::hvip;
 use spin::Once;
 
 /*
@@ -67,7 +67,7 @@ pub fn percpu_init() {
 }
 
 pub fn inject_irq(irq: usize, is_hardware: bool) {
-    // warn!("inject_irq: {} is_hardware: {}", irq, is_hardware);
+    debug!("inject_irq: {} is_hardware: {}", irq, is_hardware);
     let vcontext_id = pcontext_to_vcontext(this_cpu_data().id * 2 + 1);
     this_cpu_data()
         .zone
@@ -153,16 +153,28 @@ pub fn update_hart_line() {
 
 impl Zone {
     pub fn arch_irqchip_reset(&self) {
+        // We should make sure only one cpu to do this.
+        // This func will only be called by one root zone's cpu.
         let host_plic = host_plic();
+        let vplic = self.vplic.as_ref().unwrap();
         for (index, &word) in self.irq_bitmap.iter().enumerate() {
             for bit_position in 0..32 {
                 if word & (1 << bit_position) != 0 {
                     let irq_id = index * 32 + bit_position;
+                    // Skip the irq_id which is not in HW_IRQS
+                    if !HW_IRQS.iter().any(|&x| x == irq_id as _) {
+                        continue;
+                    }
                     // Reset priority
+                    info!("Reset irq_id {} priority to 0", irq_id);
                     host_plic.set_priority(irq_id, 0);
                     // Reset enable
                     self.cpu_set.iter().for_each(|cpuid| {
                         let pcontext_id = cpuid * 2 + 1;
+                        info!(
+                            "Reset pcontext_id {} irq_id {} enable to false",
+                            pcontext_id, irq_id
+                        );
                         host_plic.set_enable_num(pcontext_id, irq_id, false);
                     });
                 }
@@ -171,8 +183,10 @@ impl Zone {
         self.cpu_set.iter().for_each(|cpuid| {
             // Reset threshold
             let pcontext_id = cpuid * 2 + 1;
+            info!("Reset pcontext_id {} threshold to 0", pcontext_id);
             host_plic.set_threshold(pcontext_id, 0);
             // At the same time, clear the events related to this cpu.
+            info!("Clear events related to cpu {}", cpuid);
             crate::event::clear_events(cpuid);
         });
     }
diff --git a/src/device/irqchip/plic/vplic.rs b/src/device/irqchip/plic/vplic.rs
index 6bd0d003..15460c3b 100644
--- a/src/device/irqchip/plic/vplic.rs
+++ b/src/device/irqchip/plic/vplic.rs
@@ -77,6 +77,12 @@ impl VirtualPLIC {
         inner.vplic_set_hw(intr_id, hw);
     }
 
+    /// Get one interrupt as hardware interrupt.
+    pub fn vplic_get_hw(&self, intr_id: usize) -> bool {
+        let inner = self.inner.lock();
+        inner.vplic_get_hw(intr_id)
+    }
+
     /// Inject an interrupt into the vPLIC.
     pub fn inject_irq(&self, vcontext_id: usize, intr_id: usize, hw: bool) {
         debug!("Inject interrupt {} to vcontext {}", intr_id, vcontext_id);
@@ -393,11 +399,11 @@ impl VirtualPLICInner {
             let irq_id = self.vplic_get_next_pending(vcontext_id);
             if irq_id != 0 {
                 unsafe {
-                    riscv::register::hvip::set_vseip();
+                    riscv_h::register::hvip::set_vseip();
                 }
             } else {
                 unsafe {
-                    riscv::register::hvip::clear_vseip();
+                    riscv_h::register::hvip::clear_vseip();
                 }
             }
         } else {
diff --git a/src/device/uart/mod.rs b/src/device/uart/mod.rs
index 37928f0d..3bca25ba 100644
--- a/src/device/uart/mod.rs
+++ b/src/device/uart/mod.rs
@@ -31,7 +31,9 @@ mod xuartps;
 pub use xuartps::{console_getchar, console_putchar};
 
 #[cfg(target_arch = "riscv64")]
-pub use crate::arch::riscv64::sbi::{console_getchar, console_putchar};
+pub use crate::arch::riscv64::sbi::{
+    sbi_console_getchar as console_getchar, sbi_console_putchar as console_putchar,
+};
 
 #[cfg(all(feature = "loongson_uart", target_arch = "loongarch64"))]
 mod loongson_uart;
diff --git a/src/event.rs b/src/event.rs
index 107b1074..6966bdc0 100644
--- a/src/event.rs
+++ b/src/event.rs
@@ -30,6 +30,7 @@ pub const IPI_EVENT_VIRTIO_INJECT_IRQ: usize = 2;
 pub const IPI_EVENT_WAKEUP_VIRTIO_DEVICE: usize = 3;
 pub const IPI_EVENT_CLEAR_INJECT_IRQ: usize = 4;
 pub const IPI_EVENT_UPDATE_HART_LINE: usize = 5;
+pub const IPI_EVENT_SEND_IPI: usize = 6;
 
 static EVENT_MANAGER: Once<EventManager> = Once::new();
 
@@ -142,6 +143,14 @@ pub fn check_events() -> bool {
             irqchip::plic::update_hart_line();
             true
         }
+        #[cfg(target_arch = "riscv64")]
+        Some(IPI_EVENT_SEND_IPI) => {
+            // This event is different from events above, it is used to inject software interrupt.
+            // While events above will inject external interrupt.
+            use crate::arch::ipi::arch_ipi_handler;
+            arch_ipi_handler();
+            true
+        }
         _ => false,
     }
 }