feat: support booting linux kernel on single core

Cargo.lock

@@ -26,6 +26,12 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89"
 
+[[package]]
+name = "fast-trap"
+version = "0.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7fbe69badc2e0dc98ad2787648fa140b5772d24b49e9a6b180a67e1348f7544c"
+
 [[package]]
 name = "lock_api"
 version = "0.4.12"
@@ -76,6 +82,12 @@ dependencies = [
  "embedded-hal",
 ]
 
+[[package]]
+name = "riscv-decode"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bec7a6dc0b0bb96a4d23271864a45c0d24dcd9dde2a1b630a35f79fa29c588bf"
+
 [[package]]
 name = "rustsbi"
 version = "0.4.0-alpha.3"
@@ -103,10 +115,13 @@ name = "rustsbi-prototyper"
 version = "0.0.0"
 dependencies = [
  "aclint",
+ "fast-trap",
  "log",
  "panic-halt",
  "riscv",
+ "riscv-decode",
  "rustsbi",
+ "sbi-spec",
  "serde",
  "serde-device-tree",
  "sifive-test-device",

Cargo.toml

@@ -14,11 +14,15 @@ log = "0.4.21"
 panic-halt = "0.2.0"
 riscv = "0.11.1"
 rustsbi = { version = "0.4.0-alpha.3", features = ["machine"] }
+sbi-spec = { version = "0.0.7", features = ["legacy"] }
 serde = { version = "1.0.202", default-features = false, features = ["derive"]}
 serde-device-tree = { git = "https://github.com/rustsbi/serde-device-tree", default-features = false }
 sifive-test-device = "0.0.0"
 spin = "0.9.8"
 uart16550 = "0.0.1"
+riscv-decode = "0.2.1"
+fast-trap = { version = "0.0.1", features = ["riscv-m"] }
+
 
 [[bin]]
 name = "rustsbi-prototyper"

build.rs

@@ -13,7 +13,7 @@ fn main() {
 const LINKER_SCRIPT: &[u8] = b"OUTPUT_ARCH(riscv)
 ENTRY(_start) 
 SECTIONS {
-    . = 0x80000000;
+    . = 0x80100000;
     .text : ALIGN(8) { 
         *(.text.entry)
         *(.text .text.*)

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

@@ -106,7 +106,7 @@ $ git clone https://github.com/mirror/busybox.git && cd busybox && git checkout
 Clone Linux Kernel
 
 ``` shell
-git clone https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git && cd linux && git checkout v6.2 && cd ..
+$ git clone https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git && cd linux && git checkout v6.2 && cd ..
 ```
 
 ## 编译Linux Kernel
@@ -159,7 +159,7 @@ $ make -j$(nproc)
 进入busybox目录
 
 ``` shell
-cd busybox
+$ cd busybox
 ```
 
 导出环境变量
@@ -288,7 +288,7 @@ $ sudo losetup -d /dev/loop0
 进入U-Boot目录
 
 ``` shell
-cd u-boot
+$ cd u-boot
 ```
 
 导出环境变量
@@ -326,7 +326,7 @@ U-Boot 二进制文件位于 `./u-boot.bin`。
 进入OpenSBI目录
 
 ``` shell
-cd opensbi
+$ cd opensbi
 ```
 
 导出环境变量
@@ -376,7 +376,7 @@ $ qemu-system-riscv64 -M virt -smp 4 -m 256M -nographic \
 进入OpenSBI目录
 
 ``` shell
-cd opensbi
+$ cd opensbi
 ```
 
 导出环境变量
@@ -392,7 +392,7 @@ $ export CROSS_COMPILE=riscv64-linux-gnu-
 $ make all PLATFORM=generic PLATFORM_RISCV_XLEN=64 -j$(nproc)
 ```
 
-本小节将使用 QEMU 可以运行的输出文件 `build/platform/generic/firmware/fw_jump.elf`。
+本小节将使用 QEMU 可以运行的输出文件 `build/platform/generic/firmware/fw_jump.bin`。
 
 ### 使用OpenSBI `fw_jump`固件启动Linux Kernel
 
@@ -437,7 +437,7 @@ $ export CROSS_COMPILE=riscv64-linux-gnu-
 $ make all PLATFORM=generic PLATFORM_RISCV_XLEN=64 -j$(nproc)
 ```
 
-本小节将使用 QEMU 可以运行的输出文件 `build/platform/generic/firmware/fw_dynamic.elf`。
+本小节将使用 QEMU 可以运行的输出文件 `build/platform/generic/firmware/fw_dynamic.bin`。
 
 ### 编译U-Boot SPL
 

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

@@ -0,0 +1,345 @@
+# 使用RustSBI & U-Boot在QEMU中启动Linux内核
+
+本教程给出了使用RustSBI和U-Boot在QEMU中启动Linux内核的基本流程。高级用户可以在本教程中配置或构建各种内容时尝试不同的选项。
+
+请读者在其主机上安装必要的软件来尝试本教程的脚本。本教程是在Arch Linux上开发的。
+
+[环境配置](#环境配置)小节给出了本教程的环境配置方法，用户在使用本教程时需要先完成环境配置小节内容。
+
+[编译Linux Kernel](#编译linux-kernel)小节给出了Linux Kernel的编译流程，并使用编译好的Linux Kernel镜像制作启动盘。
+
+RustSBI 原型系统提供动态固件，根据前一个阶段传入的信息动态加载下一个阶段。
+
+本教程使用软件版本如下：
+
+|         软件          |  版本   |
+| :-------------------: | :-----: |
+| riscv64-linux-gnu-gcc | 14.1.0  |
+|  qemu-system-riscv64  |  9.0.1  |
+|  RustSBI Prototyper   |  0.0.0  |
+|        U-Boot         | 2024.04 |
+|     Linux Kernel      |   6.2   |
+|        busybox        | 1.36.0  |
+
+## 环境配置
+
+### 安装交叉编译器和QEMU
+
+For Arhc Linux:
+
+``` shell
+$ sudo pacman -S git riscv64-linux-gnu-gcc qemu-system-riscv
+```
+
+For Ubuntu:
+
+``` shell
+$ sudo apt-get update && sudo apt-get upgrade
+$ sudo apt-get install git qemu-system-misc gcc-riscv64-linux-gnu 
+```
+
+#### 测试是否成功安装
+
+For riscv64-linux-gnu-gcc:
+
+``` shell
+$  riscv64-linux-gnu-gcc --version
+```
+
+它将输出以下版本信息
+
+``` 
+riscv64-linux-gnu-gcc (GCC) 14.1.0
+Copyright (C) 2024 Free Software Foundation, Inc.
+This is free software; see the source for copying conditions.  There is NO warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+```
+
+For QEMU:
+
+``` shell
+$ qemu-system-riscv64  --version
+```
+
+它将输出以下版本信息
+
+``` 
+QEMU emulator version 9.0.1
+Copyright (c) 2003-2024 Fabrice Bellard and the QEMU Project developers
+```
+
+### 准备RustSBI Prototyper， U-Boot ， busybox和Linux Kernel源码
+
+创建工作目录并进入该目录
+
+``` shell
+$ mkdir workshop && cd workshop
+```
+
+Clone RustSBI Prototyper
+
+``` shell
+$ git clone https://github.com/rustsbi/prototyper.git && cd ..
+```
+
+Clone U-Boot
+
+``` shell
+$ git clone https://github.com/u-boot/u-boot.git && cd u-boot && git checkout v2024.04 && cd ..
+```
+
+Clone busybox
+
+``` shell
+$ git clone https://github.com/mirror/busybox.git && cd busybox && git checkout 1_36_0 && cd ..
+```
+
+Clone Linux Kernel
+
+``` shell
+$ git clone https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git && cd linux && git checkout v6.2 && cd ..
+```
+
+## 编译Linux Kernel
+
+进入`linux`目录
+
+``` shell
+$ cd linux
+```
+
+导出环境变量
+
+``` shell
+$ export ARCH=riscv
+$ export CROSS_COMPILE=riscv64-linux-gnu-
+```
+
+生成`.config`文件
+
+``` shell
+$ make defconfig
+```
+
+验证`.config`文件是否存在RISC-V
+
+``` shell
+$ grep --color=always -ni 'riscv' .config
+```
+
+观察到RISC-V 配置选项已启用
+
+``` 
+CONFIG_RISCV=y
+```
+
+编译Linux Kernel
+
+``` shell
+$ make -j$(nproc)
+```
+
+生成的文件`Image` 和 `Image.gz` 可以在`arch/riscv/boot/`目录找到。 `Image.gz`是 `Image` 的压缩形式。
+
+### 创建根文件系统
+
+#### 编译busybox
+
+> busybox在Ubuntu 22.04和Arch Linux系统上编译时会报错，推荐在Ubuntu 20.04系统上编译。
+
+进入busybox目录
+
+``` shell
+$ cd busybox
+```
+
+导出环境变量
+
+``` shell
+$ export ARCH=riscv
+$ export CROSS_COMPILE=riscv64-linux-gnu-
+```
+
+编译busybox
+
+``` shell
+$ make defconfig
+$ make menuconfig
+# Enable the Build static binary (no shared libs) option in Settings-->Build Options
+$ make -j $(nproc)
+$ make install
+```
+
+#### 创建启动盘
+
+在`workshop`目录运行以下命令来创建一个1 GB的磁盘镜像
+
+``` shell
+# Create a 1 GB disk image
+$ qemu-img create linux-rootfs.img 1g
+```
+
+#### 创建分区
+
+将在磁盘映像`linux-rootfs.img`上创建1个分区，这个分区是可引导的。
+
+`parted`命令将用于在镜像`linux-rootfs.img`中创建分区。在镜像中创建分区表：
+
+``` shell
+$ sudo parted linux-rootfs.img mklabel gpt
+```
+
+现在`linux-rootfs.img`中有一个分区表。将`linux-rootfs.img`挂载为loop device，以便它可以用作块设备。将`linux-rootfs.img`挂载为块设备将允许在其中创建分区。
+
+``` shell
+# Attach linux-rootfs.img with the first available loop device
+$ sudo losetup --find --show linux-rootfs.img
+```
+
+> - `find`：查找第一个未使用的loop device
+> - `show`：显示`linux-rootfs.img`附加到的loop device的名称
+
+记下循环设备的完整路径。在本教程中它是`/dev/loop0`。对`/dev/loop0`的操作将会对`linux-rootfs.img`进行操作。
+
+对`/dev/loop0`分区
+
+``` shell
+# Create a couple of primary partitions
+$ sudo parted --align minimal /dev/loop0 mkpart primary ext4 0 100%
+
+$ sudo parted /dev/loop0 print
+```
+
+#### 格式化分区
+
+通过以下命令查看分区：
+
+``` shell
+$ ls -l /dev/loop0*
+```
+
+在本教程中，分区为`/dev/loop0p1`。
+
+格式化分区并创建`ext4`文件系统，同时将分区设置为可引导分区。
+
+``` shell
+$ sudo mkfs.ext4 /dev/loop0p1
+
+# Mark first partition as bootable
+$ sudo parted /dev/loop0 set 1 boot on
+```
+
+#### 将Linux Kernel和根文件系统拷贝进启动盘
+
+``` shell
+# Mount the 1st partition
+$ sudo mkdir rootfs
+$ sudo mount /dev/loop0p1 rootfs
+$ cd rootfs
+```
+拷贝Linux Kernel镜像
+``` shell
+$ sudo cp ../linux/arch/riscv/boot/Image .
+```
+
+拷贝根文件系统
+
+``` shell
+$ sudo cp -r ../busybox/_install/* .
+$ sudo mkdir proc sys dev etc etc/init.d
+$ cd etc/init.d/
+$ sudo cat > rcS << EOF
+  #!/bin/sh
+  mount -t proc none /proc
+  mount -t sysfs none /sys
+  /sbin/mdev -s
+  EOF
+$ sudo chmod +x rcS
+```
+
+卸载`rootfs`
+
+``` shell
+$ cd workshop
+$ sudo umount rootfs
+```
+
+将`/dev/loop0`分离
+
+``` shell
+$ sudo losetup -d /dev/loop0
+```
+
+## 编译RustSBI  Prototyper
+
+进入prototyper目录
+
+``` shell
+$ 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 
+```
+
+## 编译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`文件
+
+``` shell
+# To generate .config file out of board configuration file
+$ make qemu-riscv64_spl_defconfig
+# add bootcmd value
+$ make menuconfig
+```
+
+U-Boot 配置选项将加载到终端。导航到 `Boot options` $\rightarrow$ `bootcmd value` 并将以下内容写入 `bootcmd` 值：
+
+``` 
+ext4load virtio 0:1 84000000 Image; setenv bootargs root=/dev/vda1 rw console=ttyS0; booti 0x84000000 - ${fdtcontroladdr}
+```
+
+编译U-Boot
+
+``` shell
+# To build U-Boot
+$ make -j$(nproc)
+```
+
+本小节将使用二进制文件 `./spl/u-boot-spl`和`./u-boot.itb `。
+
+## 使用RustSBI 原型系统和U-Boot启动Linux Kernel
+
+进入`workshop`目录
+
+``` shell
+$ cd workshop
+```
+
+运行下面命令
+
+``` shell
+$ qemu-system-riscv64 -M virt -smp 1 -m 256M -nographic \
+          -bios ./u-boot/spl/u-boot-spl \
+          -device loader,file=./u-boot/u-boot.itb,addr=0x80200000 \
+          -blockdev driver=file,filename=./linux-rootfs.img,node-name=hd0 \
+          -device virtio-blk-device,drive=hd0
+```
+

src/board.rs

@@ -1,64 +1,24 @@
 //! Board support, including peripheral and core drivers.
+use core::mem::MaybeUninit;
+use rustsbi::{RustSBI, SbiRet};
 
-use rustsbi::{Console, Physical, RustSBI, SbiRet};
+use crate::clint::ClintDevice;
+use crate::console::ConsoleDevice;
+use crate::hsm::Hsm;
+
+pub(crate) static mut SBI: MaybeUninit<Board> = MaybeUninit::uninit();
 
 #[derive(RustSBI, Default)]
 #[rustsbi(dynamic)]
 pub struct Board<'a> {
     #[rustsbi(console)]
-    uart16550: Option<Uart16550Device<'a>>,
+    pub uart16550: Option<ConsoleDevice<'a>>,
     #[rustsbi(ipi, timer)]
-    clint: Option<ClintDevice<'a>>,
+    pub clint: Option<ClintDevice<'a>>,
+    #[rustsbi(hsm)]
+    pub hsm: Option<Hsm>,
     #[rustsbi(reset)]
-    sifive_test: Option<SifiveTestDevice<'a>>,
-}
-
-struct Uart16550Device<'a> {
-    inner: &'a uart16550::Uart16550<u8>,
-}
-
-impl<'a> Console for Uart16550Device<'a> {
-    #[inline]
-    fn write(&self, bytes: Physical<&[u8]>) -> SbiRet {
-        // TODO verify valid memory range for a `Physical` slice.
-        let start = bytes.phys_addr_lo();
-        let buf = unsafe { core::slice::from_raw_parts(start as *const u8, bytes.num_bytes()) };
-        SbiRet::success(self.inner.write(buf))
-    }
-    #[inline]
-    fn read(&self, _bytes: Physical<&mut [u8]>) -> SbiRet {
-        todo!()
-    }
-    #[inline]
-    fn write_byte(&self, byte: u8) -> SbiRet {
-        self.inner.write(&[byte]);
-        SbiRet::success(0)
-    }
-}
-
-pub struct ClintDevice<'a> {
-    pub clint: &'a aclint::SifiveClint,
-    pub max_hart_id: usize,
-}
-
-impl<'a> rustsbi::Timer for ClintDevice<'a> {
-    #[inline]
-    fn set_timer(&self, stime_value: u64) {
-        let current_hart_id = riscv::register::mhartid::read();
-        self.clint.write_mtimecmp(current_hart_id, stime_value);
-    }
-}
-
-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) {
-                self.clint.set_msip(hart_id);
-            }
-        }
-        SbiRet::success(0)
-    }
+    pub sifive_test: Option<SifiveTestDevice<'a>>,
 }
 
 pub struct SifiveTestDevice<'a> {

