From 38f59636574c82404591ba177aa07fa3029d9384 Mon Sep 17 00:00:00 2001
From: Jan Niehusmann <jan@gondor.com>
Date: Sun, 31 Dec 2023 16:01:59 +0000
Subject: [PATCH] Add startup_delay_multiplier parameter to xosc initialization

This works the same way as PICO_XOSC_STARTUP_DELAY_MULTIPLIER in the Pico SDK
---
 rp2040-hal/src/xosc.rs | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/rp2040-hal/src/xosc.rs b/rp2040-hal/src/xosc.rs
index 255c9bb22..2285687b4 100644
--- a/rp2040-hal/src/xosc.rs
+++ b/rp2040-hal/src/xosc.rs
@@ -53,7 +53,7 @@ pub fn setup_xosc_blocking(
     xosc_dev: XOSC,
     frequency: HertzU32,
 ) -> Result<CrystalOscillator<Stable>, Error> {
-    let initialized_xosc = CrystalOscillator::new(xosc_dev).initialize(frequency)?;
+    let initialized_xosc = CrystalOscillator::new(xosc_dev).initialize(frequency, 1)?;
 
     let stable_xosc_token = nb::block!(initialized_xosc.await_stabilization()).unwrap();
 
@@ -91,7 +91,12 @@ impl CrystalOscillator<Disabled> {
     }
 
     /// Initializes the XOSC : frequency range is set, startup delay is calculated and set.
-    pub fn initialize(self, frequency: HertzU32) -> Result<CrystalOscillator<Initialized>, Error> {
+    /// Set startup_delay_multiplier to a value > 1 when using a slow-starting oscillator.
+    pub fn initialize(
+        self,
+        frequency: HertzU32,
+        startup_delay_multiplier: u32,
+    ) -> Result<CrystalOscillator<Initialized>, Error> {
         const ALLOWED_FREQUENCY_RANGE: RangeInclusive<HertzU32> =
             HertzU32::MHz(1)..=HertzU32::MHz(15);
         //1 ms = 10e-3 sec and Freq = 1/T where T is in seconds so 1ms converts to 1000Hz
@@ -102,6 +107,10 @@ impl CrystalOscillator<Disabled> {
             return Err(Error::FrequencyOutOfRange);
         }
 
+        if startup_delay_multiplier == 0 {
+            return Err(Error::BadArgument);
+        }
+
         self.device.ctrl.write(|w| {
             w.freq_range()._1_15mhz();
             w
@@ -112,6 +121,7 @@ impl CrystalOscillator<Disabled> {
         //See Chapter 2, Section 16, §3)
         //We do the calculation first.
         let startup_delay = frequency.to_Hz() / (STABLE_DELAY_AS_HZ.to_Hz() * DIVIDER);
+        let startup_delay = startup_delay.saturating_mul(startup_delay_multiplier);
 
         //Then we check if it fits into an u16.
         let startup_delay: u16 = startup_delay.try_into().map_err(|_| Error::BadArgument)?;