Merge pull request #4 from guttatus/rfence

feat: Implemented rfence extension

Cargo.lock

@@ -206,7 +206,3 @@ name = "unicode-ident"
 version = "1.0.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b"
-
-[[package]]
-name = "xtask"
-version = "0.1.0"

Cargo.toml

@@ -29,9 +29,6 @@ name = "rustsbi-prototyper"
 test = false
 bench = false
 
-[workspace]
-members = ["xtask"]
-
 [workspace.package]
 edition = "2021"
 license = "MulanPSL-2.0 OR MIT"

Makefile.toml

@@ -0,0 +1,15 @@
+[tasks.clean]
+command = "cargo"
+args = ["clean"]
+
+[tasks.build]
+command = "cargo"
+args = ["build", "--release"]
+dependencies = ["clean"]
+
+[tasks.bin]
+command = "rust-objcopy"
+args = ["--binary-architecture=riscv64", "target/riscv64imac-unknown-none-elf/release/rustsbi-prototyper",
+        "--output-target=binary", "target/riscv64imac-unknown-none-elf/release/rustsbi-prototyper.bin"
+]
+dependencies = ["build"]

docs/booting-linux-kernel-in-qemu-using-uboot-and-rustsbi.md

@@ -280,9 +280,7 @@ $ cd prototyper
 编译RustSBI  Prototyper
 
 ``` shell
-$ cargo build --release
-$ rust-objcopy --binary-architecture=riscv64 ./target/riscv64imac-unknown-none-elf/release/rustsbi-prototyper \
-  -O binary target/riscv64imac-unknown-none-elf/release/rustsbi-prototyper.bin 
+$ cargo make bin
 ```
 
 ## 编译U-Boot SPL

docs/booting-openEuler-23.09-in-qemu-using-uboot-and-rustsbi.md