src/clint.rs

@@ -0,0 +1,107 @@
+use aclint::SifiveClint;
+use core::{
+    arch::asm,
+    ptr::null_mut,
+    sync::atomic::{
+        AtomicPtr,
+        Ordering::{Relaxed, Release},
+    },
+};
+use rustsbi::SbiRet;
+
+pub(crate) static SIFIVECLINT: AtomicPtr<SifiveClint> = AtomicPtr::new(null_mut());
+
+pub(crate) fn init(base: usize) {
+    SIFIVECLINT.store(base as _, Release);
+}
+
+#[inline]
+pub fn clear() {
+    let hart_id = riscv::register::mhartid::read();
+    loop {
+        if let Some(clint) = unsafe { SIFIVECLINT.load(Relaxed).as_ref() } {
+            clint.clear_msip(hart_id);
+            clint.write_mtimecmp(hart_id, u64::MAX);
+            break;
+        } else {
+            continue;
+        }
+    }
+}
+
+#[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);
+            break;
+        } else {
+            continue;
+        }
+    }
+}
+
+#[inline]
+pub fn set_msip(hart_id: usize) {
+    loop {
+        if let Some(clint) = unsafe { SIFIVECLINT.load(Relaxed).as_ref() } {
+            clint.set_msip(hart_id);
+            break;
+        } else {
+            continue;
+        }
+    }
+}
+
+pub struct ClintDevice<'a> {
+    pub clint: &'a AtomicPtr<SifiveClint>,
+    pub max_hart_id: usize,
+}
+
+impl<'a> ClintDevice<'a> {
+    pub fn new(clint: &'a AtomicPtr<SifiveClint>, max_hart_id: usize) -> Self {
+        Self { clint, max_hart_id }
+    }
+}
+
+impl<'a> ClintDevice<'a> {
+    #[inline]
+    pub fn get_time(&self) -> usize {
+        unsafe { (*self.clint.load(Relaxed)).read_mtime() as u32 as usize }
+    }
+
+    #[inline]
+    pub fn get_timeh(&self) -> usize {
+        unsafe { ((*self.clint.load(Relaxed)).read_mtime() >> 32) as usize }
+    }
+}
+
+impl<'a> rustsbi::Timer for ClintDevice<'a> {
+    #[inline]
+    fn set_timer(&self, stime_value: u64) {
+        unsafe {
+            // TODO: 添加CPU拓展探测机制，补充无Sstc拓展时的定时器设置
+            asm!(
+                "csrrs zero, stimecmp, {}",
+                in(reg) stime_value
+            );
+            riscv::register::mie::set_mtimer();
+            // (*self.clint.load(Relaxed)).write_mtimecmp(current_hart_id, stime_value);
+        }
+    }
+}
+
+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) {
+                unsafe {
+                    (*self.clint.load(Relaxed)).set_msip(hart_id);
+                }
+            }
+        }
+        SbiRet::success(0)
+    }
+}

