Add type-c traits for getting Discovered SVIDs, Discover Identity, executing Hard Reset

examples/std/src/lib/type_c/mock_controller.rs

@@ -4,6 +4,7 @@ use embassy_sync::{channel, mutex::Mutex, signal::Signal};
 use embedded_services::GlobalRawMutex;
 use embedded_services::named::Named;
 use embedded_usb_pd::ado::Ado;
+use embedded_usb_pd::vdm::structured::command::discover_identity::{sop, sop_prime};
 use embedded_usb_pd::{LocalPortId, PdError};
 use embedded_usb_pd::{PowerRole, type_c::Current};
 use embedded_usb_pd::{type_c::ConnectionState, ucsi::lpm};
@@ -14,6 +15,7 @@ use type_c_interface::control::dp::{DpConfig, DpPinConfig, DpStatus};
 use type_c_interface::control::pd::{PdStateMachineConfig, PortStatus};
 use type_c_interface::control::power::SystemPowerState;
 use type_c_interface::control::retimer::RetimerFwUpdateState;
+use type_c_interface::control::svid::DiscoveredSvids;
 use type_c_interface::control::tbt::TbtConfig;
 use type_c_interface::control::type_c::TypeCStateMachineState;
 use type_c_interface::control::usb::UsbControlConfig;
@@ -228,6 +230,29 @@ impl type_c_interface::controller::pd::Pd for Controller<'_> {
         debug!("Set Thunderbolt config for port {port:?}: {config:?}");
         Ok(())
     }
+
+    async fn hard_reset(&mut self, port: LocalPortId) -> Result<(), PdError> {
+        debug!("Hard reset for port {port:?}");
+        Ok(())
+    }
+
+    async fn get_discovered_svids(&mut self, port: LocalPortId) -> Result<DiscoveredSvids, PdError> {
+        debug!("Get discovered SVIDs for port {port:?}");
+        Ok(DiscoveredSvids::default())
+    }
+
+    async fn get_discover_identity_sop_response(&mut self, port: LocalPortId) -> Result<sop::ResponseVdos, PdError> {
+        debug!("Get Discover Identity SOP response for port {port:?}");
+        Err(PdError::Failed)
+    }
+
+    async fn get_discover_identity_sop_prime_response(
+        &mut self,
+        port: LocalPortId,
+    ) -> Result<sop_prime::ResponseVdos, PdError> {
+        debug!("Get Discover Identity SOP' response for port {port:?}");
+        Err(PdError::Failed)
+    }
 }
 
 impl type_c_interface::controller::max_sink_voltage::MaxSinkVoltage for Controller<'_> {

type-c-interface/Cargo.toml

@@ -15,6 +15,7 @@ defmt = { workspace = true, optional = true }
 embedded-services.workspace = true
 embedded-usb-pd.workspace = true
 power-policy-interface.workspace = true
+heapless.workspace = true
 
 [lints]
 workspace = true

type-c-interface/src/control/mod.rs

@@ -3,6 +3,7 @@ pub mod dp;
 pub mod pd;
 pub mod power;
 pub mod retimer;
+pub mod svid;
 pub mod tbt;
 pub mod type_c;
 pub mod usb;

type-c-interface/src/control/svid.rs

