From e9c9b890eec40b8f922d4e2ef10f87ad0d293553 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Tue, 22 Oct 2024 20:21:59 +0200 Subject: [PATCH 1/8] Redirected esp-pacs to our fork. --- esp-hal/src/soc/esp32/peripherals.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/esp-hal/src/soc/esp32/peripherals.rs b/esp-hal/src/soc/esp32/peripherals.rs index 6216b55bfac..3a25222d5d7 100644 --- a/esp-hal/src/soc/esp32/peripherals.rs +++ b/esp-hal/src/soc/esp32/peripherals.rs @@ -73,5 +73,5 @@ crate::peripherals! { UART2 <= UART2, UHCI0 <= UHCI0, UHCI1 <= UHCI1, - WIFI <= virtual, + WIFI <= WIFI, } From cf8ff5e117e2d7c014f0e17212f3d8afe6a46bf6 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Tue, 22 Oct 2024 20:29:53 +0200 Subject: [PATCH 2/8] Actually redirected esp-pacs to our fork. --- esp-hal/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/esp-hal/Cargo.toml b/esp-hal/Cargo.toml index 399c4e7222b..d26c120f07e 100644 --- a/esp-hal/Cargo.toml +++ b/esp-hal/Cargo.toml @@ -55,7 +55,7 @@ xtensa-lx = { version = "0.9.0", optional = true } # IMPORTANT: # Each supported device MUST have its PAC included below along with a # corresponding feature. -esp32 = { version = "0.33.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } +esp32 = { version = "0.33.0", git = "https://github.com/esp32-open-mac/esp-pacs.git", features = ["critical-section", "rt"], optional = true } esp32c2 = { version = "0.22.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } esp32c3 = { version = "0.25.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } esp32c6 = { version = "0.16.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } From efdfcd41aafd4f68fc2f8da5fd177cb878418584 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Wed, 13 Nov 2024 22:28:56 +0100 Subject: [PATCH 3/8] Pointed esp-pacs back upstream. --- esp-hal/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/esp-hal/Cargo.toml b/esp-hal/Cargo.toml index d26c120f07e..399c4e7222b 100644 --- a/esp-hal/Cargo.toml +++ b/esp-hal/Cargo.toml @@ -55,7 +55,7 @@ xtensa-lx = { version = "0.9.0", optional = true } # IMPORTANT: # Each supported device MUST have its PAC included below along with a # corresponding feature. -esp32 = { version = "0.33.0", git = "https://github.com/esp32-open-mac/esp-pacs.git", features = ["critical-section", "rt"], optional = true } +esp32 = { version = "0.33.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } esp32c2 = { version = "0.22.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } esp32c3 = { version = "0.25.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } esp32c6 = { version = "0.16.0", git = "https://github.com/esp-rs/esp-pacs.git", rev = "48fd400", features = ["critical-section", "rt"], optional = true } From 12c39dfe0811042a183f52194475636aa530c5e5 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Wed, 13 Nov 2024 22:30:47 +0100 Subject: [PATCH 4/8] Made WIFI peripheral virtual again. --- esp-hal/src/soc/esp32/peripherals.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/esp-hal/src/soc/esp32/peripherals.rs b/esp-hal/src/soc/esp32/peripherals.rs index 1c60292c957..c0bd6f2732b 100644 --- a/esp-hal/src/soc/esp32/peripherals.rs +++ b/esp-hal/src/soc/esp32/peripherals.rs @@ -73,5 +73,5 @@ crate::peripherals! { [Uart2] UART2 <= UART2, UHCI0 <= UHCI0, UHCI1 <= UHCI1, - WIFI <= WIFI, + WIFI <= virtual, } From c4379c19cd34eca98121f185175dfb0323e182ae Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Wed, 13 Nov 2024 22:32:32 +0100 Subject: [PATCH 5/8] Added basic radio clock handling. --- esp-hal/src/lib.rs | 4 + esp-hal/src/radio_clock_ctrl.rs | 167 ++++++++++++++++++++++++ esp-hal/src/soc/esp32/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32c2/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32c3/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32c6/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32h2/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32s2/radio_clocks.rs | 2 +- esp-hal/src/soc/esp32s3/radio_clocks.rs | 2 +- esp-hal/src/system.rs | 44 ------- 10 files changed, 178 insertions(+), 51 deletions(-) create mode 100644 esp-hal/src/radio_clock_ctrl.rs diff --git a/esp-hal/src/lib.rs b/esp-hal/src/lib.rs index 6622d34c5e3..42c504a34b7 100644 --- a/esp-hal/src/lib.rs +++ b/esp-hal/src/lib.rs @@ -224,6 +224,7 @@ pub mod rtc_cntl; pub mod sha; #[cfg(any(spi0, spi1, spi2, spi3))] pub mod spi; +#[doc(hidden)] #[cfg(any(dport, hp_sys, pcr, system))] pub mod system; pub mod time; @@ -255,6 +256,9 @@ pub mod trapframe { pub use xtensa_lx_rt::exception::Context as TrapFrame; } +#[cfg(phy)] +pub mod radio_clock_ctrl; + // The `soc` module contains chip-specific implementation details and should not // be directly exposed. mod soc; diff --git a/esp-hal/src/radio_clock_ctrl.rs b/esp-hal/src/radio_clock_ctrl.rs new file mode 100644 index 00000000000..7e10cf07dbd --- /dev/null +++ b/esp-hal/src/radio_clock_ctrl.rs @@ -0,0 +1,167 @@ +use core::{ + cell::RefCell, + sync::atomic::{AtomicBool, Ordering}, +}; + +use critical_section::Mutex; + +use crate::peripherals::RADIO_CLK; + +/// Enumeration of the available radio peripherals for this chip. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[cfg(any(bt, ieee802154, wifi))] +pub enum RadioPeripherals { + /// Represents the PHY (Physical Layer) peripheral. + Phy, + /// Represents the Bluetooth peripheral. + #[cfg(bt)] + Bt, + /// Represents the WiFi peripheral. + #[cfg(wifi)] + Wifi, + /// Represents the IEEE 802.15.4 peripheral. + #[cfg(ieee802154)] + Ieee802154, +} + +/// Control the radio peripheral clocks +/// +/// NOTE: We don't really want the user messing with the radio clocks, so this +/// is hidden. In the future, we could just make it private. +#[doc(hidden)] +pub trait RadioClockController { + /// Enable the peripheral + fn enable(&mut self, peripheral: RadioPeripherals); + + /// Disable the peripheral + fn disable(&mut self, peripheral: RadioPeripherals); + + /// Reset the MAC + fn reset_mac(&mut self); + + /// Do any common initial initialization needed + fn init_clocks(&mut self); + + /// Initialize BLE RTC clocks + fn ble_rtc_clk_init(&mut self); + + /// Reset the Resolvable Private Address (RPA). + fn reset_rpa(&mut self); +} + +static PHY_CLOCK_ENABLED: AtomicBool = AtomicBool::new(false); +pub struct ModemClocksEnabledError; + +pub struct SharedRadioClockControl { + radio_clock: Mutex>, + + #[cfg(bt)] + bt_clock_enabled: AtomicBool, + #[cfg(wifi)] + wifi_clock_enabled: AtomicBool, + #[cfg(ieee802154)] + ieee802154_clock_enabled: AtomicBool, +} +impl SharedRadioClockControl { + pub const fn new(radio_clock: RADIO_CLK) -> Self { + Self { + radio_clock: Mutex::new(RefCell::new(radio_clock)), + #[cfg(bt)] + bt_clock_enabled: AtomicBool::new(false), + #[cfg(wifi)] + wifi_clock_enabled: AtomicBool::new(false), + #[cfg(ieee802154)] + ieee802154_clock_enabled: AtomicBool::new(false), + } + } + /// Check if any modem clocks are enabled. + pub fn any_modem_clocks_enabled(&self) -> bool { + let mut any_clocks_enabled = false; + + #[cfg(bt)] + { + any_clocks_enabled |= self.bt_clock_enabled.load(Ordering::Relaxed); + } + #[cfg(wifi)] + { + any_clocks_enabled |= self.wifi_clock_enabled.load(Ordering::Relaxed); + } + #[cfg(ieee802154)] + { + any_clocks_enabled |= self.ieee802154_clock_enabled.load(Ordering::Relaxed); + } + + any_clocks_enabled + } + /// Enable or disable the clock. + fn set_clock_status(&self, enabled: bool, radio_peripheral: RadioPeripherals) { + critical_section::with(|cs| { + let mut radio_clock = self.radio_clock.borrow_ref_mut(cs); + if enabled { + radio_clock.enable(radio_peripheral) + } else { + radio_clock.disable(radio_peripheral) + } + }) + } + /// Enable or disable the PHY clock. + /// + /// If the PHY clock state was successfully changed, or the current state + /// already matched the specified one `Ok(())` is returned. + /// If the PHY clock is being disabled, but other modem clocks are still + /// active `Err(ModemClocksEnabledError)` is returned. + fn set_phy_clock_status(&self, enabled: bool) -> Result<(), ModemClocksEnabledError> { + // We shouldn't disable the PHY clock, if other clocks are still active. + if !enabled && self.any_modem_clocks_enabled() { + return Err(ModemClocksEnabledError); + } + // If the current clock status is already the same as the user provided one, we + // return. Otherwise we store the new value. + if PHY_CLOCK_ENABLED + .compare_exchange(!enabled, enabled, Ordering::Relaxed, Ordering::Relaxed) + .is_err() + { + return Ok(()); + } + self.set_clock_status(enabled, RadioPeripherals::Phy); + Ok(()) + } + /// Enable or disable a modem clock. + /// + /// If required, this will also enabled or disable the PHY clock. + /// # Panics + /// This panics, if [RadioPeripherals::Phy] is passed in, since the PHY + /// clock is controlled internally. + pub fn set_modem_clock_status(&self, enabled: bool, radio_peripheral: RadioPeripherals) { + // We first assert, that the clock we're touching isn't the PHY clock, since + // that's special. + assert_ne!(radio_peripheral, RadioPeripherals::Phy); + + // We obtain a reference to the AtomicBool tracking the state of the clock. + let modem_clock_enabled = match radio_peripheral { + #[cfg(bt)] + RadioPeripherals::Bt => &self.bt_clock_enabled, + #[cfg(wifi)] + RadioPeripherals::Wifi => &self.wifi_clock_enabled, + #[cfg(ieee802154)] + RadioPeripherals::Ieee802154 => &self.ieee802154_clock_enabled, + _ => unreachable!(), + }; + // If the states already match, we return. Otherwise the new state is stored. + if modem_clock_enabled + .compare_exchange(!enabled, !enabled, Ordering::Relaxed, Ordering::Relaxed) + .is_err() + { + return; + } + + if enabled { + // If the PHY clock isn't enabled yet, we disable it. + let _ = self.set_phy_clock_status(true); + self.set_clock_status(true, radio_peripheral); + } else { + self.set_clock_status(false, radio_peripheral); + let _ = self.set_phy_clock_status(false); + } + } +} diff --git a/esp-hal/src/soc/esp32/radio_clocks.rs b/esp-hal/src/soc/esp32/radio_clocks.rs index 828d8015860..6740421cfc8 100644 --- a/esp-hal/src/soc/esp32/radio_clocks.rs +++ b/esp-hal/src/soc/esp32/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; const DPORT_WIFI_CLK_WIFI_BT_COMMON_M: u32 = 0x000003c9; const DPORT_WIFI_CLK_WIFI_EN_M: u32 = 0x00000406; diff --git a/esp-hal/src/soc/esp32c2/radio_clocks.rs b/esp-hal/src/soc/esp32c2/radio_clocks.rs index 62b5fea634c..d94bb77ff59 100644 --- a/esp-hal/src/soc/esp32c2/radio_clocks.rs +++ b/esp-hal/src/soc/esp32c2/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; // Mask for clock bits used by both WIFI and Bluetooth, 0, 1, 2, 3, 7, 8, 9, 10, // 19, 20, 21, 22, 23 diff --git a/esp-hal/src/soc/esp32c3/radio_clocks.rs b/esp-hal/src/soc/esp32c3/radio_clocks.rs index 5ec9cf9e3c8..98a25add1ae 100644 --- a/esp-hal/src/soc/esp32c3/radio_clocks.rs +++ b/esp-hal/src/soc/esp32c3/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; // Mask for clock bits used by both WIFI and Bluetooth, 0, 1, 2, 3, 7, 8, 9, 10, // 19, 20, 21, 22, 23 diff --git a/esp-hal/src/soc/esp32c6/radio_clocks.rs b/esp-hal/src/soc/esp32c6/radio_clocks.rs index f9319969bcc..66e58b046a5 100644 --- a/esp-hal/src/soc/esp32c6/radio_clocks.rs +++ b/esp-hal/src/soc/esp32c6/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; impl RadioClockController for crate::peripherals::RADIO_CLK { fn enable(&mut self, peripheral: RadioPeripherals) { diff --git a/esp-hal/src/soc/esp32h2/radio_clocks.rs b/esp-hal/src/soc/esp32h2/radio_clocks.rs index 101e7ee20ad..2d4d0c55ead 100644 --- a/esp-hal/src/soc/esp32h2/radio_clocks.rs +++ b/esp-hal/src/soc/esp32h2/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; impl RadioClockController for crate::peripherals::RADIO_CLK { fn enable(&mut self, peripheral: RadioPeripherals) { diff --git a/esp-hal/src/soc/esp32s2/radio_clocks.rs b/esp-hal/src/soc/esp32s2/radio_clocks.rs index 32d942d2f24..6f23d46954c 100644 --- a/esp-hal/src/soc/esp32s2/radio_clocks.rs +++ b/esp-hal/src/soc/esp32s2/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; // Mask for clock bits used by both WIFI and Bluetooth, bit 0, 3, 6, 7, 8, 9 const DPORT_WIFI_CLK_WIFI_BT_COMMON_M: u32 = 0x000003c9; diff --git a/esp-hal/src/soc/esp32s3/radio_clocks.rs b/esp-hal/src/soc/esp32s3/radio_clocks.rs index f14a0c09a1e..9d1e6e53822 100644 --- a/esp-hal/src/soc/esp32s3/radio_clocks.rs +++ b/esp-hal/src/soc/esp32s3/radio_clocks.rs @@ -11,7 +11,7 @@ //! `RadioClockControl` struct. This trait provides methods to enable, disable, //! reset the MAC, initialize clocks and perform other related operations. -use crate::system::{RadioClockController, RadioPeripherals}; +use crate::radio_clock_ctrl::{RadioClockController, RadioPeripherals}; // Note: this comment has been copied from esp-idf, including the mistake. // Mask for clock bits used by both WIFI and Bluetooth, 0, 1, 2, 3, 7, diff --git a/esp-hal/src/system.rs b/esp-hal/src/system.rs index 0406d89d0f4..21f2f3c3249 100755 --- a/esp-hal/src/system.rs +++ b/esp-hal/src/system.rs @@ -1,9 +1,4 @@ //! # System Control -//! -//! ## Overview -//! -//! This `system` module defines the available radio peripherals and provides an -//! interface to control and configure radio clocks. use crate::peripherals::SYSTEM; @@ -985,42 +980,3 @@ impl PeripheralClockControl { } } } - -/// Enumeration of the available radio peripherals for this chip. -#[cfg(any(bt, ieee802154, wifi))] -pub enum RadioPeripherals { - /// Represents the PHY (Physical Layer) peripheral. - #[cfg(phy)] - Phy, - /// Represents the Bluetooth peripheral. - #[cfg(bt)] - Bt, - /// Represents the WiFi peripheral. - #[cfg(wifi)] - Wifi, - /// Represents the IEEE 802.15.4 peripheral. - #[cfg(ieee802154)] - Ieee802154, -} - -/// Control the radio peripheral clocks -#[cfg(any(bt, ieee802154, wifi))] -pub trait RadioClockController { - /// Enable the peripheral - fn enable(&mut self, peripheral: RadioPeripherals); - - /// Disable the peripheral - fn disable(&mut self, peripheral: RadioPeripherals); - - /// Reset the MAC - fn reset_mac(&mut self); - - /// Do any common initial initialization needed - fn init_clocks(&mut self); - - /// Initialize BLE RTC clocks - fn ble_rtc_clk_init(&mut self); - - /// Reset the Resolvable Private Address (RPA). - fn reset_rpa(&mut self); -} From 74dba57ff463c960c536069bd75b7850bf80e525 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Wed, 13 Nov 2024 23:03:13 +0100 Subject: [PATCH 6/8] Fixed compilation errors. --- esp-hal/src/radio_clock_ctrl.rs | 12 ++++++++---- esp-hal/src/rtc_cntl/rtc/esp32c6.rs | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/esp-hal/src/radio_clock_ctrl.rs b/esp-hal/src/radio_clock_ctrl.rs index 7e10cf07dbd..8c6497de036 100644 --- a/esp-hal/src/radio_clock_ctrl.rs +++ b/esp-hal/src/radio_clock_ctrl.rs @@ -1,14 +1,14 @@ -use core::{ - cell::RefCell, - sync::atomic::{AtomicBool, Ordering}, -}; +//! Control over the radio clocs. +use core::cell::RefCell; use critical_section::Mutex; +use portable_atomic::{AtomicBool, Ordering}; use crate::peripherals::RADIO_CLK; /// Enumeration of the available radio peripherals for this chip. #[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] #[cfg(any(bt, ieee802154, wifi))] pub enum RadioPeripherals { /// Represents the PHY (Physical Layer) peripheral. @@ -50,8 +50,11 @@ pub trait RadioClockController { } static PHY_CLOCK_ENABLED: AtomicBool = AtomicBool::new(false); +/// The PHY clock couldn't be disabled, because some modem clocks are still +/// active. pub struct ModemClocksEnabledError; +/// This struct allows shared access to the radio clocks. pub struct SharedRadioClockControl { radio_clock: Mutex>, @@ -63,6 +66,7 @@ pub struct SharedRadioClockControl { ieee802154_clock_enabled: AtomicBool, } impl SharedRadioClockControl { + /// Create a new [SharedRadioClockControl]. pub const fn new(radio_clock: RADIO_CLK) -> Self { Self { radio_clock: Mutex::new(RefCell::new(radio_clock)), diff --git a/esp-hal/src/rtc_cntl/rtc/esp32c6.rs b/esp-hal/src/rtc_cntl/rtc/esp32c6.rs index 66d72abf62c..ee31d9e4b87 100644 --- a/esp-hal/src/rtc_cntl/rtc/esp32c6.rs +++ b/esp-hal/src/rtc_cntl/rtc/esp32c6.rs @@ -21,9 +21,9 @@ use crate::{ XtalClock, }, peripherals::TIMG0, + radio_clock_ctrl::RadioPeripherals, rtc_cntl::RtcClock, soc::efuse::Efuse, - system::RadioPeripherals, }; const I2C_DIG_REG: u8 = 0x6d; From c346e730b5f81eb5cffa59e1cd21e4e021a217c8 Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Thu, 14 Nov 2024 13:51:00 +0100 Subject: [PATCH 7/8] Removed useless cfg. --- esp-hal/src/radio_clock_ctrl.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/esp-hal/src/radio_clock_ctrl.rs b/esp-hal/src/radio_clock_ctrl.rs index 8c6497de036..8931c80232c 100644 --- a/esp-hal/src/radio_clock_ctrl.rs +++ b/esp-hal/src/radio_clock_ctrl.rs @@ -9,7 +9,6 @@ use crate::peripherals::RADIO_CLK; /// Enumeration of the available radio peripherals for this chip. #[derive(Clone, Copy, Debug, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[cfg(any(bt, ieee802154, wifi))] pub enum RadioPeripherals { /// Represents the PHY (Physical Layer) peripheral. Phy, From d8d015aeb947619ab5c7ae2442d6125b5d8d006f Mon Sep 17 00:00:00 2001 From: Frostie314159 Date: Thu, 14 Nov 2024 14:08:29 +0100 Subject: [PATCH 8/8] Moved modem clock setting to separate internal function. --- esp-hal/src/radio_clock_ctrl.rs | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/esp-hal/src/radio_clock_ctrl.rs b/esp-hal/src/radio_clock_ctrl.rs index 8931c80232c..ebc20a4e6c8 100644 --- a/esp-hal/src/radio_clock_ctrl.rs +++ b/esp-hal/src/radio_clock_ctrl.rs @@ -96,8 +96,8 @@ impl SharedRadioClockControl { any_clocks_enabled } - /// Enable or disable the clock. - fn set_clock_status(&self, enabled: bool, radio_peripheral: RadioPeripherals) { + /// Enable or disable the clock, without changing the PHY clock status. + fn set_clock_status_raw(&self, enabled: bool, radio_peripheral: RadioPeripherals) { critical_section::with(|cs| { let mut radio_clock = self.radio_clock.borrow_ref_mut(cs); if enabled { @@ -126,9 +126,22 @@ impl SharedRadioClockControl { { return Ok(()); } - self.set_clock_status(enabled, RadioPeripherals::Phy); + self.set_clock_status_raw(enabled, RadioPeripherals::Phy); Ok(()) } + /// Set the status of the modem clock, enabling or disabling the PHY if + /// needed. + fn set_modem_clock_status_internal(&self, enabled: bool, radio_peripheral: RadioPeripherals) { + // Depending on if we're enabling or disabling, we need to set the PHY and modem + // clock status in a different order. + if enabled { + let _ = self.set_phy_clock_status(true); + self.set_clock_status_raw(true, radio_peripheral); + } else { + self.set_clock_status_raw(false, radio_peripheral); + let _ = self.set_phy_clock_status(false); + } + } /// Enable or disable a modem clock. /// /// If required, this will also enabled or disable the PHY clock. @@ -152,19 +165,11 @@ impl SharedRadioClockControl { }; // If the states already match, we return. Otherwise the new state is stored. if modem_clock_enabled - .compare_exchange(!enabled, !enabled, Ordering::Relaxed, Ordering::Relaxed) + .compare_exchange(!enabled, enabled, Ordering::Relaxed, Ordering::Relaxed) .is_err() { return; } - - if enabled { - // If the PHY clock isn't enabled yet, we disable it. - let _ = self.set_phy_clock_status(true); - self.set_clock_status(true, radio_peripheral); - } else { - self.set_clock_status(false, radio_peripheral); - let _ = self.set_phy_clock_status(false); - } + self.set_modem_clock_status_internal(enabled, radio_peripheral); } }