src/console.rs

@@ -1,8 +1,72 @@
-use core::{fmt, str::FromStr};
+use core::{fmt, ptr::null, str::FromStr};
 use log::{Level, LevelFilter};
+use rustsbi::{Console, Physical, SbiRet};
 use spin::Mutex;
 use uart16550::Uart16550;
 
+pub struct ConsoleDevice<'a> {
+    inner: &'a Mutex<MachineConsole>,
+}
+
+impl<'a> ConsoleDevice<'a> {
+    pub fn new(inner: &'a Mutex<MachineConsole>) -> Self {
+        Self { inner }
+    }
+}
+
+#[doc(hidden)]
+pub(crate) static CONSOLE: Mutex<MachineConsole> = Mutex::new(MachineConsole::Uart16550(null()));
+
+pub fn init(base: usize) {
+    *CONSOLE.lock() = MachineConsole::Uart16550(base as _);
+    log_init();
+}
+
+
+impl<'a> Console for ConsoleDevice<'a> {
+    #[inline]
+    fn write(&self, bytes: Physical<&[u8]>) -> SbiRet {
+        // TODO verify valid memory range for a `Physical` slice.
+        let start = bytes.phys_addr_lo();
+        let buf = unsafe { core::slice::from_raw_parts(start as *const u8, bytes.num_bytes()) };
+        let console = self.inner.lock();
+        match *console {
+            MachineConsole::Uart16550(uart16550) => unsafe {
+                (*uart16550).write(buf);
+            },
+        }
+        drop(console);
+        SbiRet::success(0)
+    }
+
+    #[inline]
+    fn read(&self, bytes: Physical<&mut [u8]>) -> SbiRet {
+        // TODO verify valid memory range for a `Physical` slice.
+        let start = bytes.phys_addr_lo();
+        let buf = unsafe { core::slice::from_raw_parts_mut(start as *mut u8, bytes.num_bytes()) };
+        let console = self.inner.lock();
+        let bytes_num: usize = match *console {
+            MachineConsole::Uart16550(uart16550) => unsafe {
+               (*uart16550).read(buf)
+            },
+        };
+        drop(console);
+        SbiRet::success(bytes_num)
+    }
+
+    #[inline]
+    fn write_byte(&self, byte: u8) -> SbiRet {
+        let console = self.inner.lock();
+        match *console {
+            MachineConsole::Uart16550(uart16550) => unsafe {
+                (*uart16550).write(&[byte]);
+            },
+        }
+        drop(console);
+        SbiRet::success(0)
+    }
+}
+
 #[doc(hidden)]
 pub enum MachineConsole {
     Uart16550(*const Uart16550<u8>),
@@ -27,11 +91,7 @@ impl fmt::Write for MachineConsole {
 unsafe impl Send for MachineConsole {}
 unsafe impl Sync for MachineConsole {}
 
-#[doc(hidden)]
-pub static CONSOLE: Mutex<MachineConsole> =
-    Mutex::new(MachineConsole::Uart16550(0x10000000 as *const _));
-
-pub fn init() {
+pub fn log_init() {
     log::set_max_level(
         option_env!("RUST_LOG")
             .and_then(|s| LevelFilter::from_str(s).ok())

src/dynamic.rs

@@ -1,6 +1,6 @@
 //! Frequently used first boot stage dynamic information on RISC-V.
 
-use core::{mem::size_of, ops::Range};
+use core::ops::Range;
 
 use riscv::register::mstatus;
 
@@ -25,7 +25,7 @@ pub struct DynamicInfo {
 // Definition of `boot_hart` can be found at:
 // https://github.com/riscv-software-src/opensbi/blob/019a8e69a1dc0c0f011fabd0372e1ba80e40dd7c/include/sbi/fw_dynamic.h#L75
 
-const DYNAMIC_INFO_VALID_ADDRESSES: Range<usize> = 0x1000..0xf000;
+const DYNAMIC_INFO_INVALID_ADDRESSES: usize = 0x00000000;
 const NEXT_ADDR_VALID_ADDRESSES: Range<usize> = 0x80000000..0x90000000;
 pub(crate) const MAGIC: usize = 0x4942534f;
 const SUPPORTED_VERSION: Range<usize> = 2..3;
@@ -44,13 +44,13 @@ pub fn read_paddr(paddr: usize) -> Result<DynamicInfo, DynamicReadError> {
         bad_version: None,
     };
     // check pointer before dereference
-    if !DYNAMIC_INFO_VALID_ADDRESSES.contains(&paddr)
-        || !DYNAMIC_INFO_VALID_ADDRESSES.contains(&(paddr + size_of::<DynamicInfo>()))
+    if DYNAMIC_INFO_INVALID_ADDRESSES == paddr
     {
         error.bad_paddr = Some(paddr);
         return Err(error);
     }
     let ans = unsafe { *(paddr as *const DynamicInfo) };
+    
     if ans.magic != MAGIC {
         error.bad_magic = Some(ans.magic);
     }

src/hsm.rs

@@ -0,0 +1,245 @@
+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::trap_stack::ROOT_STACK;
+use crate::clint;
+
+const HART_STATE_START_PENDING_EXT: usize = usize::MAX;
+
+
+type HsmState = AtomicUsize;
+
+pub(crate) struct HsmCell<T> {
+    status: HsmState,
+    inner: UnsafeCell<Option<T>>,
+}
+
+impl<T> HsmCell<T> {
+    /// 创建一个新的共享对象。
+    pub const fn new() -> Self {
+        Self {
+            status: HsmState::new(hart_state::STOPPED),
+            inner: UnsafeCell::new(None),
+        }
+    }
+
+    /// 从当前硬件线程的状态中获取线程间共享对象。
+    ///
+    /// # Safety
+    ///
+    /// 用户需要确保对象属于当前硬件线程。
+    #[inline]
+    pub unsafe fn local(&self) -> LocalHsmCell<'_, T> {
+        LocalHsmCell(self)
+    }
+
+    /// 取出共享对象。
+    #[inline]
+    pub fn remote(&self) -> RemoteHsmCell<'_, T> {
+        RemoteHsmCell(self)
+    }
+}
+
+/// 硬件线程上下文。
+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>);
+
+/// 任意硬件线程的共享对象。
+pub struct RemoteHsmCell<'a, T>(&'a HsmCell<T>);
+
+unsafe impl<T: Send> Sync for HsmCell<T> {}
+unsafe impl<T: Send> Send for HsmCell<T> {}
+
+impl<T> LocalHsmCell<'_, T> {
+    /// 从启动挂起状态的硬件线程取出共享数据，并将其状态设置为启动，如果成功返回取出的数据，否则返回当前状态。
+    #[inline]
+    pub fn start(&self) -> Result<T, usize> {
+        loop {
+            match self.0.status.compare_exchange(
+                hart_state::START_PENDING,
+                hart_state::STARTED,
+                Ordering::AcqRel,
+                Ordering::Relaxed,
+            ) {
+                Ok(_) => break Ok(unsafe { (*self.0.inner.get()).take().unwrap() }),
+                Err(HART_STATE_START_PENDING_EXT) => spin_loop(),
+                Err(s) => break Err(s),
+            }
+        }
+    }
+
+    /// 关闭。
+    #[allow(unused)]
+    #[inline]
+    pub fn stop(&self) {
+        self.0.status.store(hart_state::STOPPED, Ordering::Release)
+    }
+
+    /// 关闭。
+    #[allow(unused)]
+    #[inline]
+    pub fn suspend(&self) {
+        self.0
+            .status
+            .store(hart_state::SUSPENDED, Ordering::Relaxed)
+    }
+
+    /// 关闭。
+    #[allow(unused)]
+    #[inline]
+    pub fn resume(&self) {
+        self.0.status.store(hart_state::STARTED, Ordering::Relaxed)
+    }
+}
+
+impl<T> RemoteHsmCell<'_, T> {
+    /// 向关闭状态的硬件线程传入共享数据，并将其状态设置为启动挂起，返回是否放入成功。
+    #[inline]
+    pub fn start(self, t: T) -> bool {
+        if self
+            .0
+            .status
+            .compare_exchange(
+                hart_state::STOPPED,
+                HART_STATE_START_PENDING_EXT,
+                Ordering::Acquire,
+                Ordering::Relaxed,
+            )
+            .is_ok()
+        {
+            unsafe { *self.0.inner.get() = Some(t) };
+            self.0
+                .status
+                .store(hart_state::START_PENDING, Ordering::Release);
+            true
+        } else {
+            false
+        }
+    }
+
+    /// 取出当前状态。
+    #[allow(unused)]
+    #[inline]
+    pub fn sbi_get_status(&self) -> usize {
+        match self.0.status.load(Ordering::Relaxed) {
+            HART_STATE_START_PENDING_EXT => hart_state::START_PENDING,
+            normal => normal,
+        }
+    }
+
+    /// 判断这个 HART 能否接收 IPI。
+    #[allow(unused)]
+    #[inline]
+    pub fn allow_ipi(&self) -> bool {
+        matches!(
+            self.0.status.load(Ordering::Relaxed),
+            hart_state::STARTED | hart_state::SUSPENDED
+        )
+    }
+}
+
+
+/// 获取此 hart 的 local hsm 对象。
+pub(crate) fn local_hsm() -> LocalHsmCell<'static, NextStage> {
+    unsafe {
+        ROOT_STACK
+            .get_unchecked_mut(hart_id())
+            .hart_context()
+            .hsm
+            .local()
+    }
+}
+
+/// 获取此 hart 的 remote hsm 对象。
+pub(crate) fn local_remote_hsm() -> RemoteHsmCell<'static, NextStage> {
+    unsafe {
+        ROOT_STACK
+            .get_unchecked_mut(hart_id())
+            .hart_context()
+            .hsm
+            .remote()
+    }
+}
+
+/// 获取任意 hart 的 remote hsm 对象。
+#[allow(unused)]
+pub(crate) fn remote_hsm(hart_id: usize) -> Option<RemoteHsmCell<'static, NextStage>> {
+    unsafe {
+        ROOT_STACK
+            .get_mut(hart_id)
+            .map(|x| x.hart_context().hsm.remote())
+    }
+}
+
+
+
+/// HSM 
+pub(crate) struct Hsm;
+
+impl rustsbi::Hsm for Hsm {
+    fn hart_start(&self, hartid: usize, start_addr: usize, opaque: usize) -> SbiRet {
+        match remote_hsm(hartid) {
+            Some(remote) => {
+                if remote.start(NextStage { start_addr, opaque, next_mode: MPP::Supervisor }) {
+                    clint::set_msip(hartid);
+                    SbiRet::success(0)
+                } else {
+                    SbiRet::already_started()
+                }
+            }
+            None => SbiRet::invalid_param(),
+        }
+    }
+
+    #[inline]
+    fn hart_stop(&self) -> SbiRet {
+        local_hsm().stop();
+        SbiRet::success(0)
+    }
+
+    #[inline]
+    fn hart_get_status(&self, hartid: usize) -> SbiRet {
+        match remote_hsm(hartid) {
+            Some(remote) => SbiRet::success(remote.sbi_get_status()),
+            None => SbiRet::invalid_param(),
+        }
+    }
+
+    fn hart_suspend(&self, suspend_type: u32, _resume_addr: usize, _opaque: usize) -> SbiRet {
+        use rustsbi::spec::hsm::suspend_type::{NON_RETENTIVE, RETENTIVE};
+        if matches!(suspend_type, NON_RETENTIVE | RETENTIVE) {
+            local_hsm().suspend();
+            riscv::asm::wfi() ;
+            local_hsm().resume();
+            SbiRet::success(0)
+        } else {
+            SbiRet::not_supported()
+        }
+    }
+}

