baseline

recipes/core/kernel/source/src/arch/x86_shared/cpuidle.rs

@@ -0,0 +1,186 @@
+use core::cell::SyncUnsafeCell;
+use core::sync::atomic::{AtomicUsize, Ordering};
+
+use crate::arch::cpuid::cpuid;
+use crate::syscall::error::{Error, Result, EINVAL};
+
+#[repr(align(64))]
+struct MonitorTarget {
+    value: AtomicUsize,
+}
+
+static MONITOR_TARGET: MonitorTarget = MonitorTarget {
+    value: AtomicUsize::new(0),
+};
+
+bitflags::bitflags! {
+    #[derive(Clone, Copy, Debug, PartialEq, Eq)]
+    pub struct CStateFlags: u32 {
+        const NEEDS_MONITOR = 1;
+        const NEEDS_WBINVD = 2;
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub struct CState {
+    pub name: &'static str,
+    pub typ: u32,
+    pub latency: u32,
+    pub power: u32,
+    pub mwait_hint: u32,
+    pub flags: CStateFlags,
+}
+
+const MAX_CSTATES: usize = 8;
+static CPUIDLE_STATES: SyncUnsafeCell<[Option<CState>; MAX_CSTATES]> =
+    SyncUnsafeCell::new([None; MAX_CSTATES]);
+static NUM_CPUIDLE_STATES: AtomicUsize = AtomicUsize::new(0);
+
+static CSTATE_POLICY_MAX: AtomicUsize = AtomicUsize::new(0);
+
+fn has_mwait() -> bool {
+    cpuid().get_feature_info().map_or(false, |info| info.has_monitor_mwait())
+}
+
+fn add_state(index: usize, state: CState) {
+    unsafe {
+        (*CPUIDLE_STATES.get())[index] = Some(state);
+    }
+}
+
+pub fn init() {
+    add_state(0, CState {
+        name: "C1",
+        typ: 1,
+        latency: 1,
+        power: 1000,
+        mwait_hint: 0x00,
+        flags: CStateFlags::empty(),
+    });
+    let mut count = 1;
+
+    if has_mwait() {
+        add_state(count, CState {
+            name: "C1E",
+            typ: 1,
+            latency: 2,
+            power: 800,
+            mwait_hint: 0x01,
+            flags: CStateFlags::NEEDS_MONITOR,
+        });
+        count += 1;
+
+        add_state(count, CState {
+            name: "C2",
+            typ: 2,
+            latency: 10,
+            power: 500,
+            mwait_hint: 0x10,
+            flags: CStateFlags::NEEDS_MONITOR,
+        });
+        count += 1;
+
+        add_state(count, CState {
+            name: "C3",
+            typ: 3,
+            latency: 50,
+            power: 100,
+            mwait_hint: 0x20,
+            flags: CStateFlags::NEEDS_MONITOR | CStateFlags::NEEDS_WBINVD,
+        });
+        count += 1;
+
+        add_state(count, CState {
+            name: "C6",
+            typ: 6,
+            latency: 100,
+            power: 50,
+            mwait_hint: 0x50,
+            flags: CStateFlags::NEEDS_MONITOR | CStateFlags::NEEDS_WBINVD,
+        });
+        count += 1;
+
+        add_state(count, CState {
+            name: "C7",
+            typ: 7,
+            latency: 200,
+            power: 30,
+            mwait_hint: 0x60,
+            flags: CStateFlags::NEEDS_MONITOR | CStateFlags::NEEDS_WBINVD,
+        });
+        count += 1;
+    }
+
+    NUM_CPUIDLE_STATES.store(count, Ordering::SeqCst);
+    log::info!("cpuidle: initialized {} states (mwait={})", count, has_mwait());
+}
+
+pub fn policy_read() -> usize {
+    CSTATE_POLICY_MAX.load(Ordering::Relaxed)
+}
+
+pub fn policy_write(buf: &[u8]) -> Result<usize> {
+    let s = core::str::from_utf8(buf).map_err(|_| Error::new(EINVAL))?;
+    let s = s.trim();
+    let val: usize = s.parse().map_err(|_| Error::new(EINVAL))?;
+    let num_states = NUM_CPUIDLE_STATES.load(Ordering::Relaxed);
+    if val >= num_states {
+        return Err(Error::new(EINVAL));
+    }
+    CSTATE_POLICY_MAX.store(val, Ordering::Relaxed);
+    log::info!("cpuidle: policy set to max state {}", val);
+    Ok(s.len())
+}
+
+pub fn resource() -> Result<alloc::vec::Vec<u8>> {
+    let mut output = alloc::string::String::new();
+    let num_states = NUM_CPUIDLE_STATES.load(Ordering::Relaxed);
+    let policy = CSTATE_POLICY_MAX.load(Ordering::Relaxed);
+    output.push_str(&format!("policy_max: {}\n", policy));
+    output.push_str(&format!("num_states: {}\n", num_states));
+    for i in 0..num_states {
+        if let Some(state) = unsafe { (*CPUIDLE_STATES.get())[i] } {
+            output.push_str(&format!(
+                "state{}: name={} type={} latency={}us power={} hint={:#x} flags={:?}\n",
+                i, state.name, state.typ, state.latency, state.power, state.mwait_hint, state.flags
+            ));
+        }
+    }
+    Ok(output.into_bytes())
+}
+
+pub unsafe fn enter_idle() {
+    let policy_max = CSTATE_POLICY_MAX.load(Ordering::Relaxed);
+    let num_states = NUM_CPUIDLE_STATES.load(Ordering::Relaxed);
+    let target_index = if num_states == 0 {
+        0
+    } else {
+        core::cmp::min(policy_max, num_states - 1)
+    };
+
+    if target_index == 0 {
+        unsafe { crate::arch::interrupt::enable_and_halt(); }
+        return;
+    }
+
+    let state = match unsafe { (*CPUIDLE_STATES.get())[target_index] } {
+        Some(s) => s,
+        None => {
+            unsafe { crate::arch::interrupt::enable_and_halt(); }
+            return;
+        }
+    };
+
+    if state.flags.contains(CStateFlags::NEEDS_MONITOR) {
+        let addr = &MONITOR_TARGET.value as *const AtomicUsize as *const u8;
+        unsafe { crate::arch::interrupt::monitor(addr, 0, 0); }
+    }
+
+    if state.flags.contains(CStateFlags::NEEDS_WBINVD) {
+        unsafe { core::arch::asm!("wbinvd", options(nostack)); }
+    }
+
+    unsafe {
+        crate::arch::interrupt::enable_and_mwait(state.mwait_hint, 0);
+    }
+}