feat(prototyper): detectoin privileged version

Signed-off-by: Woshiluo Luo <woshiluo.luo@outlook.com>

prototyper/src/board.rs

@@ -100,7 +100,7 @@ impl Board {
     }
 
     pub fn have_ipi(&self) -> bool {
-        self.sbi.ipi.is_some() 
+        self.sbi.ipi.is_some()
     }
 
     pub fn have_hsm(&self) -> bool {
@@ -108,7 +108,7 @@ impl Board {
     }
 
     pub fn have_rfence(&self) -> bool {
-        self.sbi.rfence.is_some() 
+        self.sbi.rfence.is_some()
     }
 
     pub fn ready(&self) -> bool {
@@ -143,7 +143,8 @@ impl Board {
                             return true;
                         }
                         if UART16650U32_COMPATIBLE.contains(&device_id) {
-                            self.info.console = Some((regs.start, MachineConsoleType::Uart16550U32));
+                            self.info.console =
+                                Some((regs.start, MachineConsoleType::Uart16550U32));
                             return true;
                         }
                         if UARTAXILITE_COMPATIBLE.contains(&device_id) {

prototyper/src/macros.rs

@@ -24,3 +24,27 @@ macro_rules! println {
         }
     }}
 }
+
+#[allow(unused)]
+macro_rules! csr_test {
+    ($($x: expr)*) => {{
+            use core::arch::asm;
+            use riscv::register::mtvec;
+            let res: usize;
+            unsafe {
+                // Backup old mtvec
+                let mtvec = mtvec::read().bits();
+                // Write expected_trap
+                mtvec::write(expected_trap as _, mtvec::TrapMode::Direct);
+                asm!("addi a0, zero, 0",
+                    "addi a1, zero, 0",
+                    "csrr a2, {}",
+                    "mv {}, a0",
+                    const $($x)*,
+                    out(reg) res,
+                    options(nomem));
+                asm!("csrw mtvec, {}", in(reg) mtvec);
+            }
+            res == 0
+    }};
+}

prototyper/src/main.rs

@@ -20,7 +20,10 @@ use core::arch::asm;
 
 use crate::board::BOARD;
 use crate::riscv_spec::{current_hartid, menvcfg};
-use crate::sbi::extensions::{hart_extension_probe, Extension};
+use crate::sbi::extensions::{
+    hart_extension_probe, hart_privileged_version, privileged_version_detection, Extension,
+    PrivilegedVersion,
+};
 use crate::sbi::hart_context::NextStage;
 use crate::sbi::hsm::local_remote_hsm;
 use crate::sbi::ipi;
@@ -77,6 +80,8 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
         firmware::set_pmp(unsafe { BOARD.info.memory_range.as_ref().unwrap() });
     }
 
+    // Detection Priv Ver
+    privileged_version_detection();
     // Clear all pending IPIs.
     ipi::clear_all();
 
@@ -90,14 +95,16 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
         // Keep supervisor environment calls and illegal instructions in M-mode.
         medeleg::clear_supervisor_env_call();
         medeleg::clear_illegal_instruction();
-        // Configure environment features based on available extensions.
-        // if hart_extension_probe(current_hartid(), Extension::Sstc) {
-        //     menvcfg::set_bits(
-        //         menvcfg::STCE | menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE,
-        //     );
-        // } else {
-        //     menvcfg::set_bits(menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE);
-        // }
+        if hart_privileged_version(current_hartid()) >= PrivilegedVersion::Version1_12 {
+            // Configure environment features based on available extensions.
+            if hart_extension_probe(current_hartid(), Extension::Sstc) {
+                menvcfg::set_bits(
+                    menvcfg::STCE | menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE,
+                );
+            } else {
+                menvcfg::set_bits(menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE);
+            }
+        }
         // Set up vectored trap handling.
         mtvec::write(trap_vec as _, mtvec::TrapMode::Vectored);
     }

prototyper/src/sbi/extensions.rs

@@ -1,13 +1,27 @@
 use serde_device_tree::buildin::NodeSeq;
 
+use crate::riscv_spec::current_hartid;
+use crate::sbi::trap::expected_trap;
 use crate::sbi::trap_stack::ROOT_STACK;
-pub struct HartExtensions([bool; Extension::COUNT]);
+
+pub struct HartFeatures {
+    extension: [bool; Extension::COUNT],
+    privileged_version: PrivilegedVersion,
+}
 
 #[derive(Copy, Clone)]
 pub enum Extension {
     Sstc = 0,
 }
 
