feat: support booting linux kernel on multicores

src/clint.rs

@@ -1,6 +1,5 @@
 use aclint::SifiveClint;
 use core::{
-    arch::asm,
     ptr::null_mut,
     sync::atomic::{
         AtomicPtr,
@@ -9,6 +8,8 @@ use core::{
 };
 use rustsbi::SbiRet;
 
+use crate::riscv_spec::stimecmp;
+
 pub(crate) static SIFIVECLINT: AtomicPtr<SifiveClint> = AtomicPtr::new(null_mut());
 
 pub(crate) fn init(base: usize) {
@@ -82,10 +83,7 @@ impl<'a> rustsbi::Timer for ClintDevice<'a> {
     fn set_timer(&self, stime_value: u64) {
         unsafe {
             // TODO: 添加CPU拓展探测机制，补充无Sstc拓展时的定时器设置
-            asm!(
-                "csrrs zero, stimecmp, {}",
-                in(reg) stime_value
-            );
+            stimecmp::set(stime_value);
             riscv::register::mie::set_mtimer();
             // (*self.clint.load(Relaxed)).write_mtimecmp(current_hart_id, stime_value);
         }

src/main.rs

@@ -20,84 +20,103 @@ mod trap;
 mod trap_stack;
 
 use clint::ClintDevice;
+use core::sync::atomic::{AtomicBool, Ordering};
 use core::{arch::asm, mem::MaybeUninit};
 use riscv::register::mstatus;
 
 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::hsm::{local_remote_hsm, Hsm};
 use crate::riscv_spec::menvcfg;
+use crate::trap::trap_vec;
 use crate::trap_stack::NUM_HART_MAX;
 
 #[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>")
-    );
-
-    // 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,
+    static GENESIS: AtomicBool = AtomicBool::new(true);
+
+    let is_boot_hart = if info.boot_hart == usize::MAX {
+        GENESIS.swap(false, Ordering::AcqRel)
+    } else {
+        hart_id() == info.boot_hart
+    };
+
+    if is_boot_hart {
+        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>")
+        );
+
+        // 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,
+            });
+        }
+
+        // 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);
+        }
+
+        // 设置陷入栈
+        trap_stack::prepare_for_trap();
+
+        // 设置内核入口
+        local_remote_hsm().start(NextStage {
+            start_addr: next_addr,
+            next_mode: mpp,
+            opaque,
         });
-    }
 
-    // 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.", hart_id(), next_addr, mpp);
+    } else {
+        // 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);
+        }
+
+        // 设置陷入栈
+        trap_stack::prepare_for_trap();
     }
 
-    // 设置陷入栈
-    trap_stack::prepare_for_trap();
-
-    // 设置内核入口
-    local_remote_hsm().start(NextStage {
-        start_addr: next_addr,
-        next_mode: mpp,
-        opaque,
-    });
-
-    info!("Redirecting harts to 0x{:x} in {:?} mode.", next_addr, mpp);
-
     clint::clear();
     unsafe {
         asm!("csrw mideleg,    {}", in(reg) !0);
@@ -107,12 +126,13 @@ extern "C" fn rust_main(_hart_id: usize, opaque: usize, nonstandard_a2: usize) {
         medeleg::clear_supervisor_env_call();
         medeleg::clear_illegal_instruction();
         menvcfg::set_stce();
-        menvcfg::set_bits(menvcfg::STCE | menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE);
+        menvcfg::set_bits(
+            menvcfg::STCE | menvcfg::CBIE_INVALIDATE | menvcfg::CBCFE | menvcfg::CBZE,
+        );
         mtvec::write(trap_vec as _, mtvec::TrapMode::Vectored);
     }
 }
 
-
 #[naked]
 #[link_section = ".text.entry"]
 #[export_name = "_start"]
@@ -125,11 +145,32 @@ unsafe extern "C" fn start() -> ! {
         "   csrr    t0, mhartid",
         "   ld      t1, 0(a2)",
         "   li      t2, {magic}",
-        "   bne     t1, t2, 1f",
-        "   j       2f",
-        "1:",
-        "   j       1b", // TODO multi hart preempt for runtime init
+        "   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:",
+         // 3. Prepare stack for each hart
         "   call    {locate_stack}",
         "   call    {main}",
         "   j       {trap}",

src/riscv_spec.rs

@@ -7,13 +7,13 @@ 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 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;
+    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() {
@@ -22,8 +22,16 @@ pub mod menvcfg {
 
     pub fn set_bits(option: usize) {
         let mut bits: usize;
-        unsafe { asm!("csrr {}, menvcfg", out(reg) bits, options(nomem)) };
+        unsafe { asm!("csrr {}, menvcfg", out(reg) bits, options(nomem)); }
         bits |= option;
-        unsafe { asm!("csrw menvcfg, {}", in(reg) bits, options(nomem)) };
+        unsafe { asm!("csrw menvcfg, {}", in(reg) bits, options(nomem)); }
+    }
+}
+
+pub mod stimecmp {
+    use core::arch::asm;
+
+    pub fn set(value: u64) {
+        unsafe { asm!("csrrs zero, stimecmp, {}", in(reg) value, options(nomem)); }
     }
 }

src/trap.rs

@@ -8,6 +8,7 @@ use rustsbi::RustSBI;
 use crate::board::SBI;
 use crate::clint::{self, SIFIVECLINT};
 use crate::console::{MachineConsole, CONSOLE};
+use crate::hart_id;
 use crate::hsm::local_hsm;
 use crate::riscv_spec::{CSR_TIME, CSR_TIMEH};
 
@@ -154,7 +155,7 @@ pub extern "C" fn fast_handler(
             sstatus::clear_sie();
             satp::write(0);
         }
-        ctx.regs().a[0] = 0;
+        ctx.regs().a[0] = hart_id();
         ctx.regs().a[1] = opaque;
         ctx.regs().pc = start_addr;
         ctx.call(2)
@@ -239,13 +240,13 @@ pub extern "C" fn fast_handler(
                     break ctx.restore();
                 }
                 T::Exception(E::IllegalInstruction) => {
-                    if mstatus::read().mpp() != mstatus::MPP::Supervisor {
-                        panic!("only can handle illegal instruction exception from S-MODE");
+                    if mstatus::read().mpp() == mstatus::MPP::Machine {
+                        panic!("Cannot handle illegal instruction exception from M-MODE");
                     }
 
                     ctx.regs().a = [ctx.a0(), a1, a2, a3, a4, a5, a6, a7];
                     if !illegal_instruction_handler(&mut ctx) {
-                        delegate(&mut ctx);
+                        delegate();
                     }
                     break ctx.restore();
                 }
@@ -270,7 +271,7 @@ pub extern "C" fn fast_handler(
 }
 
 #[inline]
-fn delegate(_ctx: &mut FastContext) {
+fn delegate() {
     use riscv::register::{sepc, scause, stval, stvec, sstatus, mepc, mcause, mtval};
     unsafe {
 
@@ -279,6 +280,11 @@ fn delegate(_ctx: &mut FastContext) {
         scause::write(mcause::read().bits());
         stval::write(mtval::read());
         sstatus::clear_sie();
+         if mstatus::read().mpp() == mstatus::MPP::Supervisor {
+            sstatus::set_spp(sstatus::SPP::Supervisor);
+        } else {
+            sstatus::set_spp(sstatus::SPP::User);
+        }
         mepc::write(stvec::read().address());
     }
 }
@@ -309,16 +315,6 @@ fn illegal_instruction_handler(ctx: &mut FastContext) -> bool {
             }
             _ => 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);