Add Frequency Service

2 files changed, 314 insertions, 0 deletions

diff --git a/src/freq.rs b/src/freq.rs
new file mode 100644
index 0000000..239aba4
--- /dev/null
+++ b/src/freq.rs
@@ -0,0 +1,313 @@
+use std::fs::File;
+use std::fs::{self, OpenOptions};
+use std::io::{self, Read, Seek, SeekFrom, Write};
+use std::ops::{Range, RangeInclusive};
+use std::sync::{mpsc, Arc, RwLock};
+use std::thread;
+use std::time::Duration;
+
+pub type FrequencyKHZ = u32;
+
+pub enum Request {
+    GetPossibleCPUFrequencyRange,
+    GetPolicyCPUFrequency,
+    GetCurrentFrequencies,
+    SetFrequencyRangeAllCores(RangeInclusive<FrequencyKHZ>),
+    SetFrequencyRangeForCore(u32, RangeInclusive<FrequencyKHZ>),
+    SetTargetFrequencyForCore(u32, FrequencyKHZ),
+    SetTargetFrequencyAllCores(FrequencyKHZ),
+}
+
+pub trait FrequencyService {
+    fn get_possible_cpu_frequency_range(&mut self)
+        -> io::Result<Vec<RangeInclusive<FrequencyKHZ>>>;
+    fn get_policy_cpu_frequency_ranges(&mut self) -> io::Result<Vec<RangeInclusive<FrequencyKHZ>>>;
+    fn get_current_frequencies(&mut self) -> io::Result<Vec<FrequencyKHZ>>;
+    fn set_frequency_range_all_cores(
+        &mut self,
+        frequency: &RangeInclusive<FrequencyKHZ>,
+    ) -> io::Result<()>;
+    fn set_frequency_range_for_core(
+        &mut self,
+        cpu: u32,
+        frequency: &RangeInclusive<FrequencyKHZ>,
+    ) -> io::Result<()>;
+    fn set_target_frequency_for_core(
+        &mut self,
+        cpu: u32,
+        frequency: FrequencyKHZ,
+    ) -> io::Result<()>;
+    fn set_target_frequency_all_cores(&mut self, frequency: FrequencyKHZ) -> io::Result<()>;
+}
+
+pub struct SysFSFrequencyService {
+    cpus: Range<u32>,
+    frequency_ranges: Vec<RangeInclusive<FrequencyKHZ>>,
+    cpu_descriptors: Vec<CPUDescriptors>,
+    request_receiver: mpsc::Receiver<Request>,
+    cpu_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+    policy_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+    cpu_current_frequencies: Arc<RwLock<Vec<FrequencyKHZ>>>,
+    update_interval: Duration,
+}
+
+struct CPUDescriptors {
+    scaling_min_freq: File,
+    scaling_max_freq: File,
+    scaling_set_speed: File,
+    scaling_cur_freq: File,
+}
+
+impl CPUDescriptors {
+    pub fn new(cpu: u32) -> io::Result<Self> {
+        let scaling_min_freq = OpenOptions::new().read(true).write(true).open(format!(
+            "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_min_freq",
+            cpu
+        ))?;
+
+        let scaling_max_freq = OpenOptions::new().read(true).write(true).open(format!(
+            "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_max_freq",
+            cpu
+        ))?;
+
+        let scaling_set_speed = OpenOptions::new().write(true).open(format!(
+            "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_setspeed",
+            cpu
+        ))?;
+
+        let scaling_cur_freq = OpenOptions::new().read(true).open(format!(
+            "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_cur_freq",
+            cpu
+        ))?;
+
+        Ok(CPUDescriptors {
+            scaling_min_freq,
+            scaling_max_freq,
+            scaling_set_speed,
+            scaling_cur_freq,
+        })
+    }
+
+    pub fn new_range(cpus: &Range<u32>) -> io::Result<Vec<Self>> {
+        let mut ret = Vec::new();
+        for cpu in cpus.clone() {
+            ret.push(Self::new(cpu)?);
+        }
+        Ok(ret)
+    }
+}
+
+impl FrequencyService for SysFSFrequencyService {
+    fn get_possible_cpu_frequency_range(
+        &mut self,
+    ) -> io::Result<Vec<RangeInclusive<FrequencyKHZ>>> {
+        Ok(self.frequency_ranges.clone())
+    }
+    fn get_policy_cpu_frequency_ranges(&mut self) -> io::Result<Vec<RangeInclusive<FrequencyKHZ>>> {
+        let mut ret = Vec::new();
+        for cpu in &mut self.cpu_descriptors {
+            let mut min_freq = String::new();
+            cpu.scaling_min_freq.read_to_string(&mut min_freq)?;
+            cpu.scaling_min_freq.seek(SeekFrom::Start(0))?;
+            let mut max_freq = String::new();
+            cpu.scaling_max_freq.read_to_string(&mut max_freq)?;
+            cpu.scaling_max_freq.seek(SeekFrom::Start(0))?;
+            let min_freq: FrequencyKHZ = min_freq.trim().parse().unwrap();
+            let max_freq: FrequencyKHZ = max_freq.trim().parse().unwrap();
+            ret.push(min_freq..=max_freq);
+        }
+        Ok(ret)
+    }
+    fn get_current_frequencies(&mut self) -> io::Result<Vec<FrequencyKHZ>> {
+        let mut ret = Vec::new();
+        for cpu in &mut self.cpu_descriptors {
+            let mut cur_freq = String::new();
+            cpu.scaling_cur_freq.read_to_string(&mut cur_freq)?;
+            cpu.scaling_cur_freq.seek(SeekFrom::Start(0))?;
+            let cur_freq: FrequencyKHZ = cur_freq.trim().parse().unwrap();
+            ret.push(cur_freq);
+        }
+        Ok(ret)
+    }
+    fn set_frequency_range_all_cores(
+        &mut self,
+        frequency: &RangeInclusive<FrequencyKHZ>,
+    ) -> io::Result<()> {
+        for cpu in self.cpus.clone() {
+            self.set_frequency_range_for_core(cpu, frequency)?;
+        }
+        Ok(())
+    }
+    fn set_frequency_range_for_core(
+        &mut self,
+        cpu: u32,
+        frequency: &RangeInclusive<FrequencyKHZ>,
+    ) -> io::Result<()> {
+        self.cpu_descriptors[cpu as usize]
+            .scaling_min_freq
+            .write_fmt(format_args!("{}", frequency.start()))?;
+        self.cpu_descriptors[cpu as usize]
+            .scaling_min_freq
+            .seek(SeekFrom::Start(0))?;
+        self.cpu_descriptors[cpu as usize]
+            .scaling_max_freq
+            .write_fmt(format_args!("{}", frequency.end()))?;
+        self.cpu_descriptors[cpu as usize]
+            .scaling_max_freq
+            .seek(SeekFrom::Start(0))?;
+        Ok(())
+    }
+    fn set_target_frequency_for_core(
+        &mut self,
+        cpu: u32,
+        frequency: FrequencyKHZ,
+    ) -> io::Result<()> {
+        self.cpu_descriptors[cpu as usize]
+            .scaling_set_speed
+            .write_fmt(format_args!("{}", frequency))?;
+        self.cpu_descriptors[cpu as usize]
+            .scaling_set_speed
+            .seek(SeekFrom::Start(0))?;
+        Ok(())
+    }
+    fn set_target_frequency_all_cores(&mut self, frequency: FrequencyKHZ) -> io::Result<()> {
+        for cpu in self.cpus.clone() {
+            self.set_target_frequency_for_core(cpu, frequency)?;
+        }
+        Ok(())
+    }
+}
+
+impl SysFSFrequencyService {
+    pub fn new(
+        cpus: Range<u32>,
+        request_receiver: mpsc::Receiver<Request>,
+        cpu_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+        policy_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+        cpu_current_frequencies: Arc<RwLock<Vec<FrequencyKHZ>>>,
+        update_interval: Duration,
+    ) -> io::Result<Self> {
+        Ok(SysFSFrequencyService {
+            cpus,
+            frequency_ranges: Vec::new(),
+            cpu_descriptors: Vec::new(),
+            request_receiver,
+            cpu_frequency_ranges,
+            policy_frequency_ranges,
+            cpu_current_frequencies,
+            update_interval,
+        })
+    }
+
+    fn run(mut self) {
+        thread::spawn(move || {
+            self.cpu_descriptors = CPUDescriptors::new_range(&self.cpus).unwrap();
+            let ranges = self.get_freq_limits().unwrap();
+            self.frequency_ranges = ranges;
+            self.switch_to_userspace_governor().unwrap();
+
+            loop {
+                self.handle_requests().unwrap();
+                thread::sleep(self.update_interval);
+            }
+        });
+    }
+
+    fn handle_requests(&mut self) -> io::Result<()> {
+        while let Ok(request) = self.request_receiver.try_recv() {
+            match request {
+                Request::GetPossibleCPUFrequencyRange => {
+                    *self.cpu_frequency_ranges.write().unwrap() =
+                        self.get_possible_cpu_frequency_range()?;
+                }
+                Request::GetPolicyCPUFrequency => {
+                    *self.policy_frequency_ranges.write().unwrap() =
+                        self.get_policy_cpu_frequency_ranges()?;
+                }
+                Request::GetCurrentFrequencies => {
+                    *self.cpu_current_frequencies.write().unwrap() =
+                        self.get_current_frequencies()?;
+                }
+                Request::SetFrequencyRangeAllCores(frequency) => {
+                    self.set_frequency_range_all_cores(&frequency)?;
+                }
+                Request::SetFrequencyRangeForCore(cpu, frequency) => {
+                    self.set_frequency_range_for_core(cpu, &frequency)?;
+                }
+                Request::SetTargetFrequencyForCore(cpu, frequency) => {
+                    self.set_target_frequency_for_core(cpu, frequency)?;
+                }
+                Request::SetTargetFrequencyAllCores(frequency) => {
+                    self.set_target_frequency_all_cores(frequency)?;
+                }
+            }
+        }
+        Ok(())
+    }
+
+    fn get_freq_limits(&self) -> io::Result<Vec<RangeInclusive<FrequencyKHZ>>> {
+        let mut ranges = Vec::new();
+        for cpu in self.cpus.clone() {
+            let min_freq = fs::read_to_string(format!(
+                "/sys/devices/system/cpu/cpu{}/cpufreq/cpuinfo_min_freq",
+                cpu
+            ))?;
+            let max_freq = fs::read_to_string(format!(
+                "/sys/devices/system/cpu/cpu{}/cpufreq/cpuinfo_max_freq",
+                cpu
+            ))?;
+            let min_freq: FrequencyKHZ = min_freq.trim().parse().unwrap();
+            let max_freq: FrequencyKHZ = max_freq.trim().parse().unwrap();
+            ranges.push(min_freq..=max_freq);
+        }
+        Ok(ranges)
+    }
+
+    fn switch_to_userspace_governor(&self) -> io::Result<()> {
+        let available_governor =
+            fs::read_to_string("/sys/devices/system/cpu/cpu0/cpufreq/scaling_available_governors")?;
+        if !available_governor.contains("userspace") {
+            io::Error::new(io::ErrorKind::NotFound, "userspace");
+        }
+
+        for cpu in self.cpus.clone() {
+            let current_governor = fs::read_to_string(format!(
+                "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_governor",
+                cpu
+            ))?;
+            if current_governor != "userspace" {
+                fs::write(
+                    format!(
+                        "/sys/devices/system/cpu/cpu{}/cpufreq/scaling_governor",
+                        cpu
+                    ),
+                    "userspace",
+                )?;
+            }
+        }
+        Ok(())
+    }
+}
+
+pub fn start_frequency_service(
+    cpus: Range<u32>,
+    cpu_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+    policy_frequency_ranges: Arc<RwLock<Vec<RangeInclusive<FrequencyKHZ>>>>,
+    cpu_current_frequencies: Arc<RwLock<Vec<FrequencyKHZ>>>,
+    update_interval: Duration,
+) -> io::Result<mpsc::SyncSender<Request>> {
+    let (request_sender, request_receiver) = mpsc::sync_channel(100);
+
+    let service = SysFSFrequencyService::new(
+        cpus,
+        request_receiver,
+        cpu_frequency_ranges,
+        policy_frequency_ranges,
+        cpu_current_frequencies,
+        update_interval,
+    )?;
+
+    service.run();
+
+    Ok(request_sender)
+}
diff --git a/src/main.rs b/src/main.rs
index 0bf1c12..d3c9628 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,6 +4,7 @@ pub mod bpf_intf;
 
 mod e_core_selector;
 mod energy;
+mod freq;
 mod scheduler;
 
 #[rustfmt::skip]