+#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord)]
+pub enum PrivilegedVersion {
+    Unknown = 0,
+    Version1_10 = 1,
+    Version1_11 = 2,
+    Version1_12 = 3,
+}
+
 impl Extension {
     const COUNT: usize = 1;
     const ITER: [Self; Extension::COUNT] = [Extension::Sstc];
@@ -28,7 +42,16 @@ pub fn hart_extension_probe(hart_id: usize, ext: Extension) -> bool {
     unsafe {
         ROOT_STACK
             .get_mut(hart_id)
-            .map(|x| x.hart_context().extensions.0[ext.index()])
+            .map(|x| x.hart_context().features.extension[ext.index()])
+            .unwrap()
+    }
+}
+
+pub fn hart_privileged_version(hart_id: usize) -> PrivilegedVersion {
+    unsafe {
+        ROOT_STACK
+            .get_mut(hart_id)
+            .map(|x| x.hart_context().features.privileged_version)
             .unwrap()
     }
 }
@@ -56,11 +79,35 @@ pub fn init(cpus: &NodeSeq) {
         unsafe {
             ROOT_STACK
                 .get_mut(hart_id)
-                .map(|stack| stack.hart_context().extensions = HartExtensions(hart_exts))
+                .map(|stack| stack.hart_context().features.extension = hart_exts)
                 .unwrap()
         }
     }
 }
+pub fn privileged_version_detection() {
+    let mut current_priv_ver = PrivilegedVersion::Unknown;
+    {
+        const CSR_MCOUNTEREN: u64 = 0x306;
+        const CSR_MCOUNTINHIBIT: u64 = 0x320;
+        const CSR_MENVCFG: u64 = 0x30a;
+
+        if csr_test!(CSR_MCOUNTEREN) {
+            current_priv_ver = PrivilegedVersion::Version1_10;
+            if csr_test!(CSR_MCOUNTINHIBIT) {
+                current_priv_ver = PrivilegedVersion::Version1_11;
+                if csr_test!(CSR_MENVCFG) {
+                    current_priv_ver = PrivilegedVersion::Version1_12;
+                }
+            }
+        }
+    }
+    unsafe {
+        ROOT_STACK
+            .get_mut(current_hartid())
+            .map(|stack| stack.hart_context().features.privileged_version = current_priv_ver)
+            .unwrap()
+    }
+}
 
 #[cfg(feature = "nemu")]
 pub fn init(cpus: &NodeSeq) {
@@ -70,7 +117,7 @@ pub fn init(cpus: &NodeSeq) {
         unsafe {
             ROOT_STACK
                 .get_mut(hart_id)
-                .map(|stack| stack.hart_context().extensions = HartExtensions(hart_exts))
+                .map(|stack| stack.hart_context().extensions = HartFeatures(hart_exts))
                 .unwrap()
         }
     }

prototyper/src/sbi/hart_context.rs

@@ -1,4 +1,4 @@
-use crate::sbi::extensions::HartExtensions;
+use crate::sbi::extensions::HartFeatures;
 use crate::sbi::hsm::HsmCell;
 use crate::sbi::rfence::RFenceCell;
 use core::ptr::NonNull;
@@ -16,8 +16,8 @@ pub(crate) struct HartContext {
     pub rfence: RFenceCell,
     /// Type of inter-processor interrupt pending.
     pub ipi_type: AtomicU8,
-    /// Supported hart extensions.
-    pub extensions: HartExtensions,
+    /// Supported hart features.
+    pub features: HartFeatures,
 }
 
 impl HartContext {

prototyper/src/sbi/trap.rs

@@ -45,6 +45,25 @@ pub(crate) unsafe extern "C" fn trap_vec() {
     )
 }
 
+/// When you expected some insts will cause trap, use this.
+/// If trap happend, a0 will set to 1, otherwise will be 0.
+///
+/// This function will change a0 and a1 and will NOT change them back.
+#[naked]
+#[repr(align(16))]
+pub(crate) unsafe extern "C" fn expected_trap() {
+    asm!(
+        "add a0, zero, zero",
+        "add a1, zero, zero",
+        "csrr a1, mepc",
+        "addi a1, a1, 4",
+        "csrw mepc, a1",
+        "addi a0, zero, 1",
+        "mret",
+        options(noreturn)
+    )
+}
+
 /// Machine timer interrupt handler.
 /// Saves context, clears mtimecmp, sets STIP bit, and restores context.
 ///