perf(kernel): add PortThreading::try_enter_cpu_lock

This operation can be optimized into a single CSR read-modify-write
instruction on RISC-V.

Author: yvt

src/constance/src/kernel.rs

@@ -268,12 +268,12 @@ pub trait Kernel: Port + KernelCfg2 + Sized + 'static {
 
 impl<T: Port + KernelCfg2 + 'static> Kernel for T {
     fn acquire_cpu_lock() -> Result<(), CpuLockError> {
-        if Self::is_cpu_lock_active() {
-            Err(CpuLockError::BadContext)
-        } else {
-            // Safety: CPU Lock inactive
-            unsafe { Self::enter_cpu_lock() };
+        // Safety: `try_enter_cpu_lock` is only meant to be called by
+        //         the kernel
+        if unsafe { Self::try_enter_cpu_lock() } {
             Ok(())
+        } else {
+            Err(CpuLockError::BadContext)
         }
     }
 
@@ -441,6 +441,18 @@ pub unsafe trait PortThreading: KernelCfg1 {
     /// Precondition: CPU Lock active
     unsafe fn leave_cpu_lock();
 
+    /// Activate CPU Lock. Return `true` iff CPU Lock was inactive before the
+    /// call.
+    unsafe fn try_enter_cpu_lock() -> bool {
+        if Self::is_cpu_lock_active() {
+            false
+        } else {
+            // Safety: CPU Lock inactive
+            unsafe { Self::enter_cpu_lock() };
+            true
+        }
+    }
+
     /// Prepare the task for activation. More specifically, set the current
     /// program counter to [`TaskAttr::entry_point`] and the current stack
     /// pointer to either end of [`TaskAttr::stack`], ensuring the task will

src/constance/src/kernel/utils.rs

@@ -111,16 +111,13 @@ impl<'a, Element: Clone, System: Kernel> CellLike<CpuLockGuardBorrowMut<'a, Syst
 /// Attempt to enter a CPU Lock state and get an RAII guard.
 /// Return `BadContext` if the kernel is already in a CPU Lock state.
 pub(super) fn lock_cpu<System: Kernel>() -> Result<CpuLockGuard<System>, BadContextError> {
-    expect_cpu_lock_inactive::<System>()?;
-
-    // Safety: CPU Lock is currently inactive, and it's us (the kernel) who
-    // are currently controlling the CPU Lock state
-    unsafe {
-        System::enter_cpu_lock();
+    // Safety: `try_enter_cpu_lock` is only meant to be called by the kernel
+    if unsafe { System::try_enter_cpu_lock() } {
+        // Safety: We just entered a CPU Lock state
+        Ok(unsafe { assume_cpu_lock() })
+    } else {
+        Err(BadContextError::BadContext)
     }
-
-    // Safety: We just entered a CPU Lock state
-    Ok(unsafe { assume_cpu_lock() })
 }
 
 /// Assume a CPU Lock state and get `CpuLockGuard`.

src/constance_port_riscv/src/threading/cfg.rs

@@ -95,6 +95,11 @@ macro_rules! use_port {
                     PORT_STATE.enter_cpu_lock::<Self>()
                 }
 
+                #[inline(always)]
+                unsafe fn try_enter_cpu_lock() -> bool {
+                    PORT_STATE.try_enter_cpu_lock::<Self>()
+                }
+
                 #[inline(always)]
                 unsafe fn leave_cpu_lock() {
                     PORT_STATE.leave_cpu_lock::<Self>()

src/constance_port_riscv/src/threading/imp.rs

@@ -32,6 +32,13 @@ mod mstatus {
         unsafe { asm!("csrsi mstatus, {}", const VALUE) };
     }
 
+    #[inline(always)]
+    pub fn fetch_clear_i<const VALUE: usize>() -> usize {
+        let read: usize;
+        unsafe { asm!("csrrci {}, mstatus, {}", lateout(reg) read, const VALUE) };
+        read
+    }
+
     #[inline(always)]
     pub fn read() -> usize {
         let read: usize;
@@ -567,6 +574,11 @@ impl State {
         mstatus::clear_i::<{ mstatus::MIE }>();
     }
 
+    #[inline(always)]
+    pub unsafe fn try_enter_cpu_lock<System: PortInstance>(&self) -> bool {
+        (mstatus::fetch_clear_i::<{ mstatus::MIE }>() & mstatus::MIE) != 0
+    }
+
     #[inline(always)]
     pub unsafe fn leave_cpu_lock<System: PortInstance>(&'static self) {
         mstatus::set_i::<{ mstatus::MIE }>();