diff --git a/libs/bao1x-hal/src/clocks.rs b/libs/bao1x-hal/src/clocks.rs index 47c9a6017..dab5bb9a0 100644 --- a/libs/bao1x-hal/src/clocks.rs +++ b/libs/bao1x-hal/src/clocks.rs @@ -2,6 +2,8 @@ use bao1x_api::find_pll_params; #[cfg(all(feature = "std", feature = "board-baosec"))] use bao1x_api::{IoxHal, IoxPort}; use bitbybit::bitfield; +#[cfg(not(feature = "std"))] +use num_traits::float::FloatCore; use utralib::*; #[cfg(all(feature = "board-baosec", not(feature = "oem-baosec-lite")))] @@ -270,6 +272,31 @@ pub fn divide_by_fd(fd: u32, in_freq_hz: u32) -> u32 { out_freq_khz * 1_000 } +/// Returns the floating point number to multiply the VCO frequency by to get the +/// resulting frequency. i.e. if the FCO is at 700 MHz, the net frequency is +/// equal to `(700_000_000f32 * fd_to_ratio()) as u32` +pub fn fd_to_ratio(fd: u32) -> f32 { (fd as f32 + 1.0) / (FD_MAX as f32) } + +/// when `round_freq_up` is `true`, the resulting divider will result in a frequency that is +/// rounded up from the target ratio, if it can't be perfectly achieved. +/// +/// Otherwise, the result will round to the next lowest frequency that can be achieved. +/// +/// Note that any divide ratio > 1.0 will result in a divide ration of 1.0: this is a +/// *divide* ratio, not a multiplier. It can only strictly result in frequency that are lower +/// than the input. +/// +/// This will never return a result that causes the clock to stop. So any divisors smaller +/// than 1/128 will result in a divisor of 1/128. +pub fn divide_ratio_to_fd(ratio: f32, round_freq_up: bool) -> u8 { + let target_div = ((FD_MAX as f32 * ratio) - 1.0).max(1.0); + if round_freq_up { + (target_div.floor() as u32).min(255) as u8 + } else { + (target_div.ceil() as u32).min(255) as u8 + } +} + #[cfg(feature = "std")] pub struct ClockManagerImpl { pub vco_freq: u32, @@ -414,6 +441,56 @@ impl ClockManagerImpl { self.perclk = divide_by_fd(perclk_fd, pll0_freq) / 2; } + pub fn fclk_fd(&self) -> u8 { (self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0) & 0xFF) as u8 } + + pub fn aclk_fd(&self) -> u8 { (self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1) & 0xFF) as u8 } + + pub fn hclk_fd(&self) -> u8 { (self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2) & 0xFF) as u8 } + + pub fn iclk_fd(&self) -> u8 { (self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3) & 0xFF) as u8 } + + pub fn pclk_fd(&self) -> u8 { (self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4) & 0xFF) as u8 } + + pub fn set_fclk_fd(&mut self, fd: u8) { + self.sysctrl.wo( + utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0, + self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0) & !0xFF | fd as u32, + ); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + } + + pub fn set_aclk_fd(&mut self, fd: u8) { + self.sysctrl.wo( + utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1, + self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1) & !0xFF | fd as u32, + ); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + } + + pub fn set_hclk_fd(&mut self, fd: u8) { + self.sysctrl.wo( + utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2, + self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2) & !0xFF | fd as u32, + ); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + } + + pub fn set_iclk_fd(&mut self, fd: u8) { + self.sysctrl.wo( + utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3, + self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3) & !0xFF | fd as u32, + ); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + } + + pub fn set_pclk_fd(&mut self, fd: u8) { + self.sysctrl.wo( + utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4, + self.sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4) & !0xFF | fd as u32, + ); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + } + /// Safety: this pulls out the hardware base for the susres block. The receiver of this /// address must manually manage all concurrency issues that could happen with respect to /// using the susres block. diff --git a/services/bao1x-hal-service/src/servers/susres.rs b/services/bao1x-hal-service/src/servers/susres.rs index a68520617..f21e4936a 100644 --- a/services/bao1x-hal-service/src/servers/susres.rs +++ b/services/bao1x-hal-service/src/servers/susres.rs @@ -180,15 +180,48 @@ fn susres_service() { use bao1x_api::bio::BioApi; let mut bio = bao1x_hal::bio::Bio::new(); + // hard-coded to the value of the external crystal - this is a hardware // reference value that should never change; eg, USB doesn't work if this // isn't 48 MHz. - bio.update_bio_freq(48_000_000); + let fclk_fd_backup = clk_mgr.fclk_fd(); + + // TODO: this is a hack to get past some issue with clock rate changes + // + // In theory: + // - the set_clk_fd(0xff) call should reset the fclk divider to 1:1 + // - this would mean the bio_freq is getting 48MHz + // In practice: + // - the fd seems "stuck" at /2 for fd settings from 0xff-0x3f + // - so the pulse width outputs on BIO is the same for 0xff, 0x7f, 0x3f + // - if I reduce reduce fd setting to 0x1f, then, I can see the clock rate is 1/8th + // What has been tried: + // - Forcing the clocks to a fixed 0xFF setting regardless of input - I can see that + // BIO clocks, after wfi, are incorrect in this case. Which means that set_fclk_fd + // is "getting through" + // - Adding wait states, etc. around the CGUSET call does not seem to change the + // situation + // + // The compromise for now is to detect the "fast_bio" setting at init by heuristically + // looking at the fd_backup field, and then specifying an offset frequency to the + // update_bio_freq that results in the correct waveform. + // + // Possible cause: + // - The update_bio_freq routine is being "too clever" and compensating for the fd + // setting. But it seems like the routine just takes the putative fclk setting as + // the argument to update_bio_freq(), I don't see any compensation happening... + if fclk_fd_backup == 0xff { + bio.update_bio_freq(48_000_000); + } else { + bio.update_bio_freq(24_000_000); + } + clk_mgr.set_fclk_fd(0xff); // set divider to 1:1 ratio clk_mgr.wfi(); // ~~~ time passes, but we're on carbonite so we don't notice ~~~ clk_mgr.restore_wfi(); + clk_mgr.set_fclk_fd(fclk_fd_backup); // restore the FD before restoring potentially fast PLL setting bio.update_bio_freq(clk_mgr.fclk); // when wfi() returns, it means we've resumed