@@ -0,0 +1,107 @@
+# 使用RustSBI & U-Boot在QEMU中启动openEuler 23.09
+
+本教程给出了使用RustSBI和U-Boot在QEMU中启动openEuler 23.09的基本流程。
+
+本教程使用软件版本如下：
+
+|         软件          |  版本   |
+| :-------------------: | :-----: |
+| riscv64-linux-gnu-gcc | 14.1.0  |
+|  qemu-system-riscv64  |  9.0.1  |
+|  RustSBI Prototyper   |  0.0.0  |
+|        U-Boot         | 2024.04 |
+
+## 准备RustSBI Prototyper， U-Boot ，openEuler 23.09
+创建工作目录并进入该目录
+
+``` shell
+$ mkdir workshop && cd workshop
+```
+
+### Clone RustSBI Prototyper
+
+``` shell
+$ git clone https://github.com/rustsbi/prototyper.git && cd prototyper && git checkout main && cd ..
+```
+
+### Clone U-Boot
+
+``` shell
+$ git clone https://github.com/u-boot/u-boot.git && cd u-boot && git checkout v2024.04 && cd ..
+```
+### 下载openEuler 23.09 Qemu磁盘镜像文件
+
+下载链接：[openEuler 23.09](https://mirror.iscas.ac.cn/openeuler-sig-riscv/openEuler-RISC-V/preview/openEuler-23.09-V1-riscv64/QEMU/openEuler-23.09-V1-base-qemu-preview.qcow2.zst)
+```shell
+ $ unzstd openEuler-23.09-V1-base-qemu-preview.qcow2.zst
+```
+- The password of user `root` is `openEuler12#$`.
+- The password of the default user `openeuler` is `openEuler12#$`.
+
+
+## 编译RustSBI  Prototyper
+
+进入prototyper目录
+
+``` shell
+$ cd prototyper
+```
+
+编译RustSBI  Prototyper
+
+``` shell
+$ cargo make bin
+```
+
+## 编译U-Boot SPL
+
+进入U-Boot目录
+
+``` shell
+$ cd u-boot
+```
+
+导出环境变量
+
+``` shell
+$ export ARCH=riscv
+$ export CROSS_COMPILE=riscv64-linux-gnu-
+$ export OPENSBI=../prototyper/target/riscv64imac-unknown-none-elf/release/rustsbi-prototyper.bin 
+```
+
+生成`.config`文件,编译U-Boot
+
+``` shell
+# To generate .config file out of board configuration file
+$ make qemu-riscv64_spl_defconfig
+$ ./scripts/config -e CMD_BTRFS -e FS_BTRFS
+$ make olddefconfig
+$ sed -i.bak 's/# CONFIG_USE_BOOTARGS is not set/CONFIG_USE_BOOTARGS=y\nCONFIG_BOOTARGS="root=\/dev\/vda1 rw console=ttyS0 swiotlb=1 loglevel=7 systemd.default_timeout_start_sec=600 selinux=0 highres=off earlycon"/' .config
+$ make -j$(nproc)
+```
+
+## 使用RustSBI 原型系统和U-Boot启动openEuler 23.09
+
+进入`workshop`目录
+
+``` shell
+$ cd workshop
+```
+
+运行下面命令
+
+``` shell
+$ qemu-system-riscv64 \
+    -nographic -machine virt \
+    -smp 4 -m 8G \
+    -bios ./u-boot/spl/u-boot-spl  \
+    -device loader,file=./u-boot/u-boot.itb,addr=0x80200000 \
+    -drive file=openEuler-23.09-V1-base-qemu-preview.qcow2,format=qcow2,id=hd0 \
+    -object rng-random,filename=/dev/urandom,id=rng0 \
+    -device virtio-vga \
+    -device virtio-rng-device,rng=rng0 \
+    -device virtio-blk-device,drive=hd0 \
+    -device virtio-net-device,netdev=usernet \
+    -netdev user,id=usernet,hostfwd=tcp::12055-:22 \
+    -device qemu-xhci -usb -device usb-kbd -device usb-tablet
+```

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

@@ -8,7 +8,8 @@ use core::{
 };
 use rustsbi::SbiRet;
 
-use crate::riscv_spec::stimecmp;
+use crate::riscv_spec::{stimecmp,current_hartid};
+use crate::hsm::remote_hsm;
 
 pub(crate) static SIFIVECLINT: AtomicPtr<SifiveClint> = AtomicPtr::new(null_mut());
 
@@ -18,7 +19,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 +33,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 +94,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,15 +1,14 @@
 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;
 use crate::trap_stack::ROOT_STACK;
+use crate::riscv_spec::current_hartid;
 use crate::clint;
 
 const HART_STATE_START_PENDING_EXT: usize = usize::MAX;
@@ -48,24 +47,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 +99,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
@@ -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,7 +30,8 @@ use crate::board::{Board, SBI};
 use crate::clint::SIFIVECLINT;
 use crate::console::{ConsoleDevice, CONSOLE};
 use crate::hsm::{local_remote_hsm, Hsm};
-use crate::riscv_spec::menvcfg;
+use crate::rfence::RFence;
+use crate::riscv_spec::{menvcfg,current_hartid};
 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,19 +176,15 @@ 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 {
-    riscv::register::mhartid::read()
-}
 
 #[derive(Debug)]
 pub struct NextStage {

src/rfence.rs

@@ -0,0 +1,202 @@
+use rustsbi::Ipi;
+use rustsbi::{HartMask, SbiRet};
+use spin::Mutex;
+
+use crate::board::SBI;
+use crate::hsm::remote_hsm;
+use crate::riscv_spec::current_hartid;
+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>>,
+}
+
+#[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,
+}
+
+#[allow(unused)]
+#[derive(Clone, Copy, Debug)]
+pub(crate) enum RFenceType {
+    FenceI,
+    SFenceVma,
+    SFenceVmaAsid,
+    HFenceGvmaVmid,
+    HFenceGvma,
+    HFenceVvmaAsid,
+    HFenceVvma,
+}
+
+impl RFenceCell {
+    pub fn new() -> Self {
+        Self {
+            inner: Mutex::new(None),
+        }
+    }
+
+    #[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的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 clear(&self) {
+        *self.0.inner.lock() = None;
+    }
+    pub fn get(&self) -> Option<RFenceCTX> {
+        (*self.0.inner.lock())
+    }
+    pub fn set(&self, ctx: RFenceCTX) {
+        *self.0.inner.lock() = Some(ctx);
+    }
+}
+
+#[allow(unused)]
+impl RemoteRFenceCell<'_> {
+    pub fn set(&self, ctx: RFenceCTX) {
+        *self.0.inner.lock() = Some(ctx);
+    }
+}
+
+/// 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();
+    }
+
+    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: return SBI_ERR_INVALID_ADDRESS, when start_addr or size is not valid.
+        let flush_size = match start_addr.checked_add(size) {
+            None => usize::MAX,
+            Some(_) => size,
+        };
+        remote_fence_process(
+            RFenceCTX {
+                start_addr,
+                size: flush_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: return SBI_ERR_INVALID_ADDRESS, when start_addr or size is not valid.
+        let flush_size = match start_addr.checked_add(size) {
+            None => usize::MAX,
+            Some(_) => size,
+        };
+        remote_fence_process(
+            RFenceCTX {
+                start_addr,
+                size: flush_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 {
+        return HartMask::from_mask_base(mask & (!(1 << hart_id)), 0);
+    }
+    let Some(idx) = hart_id.checked_sub(mask_base) else {
+        return hart_mask;
+    };
+    if idx >= usize::BITS as usize {
+        return hart_mask;
+    }
+    HartMask::from_mask_base(mask & (!(1 << hart_id)), mask_base)
+}

src/riscv_spec.rs

@@ -35,3 +35,9 @@ pub mod stimecmp {
         unsafe { asm!("csrrs zero, stimecmp, {}", in(reg) value, options(nomem)); }
     }
 }
+
+
+#[inline]
+pub fn current_hartid() -> usize {
+    riscv::register::mhartid::read()
+}

src/trap.rs

@@ -2,15 +2,19 @@ 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::hsm::local_hsm;
-use crate::riscv_spec::{CSR_TIME, CSR_TIMEH};
+use crate::rfence::{local_rfence, RFenceType};
+use crate::riscv_spec::{CSR_TIME, CSR_TIMEH, current_hartid};
+
+const PAGE_SIZE: usize = 4096;
 
 #[naked]
 pub(crate) unsafe extern "C" fn trap_vec() {
@@ -32,8 +36,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 +95,184 @@ 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)
-    )
+    );
 }
 
-/// Fast Trap
+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();
+            }
+            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();
+        }
+    }
+}
+
+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().clear();
+                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().clear();
+                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().clear();
+                clint::clear_msip();
+            }
+            rfencetype => {
+                error!("Unsupported RFence Type: {:?}!", rfencetype);
+                local_rfence().clear();
+                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 +283,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,6 +297,7 @@ 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);
@@ -172,11 +307,11 @@ pub extern "C" fn fast_handler(
                 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 +398,7 @@ pub extern "C" fn fast_handler(
                         mepc::read(),
                         mtval::read()
                     );
-                    panic!("stopped with unsupported trap")
+                    panic!("Stopped with unsupported trap")
                 }
             },
         }
@@ -272,15 +407,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 +425,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 +437,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 +449,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 +462,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::riscv_spec::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() };
 }
 
 /// 类型化栈。

xtask/Cargo.toml

@@ -1,6 +0,0 @@
-[package]
-name = "xtask"
-version = "0.1.0"
-edition = "2021"
-
-[dependencies]

xtask/src/main.rs

@@ -1,3 +0,0 @@
-fn main() {
-    println!("Hello, world!");
-}