Add port commands for getting Discovered SVIDs, Discover Identity, executing Hard Reset

embedded-service/src/type_c/controller.rs

@@ -11,7 +11,9 @@ use embedded_usb_pd::{
     ado::Ado,
     pdinfo::{AltMode, PowerPathStatus},
     type_c::ConnectionState,
+    vdm::structured::Svid,
 };
+use heapless::Vec;
 
 use super::{ATTN_VDM_LEN, ControllerId, OTHER_VDM_LEN, external};
 use crate::ipc::deferred;
@@ -262,6 +264,66 @@ pub enum TypeCStateMachineState {
     Disabled,
 }
 
+/// 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 [`PortResponseData::DiscoveredSvids`] response.
+    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)
+    }
+}
+
 /// Port-specific command data
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
@@ -319,6 +381,14 @@ pub enum PortCommandData {
     },
     /// Set the system power state
     SetSystemPowerState(SystemPowerState),
+    /// Get the port's discovered SVIDs
+    GetDiscoveredSvids,
+    /// Trigger a hard reset on the given port.
+    HardReset,
+    /// Get the response to a Discover Identity command sent to the given port with SOP
+    GetDiscoverIdentitySop,
+    /// Get the response to a Discover Identity command sent to the given port with SOP'
+    GetDiscoverIdentitySopPrime,
 }
 
 /// Port-specific commands
@@ -363,6 +433,12 @@ pub enum PortResponseData {
     DpStatus(DpStatus),
     /// UCSI response
     UcsiResponse(Result<Option<lpm::ResponseData>, PdError>),
+    /// Discovered SVIDs
+    DiscoveredSvids(DiscoveredSvids),
+    /// Discover Identity SOP response
+    DiscoverIdentitySop(embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos),
+    /// Discover Identity SOP' response
+    DiscoverIdentitySopPrime(embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos),
 }
 
 impl PortResponseData {
@@ -711,6 +787,37 @@ pub trait Controller {
         port: LocalPortId,
         state: SystemPowerState,
     ) -> impl Future<Output = Result<(), Error<Self::BusError>>>;
+
+    /// Get the discovered SVIDs for the given port.
+    fn get_discovered_svids(
+        &mut self,
+        port: LocalPortId,
+    ) -> impl Future<Output = Result<DiscoveredSvids, Error<Self::BusError>>>;
+
+    /// Trigger a hard reset on the given port.
+    fn hard_reset(&mut self, port: LocalPortId) -> impl Future<Output = Result<(), Error<Self::BusError>>>;
+
+    /// 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<
+            embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos,
+            Error<Self::BusError>,
+        >,
+    >;
+
+    /// 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<
+            embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos,
+            Error<Self::BusError>,
+        >,
+    >;
 }
 
 /// Internal context for managing PD controllers
@@ -771,6 +878,19 @@ pub(super) async fn lookup_controller(controller_id: ControllerId) -> Result<&'s
         .ok_or(PdError::InvalidController)
 }
 
+/// Lookup the controller and local port ID for a given global port ID.
+pub(super) async fn lookup_global_port(
+    port_id: GlobalPortId,
+) -> Result<(&'static Device<'static>, LocalPortId), PdError> {
+    for controller in CONTEXT.controllers.iter_only::<Device>() {
+        if let Ok(local_port) = controller.lookup_local_port(port_id) {
+            return Ok((controller, local_port));
+        }
+    }
+
+    Err(PdError::InvalidPort)
+}
+
 /// Get total number of ports on the system
 pub(super) fn get_num_ports() -> usize {
     CONTEXT
@@ -1285,6 +1405,65 @@ impl ContextToken {
         }
     }
 