src/main.rs

@@ -8,54 +8,110 @@ extern crate log;
 mod macros;
 
 mod board;
+mod clint;
 mod console;
 mod dt;
 mod dynamic;
 mod fail;
+mod hsm;
 mod reset;
+mod riscv_spec;
 mod trap;
+mod trap_stack;
 
-use panic_halt as _;
+use clint::ClintDevice;
+use core::{arch::asm, mem::MaybeUninit};
 use riscv::register::mstatus;
 
-extern "C" fn main(hart_id: usize, opaque: usize, nonstandard_a2: usize) -> usize {
-    let _ = (hart_id, opaque);
-    console::init();
+use crate::board::{Board, SBI};
+use crate::clint::SIFIVECLINT;
+use crate::console::{ConsoleDevice, CONSOLE};
+use crate::hsm::{Hsm, local_remote_hsm};
+use crate::trap::trap_vec;
+use crate::riscv_spec::menvcfg;
+use crate::trap_stack::NUM_HART_MAX;
 
-    info!("RustSBI version {}", rustsbi::VERSION);
-    rustsbi::LOGO.lines().for_each(|line| info!("{}", line));
-    info!("Initializing RustSBI machine-mode environment.");
+#[no_mangle]
+extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
+    extern "C" {
+        fn sbss();
+        fn ebss();
+    }
+    (sbss as usize..ebss as usize).for_each(|a| unsafe { (a as *mut u8).write_volatile(0) });
+
+    // parse dynamic information
+    let info = dynamic::read_paddr(nonstandard_a2).unwrap_or_else(fail::no_dynamic_info_available);
+    let (mpp, next_addr) = dynamic::mpp_next_addr(&info).unwrap_or_else(fail::invalid_dynamic_data);
 
+    // parse the device tree
     let dtb = dt::parse_device_tree(opaque).unwrap_or_else(fail::device_tree_format);
     let dtb = dtb.share();
     let tree = serde_device_tree::from_raw_mut(&dtb).unwrap_or_else(fail::device_tree_deserialize);
+
+    // TODO: The device base address needs to be parsed from FDT
+    console::init(0x10000000);
+    clint::init(0x2000000);
+
+    info!("RustSBI version {}", rustsbi::VERSION);
+    rustsbi::LOGO.lines().for_each(|line| info!("{}", line));
+    info!("Initializing RustSBI machine-mode environment.");
     if let Some(model) = tree.model {
         info!("Model: {}", model.iter().next().unwrap_or("<unspecified>"));
     }
-    info!("Chosen stdout item: {}", tree.chosen.stdout_path.iter().next().unwrap_or("<unspecified>"));
-    // TODO handle unspecified by parsing into &'a str
+    info!(
+        "Chosen stdout item: {}",
+        tree.chosen
+            .stdout_path
+            .iter()
+            .next()
+            .unwrap_or("<unspecified>")
+    );
 
-    let info = dynamic::read_paddr(nonstandard_a2).unwrap_or_else(fail::no_dynamic_info_available);
+    // Init SBI
+    unsafe {
+        SBI = MaybeUninit::new(Board {
+            uart16550: Some(ConsoleDevice::new(&CONSOLE)),
+            clint: Some(ClintDevice::new(&SIFIVECLINT, NUM_HART_MAX)),
+            hsm: Some(Hsm),
+            sifive_test: None,
+        });
+    }
 
-    let (mpp, next_addr) = dynamic::mpp_next_addr(&info).unwrap_or_else(fail::invalid_dynamic_data);
+    // TODO: PMP configuration needs to be obtained through the memory range in the device tree
+    use riscv::register::*;
+    unsafe {
+        pmpcfg0::set_pmp(0, Range::OFF, Permission::NONE, false);
+        pmpaddr0::write(0);
+        pmpcfg0::set_pmp(1, Range::TOR, Permission::RWX, false);
+        pmpaddr1::write(usize::MAX >> 2);
+    }
 
-    info!("Redirecting harts to 0x{:x} in {:?} mode.", next_addr, mpp);
+    // 设置陷入栈
+    trap_stack::prepare_for_trap();
 
-    trap::init();
-    unsafe { mstatus::set_mpp(mpp) };
-    next_addr
-}
+    // 设置内核入口
+    local_remote_hsm().start(NextStage {
+        start_addr: next_addr,
+        next_mode: mpp,
+        opaque,
+    });
 
