Merge pull request #136 from chenzongyao200127/feat/hfence

feat(prototyper): add hfence support for virtualized TLB sync

prototyper/README.md

@@ -32,7 +32,7 @@ These are necessary for building the firmware and handling RISC-V binary outputs
 #### Options
 
 - `-f, --features <FEATURES>`  
-  Enable specific features during the build (supports multiple values, e.g., `--features "feat1,feat2"`).
+  Enable specific features during the build (supports multiple values, e.g., `--features "hypervisor,feat2"`).
 - `--fdt <PATH>`  
   Specify the path to a Flattened Device Tree (FDT) file.  
   [Environment Variable: `PROTOTYPER_FDT_PATH`]
@@ -52,7 +52,7 @@ These are necessary for building the firmware and handling RISC-V binary outputs
 
 > #### Note on FDT Files
 > 
-> Regardless of the mode (Dynamic Firmware, Payload Firmware, or Jump Firmware), specifying an FDT file with `--fdt` ensures it is used to initialize the hardware platform configuration. The FDT file provides essential hardware setup details and overrides the bootloader’s default settings.
+> Regardless of the mode (Dynamic Firmware, Payload Firmware, or Jump Firmware), specifying an FDT file with `--fdt` ensures it is used to initialize the hardware platform configuration. The FDT file provides essential hardware setup details and overrides the bootloader's default settings.
 
 ### Firmware Compilation
 

prototyper/prototyper/Cargo.toml

@@ -6,7 +6,6 @@ license.workspace = true
 repository.workspace = true
 
 [dependencies]
-aclint = "=0.1.0"
 log = "0.4"
 panic-halt = "1.0.0"
 riscv = "0.12.1"
@@ -23,7 +22,8 @@ sbi-spec = { version = "0.0.8", features = [
     "legacy",
 ], path = "../../library/sbi-spec" }
 serde = { version = "1.0.202", default-features = false, features = ["derive"] }
-fast-trap = { version = "0.1.0", features = ["riscv-m"] }
+aclint = { git = "https://github.com/rustsbi/aclint", rev = "b2136a66" }
+fast-trap = { git = "https://github.com/rustsbi/fast-trap", rev = "8d855afa", features = ["riscv-m"] }
 serde-device-tree = { git = "https://github.com/rustsbi/serde-device-tree", rev = "2a5d6ab7", default-features = false }
 uart_xilinx = { git = "https://github.com/duskmoon314/uart-rs/", rev = "12be9142" }
 xuantie-riscv = { git = "https://github.com/rustsbi/xuantie", rev = "7a521c04" }
@@ -44,3 +44,4 @@ nemu = []
 payload = []
 jump = []
 fdt = []
+hypervisor = []

prototyper/prototyper/src/main.rs

@@ -1,5 +1,4 @@
 #![feature(alloc_error_handler)]
-#![feature(naked_functions)]
 #![feature(fn_align)]
 #![no_std]
 #![no_main]
@@ -141,95 +140,91 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
     }
 }
 
-#[naked]
+#[unsafe(naked)]
 #[unsafe(link_section = ".text.entry")]
 #[unsafe(export_name = "_start")]
 unsafe extern "C" fn start() -> ! {
-    unsafe {
-        naked_asm!(
-            ".option arch, +a",
-            // 1. Turn off interrupt.
-            "
-            csrw    mie, zero",
-            // 2. Initialize programming language runtime.
-            // only clear bss if hartid matches preferred boot hart id.
-            // Race
-            "
+    naked_asm!(
+        ".option arch, +a",
+        // 1. Turn off interrupt.
+        "
+        csrw    mie, zero",
+        // 2. Initialize programming language runtime.
+        // only clear bss if hartid matches preferred boot hart id.
+        // Race
+        "
             lla      t0, 6f
             li       t1, 1
             amoadd.w t0, t1, 0(t0)
             bnez     t0, 4f
             call     {relocation_update}",
-            // 3. Boot hart clear bss segment.
-            "1:
+        // 3. Boot hart clear bss segment.
+        "1:
             lla     t0, sbi_bss_start
             lla     t1, sbi_bss_end",
-            "2:
+        "2:
             bgeu    t0, t1, 3f
             sd      zero, 0(t0)
             addi    t0, t0, 8
             j       2b",
-            // 3.1 Boot hart set bss ready signal.
-            "3:
+        // 3.1 Boot hart set bss ready signal.
+        "3:
             lla     t0, 7f
             li      t1, 1
             amoadd.w t0, t1, 0(t0)
             j       5f",
-            // 3.2 Other harts are waiting for bss ready signal.
-            "4:
+        // 3.2 Other harts are waiting for bss ready signal.
+        "4:
             lla     t0, 7f
             lw      t0, 0(t0)
             beqz    t0, 4b",
-            // 4. Prepare stack for each hart.
-            "5:
+        // 4. Prepare stack for each hart.
+        "5:
             call    {locate_stack}
             call    {main}
             csrw    mscratch, sp
             j       {hart_boot}
             .balign  4",
-            "6:", // boot hart race signal.
-            "  .word    0",
-            "7:", // bss ready signal.
-            "  .word    0",
-            relocation_update = sym relocation_update,
-            locate_stack = sym trap_stack::locate,
-            main         = sym rust_main,
-            hart_boot    = sym trap::boot::boot,
-        )
-    }
+        "6:", // boot hart race signal.
+        "  .word    0",
+        "7:", // bss ready signal.
+        "  .word    0",
+        relocation_update = sym relocation_update,
+        locate_stack = sym trap_stack::locate,
+        main         = sym rust_main,
+        hart_boot    = sym trap::boot::boot,
+    )
 }
 
 // Handle relocations for position-independent code
