RedBear-OS/local/patches/base/P6-cpufreqd-real-impl.patch

--- a/drivers/cpufreqd/src/main.rs
+++ b/drivers/cpufreqd/src/main.rs
@@ -0,0 +1,167 @@
+use std::env;
+use std::fs;
+use std::io::{Read, Write};
+use std::thread;
+use std::time::{Duration, Instant};
+
+const IA32_PERF_CTL: u32 = 0x199;
+const IA32_PERF_STATUS: u32 = 0x198;
+const POLL_INTERVAL_MS: u64 = 100;
+
+#[derive(Clone, Copy, PartialEq)]
+enum Governor { Performance, Powersave, Ondemand }
+
+struct PState { freq_mhz: u32, power_mw: u32, latency_us: u32, ctl_value: u64 }
+
+struct CpuState { id: u32, current_pstate: usize, load: f64 }
+
+fn read_msr(cpu: u32, msr: u32) -> Option<u64> {
+    let path = format!("/dev/cpu/{}/msr", cpu);
+    let mut f = fs::OpenOptions::new().read(true).open(&path).ok()?;
+    let mut buf = [0u8; 8];
+    f.read_exact(&mut buf).ok()?;
+    Some(u64::from_ne_bytes(buf))
+}
+
+fn write_msr(cpu: u32, msr: u32, value: u64) -> bool {
+    let path = format!("/dev/cpu/{}/msr", cpu);
+    fs::OpenOptions::new().write(true).open(&path)
+        .and_then(|mut f| f.write_all(&value.to_ne_bytes()))
+        .is_ok()
+}
+
+fn read_acpi_pss(cpu: u32) -> Vec<PState> {
+    let path = format!("/scheme/acpi/processor/CPU{}/pss", cpu);
+    let data = fs::read_to_string(&path).unwrap_or_default();
+    let mut states = Vec::new();
+    for line in data.lines() {
+        let parts: Vec<&str> = line.split_whitespace().collect();
+        if parts.len() >= 6 {
+            if let (Ok(freq), Ok(power), Ok(latency), Ok(ctl)) = (
+                parts[0].parse::<u32>(), parts[2].parse::<u32>(),
+                parts[4].parse::<u32>(), u64::from_str_radix(parts[5], 16)
+            ) {
+                states.push(PState { freq_mhz: freq, power_mw: power, latency_us: latency, ctl_value: ctl });
+            }
+        }
+    }
+    if states.is_empty() {
+        states.push(PState { freq_mhz: 2400, power_mw: 35000, latency_us: 10, ctl_value: 0x1a00 });
+        states.push(PState { freq_mhz: 1200, power_mw: 15000, latency_us: 10, ctl_value: 0x0d00 });
+    }
+    states
+}
+
+fn detect_cpus() -> Vec<u32> {
+    let mut cpus = Vec::new();
+    if let Ok(entries) = fs::read_dir("/dev/cpu") {
+        for entry in entries.flatten() {
+            if let Ok(name) = entry.file_name().into_string() {
+                if let Ok(id) = name.parse::<u32>() {
+                    cpus.push(id);
+                }
+            }
+        }
+    }
+    if cpus.is_empty() { cpus.push(0); }
+    cpus
+}
+
+fn measure_cpu_load(cpu: u32, prev: &mut (u64, u64)) -> f64 {
+    let path = format!("/scheme/sys/cpu/{}/stat", cpu);
+    if let Ok(data) = fs::read_to_string(&path) {
+        let parts: Vec<u64> = data.split_whitespace().filter_map(|s| s.parse().ok()).collect();
+        if parts.len() >= 4 {
+            let total: u64 = parts.iter().sum();
+            let idle = parts.get(3).copied().unwrap_or(0);
+            let prev_total = prev.0;
+            let prev_idle = prev.1;
+            *prev = (total, idle);
+            if total > prev_total {
+                let total_delta = total - prev_total;
+                let idle_delta = idle.saturating_sub(prev_idle);
+                return 1.0 - (idle_delta as f64 / total_delta as f64);
+            }
+        }
+    }
+    0.0
+}
+
+fn choose_pstate(governor: Governor, pstates: &[PState], current: usize, load: f64) -> usize {
+    match governor {
+        Governor::Performance => 0,
+        Governor::Powersave => pstates.len() - 1,
+        Governor::Ondemand => {
+            if load > 0.8 && current > 0 { current - 1 }
+            else if load < 0.3 && current + 1 < pstates.len() { current + 1 }
+            else { current }
+        }
+    }
+}
+
+fn main() {
+    eprintln!("cpufreqd: started");
+
+    let governor = match env::var("CPUFREQ_GOVERNOR").unwrap_or_else(|_| "ondemand".to_string()).as_str() {
+        "performance" => Governor::Performance,
+        "powersave" => Governor::Powersave,
+        _ => Governor::Ondemand,
+    };
+
+    let cpus = detect_cpus();
+    eprintln!("cpufreqd: detected {} CPU(s)", cpus.len());
+
+    let all_pstates: Vec<Vec<PState>> = cpus.iter().map(|cpu| read_acpi_pss(*cpu)).collect();
+    if all_pstates.iter().all(|p| p.is_empty()) {
+        eprintln!("cpufreqd: no P-states found, cannot scale frequency");
+        return;
+    }
+
+    let mut cpu_states: Vec<CpuState> = cpus.iter().enumerate().map(|(i, &id)| {
+        CpuState { id, current_pstate: 0, load: 0.0 }
+    }).collect();
+    let mut prev_stats: Vec<(u64, u64)> = vec![(0, 0); cpus.len()];
+
+    eprintln!("cpufreqd: governor={:?}, {} P-states available", governor, all_pstates[0].len());
+    for (i, p) in all_pstates[0].iter().enumerate() {
+        eprintln!("cpufreqd:   P{}: {} MHz, {} mW, latency {} us", i, p.freq_mhz, p.power_mw, p.latency_us);
+    }
+
+    for (i, cs) in cpu_states.iter_mut().enumerate() {
+        let pstates = &all_pstates[i];
+        if !pstates.is_empty() {
+            let ctl = pstates[0].ctl_value;
+            if write_msr(cs.id, IA32_PERF_CTL, ctl) {
+                eprintln!("cpufreqd: CPU{}: set P0 ({} MHz)", cs.id, pstates[0].freq_mhz);
+            } else {
+                eprintln!("cpufreqd: CPU{}: MSR write failed, trying ACPI path", cs.id);
+            }
+        }
+    }
+
+    let poll = Duration::from_millis(POLL_INTERVAL_MS);
+
+    loop {
+        thread::sleep(poll);
+
+        for (i, cs) in cpu_states.iter_mut().enumerate() {
+            let load = measure_cpu_load(cs.id, &mut prev_stats[i]);
+            cs.load = load;
+
+            let pstates = &all_pstates[i];
+            if pstates.is_empty() { continue; }
+
+            let new_idx = choose_pstate(governor, pstates, cs.current_pstate, load);
+            if new_idx != cs.current_pstate {
+                let ctl = pstates[new_idx].ctl_value;
+                if write_msr(cs.id, IA32_PERF_CTL, ctl) {
+                    eprintln!("cpufreqd: CPU{}: P{}→P{} ({}→{} MHz, load={:.1}%)",
+                        cs.id, cs.current_pstate, new_idx,
+                        pstates[cs.current_pstate].freq_mhz, pstates[new_idx].freq_mhz,
+                        load * 100.0);
+                    cs.current_pstate = new_idx;
+                }
+            }
+        }
+    }
+}