type-c-service/max_sink_voltage: Port recovery logic from v1

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

@@ -19,4 +19,6 @@ pub enum Event {
 pub enum Loopback {
     /// Port event
     PortEvent(PortEventBitfield),
+    /// Sink ready deadline invalidated
+    SinkReadyDeadlineInvalidated,
 }

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

@@ -1,7 +1,7 @@
 //! This module contains event receiver types for the controller wrapper.
 use core::array;
 use core::future::pending;
-use embassy_futures::select::{Either, select};
+use embassy_futures::select::{Either3, select3};
 use embassy_time::Timer;
 use embedded_services::error;
 use embedded_services::event::{NonBlockingSender, Receiver};
@@ -40,49 +40,6 @@ impl<const N: usize, S: NonBlockingSender<PortEventBitfield>> PortEventSplitter<
     }
 }
 
-/// Struct to receive and stream port events from the controller.
-pub struct PortEventReceiver<R: Receiver<PortEventBitfield>, LoopbackReceiver: Receiver<Loopback>> {
-    /// Receiver for the controller's interrupt events
-    receiver: R,
-    /// Port event streaming state
-    streaming_state: Option<PortEventStreamer<array::IntoIter<PortEventBitfield, 1>>>,
-    /// Loopback receiver for software-generated events
-    loopback_receiver: LoopbackReceiver,
-}
-
-impl<R: Receiver<PortEventBitfield>, LoopbackReceiver: Receiver<Loopback>> PortEventReceiver<R, LoopbackReceiver> {
-    /// Create a new instance
-    pub fn new(receiver: R, loopback_receiver: LoopbackReceiver) -> Self {
-        Self {
-            receiver,
-            streaming_state: None,
-            loopback_receiver,
-        }
-    }
-
-    /// Wait for the next port event
-    pub async fn wait_next(&mut self) -> type_c_interface::port::event::PortEvent {
-        loop {
-            let streaming_state = if let Some(streaming_state) = &mut self.streaming_state {
-                // Yield to ensure we don't monopolize the executor
-                embassy_futures::yield_now().await;
-                streaming_state
-            } else {
-                let (Either::First(Loopback::PortEvent(events)) | Either::Second(events)) =
-                    select(self.loopback_receiver.wait_next(), self.receiver.wait_next()).await;
-                self.streaming_state
-                    .insert(PortEventStreamer::new([events].into_iter()))
-            };
-
-            if let Some((_, event)) = streaming_state.next() {
-                return event;
-            } else {
-                self.streaming_state = None;
-            }
-        }
-    }
-}
-
 /// Struct used for containing controller event receivers.
 pub struct EventReceiver<
     'a,
@@ -91,7 +48,11 @@ pub struct EventReceiver<
     LoopbackReceiver: Receiver<Loopback>,
 > {
     /// Port event receiver
-    port_event_receiver: PortEventReceiver<InterruptReceiver, LoopbackReceiver>,
+    port_event_receiver: InterruptReceiver,
+    /// Port event streaming state
+    streaming_state: Option<PortEventStreamer<array::IntoIter<PortEventBitfield, 1>>>,
+    /// Loopback event receiver
+    loopback_receiver: LoopbackReceiver,
     /// Shared state
     shared_state: &'a State,
 }
@@ -111,30 +72,62 @@ impl<
     ) -> Self {
         Self {
             shared_state,
-            port_event_receiver: PortEventReceiver::new(port_event_receiver, loopback_receiver),
+            port_event_receiver,
+            streaming_state: None,
+            loopback_receiver,
         }
     }
 