-#[naked]
+#[unsafe(naked)]
 unsafe extern "C" fn relocation_update() {
-    unsafe {
-        naked_asm!(
-            // Get load offset.
-            "   li t0, {START_ADDRESS}",
-            "   lla t1, sbi_start",
-            "   sub t2, t1, t0",
-
-            // Foreach rela.dyn and update relocation.
-            "   lla t0, __rel_dyn_start",
-            "   lla t1, __rel_dyn_end",
-            "   li  t3, {R_RISCV_RELATIVE}",
-            "1:",
-            "   ld  t4, 8(t0)",
-            "   bne t4, t3, 2f",
-            "   ld t4, 0(t0)", // Get offset
-            "   ld t5, 16(t0)", // Get append
-            "   add t4, t4, t2", // Add load offset to offset add append
-            "   add t5, t5, t2",
-            "   sd t5, 0(t4)", // Update address
-            "   addi t0, t0, 24", // Get next rela item
-            "2:",
-            "   blt t0, t1, 1b",
-            "   fence.i",
-
-            // Return
-            "   ret",
-            R_RISCV_RELATIVE = const R_RISCV_RELATIVE,
-            START_ADDRESS = const cfg::SBI_LINK_START_ADDRESS,
-        )
-    }
+    naked_asm!(
+        // Get load offset.
+        "   li t0, {START_ADDRESS}",
+        "   lla t1, sbi_start",
+        "   sub t2, t1, t0",
+
+        // Foreach rela.dyn and update relocation.
+        "   lla t0, __rel_dyn_start",
+        "   lla t1, __rel_dyn_end",
+        "   li  t3, {R_RISCV_RELATIVE}",
+        "1:",
+        "   ld  t4, 8(t0)",
+        "   bne t4, t3, 2f",
+        "   ld t4, 0(t0)", // Get offset
+        "   ld t5, 16(t0)", // Get append
+        "   add t4, t4, t2", // Add load offset to offset add append
+        "   add t5, t5, t2",
+        "   sd t5, 0(t4)", // Update address
+        "   addi t0, t0, 24", // Get next rela item
+        "2:",
+        "   blt t0, t1, 1b",
+        "   fence.i",
+
+        // Return
+        "   ret",
+        R_RISCV_RELATIVE = const R_RISCV_RELATIVE,
+        START_ADDRESS = const cfg::SBI_LINK_START_ADDRESS,
+    )
 }

prototyper/prototyper/src/sbi/early_trap.rs

@@ -7,20 +7,18 @@ use riscv::register::mtvec;
 ///
 /// This function will change a0 and a1 and will NOT change them back.
 // TODO: Support save trap info.
-#[naked]
+#[unsafe(naked)]
 #[repr(align(16))]
 pub(crate) unsafe extern "C" fn light_expected_trap() {
-    unsafe {
-        naked_asm!(
-            "add a0, zero, zero",
-            "add a1, zero, zero",
-            "csrr a1, mepc",
-            "addi a1, a1, 4",
-            "csrw mepc, a1",
-            "addi a0, zero, 1",
-            "mret",
-        )
-    }
+    naked_asm!(
+        "add a0, zero, zero",
+        "add a1, zero, zero",
+        "csrr a1, mepc",
+        "addi a1, a1, 4",
+        "csrw mepc, a1",
+        "addi a0, zero, 1",
+        "mret",
+    )
 }
 
 #[repr(C)]
@@ -40,23 +38,21 @@ impl Default for TrapInfo {
     }
 }
 
