feat(sbi): 支持可恢复挂起

YdrMaster · YdrMaster · commit 55ef7c4d8276 · 2022-05-28T07:52:11.000+08:00
diff --git a/rustsbi-qemu/src/execute/mod.rs b/rustsbi-qemu/src/execute/mod.rs
@@ -1,11 +1,15 @@
-use crate::{clint, hart_id, Supervisor};
+use crate::{
+    clint, hart_id,
+    qemu_hsm::{QemuHsm, SUSPEND_RETENTIVE},
+    Supervisor,
+};
 
 mod context;
 mod transfer_trap;
 
 use context::Context;
 
-pub(crate) fn execute_supervisor(supervisor: Supervisor) {
+pub(crate) fn execute_supervisor(hsm: &QemuHsm, supervisor: Supervisor) {
     use core::arch::asm;
     use riscv::register::{medeleg, mie, mstatus};
 
@@ -26,13 +30,14 @@ pub(crate) fn execute_supervisor(supervisor: Supervisor) {
         medeleg::clear_supervisor_env_call();
         medeleg::clear_machine_env_call();
 
-        crate::set_mtcev(s_to_m as usize);
+        crate::set_mtvec(s_to_m as usize);
         mie::set_mext();
         mie::set_msoft();
     }
 
+    hsm.record_current_start_finished();
     loop {
-        use crate::qemu_hsm::{EID_HSM, FID_HART_STOP, FID_HART_SUSPEND, SUSPEND_NON_RETENTIVE};
+        use crate::qemu_hsm::{EID_HSM, FID_HART_STOP, FID_HART_SUSPEND};
         use riscv::register::{
             mcause::{self, Exception as E, Interrupt as I, Trap as T},
             mip,
@@ -57,11 +62,10 @@ pub(crate) fn execute_supervisor(supervisor: Supervisor) {
                 let param = [ctx.a(0), ctx.a(1), ctx.a(2), ctx.a(3), ctx.a(4), ctx.a(5)];
                 let ans = rustsbi::ecall(ctx.a(7), ctx.a(6), param);
                 if ctx.a(7) == EID_HSM && ans.error == 0 {
-                    if ctx.a(6) == FID_HART_STOP {
-                        return;
-                    }
-                    if ctx.a(6) == FID_HART_SUSPEND && ctx.a(0) == SUSPEND_NON_RETENTIVE as usize {
-                        return;
+                    match ctx.a(6) {
+                        FID_HART_STOP => return,
+                        FID_HART_SUSPEND if ctx.a(0) == SUSPEND_RETENTIVE => return,
+                        _ => {}
                     }
                 }
                 *ctx.a_mut(0) = ans.error;
diff --git a/rustsbi-qemu/src/linker64.ld b/rustsbi-qemu/src/linker64.ld
@@ -11,9 +11,11 @@ SECTIONS
     .text : {
         *(.text.entry)
         . = ALIGN(4);
+        *(.text.early_trap)
+        . = ALIGN(4);
         *(.text.trap_handler)
         . = ALIGN(4);
-        *(.text.early_trap)
+        *(.text.awaker)
         *(.text .text.*)
     }
 
diff --git a/rustsbi-qemu/src/main.rs b/rustsbi-qemu/src/main.rs
@@ -119,7 +119,7 @@ static SMP: AtomicUsize = AtomicUsize::new(0);
 
 /// rust 入口。
 extern "C" fn rust_main(_hartid: usize, opaque: usize) {
-    unsafe { set_mtcev(early_trap as _) };
+    unsafe { set_mtvec(early_trap as _) };
 
     #[link_section = ".bss.uninit"]
     static GENESIS: AtomicBool = AtomicBool::new(false);
@@ -176,8 +176,7 @@ extern "C" fn rust_main(_hartid: usize, opaque: usize) {
     let hsm = HSM.wait();
     if let Some(supervisor) = hsm.take_supervisor() {
         set_pmp();
-        hsm.record_current_start_finished();
-        execute::execute_supervisor(supervisor);
+        execute::execute_supervisor(hsm, supervisor);
     }
 }
 
@@ -236,7 +235,7 @@ fn hart_id() -> usize {
 }
 
 #[inline(always)]
-unsafe fn set_mtcev(trap_handler: usize) {
+unsafe fn set_mtvec(trap_handler: usize) {
     use riscv::register::mtvec;
     mtvec::write(trap_handler, mtvec::TrapMode::Direct);
 }
diff --git a/rustsbi-qemu/src/qemu_hsm.rs b/rustsbi-qemu/src/qemu_hsm.rs
@@ -1,12 +1,12 @@
 //! Hart state monitor designed for QEMU
 
-use crate::{clint::Clint, entry, hart_id, set_mtcev, Supervisor, NUM_HART_MAX, SUPERVISOR_ENTRY};
+use crate::{clint::Clint, entry, hart_id, set_mtvec, Supervisor, NUM_HART_MAX, SUPERVISOR_ENTRY};
 use core::{mem::MaybeUninit, sync::atomic::AtomicU8};
 use rustsbi::SbiRet;
 use spin::Mutex;
 
-pub(crate) const SUSPEND_RETENTIVE: u32 = 0x00000000;
-pub(crate) const SUSPEND_NON_RETENTIVE: u32 = 0x80000000;
+pub(crate) const SUSPEND_RETENTIVE: usize = 0x00000000;
+pub(crate) const SUSPEND_NON_RETENTIVE: usize = 0x80000000;
 pub(crate) const EID_HSM: usize = 0x48534D;
 pub(crate) const FID_HART_STOP: usize = 1;
 pub(crate) const FID_HART_SUSPEND: usize = 3;
@@ -94,6 +94,23 @@ impl QemuHsm {
         }
     }
 
+    /// 初始化完成，转移到运行状态。
+    pub fn record_current_start_finished(&self) {
+        use core::sync::atomic::Ordering::Release;
+        self.state[hart_id()].store(STARTED, Release);
+    }
+
+    /// 如果一个核可以接受 ipi，返回 `true`。
+    ///
+    /// 运行状态的核可以接受权限低于 SBI 软件的核间中断，将转交给特权软件。
+    /// 挂起状态的核可以接受核间中断以恢复运行。
+    pub fn is_ipi_allowed(&self, hart_id: usize) -> bool {
+        use core::sync::atomic::Ordering::Acquire;
+        self.state
+            .get(hart_id)
+            .map_or(false, |s| matches!(s.load(Acquire), STARTED | SUSPEND))
+    }
+
     /// 为硬件线程准备休眠或关闭。
     ///
     /// 此时核状态必然是不可干预的 Pending 状态，中断业已关闭。
@@ -120,32 +137,59 @@ impl QemuHsm {
         // 通过软件中断重启
         unsafe {
             mie::set_msoft();
-            set_mtcev(entry as _)
+            set_mtvec(entry as _);
         };
         // 转移状态
         if let Err(unexpected) = state.compare_exchange(current, new, AcqRel, Acquire) {
             panic!("failed to reboot for a race {current:?} => {unexpected:?}")
         }
     }
 
-    /// Record that current hart id is marked as `Started` state.
-    /// It is used when hart stop command is received in interrupt handler.
-    /// The target hart (when in interrupt handler) is prepared to start, it marks itself into 'started',
-    /// and should jump to target address right away.
-    pub fn record_current_start_finished(&self) {
-        use core::sync::atomic::Ordering::Release;
-        self.state[hart_id()].store(STARTED, Release);
-    }
+    /// 可恢复挂起。
+    fn retentive_suspend(&self) {
+        use core::{
+            arch::asm,
+            sync::atomic::Ordering::{AcqRel, Acquire},
+        };
+        use riscv::{interrupt, register::mtvec};
 
-    /// 如果一个核可以接受 ipi，返回 `true`。
-    ///
-    /// 运行状态的核可以接受权限低于 SBI 软件的核间中断，将转交给特权软件。
-    /// 挂起状态的核可以接受核间中断以恢复运行。
-    pub fn is_ipi_allowed(&self, hart_id: usize) -> bool {
-        use core::sync::atomic::Ordering::Acquire;
-        self.state
-            .get(hart_id)
-            .map_or(false, |s| matches!(s.load(Acquire), STARTED | SUSPEND))
+        /// 挂起，使用 call 进入以链接 ra
+        #[naked]
+        unsafe extern "C" fn suspend() {
+            asm!("1: wfi", "j 1b", options(noreturn))
+        }
+
+        /// 恢复，利用 ra 回到挂起前位置
+        #[naked]
+        #[link_section = ".text.awaker"]
+        unsafe extern "C" fn resume() {
+            asm!("ret", options(noreturn))
+        }
+
+        let state = &self.state[hart_id()];
+        let mtvec = mtvec::read().address();
+
+        // 转移状态
+        if let Err(unexpected) = state.compare_exchange(SUSPEND_PENDING, SUSPEND, AcqRel, Acquire) {
+            panic!("failed to suspend by wrong state: {unexpected:?}")
+        }
+        // 调整中断，休眠
+        unsafe {
+            // 支持软中断或外部中断唤醒
+            let mut mie: usize = (1 << 11) | (1 << 3);
+
+            set_mtvec(resume as _);
+            asm!("csrrw {0}, mie, {0}", inlateout(reg) mie);
+            interrupt::enable();
+            suspend();
+            interrupt::disable();
+            asm!("csrw mie, {mie}", mie = in(reg) mie);
+            set_mtvec(mtvec);
+        }
+        // 恢复状态
+        if let Err(unexpected) = state.compare_exchange(SUSPEND, STARTED, AcqRel, Acquire) {
+            panic!("failed to resume by wrong state: {unexpected:?}")
+        }
     }
 }
 
@@ -214,8 +258,11 @@ impl rustsbi::Hsm for &'static QemuHsm {
     fn hart_suspend(&self, suspend_type: u32, resume_addr: usize, opaque: usize) -> SbiRet {
         use core::sync::atomic::Ordering::{AcqRel, Acquire};
         match self.state[hart_id()].compare_exchange(STARTED, SUSPEND_PENDING, AcqRel, Acquire) {
-            Ok(_) => match suspend_type {
-                SUSPEND_RETENTIVE => todo!(),
+            Ok(_) => match suspend_type as usize {
+                SUSPEND_RETENTIVE => {
+                    self.retentive_suspend();
+                    SbiRet::ok(0)
+                }
                 SUSPEND_NON_RETENTIVE => {
                     *self.supervisor[hart_id()].lock() = Some(Supervisor {
                         start_addr: resume_addr,

Original file line number	Diff line number	Diff line change
`@@ -119,7 +119,7 @@ static SMP: AtomicUsize = AtomicUsize::new(0);`
`119`	`119`
`120`	`120`	`/// rust 入口。`
`121`	`121`	`extern "C" fn rust_main(_hartid: usize, opaque: usize) {`
`122`		`- unsafe { set_mtcev(early_trap as _) };`
	`122`	`+ unsafe { set_mtvec(early_trap as _) };`
`123`	`123`
`124`	`124`	`#[link_section = ".bss.uninit"]`
`125`	`125`	`static GENESIS: AtomicBool = AtomicBool::new(false);`
`@@ -176,8 +176,7 @@ extern "C" fn rust_main(_hartid: usize, opaque: usize) {`
`176`	`176`	`let hsm = HSM.wait();`
`177`	`177`	`if let Some(supervisor) = hsm.take_supervisor() {`
`178`	`178`	`set_pmp();`
`179`		`- hsm.record_current_start_finished();`
`180`		`- execute::execute_supervisor(supervisor);`
	`179`	`+ execute::execute_supervisor(hsm, supervisor);`
`181`	`180`	`}`
`182`	`181`	`}`
`183`	`182`
`@@ -236,7 +235,7 @@ fn hart_id() -> usize {`
`236`	`235`	`}`
`237`	`236`
`238`	`237`	`#[inline(always)]`
`239`		`-unsafe fn set_mtcev(trap_handler: usize) {`
	`238`	`+unsafe fn set_mtvec(trap_handler: usize) {`
`240`	`239`	`use riscv::register::mtvec;`
`241`	`240`	`mtvec::write(trap_handler, mtvec::TrapMode::Direct);`
`242`	`241`	`}`