Merge pull request #3271 from sjoerdsimons/ucpd-sop-filters

[UCPD] Add support for non-SOP packets

3 files changed, 76 insertions, 7 deletions

diff --git a/embassy-stm32/src/ucpd.rs b/embassy-stm32/src/ucpd.rs
index 40aea75cb..ee0a2c7c1 100644
--- a/embassy-stm32/src/ucpd.rs
+++ b/embassy-stm32/src/ucpd.rs
@@ -27,7 +27,7 @@ use crate::dma::{ChannelAndRequest, TransferOptions};
 use crate::interrupt;
 use crate::interrupt::typelevel::Interrupt;
 use crate::pac::ucpd::vals::{Anamode, Ccenable, PscUsbpdclk, Txmode};
-pub use crate::pac::ucpd::vals::{Phyccsel as CcSel, TypecVstateCc as CcVState};
+pub use crate::pac::ucpd::vals::{Phyccsel as CcSel, Rxordset, TypecVstateCc as CcVState};
 use crate::rcc::{self, RccPeripheral};
 
 pub(crate) fn init(
@@ -86,6 +86,34 @@ pub enum CcPull {
     Source3_0A,
 }
 
+/// UCPD configuration
+#[non_exhaustive]
+#[derive(Copy, Clone, Debug)]
+pub struct Config {
+    /// Receive SOP packets
+    pub sop: bool,
+    /// Receive SOP' packets
+    pub sop_prime: bool,
+    /// Receive SOP'' packets
+    pub sop_double_prime: bool,
+    /// Receive SOP'_Debug packets
+    pub sop_prime_debug: bool,
+    /// Receive SOP''_Debug packets
+    pub sop_double_prime_debug: bool,
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            sop: true,
+            sop_prime: false,
+            sop_double_prime: false,
+            sop_prime_debug: false,
+            sop_double_prime_debug: false,
+        }
+    }
+}
+
 /// UCPD driver.
 pub struct Ucpd<'d, T: Instance> {
     cc_phy: CcPhy<'d, T>,
@@ -98,6 +126,7 @@ impl<'d, T: Instance> Ucpd<'d, T> {
         _irq: impl interrupt::typelevel::Binding<T::Interrupt, InterruptHandler<T>> + 'd,
         cc1: impl Peripheral<P = impl Cc1Pin<T>> + 'd,
         cc2: impl Peripheral<P = impl Cc2Pin<T>> + 'd,
+        config: Config,
     ) -> Self {
         into_ref!(cc1, cc2);
         cc1.set_as_analog();
@@ -129,9 +158,15 @@ impl<'d, T: Instance> Ucpd<'d, T> {
             // 1.75us * 17 = ~30us
             w.set_ifrgap(17 - 1);
 
-            // TODO: Currently only hard reset and SOP messages can be received.
             // UNDOCUMENTED: This register can only be written while UCPDEN=0 (found by testing).
-            w.set_rxordseten(0b1001);
+            let rxordset = (config.sop as u16) << 0
+                | (config.sop_prime as u16) << 1
+                | (config.sop_double_prime as u16) << 2
+                // Hard reset
+                | 0x1 << 3
+                | (config.sop_prime_debug as u16) << 4
+                | (config.sop_double_prime_debug as u16) << 5;
+            w.set_rxordseten(rxordset);
 
             // Enable DMA
             w.set_txdmaen(true);
@@ -288,6 +323,22 @@ impl<'d, T: Instance> CcPhy<'d, T> {
     }
 }
 
+/// Receive SOP.
+#[derive(Debug, Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum Sop {
+    /// SOP
+    Sop,
+    /// SOP'
+    SopPrime,
+    /// SOP''
+    SopDoublePrime,
+    /// SOP'_Debug
+    SopPrimeDebug,
+    /// SOP''_Debug
+    SopDoublePrimeDebug,
+}
+
 /// Receive Error.
 #[derive(Debug, Clone, Copy)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
@@ -340,6 +391,13 @@ impl<'d, T: Instance> PdPhy<'d, T> {
     ///
     /// Returns the number of received bytes or an error.
     pub async fn receive(&mut self, buf: &mut [u8]) -> Result<usize, RxError> {
+        self.receive_with_sop(buf).await.map(|(_sop, size)| size)
+    }
+
+    /// Receives SOP and a PD message into the provided buffer.
+    ///
+    /// Returns the start of packet type and number of received bytes or an error.
+    pub async fn receive_with_sop(&mut self, buf: &mut [u8]) -> Result<(Sop, usize), RxError> {
         let r = T::REGS;
 
         let dma = unsafe {
@@ -388,7 +446,18 @@ impl<'d, T: Instance> PdPhy<'d, T> {
             }
         }
 
-        Ok(r.rx_payszr().read().rxpaysz().into())
+        let sop = match r.rx_ordsetr().read().rxordset() {
+            Rxordset::SOP => Sop::Sop,
+            Rxordset::SOPPRIME => Sop::SopPrime,
+            Rxordset::SOPDOUBLEPRIME => Sop::SopDoublePrime,
+            Rxordset::SOPPRIMEDEBUG => Sop::SopPrimeDebug,
+            Rxordset::SOPDOUBLEPRIMEDEBUG => Sop::SopDoublePrimeDebug,
+            Rxordset::CABLERESET => return Err(RxError::HardReset),
+            // Extension headers are not supported
+            _ => unreachable!(),
+        };
+
+        Ok((sop, r.rx_payszr().read().rxpaysz().into()))
     }
 
     fn enable_rx_interrupt(enable: bool) {
diff --git a/examples/stm32g4/src/bin/usb_c_pd.rs b/examples/stm32g4/src/bin/usb_c_pd.rs
index 7caea634f..2e87d3931 100644
--- a/examples/stm32g4/src/bin/usb_c_pd.rs
+++ b/examples/stm32g4/src/bin/usb_c_pd.rs
@@ -55,7 +55,7 @@ async fn main(_spawner: Spawner) {
 
     info!("Hello World!");
 
-    let mut ucpd = Ucpd::new(p.UCPD1, Irqs {}, p.PB6, p.PB4);
+    let mut ucpd = Ucpd::new(p.UCPD1, Irqs {}, p.PB6, p.PB4, Default::default());
     ucpd.cc_phy().set_pull(CcPull::Sink);
 
     info!("Waiting for USB connection...");
diff --git a/tests/stm32/src/bin/ucpd.rs b/tests/stm32/src/bin/ucpd.rs
index a6d13b34a..bd7b35d6b 100644
--- a/tests/stm32/src/bin/ucpd.rs
+++ b/tests/stm32/src/bin/ucpd.rs
@@ -106,8 +106,8 @@ async fn main(_spawner: Spawner) {
     info!("Hello World!");
 
     // Wire between PD0 and PA8
-    let ucpd1 = Ucpd::new(p.UCPD1, Irqs {}, p.PA8, p.PB15);
-    let ucpd2 = Ucpd::new(p.UCPD2, Irqs {}, p.PD0, p.PD2);
+    let ucpd1 = Ucpd::new(p.UCPD1, Irqs {}, p.PA8, p.PB15, Default::default());
+    let ucpd2 = Ucpd::new(p.UCPD2, Irqs {}, p.PD0, p.PD2, Default::default());
 
     join(
         source(ucpd1, p.DMA1_CH1, p.DMA1_CH2),