-#[naked]
+#[unsafe(naked)]
 #[repr(align(16))]
 pub(crate) unsafe extern "C" fn expected_trap() {
-    unsafe {
-        naked_asm!(
-            "csrr a4, mepc",
-            "sd a4, 0*8(a3)",
-            "csrr a4, mcause",
-            "sd a4, 1*8(a3)",
-            "csrr a4, mtval",
-            "sd a4, 2*8(a3)",
-            "csrr a4, mepc",
-            "addi a4, a4, 4",
-            "csrw mepc, a4",
-            "mret",
-        )
-    }
+    naked_asm!(
+        "csrr a4, mepc",
+        "sd a4, 0*8(a3)",
+        "csrr a4, mcause",
+        "sd a4, 1*8(a3)",
+        "csrr a4, mtval",
+        "sd a4, 2*8(a3)",
+        "csrr a4, mepc",
+        "addi a4, a4, 4",
+        "csrw mepc, a4",
+        "mret",
+    )
 }
 
 pub(crate) unsafe fn csr_read_allow<const CSR_NUM: u16>(trap_info: *mut TrapInfo) -> usize {

prototyper/prototyper/src/sbi/features.rs

@@ -9,23 +9,18 @@ use crate::sbi::trap_stack::{hart_context, hart_context_mut};
 use super::early_trap::csr_swap;
 
 pub struct HartFeatures {
-    extension: [bool; Extension::COUNT],
+    extensions: [bool; Extension::COUNT],
     privileged_version: PrivilegedVersion,
     mhpm_mask: u32,
     mhpm_bits: u32,
 }
 
 impl HartFeatures {
-    pub fn privileged_version(&self) -> PrivilegedVersion {
+    pub const fn privileged_version(&self) -> PrivilegedVersion {
         self.privileged_version
     }
 }
 
-#[derive(Copy, Clone)]
-pub enum Extension {
-    Sstc = 0,
-}
-
 #[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
 pub enum PrivilegedVersion {
     Unknown = 0,
@@ -34,57 +29,100 @@ pub enum PrivilegedVersion {
     Version1_12 = 3,
 }
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub enum Extension {
+    Sstc = 0,
+    Hypervisor = 1,
+}
+
 impl Extension {
-    const COUNT: usize = 1;
-    const ITER: [Self; Extension::COUNT] = [Extension::Sstc];
+    pub const COUNT: usize = 2;
 
-    pub fn as_str(&self) -> &'static str {
+    pub const fn as_str(&self) -> &'static str {
         match self {
-            Extension::Sstc => "sstc",
+            Self::Sstc => "sstc",
+            Self::Hypervisor => "h",
         }
     }
 
     #[inline]
-    pub fn index(&self) -> usize {
+    pub const fn index(&self) -> usize {
         *self as usize
     }
+
+    pub fn iter() -> impl Iterator<Item = Self> {
+        [Self::Sstc, Self::Hypervisor].into_iter()
+    }
 }
 
-/// access hart feature
+/// Probes if a specific extension is supported for the given hart.
+#[inline]
 pub fn hart_extension_probe(hart_id: usize, ext: Extension) -> bool {
-    hart_context(hart_id).features.extension[ext.index()]
+    hart_context(hart_id).features.extensions[ext.index()]
 }
 
+/// Gets the privileged version for the given hart.
+#[inline]
 pub fn hart_privileged_version(hart_id: usize) -> PrivilegedVersion {
     hart_context(hart_id).features.privileged_version
 }
 
+/// Gets the MHPM mask for the given hart.
+#[inline]
 pub fn hart_mhpm_mask(hart_id: usize) -> u32 {
     hart_context(hart_id).features.mhpm_mask
 }
 
-/// Hart features detection
+/// Detects RISC-V extensions from the device tree for all harts.
 #[cfg(not(feature = "nemu"))]
 pub fn extension_detection(cpus: &NodeSeq) {
     use crate::devicetree::Cpu;
+
     for cpu_iter in cpus.iter() {
-        let cpu = cpu_iter.deserialize::<Cpu>();
-        let hart_id = cpu.reg.iter().next().unwrap().0.start;
-        let mut hart_exts = [false; Extension::COUNT];
-        if cpu.isa_extensions.is_some() {
-            let isa = cpu.isa_extensions.unwrap();
-            Extension::ITER.iter().for_each(|ext| {
-                hart_exts[ext.index()] = isa.iter().any(|e| e == ext.as_str());
-            });
-        } else if cpu.isa.is_some() {
-            let isa_iter = cpu.isa.unwrap();
-            let isa = isa_iter.iter().next().unwrap_or_default();
-            Extension::ITER.iter().for_each(|ext| {
-                hart_exts[ext.index()] = isa.contains(ext.as_str());
-            })
+        let cpu_data = cpu_iter.deserialize::<Cpu>();
+        let hart_id = cpu_data.reg.iter().next().unwrap().0.start;
+        let mut extensions = [false; Extension::COUNT];
+
+        for ext in Extension::iter() {
+            let ext_index = ext.index();
+            let ext_name = ext.as_str();
+
+            let dt_supported = check_extension_in_device_tree(ext_name, &cpu_data);
+            extensions[ext_index] = match ext {
+                Extension::Hypervisor if hart_id == current_hartid() => {
+                    let misa_supported = unsafe { misa_check_hypervisor_extension() };
+                    if dt_supported != misa_supported {
+                        warn!(
+                            "Device tree and MISA disagree on 'H' support for hart {}",
+                            hart_id
+                        );
+                    }
+                    misa_supported
+                }
+                _ => dt_supported,
+            };
         }
-        hart_context_mut(hart_id).features.extension = hart_exts;
+
+        hart_context_mut(hart_id).features.extensions = extensions;
+    }
+}
+
+fn check_extension_in_device_tree(ext: &str, cpu: &crate::devicetree::Cpu) -> bool {
+    // Check isa-extensions first (preferred, list of strings)
+    if let Some(isa_exts) = &cpu.isa_extensions {
+        return isa_exts.iter().any(|e| e == ext);
     }
+
+    // Fallback to isa (take first string, default to empty)
+    cpu.isa
+        .iter()
+        .next()
+        .and_then(|isa| isa.iter().next())
+        .map(|isa| {
+            isa.split('_')
+                .any(|part| part == ext || (ext.len() == 1 && part.contains(ext)))
+        })
+        .unwrap_or(false)
 }
 
 fn privileged_version_detection() {
@@ -139,6 +177,15 @@ fn mhpm_detection() {
     hart_context_mut(current_hartid()).features.mhpm_bits = 64;
 }
 
+/// Checks if the Hypervisor ('H') extension is supported via the `misa` CSR.
+pub unsafe fn misa_check_hypervisor_extension() -> bool {
+    let misa_val: usize;
+    unsafe {
+        core::arch::asm!("csrr {}, misa", out(reg) misa_val, options(nomem, nostack));
+    }
+    misa_val != 0 && (misa_val >> 7) & 1 != 0 // H extension bit is at position 7
+}
+
 pub fn hart_features_detection() {
     privileged_version_detection();
     mhpm_detection();

prototyper/prototyper/src/sbi/pmu.rs

@@ -1072,30 +1072,59 @@ impl Iterator for CounterMask {
 
 // TODO: If all firmware events are implemented,
 // `PMU_FIRMWARE_EVENT_SUPPORTED` should be removed.
-const PMU_FIRMWARE_EVENT_SUPPORTED: [bool; 22] = [
-    true,  // SBI_PMU_FW_MISALIGNED_LOAD
-    true,  // SBI_PMU_FW_MISALIGNED_STORE
-    false, // SBI_PMU_FW_ACCESS_LOAD
-    false, // SBI_PMU_FW_ACCESS_STORE
-    true,  // SBI_PMU_FW_ILLEGAL_INSN
-    true,  // SBI_PMU_FW_SET_TIMER
-    true,  // SBI_PMU_FW_IPI_SENT
-    true,  // SBI_PMU_FW_IPI_RECEIVED
-    true,  // SBI_PMU_FW_FENCE_I_SENT
-    true,  // SBI_PMU_FW_FENCE_I_RECEIVED
-    true,  // SBI_PMU_FW_SFENCE_VMA_SENT
-    true,  // SBI_PMU_FW_SFENCE_VMA_RECEIVED
-    true,  // SBI_PMU_FW_SFENCE_VMA_ASID_SENT
-    true,  // SBI_PMU_FW_SFENCE_VMA_ASID_RECEIVED
-    false, // SBI_PMU_FW_HFENCE_GVMA_SENT
-    false, // SBI_PMU_FW_HFENCE_GVMA_RECEIVED
-    false, // SBI_PMU_FW_HFENCE_GVMA_VMID_SENT
-    false, // SBI_PMU_FW_HFENCE_GVMA_VMID_RECEIVED
-    false, // SBI_PMU_FW_HFENCE_VVMA_SENT
-    false, // SBI_PMU_FW_HFENCE_VVMA_RECEIVED
-    false, // SBI_PMU_FW_HFENCE_VVMA_ASID_SENT
-    false, // SBI_PMU_FW_HFENCE_VVMA_ASID_RECEIVED
-];
+cfg_if::cfg_if! {
+    if #[cfg(feature = "hypervisor")] {
+        const PMU_FIRMWARE_EVENT_SUPPORTED: [bool; 22] = [
+            true,  // SBI_PMU_FW_MISALIGNED_LOAD
+            true,  // SBI_PMU_FW_MISALIGNED_STORE
+            false, // SBI_PMU_FW_ACCESS_LOAD
+            false, // SBI_PMU_FW_ACCESS_STORE
+            true,  // SBI_PMU_FW_ILLEGAL_INSN
+            true,  // SBI_PMU_FW_SET_TIMER
+            true,  // SBI_PMU_FW_IPI_SENT
+            true,  // SBI_PMU_FW_IPI_RECEIVED
+            true,  // SBI_PMU_FW_FENCE_I_SENT
+            true,  // SBI_PMU_FW_FENCE_I_RECEIVED
+            true,  // SBI_PMU_FW_SFENCE_VMA_SENT
+            true,  // SBI_PMU_FW_SFENCE_VMA_RECEIVED
+            true,  // SBI_PMU_FW_SFENCE_VMA_ASID_SENT
+            true,  // SBI_PMU_FW_SFENCE_VMA_ASID_RECEIVED
+            true,  // SBI_PMU_FW_HFENCE_GVMA_SENT
+            true,  // SBI_PMU_FW_HFENCE_GVMA_RECEIVED
+            true,  // SBI_PMU_FW_HFENCE_GVMA_VMID_SENT
+            true,  // SBI_PMU_FW_HFENCE_GVMA_VMID_RECEIVED
+            true,  // SBI_PMU_FW_HFENCE_VVMA_SENT
+            true,  // SBI_PMU_FW_HFENCE_VVMA_RECEIVED
+            true,  // SBI_PMU_FW_HFENCE_VVMA_ASID_SENT
+            true,  // SBI_PMU_FW_HFENCE_VVMA_ASID_RECEIVED
+        ];
+    } else {
+        const PMU_FIRMWARE_EVENT_SUPPORTED: [bool; 22] = [
+            true,  // SBI_PMU_FW_MISALIGNED_LOAD
+            true,  // SBI_PMU_FW_MISALIGNED_STORE
+            false, // SBI_PMU_FW_ACCESS_LOAD
+            false, // SBI_PMU_FW_ACCESS_STORE
+            true,  // SBI_PMU_FW_ILLEGAL_INSN
+            true,  // SBI_PMU_FW_SET_TIMER
+            true,  // SBI_PMU_FW_IPI_SENT
+            true,  // SBI_PMU_FW_IPI_RECEIVED
+            true,  // SBI_PMU_FW_FENCE_I_SENT
+            true,  // SBI_PMU_FW_FENCE_I_RECEIVED
+            true,  // SBI_PMU_FW_SFENCE_VMA_SENT
+            true,  // SBI_PMU_FW_SFENCE_VMA_RECEIVED
+            true,  // SBI_PMU_FW_SFENCE_VMA_ASID_SENT
+            true,  // SBI_PMU_FW_SFENCE_VMA_ASID_RECEIVED
+            false, // SBI_PMU_FW_HFENCE_GVMA_SENT
+            false, // SBI_PMU_FW_HFENCE_GVMA_RECEIVED
+            false, // SBI_PMU_FW_HFENCE_GVMA_VMID_SENT
+            false, // SBI_PMU_FW_HFENCE_GVMA_VMID_RECEIVED
+            false, // SBI_PMU_FW_HFENCE_VVMA_SENT
+            false, // SBI_PMU_FW_HFENCE_VVMA_RECEIVED
+            false, // SBI_PMU_FW_HFENCE_VVMA_ASID_SENT
+            false, // SBI_PMU_FW_HFENCE_VVMA_ASID_RECEIVED
+        ];
+    }
+}
 
 pub fn pmu_firmware_counter_increment(firmware_event: usize) {
     let pmu_state = &mut hart_context_mut(current_hartid()).pmu_state;

prototyper/prototyper/src/sbi/rfence.rs

@@ -39,7 +39,7 @@ pub struct RFenceContext {
 
 /// Types of remote fence operations supported.
 #[allow(unused)]
-#[derive(Clone, Copy, Debug)]
+#[derive(Clone, Copy, Debug, PartialEq, Eq)]
 pub enum RFenceType {
     /// Instruction fence.
     FenceI,
@@ -47,13 +47,17 @@ pub enum RFenceType {
     SFenceVma,
     /// Supervisor fence for virtual memory with ASID.
     SFenceVmaAsid,
+    #[cfg(feature = "hypervisor")]
     /// Hypervisor fence for guest virtual memory with VMID.
     HFenceGvmaVmid,
+    #[cfg(feature = "hypervisor")]
     /// Hypervisor fence for guest virtual memory.
     HFenceGvma,
-    /// Hypervisor fence for virtual machine virtual memory with ASID.
+    #[cfg(feature = "hypervisor")]
+    /// Hypervisor fence for guest virtual memory with ASID.
     HFenceVvmaAsid,
-    /// Hypervisor fence for virtual machine virtual memory.
+    #[cfg(feature = "hypervisor")]
+    /// Hypervisor fence for guest virtual memory.
     HFenceVvma,
 }
 
@@ -176,13 +180,13 @@ pub(crate) struct SbiRFence;
 /// Validates address range for fence operations
 #[inline(always)]
 fn validate_address_range(start_addr: usize, size: usize) -> Result<usize, SbiRet> {
-    // Check page alignment using bitwise AND instead of modulo
-    if start_addr & 0xFFF != 0 {
-        return Err(SbiRet::invalid_address());
+    if !((start_addr == 0 && size == 0) || size == usize::MAX) {
+        if start_addr & (PAGE_SIZE - 1) != 0 {
+            return Err(SbiRet::invalid_address());
+        }
     }
 
-    // Avoid checked_add by checking for overflow directly
-    if size > usize::MAX - start_addr {
+    if start_addr > usize::MAX - size {
         return Err(SbiRet::invalid_address());
     }
 
@@ -198,6 +202,11 @@ fn remote_fence_process(rfence_ctx: RFenceContext, hart_mask: HartMask) -> SbiRe
     sbi_ret
 }
 
+#[cfg(feature = "hypervisor")]
+fn supports_hypervisor_extension() -> bool {
+    super::features::hart_extension_probe(current_hartid(), super::features::Extension::Hypervisor)
+}
+
 impl rustsbi::Fence for SbiRFence {
     /// Remote instruction fence for specified harts.
     fn remote_fence_i(&self, hart_mask: HartMask) -> SbiRet {
@@ -259,71 +268,232 @@ impl rustsbi::Fence for SbiRFence {
             hart_mask,
         )
     }
+
+    #[cfg(feature = "hypervisor")]
+    fn remote_hfence_gvma_vmid(
+        &self,
+        hart_mask: HartMask,
+        start_addr: usize,
+        size: usize,
+        vmid: usize,
+    ) -> SbiRet {
+        if !supports_hypervisor_extension() {
+            return SbiRet::not_supported();
+        }
+        pmu_firmware_counter_increment(firmware_event::HFENCE_GVMA_VMID_SENT);
+
+        let flush_size = match validate_address_range(start_addr, size) {
+            Ok(s) => s,
+            Err(e) => return e,
+        };
+
+        remote_fence_process(
+            RFenceContext {
+                start_addr,
+                size: flush_size,
+                asid: 0,
+                vmid,
+                op: RFenceType::HFenceGvmaVmid,
+            },
+            hart_mask,
+        )
+    }
+
+    #[cfg(feature = "hypervisor")]
+    fn remote_hfence_gvma(&self, hart_mask: HartMask, start_addr: usize, size: usize) -> SbiRet {
+        if !supports_hypervisor_extension() {
+            return SbiRet::not_supported();
+        }
+        pmu_firmware_counter_increment(firmware_event::HFENCE_GVMA_SENT);
+
+        let flush_size = match validate_address_range(start_addr, size) {
+            Ok(s) => s,
+            Err(e) => return e,
+        };
+
+        remote_fence_process(
+            RFenceContext {
+                start_addr,
+                size: flush_size,
+                asid: 0,
+                vmid: 0,
+                op: RFenceType::HFenceGvma,
+            },
+            hart_mask,
+        )
+    }
+
+    #[cfg(feature = "hypervisor")]
+    fn remote_hfence_vvma_asid(
+        &self,
+        hart_mask: HartMask,
+        start_addr: usize,
+        size: usize,
+        asid: usize,
+    ) -> SbiRet {
+        if !supports_hypervisor_extension() {
+            return SbiRet::not_supported();
+        }
+        pmu_firmware_counter_increment(firmware_event::HFENCE_VVMA_ASID_SENT);
+
+        let flush_size = match validate_address_range(start_addr, size) {
+            Ok(s) => s,
+            Err(e) => return e,
+        };
+
+        remote_fence_process(
+            RFenceContext {
+                start_addr,
+                size: flush_size,
+                asid,
+                vmid: 0,
+                op: RFenceType::HFenceVvmaAsid,
+            },
+            hart_mask,
+        )
+    }
+
+    #[cfg(feature = "hypervisor")]
+    fn remote_hfence_vvma(&self, hart_mask: HartMask, start_addr: usize, size: usize) -> SbiRet {
+        if !supports_hypervisor_extension() {
+            return SbiRet::not_supported();
+        }
+        pmu_firmware_counter_increment(firmware_event::HFENCE_VVMA_SENT);
+
+        let flush_size = match validate_address_range(start_addr, size) {
+            Ok(s) => s,
+            Err(e) => return e,
+        };
+
+        remote_fence_process(
+            RFenceContext {
+                start_addr,
+                size: flush_size,
+                asid: 0,
+                vmid: 0,
+                op: RFenceType::HFenceVvma,
+            },
+            hart_mask,
+        )
+    }
 }
 
 /// Handles a single remote fence operation.
 #[inline]
 pub fn rfence_single_handler() {
-    let rfence_context = local_rfence().unwrap().get();
-    if let Some((ctx, id)) = rfence_context {
+    let local_rf = match local_rfence() {
+        Some(lr) => lr,
+        // TODO: Or return an error, depending on expected invariants
+        None => panic!("rfence_single_handler called with no local rfence context"),
+    };
+
+    if let Some((ctx, source_hart_id)) = local_rf.get() {
+        let full_flush = (ctx.start_addr == 0 && ctx.size == 0)
+            || (ctx.size == usize::MAX)
+            || (ctx.size > TLB_FLUSH_LIMIT && ctx.size != usize::MAX);
+
         match ctx.op {
-            // Handle instruction fence
-            RFenceType::FenceI => unsafe {
+            RFenceType::FenceI => {
                 pmu_firmware_counter_increment(firmware_event::FENCE_I_RECEIVED);
-                asm!("fence.i");
-                remote_rfence(id).unwrap().sub();
-            },
-            // Handle virtual memory address fence
+                unsafe { asm!("fence.i") };
+                remote_rfence(source_hart_id).unwrap().sub();
+            }
             RFenceType::SFenceVma => {
                 pmu_firmware_counter_increment(firmware_event::SFENCE_VMA_RECEIVED);
-                // If the flush size is greater than the maximum limit then simply flush all
-                if (ctx.start_addr == 0 && ctx.size == 0)
-                    || (ctx.size == usize::MAX)
-                    || (ctx.size > TLB_FLUSH_LIMIT)
-                {
-                    unsafe {
-                        asm!("sfence.vma");
-                    }
+                if full_flush {
+                    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);
-                        }
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("sfence.vma {}", in(reg) addr) };
                     }
                 }
-                remote_rfence(id).unwrap().sub();
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
             }
-            // Handle virtual memory address fence with ASID
             RFenceType::SFenceVmaAsid => {
                 pmu_firmware_counter_increment(firmware_event::SFENCE_VMA_ASID_RECEIVED);
                 let asid = ctx.asid;
-                // If the flush size is greater than the maximum limit then simply flush all
-                if (ctx.start_addr == 0 && ctx.size == 0)
-                    || (ctx.size == usize::MAX)
-                    || (ctx.size > TLB_FLUSH_LIMIT)
-                {
-                    unsafe {
-                        asm!("sfence.vma x0, {}", in(reg) asid);
+                if full_flush {
+                    unsafe { asm!("sfence.vma x0, {}", in(reg) asid) };
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("sfence.vma {}, {}", in(reg) addr, in(reg) asid) };
+                    }
+                }
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
+            }
+            #[cfg(feature = "hypervisor")]
+            RFenceType::HFenceGvmaVmid => {
+                pmu_firmware_counter_increment(firmware_event::HFENCE_GVMA_VMID_RECEIVED);
+                let vmid = ctx.vmid;
+                if full_flush {
+                    unsafe { asm!("hfence.gvma x0, {}", in(reg) vmid) };
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("hfence.gvma {}, {}", in(reg) addr, in(reg) vmid) };
+                    }
+                }
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
+            }
+            #[cfg(feature = "hypervisor")]
+            RFenceType::HFenceGvma => {
+                pmu_firmware_counter_increment(firmware_event::HFENCE_GVMA_RECEIVED);
+                if full_flush {
+                    unsafe { asm!("hfence.gvma x0, x0") };
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("hfence.gvma {}, x0", in(reg) addr) };
                     }
+                }
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
+            }
+            #[cfg(feature = "hypervisor")]
+            RFenceType::HFenceVvmaAsid => {
+                pmu_firmware_counter_increment(firmware_event::HFENCE_VVMA_ASID_RECEIVED);
+                let asid = ctx.asid;
+                if full_flush {
+                    unsafe { asm!("hfence.vvma x0, {}", 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);
-                        }
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("hfence.vvma {}, {}", in(reg) addr, in(reg) asid) };
                     }
                 }
-                remote_rfence(id).unwrap().sub();
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
             }
-            rfencetype => {
-                error!("Unsupported RFence Type: {:?}!", rfencetype);
+            #[cfg(feature = "hypervisor")]
+            RFenceType::HFenceVvma => {
+                pmu_firmware_counter_increment(firmware_event::HFENCE_VVMA_RECEIVED);
+                if full_flush {
+                    unsafe { asm!("hfence.vvma x0, x0") };
+                } else {
+                    for offset in (0..ctx.size).step_by(PAGE_SIZE) {
+                        let addr = ctx.start_addr.wrapping_add(offset);
+                        unsafe { asm!("hfence.vvma {}, x0", in(reg) addr) };
+                    }
+                }
+                if let Some(remote_cell) = remote_rfence(source_hart_id) {
+                    remote_cell.sub();
+                }
             }
         }
     }
 }
 
-/// Process all pending remote fence operations.
+/// Process all pending remote fence operations on the current hart.
 #[inline]
 pub fn rfence_handler() {
     while !local_rfence().unwrap().is_empty() {

prototyper/prototyper/src/sbi/trap/boot.rs

@@ -8,35 +8,33 @@ use riscv::register::{mie, mstatus, satp, sstatus};
 /// Boot Function.
 /// After boot, this flow will never back again,
 /// so we can store a0, a1 and mepc only.
-#[naked]
+#[unsafe(naked)]
 pub unsafe extern "C" fn boot() -> ! {
-    unsafe {
-        naked_asm!(
-            ".align 2",
-            // Reset hart local stack
-            "call    {locate_stack}",
-            "csrw    mscratch, sp",
-            // Allocate stack space
-            "addi   sp, sp, -3*8",
-            // Call handler with context pointer
-            "mv     a0, sp",
-            "call   {boot_handler}",
-            // Restore mepc
-            "ld     t0, 0*8(sp)
-            csrw    mepc, t0",
-            // Restore registers
-            "ld      a0, 1*8(sp)",
-            "ld      a1, 2*8(sp)",
-            // Restore stack pointer
-            "add     sp, sp, 3*8",
-            // Switch stacks back
-            "csrrw  sp, mscratch, sp",
-            // Return from machine mode
-            "mret",
-            locate_stack = sym trap_stack::locate,
-            boot_handler = sym boot_handler,
-        );
-    }
+    naked_asm!(
+        ".align 2",
+        // Reset hart local stack
+        "call    {locate_stack}",
+        "csrw    mscratch, sp",
+        // Allocate stack space
+        "addi   sp, sp, -3*8",
+        // Call handler with context pointer
+        "mv     a0, sp",
+        "call   {boot_handler}",
+        // Restore mepc
+        "ld     t0, 0*8(sp)
+        csrw    mepc, t0",
+        // Restore registers
+        "ld      a0, 1*8(sp)",
+        "ld      a1, 2*8(sp)",
+        // Restore stack pointer
+        "add     sp, sp, 3*8",
+        // Switch stacks back
+        "csrrw  sp, mscratch, sp",
+        // Return from machine mode
+        "mret",
+        locate_stack = sym trap_stack::locate,
+        boot_handler = sym boot_handler,
+    );
 }
 
 /// Boot Handler.

prototyper/prototyper/src/sbi/trap_stack.rs

@@ -12,11 +12,10 @@ pub(crate) static mut ROOT_STACK: [Stack; NUM_HART_MAX] = [Stack::ZERO; NUM_HART
 /// Locates and initializes stack for each hart.
 ///
 /// This is a naked function that sets up the stack pointer based on hart ID.
-#[naked]
+#[unsafe(naked)]
 pub(crate) unsafe extern "C" fn locate() {
-    unsafe {
-        core::arch::naked_asm!(
-            "   la   sp, {stack}            // Load stack base address
+    core::arch::naked_asm!(
+        "   la   sp, {stack}            // Load stack base address
             li   t0, {per_hart_stack_size} // Load stack size per hart
             csrr t1, mhartid            // Get current hart ID
             addi t1, t1,  1             // Add 1 to hart ID
@@ -26,11 +25,10 @@ pub(crate) unsafe extern "C" fn locate() {
             call t1, {move_stack}       // Call stack reuse function
             ret                         // Return
         ",
-            per_hart_stack_size = const STACK_SIZE_PER_HART,
-            stack               =   sym ROOT_STACK,
-            move_stack          =   sym fast_trap::reuse_stack_for_trap,
-        )
-    }
+        per_hart_stack_size = const STACK_SIZE_PER_HART,
+        stack               =   sym ROOT_STACK,
+        move_stack          =   sym fast_trap::reuse_stack_for_trap,
+    )
 }
 
 /// Prepares trap stack for current hart

prototyper/test-kernel/src/main.rs

@@ -1,6 +1,5 @@
 #![no_std]
 #![no_main]
-#![feature(naked_functions)]
 #![allow(static_mut_refs)]
 
 #[macro_use]
@@ -25,39 +24,32 @@ const RISCV_IMAGE_MAGIC: u64 = 0x5643534952; /* Magic number, little endian, "RI
 const RISCV_IMAGE_MAGIC2: u32 = 0x05435352; /* Magic number 2, little endian, "RSC\x05" */
 
 /// boot header
-#[naked]
+#[unsafe(naked)]
 #[unsafe(no_mangle)]
 #[unsafe(link_section = ".head.text")]
 unsafe extern "C" fn _boot_header() -> ! {
-    unsafe {
-        naked_asm!(
-            "j _start",
-            ".word 0",
-            ".balign 8",
-            ".dword 0x200000",
-            ".dword iend - istart",
-            ".dword {RISCV_HEAD_FLAGS}",
-            ".word  {RISCV_HEADER_VERSION}",
-            ".word  0",
-            ".dword 0",
-            ".dword {RISCV_IMAGE_MAGIC}",
-            ".balign 4",
-            ".word  {RISCV_IMAGE_MAGIC2}",
-            ".word  0",
-            RISCV_HEAD_FLAGS = const RISCV_HEAD_FLAGS,
-            RISCV_HEADER_VERSION = const RISCV_HEADER_VERSION,
-            RISCV_IMAGE_MAGIC = const RISCV_IMAGE_MAGIC,
-            RISCV_IMAGE_MAGIC2 = const RISCV_IMAGE_MAGIC2,
-        );
-    }
+    naked_asm!(
+        "j _start",
+        ".word 0",
+        ".balign 8",
+        ".dword 0x200000",
+        ".dword iend - istart",
+        ".dword {RISCV_HEAD_FLAGS}",
+        ".word  {RISCV_HEADER_VERSION}",
+        ".word  0",
+        ".dword 0",
+        ".dword {RISCV_IMAGE_MAGIC}",
+        ".balign 4",
+        ".word  {RISCV_IMAGE_MAGIC2}",
+        ".word  0",
+        RISCV_HEAD_FLAGS = const RISCV_HEAD_FLAGS,
+        RISCV_HEADER_VERSION = const RISCV_HEADER_VERSION,
+        RISCV_IMAGE_MAGIC = const RISCV_IMAGE_MAGIC,
+        RISCV_IMAGE_MAGIC2 = const RISCV_IMAGE_MAGIC2,
+    );
 }
 
-/// 内核入口。
-///
-/// # Safety
-///
-/// 裸函数。
-#[naked]
+#[unsafe(naked)]
 #[unsafe(no_mangle)]
 #[unsafe(link_section = ".text.entry")]
 unsafe extern "C" fn _start(hartid: usize, device_tree_paddr: usize) -> ! {
@@ -66,23 +58,21 @@ unsafe extern "C" fn _start(hartid: usize, device_tree_paddr: usize) -> ! {
     #[unsafe(link_section = ".bss.uninit")]
     static mut STACK: [u8; STACK_SIZE] = [0u8; STACK_SIZE];
 
-    unsafe {
-        naked_asm!(
-            // clear bss segment
-            "   la      t0, sbss
+    naked_asm!(
+        // clear bss segment
+        "   la      t0, sbss
             la      t1, ebss
         1:  bgeu    t0, t1, 2f
             sd      zero, 0(t0)
             addi    t0, t0, 8
             j       1b",
-            "2:",
-            "   la sp, {stack} + {stack_size}",
-            "   j  {main}",
-            stack_size = const STACK_SIZE,
-            stack      =   sym STACK,
-            main       =   sym rust_main,
-        )
-    }
+        "2:",
+        "   la sp, {stack} + {stack_size}",
+        "   j  {main}",
+        stack_size = const STACK_SIZE,
+        stack      =   sym STACK,
+        main       =   sym rust_main,
+    )
 }
 
 extern "C" fn rust_main(hartid: usize, dtb_pa: usize) -> ! {
@@ -117,6 +107,7 @@ extern "C" fn rust_main(hartid: usize, dtb_pa: usize) -> ! {
     let test_result = testing.test();
 
     pmu_test();
+    fence_test();
 
     if test_result {
         sbi::system_reset(sbi::Shutdown, sbi::NoReason);
@@ -292,6 +283,35 @@ fn pmu_test() {
     assert_eq!(ipi_num.value, 27);
 }
 
+#[inline]
+// Fence and HFence test
+fn fence_test() {
+    // Fence.i test (should succeed, no-op for PMU, but should not panic)
+    let ret = sbi::remote_fence_i(HartMask::from_mask_base(0x1, 0));
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    // SFence.vma test (should succeed, no-op for PMU, but should not panic)
+    let ret = sbi::remote_sfence_vma(HartMask::from_mask_base(0x1, 0), 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    // SFence.vma with asid test
+    let ret = sbi::remote_sfence_vma_asid(HartMask::from_mask_base(0x1, 0), 0, 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    // HFence tests (if supported)
+    let ret = sbi::remote_hfence_gvma(HartMask::from_mask_base(0x1, 0), 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    let ret = sbi::remote_hfence_gvma_vmid(HartMask::from_mask_base(0x1, 0), 0, 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    let ret = sbi::remote_hfence_vvma(HartMask::from_mask_base(0x1, 0), 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+
+    let ret = sbi::remote_hfence_vvma_asid(HartMask::from_mask_base(0x1, 0), 0, 0, 0);
+    assert!(ret.is_ok() || ret == SbiRet::not_supported());
+}
+
 #[cfg_attr(not(test), panic_handler)]
 fn panic(info: &core::panic::PanicInfo) -> ! {
     let (hart_id, pc): (usize, usize);

rust-toolchain.toml

@@ -1,6 +1,4 @@
 [toolchain]
 profile = "minimal"
-# channel = "nightly-2025-02-08"
 channel = "nightly"
-components = ["rustfmt", "llvm-tools-preview", "clippy", "rust-src"]
-# targets = ["riscv64imac-unknown-none-elf"]
+components = ["rustfmt", "clippy", "llvm-tools-preview", "rust-src"]

xtask/src/prototyper.rs

@@ -89,12 +89,19 @@ fn setup_config_file(target_config_toml: &PathBuf, arg: &PrototyperArg) -> Optio
 fn build_prototyper(arg: &PrototyperArg) -> Option<ExitStatus> {
     info!("Building Prototyper");
 
+    let enable_h = arg.features.iter().any(|f| f == "hypervisor");
+    let rustflags = if enable_h {
+        "-C relocation-model=pie -C link-arg=-pie -C target-feature=+h"
+    } else {
+        "-C relocation-model=pie -C link-arg=-pie"
+    };
+
     // Build the prototyper
     let status = cargo::Cargo::new("build")
         .package(PACKAGE_NAME)
         .target(ARCH)
         .unstable("build-std", ["core", "alloc"])
-        .env("RUSTFLAGS", "-C relocation-model=pie -C link-arg=-pie")
+        .env("RUSTFLAGS", rustflags)
         .features(&arg.features)
         .optional(arg.fdt.is_some(), |cargo| {
             cargo.env("PROTOTYPER_FDT_PATH", arg.fdt.as_ref().unwrap());
@@ -137,9 +144,9 @@ fn build_prototyper(arg: &PrototyperArg) -> Option<ExitStatus> {
     if result.is_none() {
         error!(
             "Failed to execute rust-objcopy. Command not found or failed to start.\n\
-             Source: {}\n\
-             Destination: {}\n\
-             Please install cargo-binutils with cmd: cargo install cargo-binutils",
+            Source: {}\n\
+            Destination: {}\n\
+            Please install cargo-binutils with cmd: cargo install cargo-binutils",
             elf_path.display(),
             bin_path.display()
         );