@@ -0,0 +1,62 @@
+use embedded_usb_pd::vdm::structured::Svid;
+use heapless::Vec;
+
+/// Response from the `Discover SVIDs REQ` message and the PortCommandData::GetDiscoveredSvids command.
+// Could be changed to hold the heapless::Vec directly if they were Copy or if PortResponseData was not Copy
+#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DiscoveredSvids {
+    num_sop: usize,
+    sop: [Svid; Self::NUM_SVIDS],
+
+    num_sop_prime: usize,
+    sop_prime: [Svid; Self::NUM_SVIDS],
+}
+
+impl DiscoveredSvids {
+    /// The number of SVIDs that can be reported in a single DiscoveredSvids response.
+    pub const NUM_SVIDS: usize = 8;
+
+    /// Create a new response object from `sop` and `sop_prime`.
+    pub fn new(sop: Vec<Svid, { Self::NUM_SVIDS }>, sop_prime: Vec<Svid, { Self::NUM_SVIDS }>) -> Self {
+        let num_sop = sop.len();
+        let num_sop_prime = sop_prime.len();
+
+        let mut sop_array = [Svid(0); _];
+        for (svid, dest) in sop.into_iter().zip(sop_array.iter_mut()) {
+            *dest = svid;
+        }
+
+        let mut sop_prime_array = [Svid(0); _];
+        for (svid, dest) in sop_prime.into_iter().zip(sop_prime_array.iter_mut()) {
+            *dest = svid;
+        }
+
+        Self {
+            num_sop,
+            sop: sop_array,
+            num_sop_prime,
+            sop_prime: sop_prime_array,
+        }
+    }
+
+    /// Returns the number of SVIDs discovered on the SOP port partner.
+    pub fn number_sop_svids(&self) -> usize {
+        self.num_sop
+    }
+
+    /// Returns an iterator over the SVIDs discovered on the SOP port partner.
+    pub fn svid_sop(&self) -> impl ExactSizeIterator<Item = Svid> {
+        self.sop.iter().copied().take(self.num_sop)
+    }
+
+    /// Returns the number of SVIDs discovered on the SOP' cable plug.
+    pub fn number_sop_prime_svids(&self) -> usize {
+        self.num_sop_prime
+    }
+
+    /// Returns an iterator over the SVIDs discovered on the SOP' cable plug.
+    pub fn svid_sop_prime(&self) -> impl ExactSizeIterator<Item = Svid> {
+        self.sop_prime.iter().copied().take(self.num_sop_prime)
+    }
+}

type-c-interface/src/controller/pd.rs

@@ -1,9 +1,11 @@
 use embedded_services::named::Named;