-    /// Wait for the next port event from any port.
-    ///
-    /// Returns the local port ID and the event bitfield.
+    /// Wait for the next port event from a single port.
     pub async fn wait_event(&mut self) -> Event {
-        let timeout = self.shared_state.lock().await.sink_ready_timeout;
-        match select(self.port_event_receiver.wait_next(), async move {
-            if let Some(timeout) = timeout {
-                Timer::at(timeout).await;
+        loop {
+            if let Some(streaming_state) = &mut self.streaming_state {
+                // If we have a streaming state, prioritize processing it before waiting for new events. This
+                // ensures that any pending events stay buffered in the receiver.
+
+                // Yield to ensure we don't monopolize the executor
+                embassy_futures::yield_now().await;
+
+                if let Some((_, event)) = streaming_state.next() {
+                    return Event::PortEvent(event);
+                }
+
+                // Done streaming, clear the state and continue to wait for new events.
+                self.streaming_state = None;
             } else {
-                pending::<()>().await;
-            }
-        })
-        .await
-        {
-            Either::First(event) => Event::PortEvent(event),
-            Either::Second(_) => {
-                let mut status_event = PortStatusEventBitfield::none();
-                status_event.set_sink_ready(true);
-                self.shared_state.lock().await.sink_ready_timeout = None;
-                Event::PortEvent(PortEvent::StatusChanged(status_event))
+                let timeout = self.shared_state.lock().await.sink_ready_deadline;
+                match select3(
+                    self.port_event_receiver.wait_next(),
+                    async move {
+                        if let Some(timeout) = timeout {
+                            Timer::at(timeout).await;
+                        } else {
+                            pending::<()>().await;
+                        }
+                    },
+                    self.loopback_receiver.wait_next(),
+                )
+                .await
+                {
+                    Either3::First(events) => {
+                        self.streaming_state = Some(PortEventStreamer::new([events].into_iter()));
+                    }
+                    Either3::Second(_) => {
+                        let mut status_event = PortStatusEventBitfield::none();
+                        status_event.set_sink_ready(true);
+                        self.shared_state.lock().await.sink_ready_deadline = None;
+                        return Event::PortEvent(PortEvent::StatusChanged(status_event));
+                    }
+                    Either3::Third(event) => match event {
+                        Loopback::PortEvent(events) => {
+                            self.streaming_state = Some(PortEventStreamer::new([events].into_iter()));
+                            // Continue, the next iteration will handle streaming the port events.
+                        }
+                        Loopback::SinkReadyDeadlineInvalidated => {
+                            // Continue, the next iteration will wait for the update sink ready deadline.
+                        }
+                    },
+                }
             }
         }
     }

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

@@ -8,7 +8,7 @@ use crate::controller::{event_receiver::EventReceiver, state};
 
 pub const DEFAULT_POWER_POLICY_CHANNEL_SIZE: usize = 2;
 pub const DEFAULT_TYPE_C_CHANNEL_SIZE: usize = 2;
-pub const DEFAULT_LOOPBACK_CHANNEL_SIZE: usize = 1;
+pub const DEFAULT_LOOPBACK_CHANNEL_SIZE: usize = 4;
 pub const DEFAULT_INTERRUPT_CHANNEL_SIZE: usize = 4;
 
 /// Components returned from port creation

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

@@ -1,4 +1,5 @@
 //! Max sink voltage port trait implementation
+use embassy_time::Instant;
 use embedded_services::{event::NonBlockingSender, sync::Lockable};
 use embedded_usb_pd::PdError;
 use power_policy_interface::capability::ConsumerDisconnect;
@@ -35,6 +36,29 @@ impl<
             debug!("({}): Disabling sink path before max sink voltage change", self.name);
             self.controller.lock().await.enable_sink_path(self.port, false).await?;
 
+            // In general it's not possible to know if setting the max sink voltage will trigger a renegotiation
+            // because the logic to select a particular contract is specific to the PD controller.
+            // Enable the sink ready timeout as a recovery mechanism. If there's no renegotiation, then the timeout
+            // will result in us broadcasting the existing contract back to the power policy.
+            {
+                let mut shared_state = self.shared_state.lock().await;
+                if shared_state.sink_ready_deadline.is_none() {
+                    shared_state.sink_ready_deadline =
+                        Some(Instant::now() + Self::check_sink_ready_timeout_duration(self.status.epr));
+                }
+
+                if self
+                    .loopback_sender
+                    .try_send(event::Loopback::SinkReadyDeadlineInvalidated)
+                    .is_none()
+                {
+                    error!(
+                        "({}): Failed to send SinkReadyDeadlineInvalidated loopback event, channel full",
+                        self.name
+                    );
+                }
+            }
+
             // Move our local state out of the consumer state and notify the power policy so it stops
             // tracking us as the active consumer and broadcasts a ConsumerDisconnected event. The
             // renegotiation flag marks this as a temporary disconnect for a recontract.

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

@@ -132,6 +132,20 @@ impl<
         Ok(())
     }
 
+    /// Returns the timeout duration for the sink ready check.
+    pub(super) fn check_sink_ready_timeout_duration(is_epr: bool) -> Duration {
+        Duration::from_millis(
+            (if is_epr {
+                T_PS_TRANSITION_EPR_MS
+            } else {
+                T_PS_TRANSITION_SPR_MS
+            }
+            .maximum
+            .0 * 2)
+                .into(),
+        )
+    }
+
     /// Check the sink ready timeout
     ///
     /// After accepting a sink contract (new contract as consumer), the PD spec guarantees that the
@@ -145,32 +159,26 @@ impl<
     ) -> Result<(), PdError> {
         let contract_changed = self.status.available_sink_contract != new_status.available_sink_contract;
         let mut shared_state = self.shared_state.lock().await;
-        let timeout = &mut shared_state.sink_ready_timeout;
+        let deadline = &mut shared_state.sink_ready_deadline;
 
         // Don't start the timeout if the sink has signaled it's ready or if the contract didn't change.
         // The latter ensures that soft resets won't continually reset the ready timeout
         debug!(
             "({}): Check sink ready: new_contract={:?}, sink_ready={:?}, contract_changed={:?}, deadline={:?}",
-            self.name, new_contract, sink_ready, contract_changed, timeout,
+            self.name, new_contract, sink_ready, contract_changed, deadline,
         );
         if new_contract && !sink_ready && contract_changed {
             // Start the timeout
             // Double the spec maximum transition time to provide a safety margin for hardware/controller delays or out-of-spec controllers.
-            let timeout_ms = if new_status.epr {
-                T_PS_TRANSITION_EPR_MS
-            } else {
-                T_PS_TRANSITION_SPR_MS
-            }
-            .maximum
-            .0 * 2;
+            let timeout = Self::check_sink_ready_timeout_duration(new_status.epr);
 
-            debug!("({}): Sink ready timeout started for {}ms", self.name, timeout_ms);
-            *timeout = Some(Instant::now() + Duration::from_millis(timeout_ms as u64));
-        } else if timeout.is_some()
+            debug!("({}): Sink ready timeout started for {}ms", self.name, timeout);
+            *deadline = Some(Instant::now() + timeout);
+        } else if deadline.is_some()
             && (!new_status.is_connected() || new_status.available_sink_contract.is_none() || sink_ready)
         {
             debug!("({}): Sink ready timeout cleared", self.name);
-            *timeout = None;
+            *deadline = None;
         }
         Ok(())
     }

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

@@ -3,21 +3,21 @@ use embassy_time::Instant;
 /// State shared between the port and event receiver
 #[derive(Copy, Clone)]
 pub struct SharedState {
-    /// Sink ready timeout
-    pub(crate) sink_ready_timeout: Option<Instant>,
+    /// Sink ready deadline
+    pub(crate) sink_ready_deadline: Option<Instant>,
 }
 
 impl SharedState {
     /// Create a new instance with default values
     pub fn new() -> Self {
         Self {
-            sink_ready_timeout: None,
+            sink_ready_deadline: None,
         }
     }
 
-    /// Get the current sink ready timeout deadline, if one is pending
-    pub fn sink_ready_timeout(&self) -> Option<Instant> {
-        self.sink_ready_timeout
+    /// Get the current sink ready deadline, if one is pending
+    pub fn sink_ready_deadline(&self) -> Option<Instant> {
+        self.sink_ready_deadline
     }
 }