-const LEN_STACK_PER_HART: usize = 16 * 1024;
-pub(crate) const NUM_HART_MAX: usize = 8;
-const LEN_STACK: usize = LEN_STACK_PER_HART * NUM_HART_MAX;
+    info!("Redirecting harts to 0x{:x} in {:?} mode.", next_addr, mpp);
 
-// TODO contribute `Stack` struct into the crate `riscv`
-#[repr(C, align(128))]
-struct Stack<const N: usize>([u8; N]);
+    clint::clear();
+    unsafe {
+        asm!("csrw mideleg,    {}", in(reg) !0);
+        asm!("csrw medeleg,    {}", in(reg) !0);
+        asm!("csrw mcounteren, {}", in(reg) !0);
+        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);
+        mtvec::write(trap_vec as _, mtvec::TrapMode::Vectored);
+    }
+}
 
-#[link_section = ".bss.uninit"]
-static STACK: Stack<LEN_STACK> = Stack([0; LEN_STACK]);
 
 #[naked]
 #[link_section = ".text.entry"]
@@ -69,50 +125,51 @@ unsafe extern "C" fn start() -> ! {
         "   csrr    t0, mhartid",
         "   ld      t1, 0(a2)",
         "   li      t2, {magic}",
-        "   bne     t1, t2, 3f",
-        "   ld      t2, 40(a2)",
-        "   bne     t0, t2, 2f",
-        "   j       4f",
-        "3:",
-        "   j       3b", // TODO multi hart preempt for runtime init
-        "4:",
-        // clear bss segment
-        "   la      t0, sbss
-            la      t1, ebss
-        1:  bgeu    t0, t1, 2f
-            sd      zero, 0(t0)
-            addi    t0, t0, 8
-            j       1b",
-        // prepare data segment
-        "   la      t3, sidata
-            la      t4, sdata
-            la      t5, edata
-        1:  bgeu    t4, t5, 2f
-            ld      t6, 0(t3)
-            sd      t6, 0(t4)
-            addi    t3, t3, 8
-            addi    t4, t4, 8
-            j       1b",
-        "2: ",
-        // TODO wait before boot-hart initializes runtime
-        // 3. Prepare stack for each hart
-        "   la      sp, {stack}",
-        "   li      t0, {stack_size_per_hart}",
-        "   csrr    t1, mhartid",
-        "   addi    t1, t1, 1",
-        "1: ",
-        "   add     sp, sp, t0",
-        "   addi    t1, t1, -1",
-        "   bnez    t1, 1b",
-        // 4. Run Rust main function
+        "   bne     t1, t2, 1f",
+        "   j       2f",
+        "1:",
+        "   j       1b", // TODO multi hart preempt for runtime init
+        "2:",
+        "   call    {locate_stack}",
         "   call    {main}",
-        // 5. Jump to following boot sequences
-        "   csrw    mepc, a0",
-        "   mret",
+        "   j       {trap}",
         magic = const dynamic::MAGIC,
-        stack_size_per_hart = const LEN_STACK_PER_HART,
-        stack = sym STACK,
-        main = sym main,
+        locate_stack = sym trap_stack::locate,
+        main         = sym rust_main,
+        trap         = sym trap::trap_vec,
         options(noreturn)
     )
 }