+use embedded_usb_pd::vdm::structured::command::discover_identity::{sop, sop_prime};
 use embedded_usb_pd::{LocalPortId, PdError, ado::Ado};
 
 use crate::control::{
     dp::{DpConfig, DpStatus},
     pd::{PdStateMachineConfig, PortStatus},
+    svid::DiscoveredSvids,
     tbt::TbtConfig,
     usb::UsbControlConfig,
     vdm::{AttnVdm, OtherVdm, SendVdm},
@@ -38,6 +40,8 @@ pub trait Pd: Named {
     fn send_vdm(&mut self, port: LocalPortId, tx_vdm: SendVdm) -> impl Future<Output = Result<(), PdError>>;
     /// Execute PD Data Reset for the given port
     fn execute_drst(&mut self, port: LocalPortId) -> impl Future<Output = Result<(), PdError>>;
+    /// Execute a Hard Reset on the given port.
+    fn hard_reset(&mut self, port: LocalPortId) -> impl Future<Output = Result<(), PdError>>;
 
     /// Get DisplayPort status for the given port
     fn get_dp_status(&mut self, port: LocalPortId) -> impl Future<Output = Result<DpStatus, PdError>>;
@@ -53,6 +57,21 @@ pub trait Pd: Named {
         port: LocalPortId,
         config: UsbControlConfig,
     ) -> impl Future<Output = Result<(), PdError>>;
+
+    /// Get the given port's discovered SVIDs
+    fn get_discovered_svids(&mut self, port: LocalPortId) -> impl Future<Output = Result<DiscoveredSvids, PdError>>;
+
+    /// Get the latest response from the Discover Identity command targeting SOP.
+    fn get_discover_identity_sop_response(
+        &mut self,
+        port: LocalPortId,
+    ) -> impl Future<Output = Result<sop::ResponseVdos, PdError>>;
+
+    /// Get the latest response from the Discover Identity command targeting SOP'.
+    fn get_discover_identity_sop_prime_response(
+        &mut self,
+        port: LocalPortId,
+    ) -> impl Future<Output = Result<sop_prime::ResponseVdos, PdError>>;
 }
 
 /// PD state machine related controller functionality

type-c-interface/src/port/pd.rs

@@ -1,9 +1,11 @@
 use embedded_services::named::Named;
+use embedded_usb_pd::vdm::structured::command::discover_identity::{sop, sop_prime};
 use embedded_usb_pd::{PdError, ado::Ado};
 
 use crate::control::{
     dp::{DpConfig, DpStatus},
     pd::{PdStateMachineConfig, PortStatus},
+    svid::DiscoveredSvids,
     tbt::TbtConfig,
     usb::UsbControlConfig,
     vdm::{AttnVdm, OtherVdm, SendVdm},
@@ -34,6 +36,8 @@ pub trait Pd: Named {
     fn send_vdm(&mut self, tx_vdm: SendVdm) -> impl Future<Output = Result<(), PdError>>;
     /// Execute PD Data Reset for this port
     fn execute_drst(&mut self) -> impl Future<Output = Result<(), PdError>>;
+    /// Execute a Hard Reset on this port.
+    fn hard_reset(&mut self) -> impl Future<Output = Result<(), PdError>>;
 
     /// Get DisplayPort status for this port
     fn get_dp_status(&mut self) -> impl Future<Output = Result<DpStatus, PdError>>;
@@ -45,6 +49,17 @@ pub trait Pd: Named {
 
     /// Set USB control configuration for this port
     fn set_usb_control(&mut self, config: UsbControlConfig) -> impl Future<Output = Result<(), PdError>>;
+
+    /// Get this port's discovered SVIDs
+    fn get_discovered_svids(&mut self) -> impl Future<Output = Result<DiscoveredSvids, PdError>>;
+
+    /// Get the latest response from the Discover Identity command targeting SOP.
+    fn get_discover_identity_sop_response(&mut self) -> impl Future<Output = Result<sop::ResponseVdos, PdError>>;
+
+    /// Get the latest response from the Discover Identity command targeting SOP'.
+    fn get_discover_identity_sop_prime_response(
+        &mut self,
+    ) -> impl Future<Output = Result<sop_prime::ResponseVdos, PdError>>;
 }
 
 /// PD state machine related controller functionality

type-c-service/src/controller/pd.rs

@@ -2,9 +2,11 @@
 use embedded_services::{event::Sender, sync::Lockable};
 use embedded_usb_pd::PdError;
 use embedded_usb_pd::ado::Ado;
+use embedded_usb_pd::vdm::structured::command::discover_identity::{sop, sop_prime};
 use type_c_interface::control::{
     dp::{DpConfig, DpStatus},
     pd::{PdStateMachineConfig, PortStatus},
+    svid::DiscoveredSvids,
     tbt::TbtConfig,
     usb::UsbControlConfig,
     vdm::{AttnVdm, OtherVdm, SendVdm},
@@ -130,6 +132,30 @@ impl<
     async fn set_usb_control(&mut self, config: UsbControlConfig) -> Result<(), PdError> {
         self.controller.lock().await.set_usb_control(self.port, config).await
     }
+
+    async fn hard_reset(&mut self) -> Result<(), PdError> {
+        self.controller.lock().await.hard_reset(self.port).await
+    }
+
+    async fn get_discovered_svids(&mut self) -> Result<DiscoveredSvids, PdError> {
+        self.controller.lock().await.get_discovered_svids(self.port).await
+    }
+
+    async fn get_discover_identity_sop_response(&mut self) -> Result<sop::ResponseVdos, PdError> {
+        self.controller
+            .lock()
+            .await
+            .get_discover_identity_sop_response(self.port)
+            .await
+    }
+
+    async fn get_discover_identity_sop_prime_response(&mut self) -> Result<sop_prime::ResponseVdos, PdError> {
+        self.controller
+            .lock()
+            .await
+            .get_discover_identity_sop_prime_response(self.port)
+            .await
+    }
 }
 
 impl<

type-c-service/src/driver/tps6699x.rs

@@ -16,6 +16,7 @@ use embedded_usb_pd::type_c::Current as TypecCurrent;
 use embedded_usb_pd::ucsi::lpm;
 use embedded_usb_pd::{DataRole, Error, LocalPortId, PdError, PlugOrientation, PowerRole};
 use fw_update_interface::basic::{Error as BasicFwUpdateError, FwUpdate as BasicFwUpdate};
+use heapless::Vec;
 use tps6699x::MAX_SUPPORTED_PORTS;
 use tps6699x::asynchronous::embassy::{self as tps6699x_drv, interrupt};
 use tps6699x::asynchronous::fw_update::UpdateTarget;
@@ -33,6 +34,7 @@ use type_c_interface::control::dp::{DpConfig, DpPinConfig, DpStatus};
 use type_c_interface::control::pd::{PdStateMachineConfig, PortStatus};
 use type_c_interface::control::power::SystemPowerState;
 use type_c_interface::control::retimer::RetimerFwUpdateState;
+use type_c_interface::control::svid::DiscoveredSvids;
 use type_c_interface::control::tbt::TbtConfig;
 use type_c_interface::control::type_c::TypeCStateMachineState;
 use type_c_interface::control::usb::UsbControlConfig;
@@ -719,6 +721,76 @@ impl<M: RawMutex, B: I2c> Pd for Tps6699x<'_, M, B> {
 
         { self.tps6699x.lock_inner().await.set_tbt_config(port, config_reg).await }.map_err(|e| self.log_error(e))
     }
+
+    async fn hard_reset(&mut self, port: LocalPortId) -> Result<(), PdError> {
+        self.guard_no_fw_update_active()?;
+        match self.tps6699x.execute_hrst(port).await.map_err(|e| self.log_error(e))? {
+            ReturnValue::Success => Ok(()),
+            r => {
+                error!("Error executing hard reset on port {}: {:#?}", port.0, r);
+                Err(PdError::InvalidResponse)
+            }
+        }
+    }
+
+    async fn get_discovered_svids(&mut self, port: LocalPortId) -> Result<DiscoveredSvids, PdError> {
+        self.guard_no_fw_update_active()?;
+        let svids = self
+            .tps6699x
+            .get_discovered_svids(port)
+            .await
+            .map_err(|e| self.log_error(e))?;
+        debug!("{:?} discovered SVIDs: {:?}", port, svids);
+        let mut sop = Vec::new();
+        for svid in svids.svid_sop().take(sop.capacity()) {
+            let _ = sop.push(svid);
+        }
+
+        let mut sop_prime = Vec::new();
+        for svid in svids.svid_sop_prime().take(sop_prime.capacity()) {
+            let _ = sop_prime.push(svid);
+        }
+
+        Ok(DiscoveredSvids::new(sop, sop_prime))
+    }
+
+    async fn get_discover_identity_sop_response(
+        &mut self,
+        port: LocalPortId,
+    ) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos, PdError> {
+        self.guard_no_fw_update_active()?;
+        let data = self
+            .tps6699x
+            .get_received_sop_identity_data(port)
+            .await
+            .map_err(|e| self.log_error(e))?;
+        match data.try_into() {
+            Ok(vdos) => Ok(vdos),
+            Err(e) => {
+                error!("Error deserializing Received SOP Identity Data: {:?}", e);
+                Err(PdError::Serialize)
+            }
+        }
+    }
+
+    async fn get_discover_identity_sop_prime_response(
+        &mut self,
+        port: LocalPortId,
+    ) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos, PdError> {
+        self.guard_no_fw_update_active()?;
+        let data = self
+            .tps6699x
+            .get_received_sop_prime_identity_data(port)
+            .await
+            .map_err(|e| self.log_error(e))?;
+        match data.try_into() {
+            Ok(vdos) => Ok(vdos),
+            Err(e) => {
+                error!("Error deserializing Received SOP Prime Identity Data: {:?}", e);
+                Err(PdError::Serialize)
+            }
+        }
+    }
 }
 
 impl<M: RawMutex, B: I2c> Retimer for Tps6699x<'_, M, B> {