From a0e056a629c166b38c536c68afbf852819f10143 Mon Sep 17 00:00:00 2001
From: Marvin Drees <marvin.drees@9elements.com>
Date: Fri, 6 Dec 2024 16:22:08 +0100
Subject: [PATCH] Update STM32U5 OTG HS clock handling

Signed-off-by: Marvin Drees <marvin.drees@9elements.com>
---
 embassy-stm32/src/rcc/u5.rs  | 29 +++++++++++++++++++++++++++++
 embassy-stm32/src/usb/mod.rs | 14 ++------------
 embassy-stm32/src/usb/otg.rs |  2 --
 3 files changed, 31 insertions(+), 14 deletions(-)

diff --git a/embassy-stm32/src/rcc/u5.rs b/embassy-stm32/src/rcc/u5.rs
index af99c77bc..dc77dc540 100644
--- a/embassy-stm32/src/rcc/u5.rs
+++ b/embassy-stm32/src/rcc/u5.rs
@@ -1,9 +1,13 @@
 pub use crate::pac::pwr::vals::Vos as VoltageScale;
+#[cfg(all(peri_usb_otg_hs))]
+pub use crate::pac::rcc::vals::Otghssel;
 pub use crate::pac::rcc::vals::{
     Hpre as AHBPrescaler, Msirange, Msirange as MSIRange, Plldiv as PllDiv, Pllm as PllPreDiv, Plln as PllMul,
     Pllsrc as PllSource, Ppre as APBPrescaler, Sw as Sysclk,
 };
 use crate::pac::rcc::vals::{Hseext, Msirgsel, Pllmboost, Pllrge};
+#[cfg(all(peri_usb_otg_hs))]
+pub use crate::pac::{syscfg::vals::Usbrefcksel, SYSCFG};
 use crate::pac::{FLASH, PWR, RCC};
 use crate::rcc::LSI_FREQ;
 use crate::time::Hertz;
@@ -295,6 +299,31 @@ pub(crate) unsafe fn init(config: Config) {
 
     let rtc = config.ls.init();
 
+    #[cfg(all(stm32u5, peri_usb_otg_hs))]
+    let usb_refck = match config.mux.otghssel {
+        Otghssel::HSE => hse,
+        Otghssel::HSE_DIV_2 => hse.map(|hse_val| hse_val / 2u8),
+        Otghssel::PLL1_P => pll1.p,
+        Otghssel::PLL1_P_DIV_2 => pll1.p.map(|pll1p_val| pll1p_val / 2u8),
+    };
+    #[cfg(all(stm32u5, peri_usb_otg_hs))]
+    let usb_refck_sel = match usb_refck {
+        Some(clk_val) => match clk_val {
+            Hertz(16_000_000) => Usbrefcksel::MHZ16,
+            Hertz(19_200_000) => Usbrefcksel::MHZ19_2,
+            Hertz(20_000_000) => Usbrefcksel::MHZ20,
+            Hertz(24_000_000) => Usbrefcksel::MHZ24,
+            Hertz(26_000_000) => Usbrefcksel::MHZ26,
+            Hertz(32_000_000) => Usbrefcksel::MHZ32,
+            _ => panic!("cannot select OTG_HS reference clock with source frequency of {} Hz, must be one of 16, 19.2, 20, 24, 26, 32 MHz", clk_val),
+        },
+        None => Usbrefcksel::MHZ24,
+    };
+    #[cfg(all(stm32u5, peri_usb_otg_hs))]
+    SYSCFG.otghsphycr().modify(|w| {
+        w.set_clksel(usb_refck_sel);
+    });
+
     let lse = config.ls.lse.map(|l| l.frequency);
     let lsi = config.ls.lsi.then_some(LSI_FREQ);
 
diff --git a/embassy-stm32/src/usb/mod.rs b/embassy-stm32/src/usb/mod.rs
index 9737b9b3f..ae5963420 100644
--- a/embassy-stm32/src/usb/mod.rs
+++ b/embassy-stm32/src/usb/mod.rs
@@ -15,7 +15,7 @@ fn common_init<T: Instance>() {
     let freq = T::frequency();
 
     // On the H7RS, the USBPHYC embeds a PLL accepting one of the input frequencies listed below and providing 48MHz to OTG_FS and 60MHz to OTG_HS internally
-    #[cfg(stm32h7rs)]
+    #[cfg(any(stm32h7rs, all(stm32u5, peri_usb_otg_hs)))]
     if ![16_000_000, 19_200_000, 20_000_000, 24_000_000, 26_000_000, 32_000_000].contains(&freq.0) {
         panic!(
             "USB clock should be one of 16, 19.2, 20, 24, 26, 32Mhz but is {} Hz. Please double-check your RCC settings.",
@@ -25,8 +25,7 @@ fn common_init<T: Instance>() {
     // Check frequency is within the 0.25% tolerance allowed by the spec.
     // Clock might not be exact 48Mhz due to rounding errors in PLL calculation, or if the user
     // has tight clock restrictions due to something else (like audio).
-    #[cfg(not(stm32h7rs))]
-    #[cfg(not(all(stm32u5, peri_usb_otg_hs)))]
+    #[cfg(not(any(stm32h7rs, all(stm32u5, peri_usb_otg_hs))))]
     if freq.0.abs_diff(48_000_000) > 120_000 {
         panic!(
             "USB clock should be 48Mhz but is {} Hz. Please double-check your RCC settings.",
@@ -34,15 +33,6 @@ fn common_init<T: Instance>() {
         )
     }
 
-    // For OTG-HS on STM32U5 only the 32MHz clock is fast enough (Table 762, Sect 73.14.4)
-    #[cfg(all(stm32u5, peri_usb_otg_hs))]
-    if freq.0.abs_diff(32_000_000) > 120_000 {
-        panic!(
-            "USB clock should be 32Mhz but is {} Hz. Please double-check your RCC settings.",
-            freq.0
-        )
-    }
-
     #[cfg(any(stm32l4, stm32l5, stm32wb, stm32u0))]
     critical_section::with(|_| crate::pac::PWR.cr2().modify(|w| w.set_usv(true)));
 
diff --git a/embassy-stm32/src/usb/otg.rs b/embassy-stm32/src/usb/otg.rs
index d1b38a558..d3c7978e4 100644
--- a/embassy-stm32/src/usb/otg.rs
+++ b/embassy-stm32/src/usb/otg.rs
@@ -315,9 +315,7 @@ impl<'d, T: Instance> Bus<'d, T> {
 
         #[cfg(all(stm32u5, peri_usb_otg_hs))]
         {
-            // Only the 32MHz clock is suitable here, which the magic number represents
             crate::pac::SYSCFG.otghsphycr().modify(|w| {
-                w.set_clksel(11);
                 w.set_en(true);
             });