+
+#[inline]
+pub fn hart_id() -> usize {
+    riscv::register::mhartid::read()
+}
+
+#[derive(Debug)]
+pub struct NextStage {
+    start_addr: usize,
+    opaque: usize,
+    next_mode: mstatus::MPP,
+}
+
+#[panic_handler]
+fn panic(info: &core::panic::PanicInfo) -> ! {
+    use riscv::register::*;
+    println!(
+        "[rustsbi-panic] hart {} {info}",
+        riscv::register::mhartid::read()
+    );
+    println!(
+        "-----------------------------
+> mcause:  {:?}
+> mepc:    {:#018x}
+> mtval:   {:#018x}
+-----------------------------",
+        mcause::read().cause(),
+        mepc::read(),
+        mtval::read()
+    );
+    println!("[rustsbi-panic] system shutdown scheduled due to RustSBI panic");
+    loop {}
+}

src/riscv_spec.rs

@@ -0,0 +1,29 @@
+#![allow(unused)]
+
+pub const CSR_TIME: u32 = 0xc01;
+pub const CSR_TIMEH: u32 = 0xc81;
+pub const CSR_STIMECMP: u32 = 0x14D;
+
+pub mod menvcfg {
+    use core::arch::asm;
+
+    pub const FIOM:            usize = 0x1 << 0;
+    pub const CBIE_FLUSH:      usize = 0x01 << 4;
+    pub const CBIE_INVALIDATE: usize = 0x11 << 4;
+    pub const CBCFE:           usize = 0x1 << 6;
+    pub const CBZE:            usize = 0x1 << 7;
+    pub const PBMTE:           usize = 0x1 << 62;
+    pub const STCE:            usize = 0x1 << 63;
+
+    #[inline(always)]
+    pub fn set_stce() {
+        set_bits(STCE);
+    }
+
+    pub fn set_bits(option: usize) {
+        let mut bits: usize;
+        unsafe { asm!("csrr {}, menvcfg", out(reg) bits, options(nomem)) };
+        bits |= option;
+        unsafe { asm!("csrw menvcfg, {}", in(reg) bits, options(nomem)) };
+    }
+}

src/trap.rs

@@ -1,141 +1,326 @@
+use aclint::SifiveClint as Clint;
 use core::arch::asm;
-
+use fast_trap::{trap_entry, FastContext, FastResult};
 use riscv::register::{
-    mstatus::Mstatus,
-    mtvec::{self, TrapMode},
-};
+    mepc, mie, mstatus};
+use rustsbi::RustSBI;
 
-// Typically, the `stvec` Will be overrided by supervisor software, e.g. kernel, hypervisor or secure environments.
-pub fn init() {
-    let mut addr = machine_trap as usize;
-    if addr & 0b10 != 0 {
-        addr += 0b10; // mtvec base address must align to 4 bytes.
-    }
-    unsafe { mtvec::write(addr, TrapMode::Direct) };
+use crate::board::SBI;
+use crate::clint::{self, SIFIVECLINT};
+use crate::console::{MachineConsole, CONSOLE};
+use crate::hsm::local_hsm;
+use crate::riscv_spec::{CSR_TIME, CSR_TIMEH};
+
+#[naked]
+pub(crate) unsafe extern "C" fn trap_vec() {
+    asm!(
+        ".align 2",
+        ".option push",
+        ".option norvc",
+        "j {default}", // exception
+        "j {default}", // supervisor software
+        "j {default}", // reserved
+        "j {msoft} ",  // machine    software
+        "j {default}", // reserved
+        "j {default}", // supervisor timer
+        "j {default}", // reserved
+        "j {mtimer}",  // machine    timer
+        "j {default}", // reserved
+        "j {default}", // supervisor external
+        "j {default}", // reserved
+        "j {default}", // machine    external
+        ".option pop",
+        default = sym trap_entry,
+        mtimer  = sym mtimer,
+        msoft   = sym msoft,
+        options(noreturn)
+    )
 }
 
-extern "C" fn rust_machine_trap(ctx: &mut SupervisorContext) {
-    error!("ctx: {:x?}", ctx);
+/// machine timer 中断代理
+///
+/// # Safety
+///
+/// 裸函数。
+#[naked]
+unsafe extern "C" fn mtimer() {
+    asm!(
+        // 换栈：
+        // sp      : M sp
+        // mscratch: S sp
+        "   csrrw sp, mscratch, sp",
+        // 保护
+        "   addi  sp, sp, -4*8
+            sd    ra, 0*8(sp)
+            sd    a0, 1*8(sp)
+            sd    a1, 2*8(sp)
+            sd    a2, 3*8(sp)
+        ",
+        // 清除 mtimecmp
+        "   la    a0, {clint_ptr}
+            ld    a0, (a0)
+            csrr  a1, mhartid
+            addi  a2, zero, -1
+            call  {set_mtimecmp}
+        ",
+        // 设置 stip
+        "   li    a0, {mip_stip}
+            csrrs zero, mip, a0
+        ",
+        // 恢复
+        "   ld    ra, 0*8(sp)
+            ld    a0, 1*8(sp)
+            ld    a1, 2*8(sp)
+            ld    a2, 3*8(sp)
+            addi  sp, sp,  4*8
+        ",
+        // 换栈：
+        // sp      : S sp
+        // mscratch: M sp
+        "   csrrw sp, mscratch, sp",
+        // 返回
+        "   mret",
+        mip_stip     = const 1 << 5,
+        clint_ptr    =   sym SIFIVECLINT,
+        //                   Clint::write_mtimecmp_naked(&self, hart_idx, val)
+        set_mtimecmp =   sym Clint::write_mtimecmp_naked,
+        options(noreturn)
+    )
 }
 
+/// machine soft 中断代理
+///
+/// # Safety
+///
+/// 裸函数。
 #[naked]
-unsafe extern "C" fn machine_trap() -> ! {
+unsafe extern "C" fn msoft() {
     asm!(
-        ".align 2",
-        "csrrw  sp, mscratch, sp",
-        "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, mstatus
-        sd      t0, 31*8(sp)",
-        "csrr   t1, mepc
-        sd      t1, 32*8(sp)",
-        "csrr   t2, mscratch",
-        "sd     t2, 1*8(sp)",
-        "mv     a0, sp",
-        "call   {rust_machine_trap}",
-        "ld     t0, 31*8(sp)
-        ld      t1, 32*8(sp)
-        csrw    mstatus, t0
-        csrw    mepc, t1",
-        "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)",
-        "csrrw  sp, mscratch, sp",
-        "mret",
-        rust_machine_trap = sym rust_machine_trap,
+        // 换栈：
+        // 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,
         options(noreturn)
-    );
+    )
 }
 
-#[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 mstatus: Mstatus, // 31
-    pub mepc: usize,      // 32
+/// Fast Trap
+pub extern "C" fn fast_handler(
+    mut ctx: FastContext,
+    a1: usize,
+    a2: usize,
+    a3: usize,
+    a4: usize,
+    a5: usize,
+    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] = 0;
+        ctx.regs().a[1] = opaque;
+        ctx.regs().pc = start_addr;
+        ctx.call(2)
+    }
+    loop {
+        match local_hsm().start() {
+            Ok(next_stage) => {
+                unsafe {
+                    mstatus::set_mpie();
+                    mstatus::set_mpp(next_stage.next_mode);
+                    mie::set_msoft();
+                    mie::set_mtimer();
+                }
+                break boot(ctx, next_stage.start_addr, next_stage.opaque);
+            }
+            Err(rustsbi::spec::hsm::HART_STOP) => {
+                unsafe {
+                    mie::set_msoft();
+                }
+                riscv::asm::wfi();
+                clint::clear_msip();
+            }
+            _ => match mcause::read().cause() {
+                // SBI call
+                T::Exception(E::SupervisorEnvCall) => {
+                    use sbi_spec::{base, hsm, legacy};
+                    let mut ret = unsafe { SBI.assume_init_mut() }.handle_ecall(
+                        a7,
+                        a6,
+                        [ctx.a0(), a1, a2, a3, a4, a5],
+                    );
+                    if ret.is_ok() {
+                        match (a7, a6) {
+                            // 关闭
+                            (hsm::EID_HSM, hsm::HART_STOP) => continue,
+                            // 不可恢复挂起
+                            (hsm::EID_HSM, hsm::HART_SUSPEND)
+                                if matches!(ctx.a0() as u32, hsm::suspend_type::NON_RETENTIVE) =>
+                            {
+                                break boot(ctx, a1, a2);
+                            }
+                            // legacy console 探测
+                            (base::EID_BASE, base::PROBE_EXTENSION)
+                                if matches!(
+                                    ctx.a0(),
+                                    legacy::LEGACY_CONSOLE_PUTCHAR | legacy::LEGACY_CONSOLE_GETCHAR
+                                ) =>
+                            {
+                                ret.value = 1;
+                            }
+                            _ => {}
+                        }
+                    } else {
+                        match a7 {
+                            legacy::LEGACY_CONSOLE_PUTCHAR => {
+                                print!("{}", ctx.a0() as u8 as char);
+                                ret.error = 0;
+                                ret.value = a1;
+                            }
+                            legacy::LEGACY_CONSOLE_GETCHAR => {
+                                let mut c = 0u8;
+                                let uart = CONSOLE.lock();
+                                match *uart {
+                                    MachineConsole::Uart16550(uart16550) => unsafe {
+                                        loop {
+                                            if (*uart16550).read(core::slice::from_mut(&mut c)) == 1
+                                            {
+                                                ret.error = c as _;
+                                                ret.value = a1;
+                                                break;
+                                            }
+                                        }
+                                    },
+                                }
+                                drop(uart);
+                            }
+                            _ => {}
+                        }
+                    }
+                    ctx.regs().a = [ret.error, ret.value, a2, a3, a4, a5, a6, a7];
+                    mepc::write(mepc::read() + 4);
+                    break ctx.restore();
+                }
+                T::Exception(E::IllegalInstruction) => {
+                    if mstatus::read().mpp() != mstatus::MPP::Supervisor {
+                        panic!("only can handle illegal instruction exception from S-MODE");
+                    }
+
+                    ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];
+                    if !illegal_instruction_handler(&mut ctx) {
+                        delegate(&mut ctx);
+                    }
+                    break ctx.restore();
+                }
+                // 其他陷入
+                trap => {
+                    println!(
+                        "
+-----------------------------
+> trap:    {trap:?}
+> mepc:    {:#018x}
+> mtval:   {:#018x}
+-----------------------------
+            ",
+                        mepc::read(),
+                        mtval::read()
+                    );
+                    panic!("stopped with unsupported trap")
+                }
+            },
+        }
+    }
+}
+
+#[inline]
+fn delegate(_ctx: &mut FastContext) {
+    use riscv::register::{sepc, scause, stval, stvec, sstatus, mepc, mcause, mtval};
+    unsafe {
+
+        // TODO: 当支持中断嵌套时，需要从ctx里获取mpec。当前ctx.reg().pc与mepc不一致
+        sepc::write(mepc::read());
+        scause::write(mcause::read().bits());
+        stval::write(mtval::read());
+        sstatus::clear_sie();
+        mepc::write(stvec::read().address());
+    }
+}
+
+#[inline]
+fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
+    use riscv_decode::{decode, Instruction};
+    use riscv::register::{mepc, mtval};
+
+    let inst = decode(mtval::read() as u32);
+    match inst {
+        Ok(Instruction::Csrrs(csr)) => match csr.csr() {
+            CSR_TIME => {
+                assert!(
+                    10 <= csr.rd() && csr.rd() <= 17,
+                    "Unsupported CSR rd: {}",
+                    csr.rd()
+                );
+                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }.clint.as_ref().unwrap().get_time();
+            }
+            CSR_TIMEH => {
+                assert!(
+                    10 <= csr.rd() && csr.rd() <= 17,
+                    "Unsupported CSR rd: {}",
+                    csr.rd()
+                );
+                ctx.regs().a[(csr.rd() - 10) as usize] = unsafe { SBI.assume_init_mut() }.clint.as_ref().unwrap().get_timeh();
+            }
+            _ => return false,
+        },
+        // Ok(Instruction::Csrrw(csr)) => match csr.csr() {
+        //     CSR_STIMECMP => {
+        //         unsafe {
+        //             asm!("csrw stimecmp, {}", in(reg) csr.rs1()); 
+        //             mip::clear_stimer();
+        //         }
+        //         
+        //     }
+        //     _ => return false
+        // }
+        _ => return false,
+    }
+    mepc::write(mepc::read() + 4);
+    true
 }

