lib: parse dynamic information

Signed-off-by: Zhouqi Jiang <luojia@hust.edu.cn>

Cargo.lock

@@ -2,17 +2,40 @@
 # It is not intended for manual editing.
 version = 3
 
+[[package]]
+name = "critical-section"
+version = "1.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216"
+
+[[package]]
+name = "embedded-hal"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89"
+
 [[package]]
 name = "panic-halt"
 version = "0.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812"
 
+[[package]]
+name = "riscv"
+version = "0.11.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2f5c1b8bf41ea746266cdee443d1d1e9125c86ce1447e1a2615abd34330d33a9"
+dependencies = [
+ "critical-section",
+ "embedded-hal",
+]
+
 [[package]]
 name = "rustsbi-prototyper"
 version = "0.0.0"
 dependencies = [
  "panic-halt",
+ "riscv",
 ]
 
 [[package]]

Cargo.toml

@@ -10,6 +10,7 @@ forced-target = "riscv64imac-unknown-none-elf"
 
 [dependencies]
 panic-halt = "0.2.0"
+riscv = "0.11.1"
 
 [[bin]]
 name = "rustsbi-prototyper"

src/dynamic.rs

@@ -0,0 +1,53 @@
+//! Frequently used first boot stage dynamic information on RISC-V.
+
+use core::{mem::size_of, ops::Range};
+
+use riscv::register::mstatus;
+
+/// M-mode firmware dynamic information.
+#[derive(Clone, Copy)]
+#[repr(C)]
+pub struct DynamicInfo {
+    /// Dynamic information magic value.
+    pub magic: usize,
+    /// Version of dynamic information.
+    pub version: usize,
+    /// Address of the next boot-loading stage.
+    pub next_addr: usize,
+    /// RISC-V privilege mode of the next boot-loading stage.
+    pub next_mode: usize,
+    /// M-mode firmware options; its definition varies between SBI implementations.
+    pub options: usize,
+}
+
+const DYNAMIC_INFO_VALID_ADDRESSES: Range<usize> = 0x1000..0xf000;
+const NEXT_ADDR_VALID_ADDRESSES: Range<usize> = 0x80000000..0x90000000;
+
+// TODO unconstrained lifetime
+pub fn try_read_dynamic(paddr: usize) -> Result<DynamicInfo, ()> {
+    // check pointer before dereference
+    if !DYNAMIC_INFO_VALID_ADDRESSES.contains(&paddr)
+        || !DYNAMIC_INFO_VALID_ADDRESSES.contains(&(paddr + size_of::<DynamicInfo>()))
+    {
+        return Err(());
+    }
+    let ans = unsafe { *(paddr as *const DynamicInfo) };
+    Ok(ans)
+}
+
+pub fn next_mode_mpp(info: &DynamicInfo) -> Result<mstatus::MPP, ()> {
+    match info.next_mode {
+        0 => Ok(mstatus::MPP::User),
+        1 => Ok(mstatus::MPP::Supervisor),
+        3 => Ok(mstatus::MPP::Machine),
+        _ => Err(()),
+    }
+}
+
+pub fn check_next_addr(info: &DynamicInfo) -> Result<usize, ()> {
+    if NEXT_ADDR_VALID_ADDRESSES.contains(&info.next_addr) {
+        Ok(info.next_addr)
+    } else {
+        Err(())
+    }
+}

src/main.rs

@@ -2,11 +2,47 @@
 #![no_std]
 #![no_main]
 
+mod dynamic;
+
 use panic_halt as _;
+use riscv::register::mstatus;
+
+extern "C" fn main(hart_id: usize, opaque: usize, nonstandard_a2: usize) -> usize {
+    let _ = (hart_id, opaque);
+
+    if let Ok(info) = dynamic::try_read_dynamic(nonstandard_a2) {
+        let mpp = dynamic::next_mode_mpp(&info).unwrap_or_else(fail_invalid_next_privilege_mode);
+        let next_addr = dynamic::check_next_addr(&info).unwrap_or_else(fail_invalid_next_address);
+
+        unsafe { mstatus::set_mpp(mpp) };
+        next_addr
+    } else {
+        fail_no_dynamic_info_available()
+    }
+}
+
+#[cold]
+fn fail_invalid_next_privilege_mode(_err: ()) -> mstatus::MPP {
+    // TODO dynamic information contains invalid privilege mode
+    loop {
+        core::hint::spin_loop()
+    }
+}
+
+#[cold]
+fn fail_invalid_next_address(_err: ()) -> usize {
+    // TODO dynamic information contains invalid next address
+    loop {
+        core::hint::spin_loop()
+    }
+}
 
-extern "C" fn main(hart_id: usize, opaque: usize, a2: usize) -> usize {
-    let _ = (hart_id, opaque, a2);
-    0 // TODO
+#[cold]
+fn fail_no_dynamic_info_available() -> ! {
+    // TODO no dynamic information available
+    loop {
+        core::hint::spin_loop()
+    }
 }
 
 const LEN_STACK_PER_HART: usize = 16 * 1024;
@@ -59,9 +95,10 @@ unsafe extern "C" fn start() -> ! {
         "   addi    t1, t1, -1",
         "   bnez    t1, 1b",
         // 4. Run Rust main function
-        "   j       {main}",
+        "   call    {main}",
         // 5. Jump to following boot sequences
-        "   jr      a0", // TODO
+        "   csrw    mepc, a0",
+        "   mret",
         stack_size_per_hart = const LEN_STACK_PER_HART,
         stack = sym STACK,
         main = sym main,