Add USBPHYC clock configuration for H7RS series

1 files changed, 18 insertions, 11 deletions

diff --git a/embassy-usb-synopsys-otg/src/lib.rs b/embassy-usb-synopsys-otg/src/lib.rs
index 3ff965149..f90403936 100644
--- a/embassy-usb-synopsys-otg/src/lib.rs
+++ b/embassy-usb-synopsys-otg/src/lib.rs
@@ -584,20 +584,27 @@ impl<'d, const MAX_EP_COUNT: usize> Bus<'d, MAX_EP_COUNT> {
         });
     }
 
+    /// Applies configuration specific to
+    /// Core ID 0x0000_5000
     pub fn config_v5(&mut self) {
         let r = self.instance.regs;
+        let phy_type = self.instance.phy_type;
 
-        r.gccfg_v3().modify(|w| {
-            w.set_vbvaloven(true);
-            w.set_vbvaloval(true);
-            w.set_vbden(self.config.vbus_detection);
-        });
-
-        // Force B-peripheral session
-        r.gotgctl().modify(|w| {
-            w.set_vbvaloen(!self.config.vbus_detection);
-            w.set_bvaloval(true);
-        });
+        if phy_type == PhyType::InternalHighSpeed {
+            r.gccfg_v3().modify(|w| {
+                w.set_vbvaloven(!self.config.vbus_detection);
+                w.set_vbvaloval(!self.config.vbus_detection);
+                w.set_vbden(self.config.vbus_detection);
+            });
+        } else {
+            r.gotgctl().modify(|w| {
+                w.set_bvaloen(!self.config.vbus_detection);
+                w.set_bvaloval(!self.config.vbus_detection);
+            });
+            r.gccfg_v3().modify(|w| {
+                w.set_vbden(self.config.vbus_detection);
+            });
+        }
     }
 
     fn init(&mut self) {