1#[cfg(idle_frequency_scaling)]
2mod implem {
3 use portable_atomic::{AtomicBool, AtomicUsize, Ordering};
4 static LOCK_COUNT: AtomicUsize = AtomicUsize::new(0);
5
6 static CLOCK_LOWERED: AtomicBool = AtomicBool::new(false);
8 #[cfg(multi_core)]
9 static IDLE_CORES: AtomicUsize = AtomicUsize::new(0);
10
11 pub(super) fn acquire() {
12 LOCK_COUNT.fetch_add(1, Ordering::AcqRel);
13 }
14
15 pub(super) fn release() {
16 let previous = LOCK_COUNT.fetch_sub(1, Ordering::AcqRel);
17 debug_assert_ne!(previous, 0, "CPU frequency lock counter underflow");
18 }
19
20 pub(crate) fn wait_for_interrupt() {
27 use crate::soc::clocks::{self, ClockTree};
28
29 let mstatus: usize;
32 unsafe { core::arch::asm!("csrrci {0}, mstatus, 8", out(reg) mstatus) };
33
34 ClockTree::with(|tree| {
35 let all_cores_idle = cfg_select! {
36 multi_core => {{
37 use crate::system::Cpu;
38
39 let idle_cores = IDLE_CORES.fetch_add(1, Ordering::Relaxed) + 1;
40 let running_cores = Cpu::all()
41 .filter(|&cpu| crate::soc::cpu_control::is_running(cpu))
42 .count();
43 idle_cores >= running_cores
44 }}
45 _ => true,
46 };
47
48 if all_cores_idle && LOCK_COUNT.load(Ordering::Acquire) == 0 {
49 CLOCK_LOWERED.store(clocks::switch_cpu_clock_to_xtal(tree), Ordering::Relaxed);
50 }
51 });
52
53 unsafe { core::arch::asm!("wfi") };
54
55 ClockTree::with(|tree| {
56 #[cfg(multi_core)]
57 IDLE_CORES.fetch_sub(1, Ordering::Relaxed);
58
59 if CLOCK_LOWERED.swap(false, Ordering::Relaxed) {
60 clocks::restore_cpu_clock(tree);
61 }
62 });
63
64 if mstatus & 0b1000 != 0 {
65 unsafe { core::arch::asm!("csrsi mstatus, 8") };
66 }
67 }
68}
69
70#[cfg(idle_frequency_scaling)]
71pub(crate) use implem::wait_for_interrupt;
72
73#[instability::unstable]
84#[derive(Debug)]
85#[cfg_attr(feature = "defmt", derive(defmt::Format))]
86#[non_exhaustive]
87pub struct CpuFrequencyLock;
88
89impl CpuFrequencyLock {
90 #[instability::unstable]
92 #[inline]
93 pub fn new() -> Self {
94 Self::acquire();
95 Self
96 }
97
98 #[instability::unstable]
100 #[inline]
101 pub fn acquire() {
102 #[cfg(idle_frequency_scaling)]
103 implem::acquire();
104 }
105
106 #[instability::unstable]
110 #[inline]
111 pub fn release() {
112 #[cfg(idle_frequency_scaling)]
113 implem::release();
114 }
115}
116
117#[instability::unstable]
118impl Clone for CpuFrequencyLock {
119 #[inline]
120 fn clone(&self) -> Self {
121 Self::new()
122 }
123}
124
125#[instability::unstable]
126impl Default for CpuFrequencyLock {
127 #[inline]
128 fn default() -> Self {
129 Self::new()
130 }
131}
132
133impl Drop for CpuFrequencyLock {
134 #[inline]
135 fn drop(&mut self) {
136 Self::release();
137 }
138}