diff --git a/rp2040-hal/CHANGELOG.md b/rp2040-hal/CHANGELOG.md index 73a76c0..c9b051b 100644 --- a/rp2040-hal/CHANGELOG.md +++ b/rp2040-hal/CHANGELOG.md @@ -13,7 +13,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed -- Updated embedded-hal alpha support to version 1.0.0-alpha.7 +- Update embedded-hal alpha support to version 1.0.0-alpha.7 +- Avoid 64-bit division in clock calculations ## [0.3.0] - 2021-12-19 diff --git a/rp2040-hal/src/clocks/macros.rs b/rp2040-hal/src/clocks/macros.rs index fc75261..e814640 100644 --- a/rp2040-hal/src/clocks/macros.rs +++ b/rp2040-hal/src/clocks/macros.rs @@ -180,19 +180,13 @@ macro_rules! clock { #[doc = "Configure `"$name"`"] fn configure_clock>(&mut self, src: &S, freq: Hertz) -> Result<(), ClockError>{ - let src_freq: Hertz = src.get_freq().into(); + let src_freq: Hertz = src.get_freq().into(); if freq.gt(&src_freq){ return Err(ClockError::CantIncreaseFreq); } - // Div register is 24.8) int.frac divider so multiply by 2^8 (left shift by 8) - let shifted_src_freq = src_freq * (1 << 8); - let div = if freq.eq(&src_freq) { - 1 << 8 - } else { - (shifted_src_freq / freq.integer() as u64).integer() as u32 - }; + let div = fractional_div(src_freq.integer(), freq.integer()).ok_or(ClockError::FrequencyTooLow)?; // If increasing divisor, set divisor before source. Otherwise set source // before divisor. This avoids a momentary overspeed when e.g. switching @@ -223,8 +217,7 @@ macro_rules! clock { self.set_div(div); // Store the configured frequency - // div contains both the integer part and the fractional part so we need to shift the src_freq equally - self.frequency = (shifted_src_freq / div as u64).try_into().map_err(|_| ClockError::FrequencyToHigh)?; + self.frequency = fractional_div(src_freq.integer(), div).ok_or(ClockError::FrequencyTooHigh)?.Hz(); Ok(()) } @@ -337,19 +330,13 @@ macro_rules! stoppable_clock { #[doc = "Configure `"$name"`"] fn configure_clock>(&mut self, src: &S, freq: Hertz) -> Result<(), ClockError>{ - let src_freq: Hertz = src.get_freq().into(); + let src_freq: Hertz = src.get_freq().into(); if freq.gt(&src_freq){ return Err(ClockError::CantIncreaseFreq); } - // Div register is 24.8) int.frac divider so multiply by 2^8 (left shift by 8) - let shifted_src_freq = src_freq * (1 << 8); - let div = if freq.eq(&src_freq) { - 1 << 8 - } else { - (shifted_src_freq / freq.integer() as u64).integer() as u32 - }; + let div = fractional_div(src_freq.integer(), freq.integer()).ok_or(ClockError::FrequencyTooLow)?; // If increasing divisor, set divisor before source. Otherwise set source // before divisor. This avoids a momentary overspeed when e.g. switching @@ -386,7 +373,7 @@ macro_rules! stoppable_clock { self.set_div(div); // Store the configured frequency - self.frequency = (shifted_src_freq / div as u64).try_into().map_err(|_| ClockError::FrequencyToHigh)?; + self.frequency = fractional_div(src_freq.integer(), div).ok_or(ClockError::FrequencyTooHigh)?.Hz(); Ok(()) } diff --git a/rp2040-hal/src/clocks/mod.rs b/rp2040-hal/src/clocks/mod.rs index b1a546c..e8cb8f8 100644 --- a/rp2040-hal/src/clocks/mod.rs +++ b/rp2040-hal/src/clocks/mod.rs @@ -70,10 +70,7 @@ use crate::{ watchdog::Watchdog, xosc::{setup_xosc_blocking, CrystalOscillator, Error as XoscError, Stable}, }; -use core::{ - convert::{Infallible, TryInto}, - marker::PhantomData, -}; +use core::{convert::Infallible, marker::PhantomData}; use embedded_time::rate::*; use pac::{CLOCKS, PLL_SYS, PLL_USB, RESETS, XOSC}; @@ -106,7 +103,9 @@ pub enum ClockError { /// The frequency desired is higher than the source frequency CantIncreaseFreq, /// The desired frequency is to high (would overflow an u32) - FrequencyToHigh, + FrequencyTooHigh, + /// The desired frequency is too low (divider can't reach the desired value) + FrequencyTooLow, } /// For clocks @@ -354,3 +353,30 @@ pub fn init_clocks_and_plls( .map_err(InitError::ClockError)?; Ok(clocks) } + +// Calculates (numerator<<8)/denominator, avoiding 64bit division +// Returns None if the result would not fit in 32 bit. +fn fractional_div(numerator: u32, denominator: u32) -> Option { + if denominator.eq(&numerator) { + return Some(1 << 8); + } + + let div_int = numerator / denominator; + if div_int >= 1 << 24 { + return None; + } + + let div_rem = numerator - (div_int * denominator); + + let div_frac = if div_rem < 1 << 24 { + // div_rem is small enough to shift it by 8 bits without overflow + (div_rem << 8) / denominator + } else { + // div_rem is too large. Shift denominator right, instead. + // As 1<<24 < div_rem < denominator, relative error caused by the + // lost lower 8 bits of denominator is smaller than 2^-16 + (div_rem) / (denominator >> 8) + }; + + Some((div_int << 8) + div_frac) +}