stm32/usb: extract common init code.

3 files changed, 53 insertions, 48 deletions

diff --git a/embassy-stm32/src/usb/mod.rs b/embassy-stm32/src/usb/mod.rs
index 974880900..130c728e6 100644
--- a/embassy-stm32/src/usb/mod.rs
+++ b/embassy-stm32/src/usb/mod.rs
@@ -4,3 +4,53 @@
 #[cfg_attr(otg, path = "otg.rs")]
 mod _version;
 pub use _version::*;
+
+use crate::interrupt::typelevel::Interrupt;
+use crate::rcc::sealed::RccPeripheral;
+
+/// clock, power initialization stuff that's common for USB and OTG.
+fn common_init<T: Instance>() {
+    #[cfg(any(stm32l4, stm32l5, stm32wb))]
+    critical_section::with(|_| crate::pac::PWR.cr2().modify(|w| w.set_usv(true)));
+
+    #[cfg(pwr_h5)]
+    critical_section::with(|_| crate::pac::PWR.usbscr().modify(|w| w.set_usb33sv(true)));
+
+    #[cfg(stm32h7)]
+    {
+        // If true, VDD33USB is generated by internal regulator from VDD50USB
+        // If false, VDD33USB and VDD50USB must be suplied directly with 3.3V (default on nucleo)
+        // TODO: unhardcode
+        let internal_regulator = false;
+
+        // Enable USB power
+        critical_section::with(|_| {
+            crate::pac::PWR.cr3().modify(|w| {
+                w.set_usb33den(true);
+                w.set_usbregen(internal_regulator);
+            })
+        });
+
+        // Wait for USB power to stabilize
+        while !crate::pac::PWR.cr3().read().usb33rdy() {}
+    }
+
+    #[cfg(stm32u5)]
+    {
+        // Enable USB power
+        critical_section::with(|_| {
+            crate::pac::PWR.svmcr().modify(|w| {
+                w.set_usv(true);
+                w.set_uvmen(true);
+            })
+        });
+
+        // Wait for USB power to stabilize
+        while !crate::pac::PWR.svmsr().read().vddusbrdy() {}
+    }
+
+    T::Interrupt::unpend();
+    unsafe { T::Interrupt::enable() };
+
+    <T as RccPeripheral>::enable_and_reset();
+}
diff --git a/embassy-stm32/src/usb/otg.rs b/embassy-stm32/src/usb/otg.rs
index d15d929f6..80a08f3c5 100644
--- a/embassy-stm32/src/usb/otg.rs
+++ b/embassy-stm32/src/usb/otg.rs
@@ -560,8 +560,7 @@ impl<'d, T: Instance> Bus<'d, T> {
 
 impl<'d, T: Instance> Bus<'d, T> {
     fn init(&mut self) {
-        #[cfg(stm32l4)]
-        critical_section::with(|_| crate::pac::PWR.cr2().modify(|w| w.set_usv(true)));
+        super::common_init::<T>();
 
         #[cfg(stm32f7)]
         {
@@ -589,22 +588,6 @@ impl<'d, T: Instance> Bus<'d, T> {
 
         #[cfg(stm32h7)]
         {
-            // If true, VDD33USB is generated by internal regulator from VDD50USB
-            // If false, VDD33USB and VDD50USB must be suplied directly with 3.3V (default on nucleo)
-            // TODO: unhardcode
-            let internal_regulator = false;
-
-            // Enable USB power
-            critical_section::with(|_| {
-                crate::pac::PWR.cr3().modify(|w| {
-                    w.set_usb33den(true);
-                    w.set_usbregen(internal_regulator);
-                })
-            });
-
-            // Wait for USB power to stabilize
-            while !crate::pac::PWR.cr3().read().usb33rdy() {}
-
             // Enable ULPI clock if external PHY is used
             let ulpien = !self.phy_type.internal();
             critical_section::with(|_| {
@@ -625,25 +608,6 @@ impl<'d, T: Instance> Bus<'d, T> {
             });
         }
 
-        #[cfg(stm32u5)]
-        {
-            // Enable USB power
-            critical_section::with(|_| {
-                crate::pac::PWR.svmcr().modify(|w| {
-                    w.set_usv(true);
-                    w.set_uvmen(true);
-                })
-            });
-
-            // Wait for USB power to stabilize
-            while !crate::pac::PWR.svmsr().read().vddusbrdy() {}
-        }
-
-        <T as RccPeripheral>::enable_and_reset();
-
-        T::Interrupt::unpend();
-        unsafe { T::Interrupt::enable() };
-
         let r = T::regs();
         let core_id = r.cid().read().0;
         trace!("Core id {:08x}", core_id);
diff --git a/embassy-stm32/src/usb/usb.rs b/embassy-stm32/src/usb/usb.rs
index b3b3470e4..1fb2c9ebb 100644
--- a/embassy-stm32/src/usb/usb.rs
+++ b/embassy-stm32/src/usb/usb.rs
@@ -12,7 +12,6 @@ use embassy_usb_driver::{
     Direction, EndpointAddress, EndpointAllocError, EndpointError, EndpointInfo, EndpointType, Event, Unsupported,
 };
 
-use crate::interrupt::typelevel::Interrupt;
 use crate::pac::usb::regs;
 use crate::pac::usb::vals::{EpType, Stat};
 use crate::pac::USBRAM;
@@ -258,18 +257,10 @@ impl<'d, T: Instance> Driver<'d, T> {
         dm: impl Peripheral<P = impl DmPin<T>> + 'd,
     ) -> Self {
         into_ref!(dp, dm);
-        T::Interrupt::unpend();
-        unsafe { T::Interrupt::enable() };
 
-        let regs = T::regs();
-
-        #[cfg(any(stm32l4, stm32l5, stm32wb))]
-        crate::pac::PWR.cr2().modify(|w| w.set_usv(true));
+        super::common_init::<T>();
 
-        #[cfg(pwr_h5)]
-        crate::pac::PWR.usbscr().modify(|w| w.set_usb33sv(true));
-
-        <T as RccPeripheral>::enable_and_reset();
+        let regs = T::regs();
 
         regs.cntr().write(|w| {
             w.set_pdwn(false);