+    /// Get the discovered SVIDs for the given port.
+    pub async fn get_discovered_svids(&self, port: GlobalPortId) -> Result<DiscoveredSvids, PdError> {
+        match self
+            .send_port_command(port, PortCommandData::GetDiscoveredSvids)
+            .await?
+        {
+            PortResponseData::DiscoveredSvids(svids) => Ok(svids),
+            r => {
+                error!("Invalid response: expected discovered SVIDs, got {:?}", r);
+                Err(PdError::InvalidResponse)
+            }
+        }
+    }
+
+    /// Trigger a hard reset on the given port.
+    pub async fn hard_reset(&self, port: GlobalPortId) -> Result<(), PdError> {
+        match self.send_port_command(port, PortCommandData::HardReset).await? {
+            PortResponseData::Complete => Ok(()),
+            _ => Err(PdError::InvalidResponse),
+        }
+    }
+
+    /// Get the latest response from the Discover Identity command targeting SOP.
+    pub async fn get_discover_identity_sop_response(
+        &self,
+        port: GlobalPortId,
+    ) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos, PdError> {
+        match self
+            .send_port_command(port, PortCommandData::GetDiscoverIdentitySop)
+            .await?
+        {
+            PortResponseData::DiscoverIdentitySop(response) => Ok(response),
+            r => {
+                error!("Invalid response: expected Discover Identity SOP response, got {:?}", r);
+                Err(PdError::InvalidResponse)
+            }
+        }
+    }
+
+    /// Get the latest response from the Discover Identity command targeting SOP'.
+    pub async fn get_discover_identity_sop_prime_response(
+        &self,
+        port: GlobalPortId,
+    ) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos, PdError> {
+        match self
+            .send_port_command(port, PortCommandData::GetDiscoverIdentitySopPrime)
+            .await?
+        {
+            PortResponseData::DiscoverIdentitySopPrime(response) => Ok(response),
+            r => {
+                error!(
+                    "Invalid response: expected Discover Identity SOP' response, got {:?}",
+                    r
+                );
+                Err(PdError::InvalidResponse)
+            }
+        }
+    }
+
     /// Broadcast a type-C message to all subscribers
     pub async fn broadcast_message(&self, message: CommsMessage) {
         CONTEXT.broadcaster.broadcast(message).await;

embedded-service/src/type_c/external.rs

@@ -6,7 +6,7 @@ use embedded_usb_pd::{GlobalPortId, LocalPortId, PdError, ucsi};
 use crate::type_c::{
     Cached,
     controller::{
-        PdStateMachineConfig, SystemPowerState, TbtConfig, TypeCStateMachineState, UsbControlConfig,
+        DiscoveredSvids, PdStateMachineConfig, SystemPowerState, TbtConfig, TypeCStateMachineState, UsbControlConfig,
         execute_external_ucsi_command,
     },
 };
@@ -15,7 +15,7 @@ use super::{
     ControllerId,
     controller::{
         ControllerStatus, DpConfig, DpStatus, PortStatus, RetimerFwUpdateState, SendVdm,
-        execute_external_controller_command, execute_external_port_command, lookup_controller,
+        execute_external_controller_command, execute_external_port_command, lookup_controller, lookup_global_port,
     },
 };
 
@@ -103,6 +103,14 @@ pub enum PortCommandData {
     },
     /// Set the system power state
     SetSystemPowerState(SystemPowerState),
+    /// Get the port's discovered SVIDs
+    GetDiscoveredSvids,
+    /// Trigger a hard reset on the given port.
+    HardReset,
+    /// Get the response to a Discover Identity command sent to the given port with SOP
+    GetDiscoverIdentitySop,
+    /// Get the response to a Discover Identity command sent to the given port with SOP'
+    GetDiscoverIdentitySopPrime,
 }
 
 /// Port-specific commands
@@ -127,6 +135,12 @@ pub enum PortResponseData {
     RetimerFwUpdateGetState(RetimerFwUpdateState),
     /// Get DisplayPort status
     GetDpStatus(DpStatus),
+    /// Get the port's discovered SVIDs
+    DiscoveredSvids(DiscoveredSvids),
+    /// Discover Identity response data for SOP
+    DiscoverIdentitySop(embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos),
+    /// Discover Identity response data for SOP'
+    DiscoverIdentitySopPrime(embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos),
 }
 
 /// Port-specific command response
@@ -247,6 +261,12 @@ pub async fn controller_port_to_global_id(
     lookup_controller(controller_id).await?.lookup_global_port(port_id)
 }
 
+/// Convert a global port ID to a (controller ID, local port ID)
+pub async fn global_port_to_controller_port(global_port: GlobalPortId) -> Result<(ControllerId, LocalPortId), PdError> {
+    let (controller, local_port) = lookup_global_port(global_port).await?;
+    Ok((controller.id(), local_port))
+}
+
 /// Get the retimer fw update status of the given port
 pub async fn port_get_rt_fw_update_status(port: GlobalPortId) -> Result<RetimerFwUpdateState, PdError> {
     match execute_external_port_command(Command::Port(PortCommand {
@@ -501,3 +521,59 @@ pub async fn set_type_c_state_machine_config(port: GlobalPortId, state: TypeCSta
         _ => Err(PdError::InvalidResponse),
     }
 }
+
+/// Get the port's discovered SVIDs
+pub async fn get_discovered_svids(port: GlobalPortId) -> Result<DiscoveredSvids, PdError> {
+    match execute_external_port_command(Command::Port(PortCommand {
+        port,
+        data: PortCommandData::GetDiscoveredSvids,
+    }))
+    .await?
+    {
+        PortResponseData::DiscoveredSvids(svids) => Ok(svids),
+        _ => Err(PdError::InvalidResponse),
+    }
+}
+
+/// Trigger a hard reset on the given port
+pub async fn hard_reset(port: GlobalPortId) -> Result<(), PdError> {
+    match execute_external_port_command(Command::Port(PortCommand {
+        port,
+        data: PortCommandData::HardReset,
+    }))
+    .await?
+    {
+        PortResponseData::Complete => Ok(()),
+        _ => Err(PdError::InvalidResponse),
+    }
+}
+
+/// Get the response to a Discover Identity command sent to the given port with SOP.
+pub async fn get_discover_identity_sop_response(
+    port: GlobalPortId,
+) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop::ResponseVdos, PdError> {
+    match execute_external_port_command(Command::Port(PortCommand {
+        port,
+        data: PortCommandData::GetDiscoverIdentitySop,
+    }))
+    .await?
+    {
+        PortResponseData::DiscoverIdentitySop(vdos) => Ok(vdos),
+        _ => Err(PdError::InvalidResponse),
+    }
+}
+
+/// Get the response to a Discover Identity command sent to the given port with SOP'.
+pub async fn get_discover_identity_sop_prime_response(
+    port: GlobalPortId,
+) -> Result<embedded_usb_pd::vdm::structured::command::discover_identity::sop_prime::ResponseVdos, PdError> {
+    match execute_external_port_command(Command::Port(PortCommand {
+        port,
+        data: PortCommandData::GetDiscoverIdentitySopPrime,
+    }))
+    .await?
+    {
+        PortResponseData::DiscoverIdentitySopPrime(vdos) => Ok(vdos),
+        _ => Err(PdError::InvalidResponse),
+    }
+}