feat: implemented rfence extension

src/board.rs

@@ -5,6 +5,7 @@ use rustsbi::{RustSBI, SbiRet};
 use crate::clint::ClintDevice;
 use crate::console::ConsoleDevice;
 use crate::hsm::Hsm;
+use crate::rfence::RFence;
 
 pub(crate) static mut SBI: MaybeUninit<Board> = MaybeUninit::uninit();
 
@@ -19,6 +20,8 @@ pub struct Board<'a> {
     pub hsm: Option<Hsm>,
     #[rustsbi(reset)]
     pub sifive_test: Option<SifiveTestDevice<'a>>,
+    #[rustsbi(fence)]
+    pub rfence: Option<RFence>
 }
 
 pub struct SifiveTestDevice<'a> {

src/clint.rs

@@ -9,6 +9,8 @@ use core::{
 use rustsbi::SbiRet;
 
 use crate::riscv_spec::stimecmp;
+use crate::current_hartid;
+use crate::hsm::remote_hsm;
 
 pub(crate) static SIFIVECLINT: AtomicPtr<SifiveClint> = AtomicPtr::new(null_mut());
 
@@ -18,7 +20,7 @@ pub(crate) fn init(base: usize) {
 
 #[inline]
 pub fn clear() {
-    let hart_id = riscv::register::mhartid::read();
+    let hart_id = current_hartid();
     loop {
         if let Some(clint) = unsafe { SIFIVECLINT.load(Relaxed).as_ref() } {
             clint.clear_msip(hart_id);
@@ -32,10 +34,9 @@ pub fn clear() {
 
 #[inline]
 pub fn clear_msip() {
-    let hart_id = riscv::register::mhartid::read();
     loop {
         if let Some(clint) = unsafe { SIFIVECLINT.load(Relaxed).as_ref() } {
-            clint.clear_msip(hart_id);
+            clint.clear_msip(current_hartid());
             break;
         } else {
             continue;
@@ -94,7 +95,7 @@ impl<'a> rustsbi::Ipi for ClintDevice<'a> {
     #[inline]
     fn send_ipi(&self, hart_mask: rustsbi::HartMask) -> SbiRet {
         for hart_id in 0..=self.max_hart_id {
-            if hart_mask.has_bit(hart_id) {
+            if hart_mask.has_bit(hart_id) && remote_hsm(hart_id).unwrap().allow_ipi() {
                 unsafe {
                     (*self.clint.load(Relaxed)).set_msip(hart_id);
                 }

src/hart_context.rs

@@ -0,0 +1,28 @@
+use core::ptr::NonNull;
+use fast_trap::FlowContext;
+
+use crate::hsm::HsmCell;
+use crate::rfence::RFenceCell;
+use crate::NextStage;
+
+
+/// 硬件线程上下文。
+pub(crate) struct HartContext {
+    /// 陷入上下文。
+    trap: FlowContext,
+    pub hsm: HsmCell<NextStage>,
+    pub rfence: RFenceCell
+}
+
+impl HartContext {
+    #[inline]
+    pub fn init(&mut self) {
+        self.hsm = HsmCell::new();
+        self.rfence = RFenceCell::new();
+    }
+
+    #[inline]
+    pub fn context_ptr(&mut self) -> NonNull<FlowContext> {
+        unsafe { NonNull::new_unchecked(&mut self.trap) }
+    }
+}

src/hsm.rs

@@ -1,14 +1,12 @@
 use core::{
     cell::UnsafeCell,
-    ptr::NonNull,
     hint::spin_loop,
     sync::atomic::{AtomicUsize, Ordering},
 };
-use fast_trap::FlowContext;
 use rustsbi::{spec::hsm::hart_state, SbiRet};
 use riscv::register::mstatus::MPP;
 
-use crate::{NextStage,hart_id};
+use crate::{NextStage,current_hartid};
 use crate::trap_stack::ROOT_STACK;
 use crate::clint;
 
@@ -48,24 +46,6 @@ impl<T> HsmCell<T> {
     }
 }
 
-/// 硬件线程上下文。
-pub struct HartContext {
-    /// 陷入上下文。
-    trap: FlowContext,
-    hsm: HsmCell<NextStage>,
-}
-
-impl HartContext {
-    #[inline]
-    pub fn init(&mut self) {
-        self.hsm = HsmCell::new();
-    }
-
-    #[inline]
-    pub fn context_ptr(&mut self) -> NonNull<FlowContext> {
-        unsafe { NonNull::new_unchecked(&mut self.trap) }
-    }
-}
 
 /// 当前硬件线程的共享对象。
 pub struct LocalHsmCell<'a, T>(&'a HsmCell<T>);
@@ -118,10 +98,10 @@ impl<T> LocalHsmCell<'_, T> {
     }
 }
 
-impl<T> RemoteHsmCell<'_, T> {
+impl<T : core::fmt::Debug> RemoteHsmCell<'_, T> {
     /// 向关闭状态的硬件线程传入共享数据，并将其状态设置为启动挂起，返回是否放入成功。
     #[inline]
-    pub fn start(self, t: T) -> bool {
+    pub fn start(&self, t: T) -> bool {
         if self
             .0
             .status
@@ -133,6 +113,7 @@ impl<T> RemoteHsmCell<'_, T> {
             )
             .is_ok()
         {
+            info!("t: {:?}",t);
             unsafe { *self.0.inner.get() = Some(t) };
             self.0
                 .status
@@ -169,7 +150,7 @@ impl<T> RemoteHsmCell<'_, T> {
 pub(crate) fn local_hsm() -> LocalHsmCell<'static, NextStage> {
     unsafe {
         ROOT_STACK
-            .get_unchecked_mut(hart_id())
+            .get_unchecked_mut(current_hartid())
             .hart_context()
             .hsm
             .local()
@@ -180,7 +161,7 @@ pub(crate) fn local_hsm() -> LocalHsmCell<'static, NextStage> {
 pub(crate) fn local_remote_hsm() -> RemoteHsmCell<'static, NextStage> {
     unsafe {
         ROOT_STACK
-            .get_unchecked_mut(hart_id())
+            .get_unchecked_mut(current_hartid())
             .hart_context()
             .hsm
             .remote()

src/main.rs

@@ -7,17 +7,19 @@ extern crate log;
 #[macro_use]
 mod macros;
 
-mod board;
 mod clint;
 mod console;
+mod hsm;
+mod reset;
+mod rfence;
+mod board;
 mod dt;
 mod dynamic;
 mod fail;
-mod hsm;
-mod reset;
 mod riscv_spec;
 mod trap;
 mod trap_stack;
+mod hart_context;
 
 use clint::ClintDevice;
 use core::sync::atomic::{AtomicBool, Ordering};
@@ -28,6 +30,7 @@ use crate::board::{Board, SBI};
 use crate::clint::SIFIVECLINT;
 use crate::console::{ConsoleDevice, CONSOLE};
 use crate::hsm::{local_remote_hsm, Hsm};
+use crate::rfence::RFence;
 use crate::riscv_spec::menvcfg;
 use crate::trap::trap_vec;
 use crate::trap_stack::NUM_HART_MAX;
@@ -41,7 +44,7 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
     let is_boot_hart = if info.boot_hart == usize::MAX {
         GENESIS.swap(false, Ordering::AcqRel)
     } else {
-        hart_id() == info.boot_hart
+        current_hartid() == info.boot_hart
     };
 
     if is_boot_hart {
@@ -80,6 +83,7 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
                 clint: Some(ClintDevice::new(&SIFIVECLINT, NUM_HART_MAX)),
                 hsm: Some(Hsm),
                 sifive_test: None,
+                rfence: Some(RFence)
             });
         }
 
@@ -102,7 +106,7 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
             opaque,
         });
 
-        info!("Redirecting harts {} to 0x{:x} in {:?} mode.", hart_id(), next_addr, mpp);
+        info!("Redirecting hart {} to 0x{:0>16x} in {:?} mode.", current_hartid(), next_addr, mpp);
     } else {
         // TODO: PMP configuration needs to be obtained through the memory range in the device tree
         use riscv::register::*;
@@ -125,7 +129,6 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
         use riscv::register::{medeleg, mtvec};
         medeleg::clear_supervisor_env_call();
         medeleg::clear_illegal_instruction();
-        menvcfg::set_stce();
         menvcfg::set_bits(
             menvcfg::STCE | menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE,
         );
@@ -173,17 +176,17 @@ unsafe extern "C" fn start() -> ! {
          // 3. Prepare stack for each hart
         "   call    {locate_stack}",
         "   call    {main}",
-        "   j       {trap}",
+        "   j       {hart_boot}",
         magic = const dynamic::MAGIC,
         locate_stack = sym trap_stack::locate,
         main         = sym rust_main,
-        trap         = sym trap::trap_vec,
+        hart_boot         = sym trap::trap_vec,
         options(noreturn)
     )
 }
 
 #[inline]
-pub fn hart_id() -> usize {
+pub fn current_hartid() -> usize {
     riscv::register::mhartid::read()
 }
 

src/rfence.rs

@@ -0,0 +1,229 @@
+use rustsbi::Ipi;
+use rustsbi::{HartMask, SbiRet};
+use spin::Mutex;
+use spin::RwLock;
+
+use crate::board::SBI;
+use crate::current_hartid;
+use crate::hsm::remote_hsm;
+use crate::trap::msoft_rfence_handler;
+use crate::trap_stack::NUM_HART_MAX;
+use crate::trap_stack::ROOT_STACK;
+
+pub(crate) struct RFenceCell {
+    inner: Mutex<Option<RFenceCTX>>,
+    sync: RwLock<bool>,
+}
+
+#[repr(C)]
+#[derive(Clone, Copy)]
+pub(crate) struct RFenceCTX {
+    pub start_addr: usize,
+    pub size: usize,
+    pub asid: usize,
+    pub vmid: usize,
+    pub op: RFenceType,
+}
+
+// impl RFenceCTX {
+//     pub fn new() -> Self {
+//         Self {
+//             start_addr: 0,
+//             size: 0,
+//             asid: 0,
+//             vmid: 0,
+//             op: RFenceType::None,
+//         }
+//     }
+// }
+
+#[allow(unused)]
+#[derive(Clone, Copy)]
+pub(crate) enum RFenceType {
+    FenceI,
+    SFenceVma,
+    SFenceVmaAsid,
+    HFenceGvmaVmid,
+    HFenceGvma,
+    HFenceVvmaAsid,
+    HFenceVvma,
+}
+
+impl RFenceCell {
+    pub fn new() -> Self {
+        Self {
+            inner: Mutex::new(None),
+            sync: RwLock::new(false),
+        }
+    }
+
+    #[inline]
+    pub unsafe fn local(&self) -> LocalRFenceCell<'_> {
+        LocalRFenceCell(self)
+    }
+
+    /// 取出共享对象。
+    #[inline]
+    pub fn remote(&self) -> RemoteRFenceCell<'_> {
+        RemoteRFenceCell(self)
+    }
+}
+
+unsafe impl Sync for RFenceCell {}
+unsafe impl Send for RFenceCell {}
+
+/// 当前硬件线程的RFence上下文。
+pub struct LocalRFenceCell<'a>(&'a RFenceCell);
+
+/// 任意硬件线程的RFence上下文。
+pub struct RemoteRFenceCell<'a>(&'a RFenceCell);
+
+/// 获取此 hart 的 rfence 上下文
+pub(crate) fn local_rfence() -> LocalRFenceCell<'static> {
+    unsafe {
+        ROOT_STACK
+            .get_unchecked_mut(current_hartid())
+            .hart_context()
+            .rfence
+            .local()
+    }
+}
+
+/// 获取任意 hart 的 remote rfence 上下文
+pub(crate) fn remote_rfence(hart_id: usize) -> Option<RemoteRFenceCell<'static>> {
+    unsafe {
+        ROOT_STACK
+            .get_mut(hart_id)
+            .map(|x| x.hart_context().rfence.remote())
+    }
+}
+
+#[allow(unused)]
+impl LocalRFenceCell<'_> {
+    pub fn sync(&self) {
+        *self.0.inner.lock() = None;
+        *self.0.sync.write() = true;
+    }
+    pub fn get(&self) -> Option<RFenceCTX> {
+        (*self.0.inner.lock()).clone()
+    }
+    pub fn set(&self, ctx: RFenceCTX) {
+        *self.0.inner.lock() = Some(ctx);
+        *self.0.sync.write() = false;
+    }
+}
+
+#[allow(unused)]
+impl RemoteRFenceCell<'_> {
+    pub fn set(&self, ctx: RFenceCTX) {
+        *self.0.inner.lock() = Some(ctx);
+        *self.0.sync.write() = false;
+    }
+
+    pub fn is_sync(&self) -> bool {
+        // info!("is sync");
+        match self.0.sync.try_read() {
+            Some(value) => *value,
+            None => false,
+        }
+    }
+}
+
+/// RFENCE
+pub(crate) struct RFence;
+
+fn remote_fence_process(rfence_ctx: RFenceCTX, hart_mask: HartMask) -> SbiRet {
+    let mut hart_mask_hsm = hart_mask_clear(hart_mask, current_hartid());
+    for hart_id in 0..=NUM_HART_MAX {
+        if hart_mask.has_bit(hart_id) {
+            if remote_hsm(hart_id).unwrap().allow_ipi() {
+                remote_rfence(hart_id).unwrap().set(rfence_ctx);
+            } else {
+                hart_mask_hsm = hart_mask_clear(hart_mask_hsm, hart_id);
+            }
+        }
+    }
+    let sbi_ret = unsafe { SBI.assume_init_mut() }
+        .clint
+        .as_ref()
+        .unwrap()
+        .send_ipi(hart_mask_hsm);
+
+    if hart_mask.has_bit(current_hartid()) {
+        msoft_rfence_handler();
+    }
+
+    // for hart_id in 0..=NUM_HART_MAX {
+    //     if hart_mask_hsm.has_bit(hart_id) {
+    //         while !remote_rfence(hart_id).unwrap().is_sync() {}
+    //     }
+    // }
+    return sbi_ret;
+}
+
+impl rustsbi::Fence for RFence {
+    fn remote_fence_i(&self, hart_mask: HartMask) -> SbiRet {
+        remote_fence_process(
+            RFenceCTX {
+                start_addr: 0,
+                size: 0,
+                asid: 0,
+                vmid: 0,
+                op: RFenceType::FenceI,
+            },
+            hart_mask,
+        )
+    }
+
+    fn remote_sfence_vma(&self, hart_mask: HartMask, start_addr: usize, size: usize) -> SbiRet {
+        // TODO: check start_addr and size
+        remote_fence_process(
+            RFenceCTX {
+                start_addr,
+                size,
+                asid: 0,
+                vmid: 0,
+                op: RFenceType::SFenceVma,
+            },
+            hart_mask,
+        )
+    }
+
+    fn remote_sfence_vma_asid(
+        &self,
+        hart_mask: HartMask,
+        start_addr: usize,
+        size: usize,
+        asid: usize,
+    ) -> SbiRet {
+        // TODO: check start_addr and size
+        remote_fence_process(
+            RFenceCTX {
+                start_addr,
+                size,
+                asid,
+                vmid: 0,
+                op: RFenceType::SFenceVmaAsid,
+            },
+            hart_mask,
+        )
+    }
+}
+
+pub fn hart_mask_clear(hart_mask: HartMask, hart_id: usize) -> HartMask {
+    let (mask, mask_base) = hart_mask.into_inner();
+    if mask_base == usize::MAX {
+        // If `hart_mask_base` equals `usize::MAX`, that means `hart_mask` is ignored
+        // and all available harts must be considered.
+        return HartMask::from_mask_base(mask & (!(1 << hart_id)), 0);
+    }
+    let Some(idx) = hart_id.checked_sub(mask_base) else {
+        // hart_id < hart_mask_base, not in current mask range
+        return hart_mask;
+    };
+    if idx >= usize::BITS as usize {
+        // hart_idx >= hart_mask_base + XLEN, not in current mask range
+        return hart_mask;
+    }
+    HartMask::from_mask_base(mask & (!(1 << hart_id)), mask_base)
+}

src/trap.rs

@@ -2,16 +2,21 @@ use aclint::SifiveClint as Clint;
 use core::arch::asm;
 use fast_trap::{trap_entry, FastContext, FastResult};
 use riscv::register::{
-    mepc, mie, mstatus};
+    mcause::{self, Exception as E, Trap as T},
+    mepc, mie, mstatus, mtval, satp, sstatus,
+};
 use rustsbi::RustSBI;
 
 use crate::board::SBI;
 use crate::clint::{self, SIFIVECLINT};
 use crate::console::{MachineConsole, CONSOLE};
-use crate::hart_id;
+use crate::current_hartid;
 use crate::hsm::local_hsm;
+use crate::rfence::{local_rfence, RFenceType};
 use crate::riscv_spec::{CSR_TIME, CSR_TIMEH};
 
+const PAGE_SIZE: usize = 4096;
+
 #[naked]
 pub(crate) unsafe extern "C" fn trap_vec() {
     asm!(
@@ -32,8 +37,8 @@ pub(crate) unsafe extern "C" fn trap_vec() {
         "j {default}", // machine    external
         ".option pop",
         default = sym trap_entry,
-        mtimer  = sym mtimer,
         msoft   = sym msoft,
+        mtimer  = sym mtimer,
         options(noreturn)
     )
 }
@@ -91,49 +96,189 @@ unsafe extern "C" fn mtimer() {
 
 /// machine soft 中断代理
 ///
-/// # Safety
-///
-/// 裸函数。
 #[naked]
-unsafe extern "C" fn msoft() {
+unsafe extern "C" fn msoft() -> ! {
     asm!(
-        // 换栈：
-        // sp      : M sp
-        // mscratch: S sp
-        "   csrrw sp, mscratch, sp",
-        // 保护
-        "   addi sp, sp, -3*8
-            sd   ra, 0*8(sp)
-            sd   a0, 1*8(sp)
-            sd   a1, 2*8(sp)
-        ",
-        // 清除 msip 设置 ssip
-        "   la   a0, {clint_ptr}
-            ld   a0, (a0)
-            csrr a1, mhartid
-            call {clear_msip}
-            csrrsi zero, mip, 1 << 1
-        ",
-        // 恢复
-        "   ld   ra, 0*8(sp)
-            ld   a0, 1*8(sp)
-            ld   a1, 2*8(sp)
-            addi sp, sp,  3*8
-        ",
-        // 换栈：
-        // sp      : S sp
-        // mscratch: M sp
-        "   csrrw sp, mscratch, sp",
-        // 返回
-        "   mret",
-        clint_ptr  = sym SIFIVECLINT,
-        //               Clint::clear_msip_naked(&self, hart_idx)
-        clear_msip = sym Clint::clear_msip_naked,
+        ".align 2",
+        "csrrw  sp, mscratch, sp",
+        "addi   sp, sp, -32*8",
+        "sd     ra, 0*8(sp)
+        sd      gp, 2*8(sp)
+        sd      tp, 3*8(sp)
+        sd      t0, 4*8(sp)
+        sd      t1, 5*8(sp)
+        sd      t2, 6*8(sp)
+        sd      s0, 7*8(sp)
+        sd      s1, 8*8(sp)
+        sd      a0, 9*8(sp)
+        sd      a1, 10*8(sp)
+        sd      a2, 11*8(sp)
+        sd      a3, 12*8(sp)
+        sd      a4, 13*8(sp)
+        sd      a5, 14*8(sp)
+        sd      a6, 15*8(sp)
+        sd      a7, 16*8(sp)
+        sd      s2, 17*8(sp)
+        sd      s3, 18*8(sp)
+        sd      s4, 19*8(sp)
+        sd      s5, 20*8(sp)
+        sd      s6, 21*8(sp)
+        sd      s7, 22*8(sp)
+        sd      s8, 23*8(sp)
+        sd      s9, 24*8(sp)
+        sd     s10, 25*8(sp)
+        sd     s11, 26*8(sp)
+        sd      t3, 27*8(sp)
+        sd      t4, 28*8(sp)
+        sd      t5, 29*8(sp)
+        sd      t6, 30*8(sp)",
+        "csrr   t0, mepc
+        sd      t0, 31*8(sp)",
+        "csrr   t2, mscratch",
+        "sd     t2, 1*8(sp)",
+        "mv     a0, sp",
+        "call   {msoft_hanlder}",
+        "ld     t0, 31*8(sp)
+        csrw    mepc, t0",
+        "ld     ra, 0*8(sp)
+        ld      gp, 2*8(sp)
+        ld      tp, 3*8(sp)
+        ld      t0, 4*8(sp)
+        ld      t1, 5*8(sp)
+        ld      t2, 6*8(sp)
+        ld      s0, 7*8(sp)
+        ld      s1, 8*8(sp)
+        ld      a0, 9*8(sp)
+        ld      a1, 10*8(sp)
+        ld      a2, 11*8(sp)
+        ld      a3, 12*8(sp)
+        ld      a4, 13*8(sp)
+        ld      a5, 14*8(sp)
+        ld      a6, 15*8(sp)
+        ld      a7, 16*8(sp)
+        ld      s2, 17*8(sp)
+        ld      s3, 18*8(sp)
+        ld      s4, 19*8(sp)
+        ld      s5, 20*8(sp)
+        ld      s6, 21*8(sp)
+        ld      s7, 22*8(sp)
+        ld      s8, 23*8(sp)
+        ld      s9, 24*8(sp)
+        ld     s10, 25*8(sp)
+        ld     s11, 26*8(sp)
+        ld      t3, 27*8(sp)
+        ld      t4, 28*8(sp)
+        ld      t5, 29*8(sp)
+        ld      t6, 30*8(sp)",
+        "addi   sp, sp, 32*8",
+        "csrrw  sp, mscratch, sp",
+        "mret",
+        msoft_hanlder = sym msoft_hanlder,
         options(noreturn)
-    )
+    );
+}
+
+pub extern "C" fn msoft_hanlder(ctx: &mut SupervisorContext) {
+    #[inline]
+    fn boot(ctx: &mut SupervisorContext, start_addr: usize, opaque: usize) {
+        unsafe {
+            sstatus::clear_sie();
+            satp::write(0);
+        }
+        ctx.a0 = current_hartid();
+        ctx.a1 = opaque;
+        ctx.mepc = start_addr;
+    }
+
+    match local_hsm().start() {
+        // HSM Start
+        Ok(next_stage) => {
+            clint::clear_msip();
+            unsafe {
+                mstatus::set_mpie();
+                mstatus::set_mpp(next_stage.next_mode);
+                mie::set_msoft();
+                mie::set_mtimer();
+            }
+            info!(
+                "hart {} => start_addr : 0x{:16x}",
+                current_hartid(),
+                next_stage.start_addr
+            );
+            boot(ctx, next_stage.start_addr, next_stage.opaque);
+        }
+        Err(rustsbi::spec::hsm::HART_STOP) => {
+            clint::clear_msip();
+            unsafe {
+                mie::set_msoft();
+            }
+            riscv::asm::wfi();
+        }
+        // RFence
+        _ => {
+            msoft_rfence_handler();
+        }
+    }
 }
 
-/// Fast Trap
+pub fn msoft_rfence_handler() {
+    let rfence_context = local_rfence().get();
+    match rfence_context {
+        Some(ctx) => match ctx.op {
+            RFenceType::FenceI => unsafe {
+                asm!("fence.i");
+                local_rfence().sync();
+                clint::clear_msip();
+            },
+            RFenceType::SFenceVma => {
+                if (ctx.start_addr == 0 && ctx.size == 0) || (ctx.size == usize::MAX) {
+                    unsafe {
+                        asm!("sfence.vma");
+                    }
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr + offset;
+                        unsafe {
+                            asm!("sfence.vma {}", in(reg) addr);
+                        }
+                    }
+                }
+                local_rfence().sync();
+                clint::clear_msip();
+            }
+            RFenceType::SFenceVmaAsid => {
+                let asid = ctx.asid;
+                if (ctx.start_addr == 0 && ctx.size == 0) || (ctx.size == usize::MAX) {
+                    unsafe {
+                        asm!("sfence.vma {}, {}", in(reg) 0, in(reg) asid);
+                    }
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr + offset;
+                        unsafe {
+                            asm!("sfence.vma {}, {}", in(reg) addr, in(reg) asid);
+                        }
+                    }
+                }
+                local_rfence().sync();
+                clint::clear_msip();
+            }
+            _ => {
+                error!("Unsupported RFence Type!");
+                local_rfence().sync();
+                clint::clear_msip();
+            }
+        },
+        None => {
+            clint::clear_msip();
+            unsafe {
+                riscv::register::mip::set_ssoft();
+            }
+        }
+    }
+}
+
+/// Fast trap
 pub extern "C" fn fast_handler(
     mut ctx: FastContext,
     a1: usize,
@@ -144,18 +289,13 @@ pub extern "C" fn fast_handler(
     a6: usize,
     a7: usize,
 ) -> FastResult {
-    use riscv::register::{
-        mcause::{self, Exception as E, Trap as T},
-        mtval, satp, sstatus,
-    };
-
     #[inline]
     fn boot(mut ctx: FastContext, start_addr: usize, opaque: usize) -> FastResult {
         unsafe {
             sstatus::clear_sie();
             satp::write(0);
         }
-        ctx.regs().a[0] = hart_id();
+        ctx.regs().a[0] = current_hartid();
         ctx.regs().a[1] = opaque;
         ctx.regs().pc = start_addr;
         ctx.call(2)
@@ -163,20 +303,26 @@ pub extern "C" fn fast_handler(
     loop {
         match local_hsm().start() {
             Ok(next_stage) => {
+                clint::clear_msip();
                 unsafe {
                     mstatus::set_mpie();
                     mstatus::set_mpp(next_stage.next_mode);
                     mie::set_msoft();
                     mie::set_mtimer();
                 }
+                info!(
+                    "hart {} => start_addr : 0x{:16x}",
+                    current_hartid(),
+                    next_stage.start_addr
+                );
                 break boot(ctx, next_stage.start_addr, next_stage.opaque);
             }
             Err(rustsbi::spec::hsm::HART_STOP) => {
+                clint::clear_msip();
                 unsafe {
                     mie::set_msoft();
                 }
                 riscv::asm::wfi();
-                clint::clear_msip();
             }
             _ => match mcause::read().cause() {
                 // SBI call
@@ -263,7 +409,7 @@ pub extern "C" fn fast_handler(
                         mepc::read(),
                         mtval::read()
                     );
-                    panic!("stopped with unsupported trap")
+                    panic!("Stopped with unsupported trap")
                 }
             },
         }
@@ -272,15 +418,14 @@ pub extern "C" fn fast_handler(
 
 #[inline]
 fn delegate() {
-    use riscv::register::{sepc, scause, stval, stvec, sstatus, mepc, mcause, mtval};
+    use riscv::register::{mcause, mepc, mtval, scause, sepc, sstatus, stval, stvec};
     unsafe {
-
         // TODO: 当支持中断嵌套时，需要从ctx里获取mpec。当前ctx.reg().pc与mepc不一致
         sepc::write(mepc::read());
         scause::write(mcause::read().bits());
         stval::write(mtval::read());
         sstatus::clear_sie();
-         if mstatus::read().mpp() == mstatus::MPP::Supervisor {
+        if mstatus::read().mpp() == mstatus::MPP::Supervisor {
             sstatus::set_spp(sstatus::SPP::Supervisor);
         } else {
             sstatus::set_spp(sstatus::SPP::User);
@@ -291,8 +436,8 @@ fn delegate() {
 
 #[inline]
 fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
-    use riscv_decode::{decode, Instruction};
     use riscv::register::{mepc, mtval};
+    use riscv_decode::{decode, Instruction};
 
     let inst = decode(mtval::read() as u32);
     match inst {
@@ -303,7 +448,11 @@ fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
                     "Unsupported CSR rd: {}",
                     csr.rd()
                 );
-                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }.clint.as_ref().unwrap().get_time();
+                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }
+                    .clint
+                    .as_ref()
+                    .unwrap()
+                    .get_time();
             }
             CSR_TIMEH => {
                 assert!(
@@ -311,7 +460,11 @@ fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
                     "Unsupported CSR rd: {}",
                     csr.rd()
                 );
-                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }.clint.as_ref().unwrap().get_timeh();
+                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }
+                    .clint
+                    .as_ref()
+                    .unwrap()
+                    .get_timeh();
             }
             _ => return false,
         },
@@ -320,3 +473,40 @@ fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
     mepc::write(mepc::read() + 4);
     true
 }
+
+#[derive(Debug)]
+#[repr(C)]
+pub struct SupervisorContext {
+    pub ra: usize, // 0
+    pub sp: usize,
+    pub gp: usize,
+    pub tp: usize,
+    pub t0: usize,
+    pub t1: usize,
+    pub t2: usize,
+    pub s0: usize,
+    pub s1: usize,
+    pub a0: usize,
+    pub a1: usize,
+    pub a2: usize,
+    pub a3: usize,
+    pub a4: usize,
+    pub a5: usize,
+    pub a6: usize,
+    pub a7: usize,
+    pub s2: usize,
+    pub s3: usize,
+    pub s4: usize,
+    pub s5: usize,
+    pub s6: usize,
+    pub s7: usize,
+    pub s8: usize,
+    pub s9: usize,
+    pub s10: usize,
+    pub s11: usize,
+    pub t3: usize,
+    pub t4: usize,
+    pub t5: usize,
+    pub t6: usize,   // 30
+    pub mepc: usize, // 31
+}

src/trap_stack.rs

@@ -1,9 +1,9 @@
 use core::mem::forget;
 use fast_trap::FreeTrapStack;
 
-use crate::hsm::HartContext;
+use crate::hart_context::HartContext;
 use crate::trap::fast_handler;
-use crate::hart_id;
+use crate::current_hartid;
 
 const LEN_STACK_PER_HART: usize = 16 * 1024;
 pub const NUM_HART_MAX: usize = 8;
@@ -34,7 +34,7 @@ pub(crate) unsafe extern "C" fn locate() {
 
 /// 预备陷入栈。
 pub(crate) fn prepare_for_trap() {
-    unsafe { ROOT_STACK.get_unchecked_mut(hart_id()).load_as_stack() };
+    unsafe { ROOT_STACK.get_unchecked_mut(current_hartid()).load_as_stack() };
 }
 
 /// 类型化栈。