2 files changed, 130 insertions, 0 deletions

diff --git a/embassy-stm32/src/ucpd.rs b/embassy-stm32/src/ucpd.rs
index ee0a2c7c1..ac31f46a4 100644
--- a/embassy-stm32/src/ucpd.rs
+++ b/embassy-stm32/src/ucpd.rs
@@ -175,6 +175,21 @@ impl<'d, T: Instance> Ucpd<'d, T> {
             w.set_ucpden(true);
         });
 
+        r.cfgr2().write(|w| {
+            w.set_rxafilten(true);
+        });
+
+        // Software trim according to RM0481, p. 2650
+        #[cfg(stm32h5)]
+        {
+            let trim_rd_cc1 = unsafe { *(0x4002_242C as *const u32) & 0xF };
+            let trim_rd_cc2 = unsafe { ((*(0x4002_242C as *const u32)) >> 8) & 0xF };
+
+            r.cfgr3().write(|w| {
+                w.set_trim_cc1_rd(trim_rd_cc1 as u8);
+                w.set_trim_cc2_rd(trim_rd_cc2 as u8);
+            });
+        }
         Self {
             cc_phy: CcPhy { _lifetime: PhantomData },
         }
@@ -278,6 +293,31 @@ impl<'d, T: Instance> CcPhy<'d, T> {
             });
         });
 
+        // Software trim according to RM0481, p. 2668
+        #[cfg(stm32h5)]
+        T::REGS.cfgr3().modify(|w| match cc_pull {
+            CcPull::Source1_5A => {
+                #[cfg(stm32h5)]
+                {
+                    let trim_1a5_cc1 = unsafe { *(0x08FF_F844 as *const u32) & 0xF };
+                    let trim_1a5_cc2 = unsafe { ((*(0x08FF_F844 as *const u32)) >> 16) & 0xF };
+
+                    w.set_trim_cc1_rp(trim_1a5_cc1 as u8);
+                    w.set_trim_cc2_rp(trim_1a5_cc2 as u8);
+                };
+            }
+            _ => {
+                #[cfg(stm32h5)]
+                {
+                    let trim_3a0_cc1 = unsafe { (*(0x4002_242C as *const u32) >> 4) & 0xF };
+                    let trim_3a0_cc2 = unsafe { ((*(0x4002_242C as *const u32)) >> 12) & 0xF };
+
+                    w.set_trim_cc1_rp(trim_3a0_cc1 as u8);
+                    w.set_trim_cc2_rp(trim_3a0_cc2 as u8);
+                };
+            }
+        });
+
         // Disable dead-battery pull-down resistors which are enabled by default on boot.
         critical_section::with(|cs| {
             init(
diff --git a/examples/stm32h5/src/bin/usb_c_pd.rs b/examples/stm32h5/src/bin/usb_c_pd.rs
new file mode 100644
index 000000000..3bf3ab258
--- /dev/null
+++ b/examples/stm32h5/src/bin/usb_c_pd.rs
@@ -0,0 +1,90 @@
+#![no_std]
+#![no_main]
+
+use defmt::{error, info, Format};
+use embassy_executor::Spawner;
+use embassy_stm32::gpio::Output;
+use embassy_stm32::ucpd::{self, CcPhy, CcPull, CcSel, CcVState, Ucpd};
+use embassy_stm32::{bind_interrupts, peripherals, Config};
+use embassy_time::{with_timeout, Duration};
+use {defmt_rtt as _, panic_probe as _};
+
+bind_interrupts!(struct Irqs {
+    UCPD1 => ucpd::InterruptHandler<peripherals::UCPD1>;
+});
+
+#[derive(Debug, Format)]
+enum CableOrientation {
+    Normal,
+    Flipped,
+    DebugAccessoryMode,
+}
+
+// Returns true when the cable
+async fn wait_attached<T: ucpd::Instance>(cc_phy: &mut CcPhy<'_, T>) -> CableOrientation {
+    loop {
+        let (cc1, cc2) = cc_phy.vstate();
+        if cc1 == CcVState::LOWEST && cc2 == CcVState::LOWEST {
+            // Detached, wait until attached by monitoring the CC lines.
+            cc_phy.wait_for_vstate_change().await;
+            continue;
+        }
+
+        // Attached, wait for CC lines to be stable for tCCDebounce (100..200ms).
+        if with_timeout(Duration::from_millis(100), cc_phy.wait_for_vstate_change())
+            .await
+            .is_ok()
+        {
+            // State has changed, restart detection procedure.
+            continue;
+        };
+
+        // State was stable for the complete debounce period, check orientation.
+        return match (cc1, cc2) {
+            (_, CcVState::LOWEST) => CableOrientation::Normal,  // CC1 connected
+            (CcVState::LOWEST, _) => CableOrientation::Flipped, // CC2 connected
+            _ => CableOrientation::DebugAccessoryMode,          // Both connected (special cable)
+        };
+    }
+}
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let config = Config::default();
+    let p = embassy_stm32::init(config);
+
+    info!("Hello World!");
+
+    // This pin controls the dead-battery mode on the attached TCPP01-M12.
+    // If low, TCPP01-M12 disconnects CC lines and presents dead-battery resistance on CC lines, thus set high.
+    let _tcpp01_m12_ndb = Output::new(p.PA9, embassy_stm32::gpio::Level::High, embassy_stm32::gpio::Speed::Low);
+
+    let mut ucpd = Ucpd::new(p.UCPD1, Irqs {}, p.PB13, p.PB14, Default::default());
+    ucpd.cc_phy().set_pull(CcPull::Sink);
+
+    info!("Waiting for USB connection...");
+    let cable_orientation = wait_attached(ucpd.cc_phy()).await;
+    info!("USB cable connected, orientation: {}", cable_orientation);
+
+    let cc_sel = match cable_orientation {
+        CableOrientation::Normal => {
+            info!("Starting PD communication on CC1 pin");
+            CcSel::CC1
+        }
+        CableOrientation::Flipped => {
+            info!("Starting PD communication on CC2 pin");
+            CcSel::CC2
+        }
+        CableOrientation::DebugAccessoryMode => panic!("No PD communication in DAM"),
+    };
+    let (_cc_phy, mut pd_phy) = ucpd.split_pd_phy(p.GPDMA1_CH0, p.GPDMA1_CH1, cc_sel);
+
+    loop {
+        // Enough space for the longest non-extended data message.
+        let mut buf = [0_u8; 30];
+        match pd_phy.receive(buf.as_mut()).await {
+            Ok(n) => info!("USB PD RX: {=[u8]:?}", &buf[..n]),
+            Err(e) => error!("USB PD RX: {}", e),
+        }
+    }
+}