src/trap_stack.rs

@@ -0,0 +1,74 @@
+use core::mem::forget;
+use fast_trap::FreeTrapStack;
+
+use crate::hsm::HartContext;
+use crate::trap::fast_handler;
+use crate::hart_id;
+
+const LEN_STACK_PER_HART: usize = 16 * 1024;
+pub const NUM_HART_MAX: usize = 8;
+/// 栈空间。
+#[link_section = ".bss.uninit"]
+pub(crate) static mut ROOT_STACK: [Stack; NUM_HART_MAX] = [Stack::ZERO; NUM_HART_MAX];
+
+/// 定位每个 hart 的栈。
+#[naked]
+pub(crate) unsafe extern "C" fn locate() {
+    core::arch::asm!(
+        "   la   sp, {stack}
+            li   t0, {per_hart_stack_size}
+            csrr t1, mhartid
+            addi t1, t1,  1
+         1: add  sp, sp, t0
+            addi t1, t1, -1
+            bnez t1, 1b
+            call t1, {move_stack}
+            ret
+        ",
+        per_hart_stack_size = const LEN_STACK_PER_HART,
+        stack               =   sym ROOT_STACK,
+        move_stack          =   sym fast_trap::reuse_stack_for_trap,
+        options(noreturn),
+    )
+}
+
+/// 预备陷入栈。
+pub(crate) fn prepare_for_trap() {
+    unsafe { ROOT_STACK.get_unchecked_mut(hart_id()).load_as_stack() };
+}
+
+/// 类型化栈。
+///
+/// 每个硬件线程拥有一个满足这样条件的内存块。
+/// 这个内存块的底部放着硬件线程状态 [`HartContext`]，顶部用于陷入处理，中间是这个硬件线程的栈空间。
+/// 不需要 M 态线程，每个硬件线程只有这一个栈。
+#[repr(C, align(128))]
+pub(crate) struct Stack([u8; LEN_STACK_PER_HART]);
+
+impl Stack {
+    /// 零初始化以避免加载。
+    const ZERO: Self = Self([0; LEN_STACK_PER_HART]);
+
+    /// 从栈上取出硬件线程状态。
+    #[inline]
+    pub fn hart_context(&mut self) -> &mut HartContext {
+        unsafe { &mut *self.0.as_mut_ptr().cast() }
+    }
+
+    fn load_as_stack(&'static mut self) {
+        let hart = self.hart_context();
+        let context_ptr = hart.context_ptr();
+        hart.init();
+        let range = self.0.as_ptr_range();
+        forget(
+            FreeTrapStack::new(
+                range.start as usize..range.end as usize,
+                |_| {},
+                context_ptr,
+                fast_handler,
+            )
+            .unwrap()
+            .load(),
+        );
+    }
+}