From e33b840514ca56d1952e8c43b0dfa4d25eafc479 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Mon, 26 Jan 2026 15:06:26 -0500 Subject: [PATCH 1/8] wip: source mode --- usbpd/src/lib.rs | 1 + usbpd/src/protocol_layer/message/data/mod.rs | 2 +- .../message/data/source_capabilities.rs | 41 +- usbpd/src/protocol_layer/mod.rs | 77 +- usbpd/src/source/device_policy_manager.rs | 223 +++ usbpd/src/source/mod.rs | 4 + usbpd/src/source/policy_engine.rs | 1362 +++++++++++++++++ 7 files changed, 1705 insertions(+), 5 deletions(-) create mode 100644 usbpd/src/source/device_policy_manager.rs create mode 100644 usbpd/src/source/mod.rs create mode 100644 usbpd/src/source/policy_engine.rs diff --git a/usbpd/src/lib.rs b/usbpd/src/lib.rs index ab0207b..06a14aa 100644 --- a/usbpd/src/lib.rs +++ b/usbpd/src/lib.rs @@ -26,6 +26,7 @@ pub(crate) mod fmt; pub(crate) mod counters; pub mod protocol_layer; pub mod sink; +pub mod source; pub mod timers; #[cfg(test)] diff --git a/usbpd/src/protocol_layer/message/data/mod.rs b/usbpd/src/protocol_layer/message/data/mod.rs index a8278ee..f2f1257 100644 --- a/usbpd/src/protocol_layer/message/data/mod.rs +++ b/usbpd/src/protocol_layer/message/data/mod.rs @@ -186,7 +186,7 @@ impl Data { pub fn to_bytes(&self, payload: &mut [u8]) -> usize { match self { Self::Unknown => 0, - Self::SourceCapabilities(_) => unimplemented!(), + Self::SourceCapabilities(caps) => caps.to_bytes(payload), Self::SinkCapabilities(caps) => caps.to_bytes(payload), Self::Request(request::PowerSource::FixedVariableSupply(data_object)) => data_object.to_bytes(payload), Self::Request(request::PowerSource::Pps(data_object)) => data_object.to_bytes(payload), diff --git a/usbpd/src/protocol_layer/message/data/source_capabilities.rs b/usbpd/src/protocol_layer/message/data/source_capabilities.rs index 8809df6..428c0f3 100644 --- a/usbpd/src/protocol_layer/message/data/source_capabilities.rs +++ b/usbpd/src/protocol_layer/message/data/source_capabilities.rs @@ -50,7 +50,12 @@ impl PowerDataObject { /// Per USB PD Spec R3.2 Section 6.5.15.1, if the SPR Capabilities Message /// contains fewer than 7 PDOs, the unused Data Objects are zero-filled. pub fn is_zero_padding(&self) -> bool { - (match self { + self.to_raw() == 0 + } + + /// Convert the PDO to its raw u32 representation. + pub fn to_raw(&self) -> u32 { + match self { PowerDataObject::FixedSupply(f) => f.0, PowerDataObject::Battery(b) => b.0, PowerDataObject::VariableSupply(v) => v.0, @@ -60,7 +65,7 @@ impl PowerDataObject { Augmented::Unknown(u) => *u, }, PowerDataObject::Unknown(u) => u.0, - }) == 0 + } } } @@ -123,6 +128,13 @@ impl FixedSupply { pub fn max_current(&self) -> ElectricCurrent { ElectricCurrent::new::(self.raw_max_current().into()) } + + pub fn vSafe5V(max_current_10ma: u16) -> Self { + FixedSupply::default() + .with_raw_voltage(100) // V = 5v = 100_u16 * 50mv + .with_raw_max_current(max_current_10ma) + .with_peak_current(0) + } } bitfield! { @@ -284,6 +296,18 @@ impl EprAdjustableVoltageSupply { pub struct SourceCapabilities(pub(crate) Vec); impl SourceCapabilities { + pub fn new_with_pdos(pdos: Vec) -> Self { + Self(pdos) + } + + pub fn new_vsafe5v_only(maximum_current_10ma: u16) -> Self { + let mut inner = Vec::new(); + inner + .push(PowerDataObject::FixedSupply(FixedSupply::vSafe5V(maximum_current_10ma))) + .ok(); + Self(inner) + } + pub fn vsafe_5v(&self) -> Option<&FixedSupply> { self.0.first().and_then(|supply| { if let PowerDataObject::FixedSupply(supply) = supply { @@ -386,6 +410,19 @@ impl SourceCapabilities { _ => false, }) } + + /// Convert to bytes for transmission. + /// + /// Each PDO is 4 bytes, little-endian. + pub fn to_bytes(&self, buffer: &mut [u8]) -> usize { + let mut offset = 0; + for pdo in &self.0 { + let raw = pdo.to_raw(); + buffer[offset..offset + 4].copy_from_slice(&raw.to_le_bytes()); + offset += 4; + } + offset + } } impl PdoKind for SourceCapabilities { diff --git a/usbpd/src/protocol_layer/mod.rs b/usbpd/src/protocol_layer/mod.rs index f147999..0511495 100644 --- a/usbpd/src/protocol_layer/mod.rs +++ b/usbpd/src/protocol_layer/mod.rs @@ -27,6 +27,7 @@ use usbpd_traits::{Driver, DriverRxError, DriverTxError}; use crate::PowerRole; use crate::counters::{Counter, CounterType, Error as CounterError}; use crate::protocol_layer::message::data::epr_mode::EprModeDataObject; +use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; use crate::protocol_layer::message::extended::Extended; use crate::protocol_layer::message::{ParseError, Payload}; use crate::timers::{Timer, TimerType}; @@ -155,12 +156,16 @@ impl ProtocolLayer { &mut self.driver } - /// Allows tests to access the default header directly. - #[cfg(test)] + /// Access the default header directly. pub fn header(&self) -> &Header { &self.default_header } + /// Change the header's data role after a data role swap + pub fn set_header_data_role(&mut self, role: crate::DataRole) { + self.default_header.set_port_data_role(role); + } + fn get_message_buffer() -> [u8; MAX_MESSAGE_SIZE] { [0u8; MAX_MESSAGE_SIZE] } @@ -696,6 +701,30 @@ impl ProtocolLayer { self.transmit(Message::new_with_data(header, Data::EprMode(mdo))).await } + /// Wait for the sink to request a capability with the a Request Message. + pub async fn wait_for_request(&mut self) -> Result { + // Only sources await a sink power request + debug_assert!(matches!(self.default_header.port_power_role(), PowerRole::Source)); + + self.receive_message_type( + &[MessageType::Data(message::header::DataMessageType::Request)], + TimerType::SenderResponse, + ) + .await + } + + /// Wait for the sink to request a capability with an EPR_Request Message + pub async fn wait_for_epr_request(&mut self) -> Result { + // Only sources await a sink power request + debug_assert!(matches!(self.default_header.port_power_role(), PowerRole::Source)); + + self.receive_message_type( + &[MessageType::Data(message::header::DataMessageType::EprRequest)], + TimerType::SenderResponse, + ) + .await + } + /// Request a certain power level from the source. pub async fn request_power(&mut self, power_source_request: request::PowerSource) -> Result<(), ProtocolError> { // Only sinks can request from a supply. @@ -806,6 +835,50 @@ impl ProtocolLayer { self.transmit(message).await } + + pub async fn transmit_source_capabilities( + &mut self, + source_capabilities: &SourceCapabilities, + ) -> Result<(), ProtocolError> { + // Only sources can send capabilities + debug_assert!(matches!(self.default_header.port_power_role(), PowerRole::Source)); + if source_capabilities.has_epr_pdo_in_spr_positions() { + return Err(ProtocolError::TxError(TxError::HardReset)); + } + + let header = Header::new_data( + self.default_header, + self.counters.tx_message, + DataMessageType::SourceCapabilities, + source_capabilities.0.len() as u8, // Raw cast OK since since len has domain of [0, 8] + ); + + let message = Message::new_with_data(header, Data::SourceCapabilities(source_capabilities.clone())); + + self.transmit(message).await + } + + pub async fn transmit_epr_source_capabilities( + &mut self, + source_capabilities: &SourceCapabilities, + ) -> Result<(), ProtocolError> { + debug_assert!(matches!(self.default_header.port_power_role(), PowerRole::Source)); + + let pdos: heapless::Vec<_, 16> = source_capabilities.0.iter().cloned().collect(); + let extended_payload = message::extended::Extended::EprSourceCapabilities(pdos); + + let header = Header::new_extended( + self.default_header, + self.counters.tx_message, + ExtendedMessageType::EprSourceCapabilities, + 0, // num_objects is 0 for extended messages + ); + + let mut message = Message::new(header); + message.payload = Some(Payload::Extended(extended_payload)); + + self.transmit(message).await + } } #[cfg(test)] diff --git a/usbpd/src/source/device_policy_manager.rs b/usbpd/src/source/device_policy_manager.rs new file mode 100644 index 0000000..bd5e85e --- /dev/null +++ b/usbpd/src/source/device_policy_manager.rs @@ -0,0 +1,223 @@ +//! The device policy manager (DPM) allows a device to control the policy engine, and be informed about status changes. +//! +//! For example, through the DPM, a device can request certain source capabilities (voltage, current), +//! or renegotiate the power contract. +use core::future::Future; + +use crate::{DataRole, protocol_layer::message::data::{ + request, + sink_capabilities::SinkCapabilities, + source_capabilities::SourceCapabilities, +}}; + +/// Events that the device policy manager can send to the policy engine. +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum Event { + /// Empty event. + None, + /// Source capabilities have changed + UpdatedSourceCapabilities, + /// Get Sink capabilities + RequestSinkCapabilities, + /// Get the Remote DRP's Source capabilities + RequestSourceCapabilities, + /// Indicate a Vconn swap needs to be done + RequestVconnSwap, + /// **EPR** Get the Remote DRP's Source EPR capabilities + RequestEprSourceCapabilities, + /// **EPR** + ExitEprMode, + /// **DRP** Indicate a data swap needs to be done + RequestDataRoleSwap, + /// **DRP** Indicate a power role swap needs to be done + RequestPowerRoleSwap, +} + +#[derive(Debug)] +/// Information that the policy engine will publish to the DPM +pub enum Info { + /// SPR Sink Capabilities + SprSinkCapabilities(Option), + /// EPR Sink Capabilities + EprSinkCapabilities(Option), + /// Remote Source Capabilities + RemoteSourceCapabilities(Option), +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// How the source DPM can respond to a request from the sink +pub enum CapabilityResponse { + /// Request is rejected + Reject, + /// Request could be met later from the power reserve & present contract is still valid + Wait, + /// Requast can be met now + Accept, +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// For defining DPM swap behavior +pub enum SwapType { + /// Vconn Port Driver Swap + Vconn, + /// **DRP** Data Role Swap (UFP <---> DFP) + Data, + /// **DRP** Power Role Swap (Source --> Sink) + Power, +} + +/// Trait for the device policy manager. +/// Functions labeled **DRP** do not need to be implemented on single-role devices. +/// +/// This entity commands the policy engine and enforces device policy. +pub trait DevicePolicyManager { + + /// Evaluate a request from the Sink + /// + /// The policy engine will use this evaluation to determine PD control flow + fn evaluate_request( + &mut self, + _request: &request::PowerSource, + ) -> impl Future { + async { CapabilityResponse::Reject } + } + + /// Evaluate a swap request: + /// - Vconn, + /// - **DRP**: Data, Power, Fast Power + fn evaluate_swap_request(&mut self, _swap_request: SwapType) -> impl Future { + async { false } + } + + /// Respond to the Policy Engine's request for this port's current source capabilities + /// + /// Defaults to only default usb capability (5v @ 3A) + fn source_capabilities(&mut self) -> SourceCapabilities { + SourceCapabilities::new_vsafe5v_only(3 * 100) + } + + /// Transition source power to a new power level. + /// + /// After, the DPM will notify the Sink on the outcome of the transition + fn transition_power(&mut self, _power_level: &request::PowerSource) -> impl Future> { + async { Ok(()) } + } + + /// Hard reset power supply to vSafe5V via vSafe0V + /// + /// A Hard Reset shall not cause any change to either the Rp/Rd resistor being asserted + fn hard_reset(&mut self) -> impl Future> { + async { Ok(()) } + } + + /// Set port to drive VCONN to 5V (true) or not (false) + fn drive_vconn(&mut self, _on: bool) -> impl Future> { + async { Ok(()) } + } + + /// **EPR** Return `true` if device is EPR capable. + /// + /// Also possible to dynamically assess EPR capability + fn epr_capable(&mut self) -> bool { false } + + /// **EPR** Return `true` if the `Cable Plug` supports EPR + fn epr_cable_good(&mut self) -> bool { false } + + /// **EPR** Return device's EPR capabilities + fn epr_source_capabilities(&mut self) -> SourceCapabilities { + self.source_capabilities() + } + + /// **DRP** Respond to the Policy Engine's request for this port's current sink capabilities + /// + /// Defaults to only default usb capability (5v @ 3A) + fn sink_capabilities(&mut self) -> impl Future { + async { SinkCapabilities::new_vsafe5v_only(3 * 100) } + } + + /// **DRP** Discharge the VBUS to vSafe5V + fn discharge_vbus(&mut self) -> impl Future { async {} } + + /// **DRP** Turn the Source off. + /// + /// This will be requested before a Role Swap to Sink + fn disable_source(&mut self) -> impl Future { async {} } + + /// **DRP** Set the CC lines to sink configuration + /// + /// This will be requested before a Role Swap to Sink + fn cc_sink(&mut self) -> impl Future { async {} } + + /// **DRP** Detect whether a fast role swap is signaled on the cc lines + /// + /// Table 1.4 - Fast Role Swap Request: + /// + /// An indication from an Initial Source to the Initial Sink that a + /// Fast Role Swap is needed. The Fast Role Swap Request is indicated by + /// driving the CC line to ground for a short period. + fn fr_swap_signaled(&mut self) -> impl Future { + async { true } + } + + /// **DRP** Swap data role + fn swap_data_role(&mut self, _role: DataRole) -> impl Future { async {} } + + /// The policy engine gets and evaluates device policy events when ready. + /// + /// By default, this is a future that never resolves. + /// + ///
+ /// The function must be safe to cancel. To determine whether your own methods are cancellation safe, + /// look for the location of uses of .await. This is because when an asynchronous method is cancelled, + /// that always happens at an .await. If your function behaves correctly even if it is restarted while waiting + /// at an .await, then it is cancellation safe. + ///
+ fn get_event(&mut self) -> impl Future { + async { core::future::pending().await } + } + + /// The policy engine publishes events for the device policy manager to handle here. + /// + /// By default, this is a future that returns immediately + fn inform(&mut self, _info: Info) -> impl Future { + async {} + } +} + + +// TODO: Split DPM traits between base & +// pub trait DualRoleDevicePolicyManager { +// /// **DRP** Respond to the Policy Engine's request for this port's current sink capabilities +// /// +// /// Defaults to only default usb capability (5v @ 3A) +// fn sink_capabilities(&mut self) -> impl Future { +// async { SinkCapabilities::new_vsafe5v_only(3 * 100) } +// } + +// /// **DRP** Discharge the VBUS to vSafe5V +// fn discharge_vbus(&mut self) -> impl Future { async {} } + +// /// **DRP** Turn the Source off. +// /// +// /// This will be requested before a Role Swap to Sink +// fn disable_source(&mut self) -> impl Future { async {} } + +// /// **DRP** Set the CC lines to sink configuration +// /// +// /// This will be requested before a Role Swap to Sink +// fn cc_sink(&mut self) -> impl Future { async {} } + +// /// **DRP** Detect whether a fast role swap is signaled on the cc lines +// /// +// /// Table 1.4 - Fast Role Swap Request: +// /// +// /// An indication from an Initial Source to the Initial Sink that a +// /// Fast Role Swap is needed. The Fast Role Swap Request is indicated by +// /// driving the CC line to ground for a short period. +// fn fr_swap_signaled(&mut self) -> impl Future { +// async { true } +// } + +// } diff --git a/usbpd/src/source/mod.rs b/usbpd/src/source/mod.rs new file mode 100644 index 0000000..d47df21 --- /dev/null +++ b/usbpd/src/source/mod.rs @@ -0,0 +1,4 @@ +//! The source implementation. + +pub mod device_policy_manager; +pub mod policy_engine; diff --git a/usbpd/src/source/policy_engine.rs b/usbpd/src/source/policy_engine.rs new file mode 100644 index 0000000..936fee5 --- /dev/null +++ b/usbpd/src/source/policy_engine.rs @@ -0,0 +1,1362 @@ +//! Policy engine for the implementation of a sink. +use core::marker::PhantomData; + +use embassy_futures::select::{Either, select}; +use embassy_futures::select::{Either3, select3}; +use usbpd_traits::Driver; + +use super::device_policy_manager::DevicePolicyManager; +use super::device_policy_manager::Event; +use crate::counters::Counter; +use crate::protocol_layer::message::Message; +use crate::protocol_layer::message::Payload; +use crate::protocol_layer::message::data::request::PowerSource; +use crate::protocol_layer::message::data::sink_capabilities::SinkCapabilities; +use crate::protocol_layer::message::data::source_capabilities::Kind; +use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; +use crate::protocol_layer::message::data::{Data, PdoKind, epr_mode, request}; +use crate::protocol_layer::message::extended::Extended; +use crate::protocol_layer::message::extended::extended_control::ExtendedControlMessageType; +use crate::protocol_layer::message::header::{ + ControlMessageType, DataMessageType, ExtendedMessageType, Header, MessageType, SpecificationRevision, +}; +use crate::protocol_layer::{ProtocolError, ProtocolLayer, RxError, TxError}; +use crate::source::device_policy_manager::SwapType; +use crate::source::device_policy_manager::{CapabilityResponse, Info}; +use crate::timers::{Timer, TimerType}; +use crate::{DataRole, PowerRole}; + +/// Source capability +#[derive(Debug, Clone, Copy, Default, PartialEq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +enum Mode { + /// The classic mode of PD operation where explicit contracts are negotiaged using SPR (A)PDOs. + #[default] + Spr, + /// A Power Delivery mode of operation where maximum allowable voltage is 48V. + Epr, +} + +#[derive(Debug, Clone, Copy, Default)] +enum Contract { + #[default] + Safe5V, + Implicit, // Only present after fast role swap. Limited to max. type C current. + TransitionToExplicit, + Explicit(PowerSource), + Invalid, +} + +/// Source states. +#[derive(Debug, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +enum State { + // States of the policy engine as given by specification. + // 8.3.3.2 Policy Engine Source Port State Diagram + Startup { + role_swap: bool, + }, + Discovery, + SendCapabilities, + NegotiateCapability(PowerSource), + TransitionSupply(PowerSource), + Ready, + Disabled, + CapabilityResponse(CapabilityResponse), + HardReset, + HardResetReceived, + TransitionToDefault, + GetSinkCap, + WaitNewCapabilities, + EprKeepAlive, + GiveSourceCap, + + // 8.3.3.4 Source Port Soft Reset + SendSoftReset, + SoftReset, + + // 8.3.3.6 Not Supported Message State + SendNotSupported, + NotSupportedReceived, + + // 8.3.3.19 Dual-Role Port + DrSwap(SwapState), + DrGetSourceCap(Mode), + DrGiveSinkCap(Mode), + + // Custom state to signal exit out of source to sink from a power swap + PrSwapToSinkStartup, + + // 8.3.3.20 Vconn Swap + VcsSendSwap(VcsSwapSource), + VcsEvaluateSwap(VcsSwapSource), + VcsAcceptSwap(VcsSwapSource), + VcsRejectSwap(VcsSwapSource), + VcsWaitForVconn(VcsSwapSource), + VcsTurnOffVconn(VcsSwapSource), + VcsTurnOnVconn(VcsSwapSource), + VcsSendPsRdy(VcsSwapSource), + VcsForceVconn(VcsSwapSource), + + // 8.3.3.26 EPR States + EprModeEntry, + EprModeEntryAck, + EprModeDiscoverCable, + EprModeEvaluateCable, + EprModeEntrySucceeded, + EprModeEntryFailed(u8), + EprModeSendExit, + EprModeExitReceived, + + ErrorRecovery, +} + +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +enum VcsSwapSource { + Message, + Epr +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// Dual Role Port Swap States +enum SwapState { + Data(DataRoleSwap), + Power(PowerRoleSwap), + FastPower(FastPowerRoleSwap), +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// State during a Data Role Swap execution (for both directions) +enum DataRoleSwap { + Evaluate, + Accept, + Change, + Send, + Reject, +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// State during a Power Role Swap execution +enum PowerRoleSwap { + Evaluate, + Accept, + TransitionToOff, + AssertRd, + WaitSourceOn, + Send, + Reject, +} + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +/// State during a Fast Power Role Swap execution +enum FastPowerRoleSwap { + Evaluate, + Accept, + TransitionToOff, + AssertRd, + WaitSourceOn, +} + +/// Implementation of the source policy engine. +/// See spec, [8.3.3.2] +#[derive(Debug)] +pub struct Source { + device_policy_manager: DPM, + protocol_layer: ProtocolLayer, + hard_reset_counter: Counter, + caps_counter: Counter, + state: State, + + dual_role: bool, + vconn_source: bool, + mode: Mode, + contract: Contract, + + _timer: PhantomData, +} + +/// Errors that can occur in the source policy engine state machine. +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Error { + /// The port partner is unresponsive. + PortPartnerUnresponsive, + /// Entered ErrorRecovery mode. This requests a disconnect. + ReconnectionRequired, + /// Easiest way to signal to device to swap to sink + SwapToSink, + /// A protocol error has occured. + Protocol(ProtocolError), +} + +impl From for Error { + fn from(protocol_error: ProtocolError) -> Self { + Error::Protocol(protocol_error) + } +} + +impl Source { + fn new_protocol_layer(driver: DRIVER) -> ProtocolLayer { + let header = Header::new_template(DataRole::Dfp, PowerRole::Source, SpecificationRevision::R3_X); + ProtocolLayer::new(driver, header) + } + + /// Create a new source policy engine with a given `driver` and set of `source_capabilities`. + pub fn new( + driver: DRIVER, + device_policy_manager: DPM, + role_swapped: bool, + ) -> Self { + Self { + device_policy_manager, + protocol_layer: Self::new_protocol_layer(driver), + hard_reset_counter: Counter::new(crate::counters::CounterType::HardReset), + caps_counter: Counter::new(crate::counters::CounterType::Caps), + + state: match role_swapped { + true => State::SendCapabilities, + false => State::Startup { role_swap: true } + }, + contract: match role_swapped { + true => Contract::Implicit, + false => Contract::default(), + }, + mode: Mode::default(), + dual_role: false, + vconn_source: true, + + _timer: PhantomData, + } + } + + /// Create a new source policy engine with dual role capabilities, + /// with a given `driver`, and set of `source_capabilities`, and set of `sink_capabilities` + /// for the port + pub fn new_dual_role( + driver: DRIVER, + device_policy_manager: DPM, + role_swapped: bool, + ) -> Self { + Self { + device_policy_manager, + protocol_layer: Self::new_protocol_layer(driver), + hard_reset_counter: Counter::new(crate::counters::CounterType::HardReset), + caps_counter: Counter::new(crate::counters::CounterType::Caps), + + state: match role_swapped { + true => State::SendCapabilities, + false => State::Startup { role_swap: true } + }, + contract: match role_swapped { + true => Contract::Implicit, + false => Contract::default(), + }, + mode: Mode::default(), + dual_role: true, + vconn_source: true, + + _timer: PhantomData, + } + } + + /// Set a new driver when re-attached. + pub fn re_attach(&mut self, driver: DRIVER) { + self.protocol_layer = Self::new_protocol_layer(driver); + } + + /// Run a single step in the policy engine state machine. + async fn run_step(&mut self) -> Result<(), Error> { + let result = self.update_state().await; + if result.is_ok() { + return Ok(()); + } + + if let Err(Error::Protocol(protocol_error)) = result { + let new_state = match (&self.mode, &self.state, protocol_error) { + // Handle when hard reset is signaled by the driver itself. + (_, _, ProtocolError::RxError(RxError::HardReset)) => Some(State::HardResetReceived), + + // Handle when hard reset is signaled by the driver itself. + (_, _, ProtocolError::TxError(TxError::HardReset)) => Some(State::HardReset), + + // Handle when soft reset is signaled by the driver itself. + (_, _, ProtocolError::RxError(RxError::SoftReset)) => Some(State::SoftReset), + + // Per spec 6.3.13: If the Soft_Reset Message fails, a Hard Reset shall be initiated. + // This handles the case where we're trying to send/receive a soft reset and it fails. + (_, State::SoftReset | State::SendSoftReset, ProtocolError::TransmitRetriesExceeded(_)) => { + Some(State::HardReset) + } + + // Per spec 8.3.3.2.3: No GoodCRC (NoResponseTimer times out) goes to Discovery or Disabled + (_, State::SendCapabilities, ProtocolError::TransmitRetriesExceeded(_)) => Some(State::Discovery), + + // Per spec 8.3.3.2.3: Failure to receive a Request Message results in + (_, State::SendCapabilities, ProtocolError::RxError(RxError::ReceiveTimeout)) => { + // FIXME: Detect when Port Partners have been PD Connected before this error or not. + // For now, using whether or not a Contract had been previously established or not + match self.contract { + Contract::Safe5V => Some(State::Discovery), + _ => Some(State::ErrorRecovery), + } + } + + // PowerSwap: Per spec 8.3.3.19.3.6, the Policy Engine shall transition to ErrorRecovery on RxTimeout or TxSendFail + // FastPowerSwap: Per spec 8.3.3.19.5.6, the Policy Engine shall transition to ErrorRecovery on RxTimeout or TxSendFail + (_, + State::DrSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)) | + State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn)), + + ProtocolError::RxError(RxError::ReceiveTimeout) | + ProtocolError::TransmitRetriesExceeded(_)) => + { + Some(State::ErrorRecovery) + } + + // Per spec 8.3.3.2.5: When any Protocol Error occurs, transition to Hard Reset + (_, State::TransitionSupply(_), _) => Some(State::HardReset), + + // Unexpected messages indicate a protocol error and demand a soft reset. + // Per spec 6.8.1 Table 6.72 (for non-power-transitioning states). + // Note: This must come AFTER TransitionSupply check above. + (_, _, ProtocolError::UnexpectedMessage) => Some(State::SendSoftReset), + + // Per Table 6.72: Unsupported messages in Ready state get Not_Supported response. + (_, State::Ready, ProtocolError::RxError(RxError::UnsupportedMessage)) => Some(State::SendNotSupported), + + // Per spec 6.6.9.1: Transmission failure (no GoodCRC after retries) triggers Soft Reset. + // Note: If we're in SoftReset/SendSoftReset state, this is caught above and escalates to Hard Reset. + (_, _, ProtocolError::TransmitRetriesExceeded(_)) => Some(State::SendSoftReset), + + // Unhandled protocol errors - log and continue. + (_, _, error) => { + error!("Protocol error {:?} in source state transition", error); + None + } + }; + + if let Some(state) = new_state { + self.state = state + } + + Ok(()) + } else { + error!("Unrecoverable result {:?} in sink state transition", result); + result + } + } + + /// Run the source's state machine continuously. + /// + /// The loop is only broken for unrecoverable errors, for example if the port partner is unresponsive. + pub async fn run(&mut self) -> Result<(), Error> { + loop { + self.run_step().await?; + } + } + + async fn update_state(&mut self) -> Result<(), Error> { + trace!("State: {:?}", &self.state); + let new_state = match &self.state { + // 8.3.3.2.1 (PE_SR_Startup): + State::Startup { role_swap } => { + self.contract = Default::default(); + self.protocol_layer.reset(); + self.caps_counter.reset(); + + if *role_swap { + self.contract = Contract::Implicit; + TimerType::get_timer::(TimerType::SwapSourceStart).await; + } + + // TODO: Sources shall remain in the Startup state until a plug is Attached + // For now, assume that a Source driver will only be ran after an attach occurs + + State::SendCapabilities + } + // 8.3.3.2.2 (PE_SRC_Discovery): + State::Discovery => { + // NOTE: Incrementing the CapsCounter here is not to spec, + // but **should** have the same behavior + if self.caps_counter.increment().is_err() { + // If the CapsCounter saturates, enter the Disabled state + State::Disabled + } else { + // Else, re-enter SendCapabilities once the SourceCapability timer times out + TimerType::get_timer::(TimerType::SourceCapability).await; + State::SendCapabilities + } + } + // 8.3.3.2.3 (PE_SRC_Send_Capabilities): + State::SendCapabilities => { + // Send capabilities message + match self.mode { + Mode::Spr => { + self.protocol_layer + .transmit_source_capabilities(&self.device_policy_manager.source_capabilities()) + .await? + } + Mode::Epr => { + self.protocol_layer + .transmit_epr_source_capabilities(&self.device_policy_manager.epr_source_capabilities()) + .await? + } + }; + + // When Capabilities were sent successfully, reset the hard reset counter + self.hard_reset_counter.reset(); + + let request = self.wait_for_sink_request().await?; + + State::NegotiateCapability(request) + } + // 8.3.3.2.4 (PE_SRC_Negotiate_Capability): + State::NegotiateCapability(power_request) => { + + // FIXME: This should be done in the protocol layer + // If the request is Unknown, attempt to match to its PDO to determine the Kind & re-type the request + let power_request = match *power_request { + PowerSource::Unknown(u) => { + match self.device_policy_manager + .source_capabilities() + .at_object_position(u.object_position()) + { + Some(q) => match q { + Kind::FixedSupply | Kind::VariableSupply => request::PowerSource::FixedVariableSupply(request::FixedVariableSupply(u.0)), + Kind::Battery => request::PowerSource::Battery(request::Battery(u.0)), + Kind::Pps => request::PowerSource::Pps(request::Pps(u.0)), + Kind::Avs => request::PowerSource::Avs(request::Avs(u.0)), + }, + None => { + trace!("Could not match Unknown Power Request to a PDO!"); + *power_request + }, + } + } + _ => *power_request + }; + + let response = self + .device_policy_manager + .evaluate_request(&power_request) + .await; + + match response { + CapabilityResponse::Accept => State::TransitionSupply(power_request), + _ => State::CapabilityResponse(response), + } + } + // 8.3.3.2.5 (PE_SRC_Transition_Supply): + State::TransitionSupply(power_request) => { + self.protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await?; + + self.contract = Contract::TransitionToExplicit; + self.device_policy_manager + .transition_power(power_request) + .await + .map_err(|_| Error::PortPartnerUnresponsive)?; + self.protocol_layer + .transmit_control_message(ControlMessageType::PsRdy) + .await?; + + self.contract = Contract::Explicit(*power_request); + State::Ready + } + // 8.3.3.2.6 (PE_SRC_Ready): + State::Ready => { + // FIXME: Entry: source shall notify the protocol layer of the end of the Atomic Message Sequence (AMS) + // FIXME: Exit: If source is initiating an AMS, notify the protocol layer that the first message in an AMS will follow + + // FIXME: Entry: The DiscoverIdentityTimer is run when this is a Vconn Source and a PD Connection w/ a Cable Plug needs + // to be established, i.e., no GoodCrc Message has yet been received in response to a Discover Identity Command. + let discover_identity_fut = async { core::future::pending::<()>().await }; + + // Entry: If current Explicit Contract is for an SPR PPS APDO, then run the SourcePPSCommTimer + let pps_periodic_fut = async { + let power_source = match self.contract { + Contract::Explicit(power_source) => Some(power_source), + _ => None, + }; + + match power_source { + Some(PowerSource::Pps(_)) => TimerType::get_timer::(TimerType::SourcePPSComm).await, + _ => core::future::pending().await, + } + }; + + // Entry: If current Explicit Contract is for EPR Mode, then run the SourceEPRKeepAliveTimer + let epr_keep_alive_fut = async { + match self.mode { + Mode::Epr => TimerType::get_timer::(TimerType::SourceEPRKeepAlive).await, + Mode::Spr => core::future::pending().await, + } + }; + + let timers_fut = async { select3(discover_identity_fut, pps_periodic_fut, epr_keep_alive_fut).await }; + + match select3( + self.protocol_layer.receive_message(), + self.device_policy_manager.get_event(), + timers_fut + ).await { + Either3::First(message) => { + let message = message?; + self.match_message_to_state(message).await? + } + + Either3::Second(dpm_event) => match dpm_event { + Event::None => State::Ready, + Event::UpdatedSourceCapabilities => State::SendCapabilities, + Event::RequestSinkCapabilities => State::GetSinkCap, + Event::RequestSourceCapabilities => State::DrGetSourceCap(Mode::Spr), + Event::RequestEprSourceCapabilities => State::DrGetSourceCap(Mode::Epr), + Event::ExitEprMode => State::EprModeSendExit, + Event::RequestVconnSwap => State::VcsSendSwap(VcsSwapSource::Message), + Event::RequestDataRoleSwap => State::DrSwap(SwapState::Data(DataRoleSwap::Send)), + Event::RequestPowerRoleSwap => State::DrSwap(SwapState::Power(PowerRoleSwap::Send)), }, + + Either3::Third(timeout_source) => match timeout_source { + // DiscoverIdentity Timeout + Either3::First(_) => State::HardReset, + // PPS Periodic Timeout + Either3::Second(_) => State::HardReset, + // EPR Keep Alive Timeout + Either3::Third(_) => State::HardReset, + }, + } + } + // 8.3.3.2.7 (PE_SRC_Disabled): + State::Disabled => { + // This **SHOULD** put the device in a Safe5V default power mode + let source_capabilities = self.device_policy_manager.source_capabilities(); + self.device_policy_manager + .transition_power( + &PowerSource::new_fixed( + request::CurrentRequest::Highest, + request::VoltageRequest::Safe5V, + &source_capabilities, + ) + .unwrap(), + ) + .await + .map_err(|_| ProtocolError::RxError(RxError::HardReset))?; + // Only respond to `HardReset` Signaling + loop { + if let Err(ProtocolError::RxError(RxError::HardReset)) = self.protocol_layer.receive_message().await + { + break; + } + } + State::HardReset + } + // 8.3.3.2.8 (PE_SRC_Capability_Response): + State::CapabilityResponse(response) => { + let message_type = match response { + CapabilityResponse::Reject => ControlMessageType::Reject, + CapabilityResponse::Wait => ControlMessageType::Wait, + _ => unreachable!(), // `Accept` does not go to this state + }; + self.protocol_layer.transmit_control_message(message_type).await?; + + match self.contract { + Contract::Invalid => + if message_type == ControlMessageType::Reject { + State::HardReset + } else { + State::WaitNewCapabilities + } + Contract::Explicit(_) => State::Ready, + _ => State::WaitNewCapabilities, + } + } + // 8.3.3.2.9 (PE_SRC_Hard_Reset): + State::HardReset => { + // Increment HardResetCounter + self.hard_reset_counter + .increment() + .map_err(|_| Error::PortPartnerUnresponsive)?; + + // Transmit Hard Reset Signaling + self.protocol_layer.hard_reset().await?; + + // Transition to TransitionToDefault when PSHardResetTimer times out + TimerType::get_timer::(TimerType::PSHardReset).await; + + State::TransitionToDefault + } + // 8.3.3.2.10 (PE_SRC_Hard_Reset_Received): + State::HardResetReceived => { + // Transition to TransitionToDefault when PSHardResetTimer times out + TimerType::get_timer::(TimerType::PSHardReset).await; + + State::TransitionToDefault + } + // 8.3.3.2.11 (PE_SRC_Transition_to_default): + State::TransitionToDefault => { + // Notify DPM about hard reset & turn off Vconn + self.device_policy_manager + .drive_vconn(false) + .await + .map_err(|_| Error::PortPartnerUnresponsive)?; + self.device_policy_manager + .hard_reset() + .await + .map_err(|_| Error::PortPartnerUnresponsive)?; + self.device_policy_manager + .drive_vconn(true) + .await + .map_err(|_| Error::PortPartnerUnresponsive)?; + + // Hard Reset shall cause EPR Mode to be exited + self.mode = Mode::Spr; + + // Reset contract to default + self.contract = Contract::Safe5V; + + State::Startup { role_swap: false } + } + // 8.3.3.2.12 (PE_SRC_Get_Sink_Cap): + State::GetSinkCap => { + // Due to request from DPM, request capabilities from Attached Sink + match self.mode { + Mode::Spr => { + self.protocol_layer + .transmit_control_message(ControlMessageType::GetSinkCap) + .await? + } + Mode::Epr => { + self.protocol_layer + .transmit_extended_control_message(ExtendedControlMessageType::EprGetSinkCap) + .await? + } + } + + // Inform the DPM of the outcome + let message = match self.mode { + Mode::Spr => { + self.protocol_layer + .receive_message_type( + &[MessageType::Data(DataMessageType::SinkCapabilities)], + TimerType::SenderResponse, + ) + .await + } + Mode::Epr => { + self.protocol_layer + .receive_message_type( + &[MessageType::Extended(ExtendedMessageType::EprSinkCapabilities)], + TimerType::SenderResponse, + ) + .await + } + }; + + let response = if let Ok(message) = message { + // Message success, deal with payload: + let capabilities = match message.payload { + Some(Payload::Data(Data::SinkCapabilities(caps))) => caps, + Some(Payload::Extended(Extended::EprSinkCapabilities(pdos))) => SinkCapabilities(pdos), + _ => unreachable!(), + }; + Some(capabilities) + } else if let Err(ProtocolError::RxError(RxError::ReceiveTimeout)) = message { + // Did not receive a capability, which should be handled explicitly here + None + } else { + // Propogate the error if it wasn't the receive timeout + message?; + unreachable!() + }; + + let info = match self.mode { + Mode::Spr => Info::SprSinkCapabilities(response), + Mode::Epr => Info::EprSinkCapabilities(response), + }; + self.device_policy_manager.inform(info).await; + + State::Ready + } + // 8.3.3.2.13 (PE_SRC_Wait_New_Capabilities): + State::WaitNewCapabilities => { + // Transition to SendCapabilities only when the DPM indicates the source capabilities have changed + let mut wait_time: usize = 0; + loop { + match select( + self.device_policy_manager.get_event(), + TIMER::after_millis(5000) + ).await { + Either::First(event) => if event == Event::UpdatedSourceCapabilities { break }, + Either::Second(_timeout) => { + wait_time += 5; + warn!("{} seconds have passed waiting for updated source capabilities!", wait_time); + }, + } + } + State::SendCapabilities + } + // 8.3.3.2.14 (PE_SRC_EPR_Keep_Alive): + State::EprKeepAlive => { + self.protocol_layer + .transmit_extended_control_message(ExtendedControlMessageType::EprKeepAlive) + .await?; + State::Ready + } + // 8.3.3.2.15 (PE_SRC_Give_Source_Cap): + State::GiveSourceCap => { + match self.mode { + Mode::Spr => { + self.protocol_layer + .transmit_source_capabilities(&self.device_policy_manager.source_capabilities()) + .await? + } + Mode::Epr => { + self.protocol_layer + .transmit_epr_source_capabilities(&self.device_policy_manager.epr_source_capabilities()) + .await? + } + } + State::Ready + } + + // 8.3.3.4 SOP Soft Reset & Protocol Error + // 8.3.3.4.1.1 (PE_SRC_Send_Soft_Reset) + State::SendSoftReset => { + // Soft reset the protocol layer and send a SoftReset message + self.protocol_layer.reset(); + self.protocol_layer + .transmit_control_message(ControlMessageType::SoftReset) + .await?; + + // Wait for an Accept message + self.protocol_layer + .receive_message_type( + &[MessageType::Control(ControlMessageType::Accept)], + TimerType::SenderResponse, + ) + .await?; + + State::SendCapabilities + } + // 8.3.3.4.1.2 (PE_SRC_Soft_Reset) + State::SoftReset => { + self.protocol_layer.reset(); + self.protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await?; + + State::SendCapabilities + } + + // 8.3.3.6 Not Supported Message + // 8.3.3.6.1.1 (PE_SRC_Not_Supported): + State::SendNotSupported => { + self.protocol_layer + .transmit_control_message(ControlMessageType::NotSupported) + .await?; + + State::Ready + } + // 8.3.3.6.1.2 (PE_SRC_Not_Supported_Received): + State::NotSupportedReceived => { + // FIXME: Entry: Inform the Device Policy Manager + State::Ready + } + + // 8.3.3.19 Dual-Role Port States + State::DrSwap(swap_state) => { + match swap_state { + SwapState::Data(dr) => + self.execute_data_role_swap_state(*dr).await?, + SwapState::Power(pr) => + self.execute_power_role_swap_state(*pr).await?, + SwapState::FastPower(fpr) => + self.execute_fast_power_role_swap_state(*fpr).await? + } + } + // 8.3.3.19.7.1 (PE_DR_SRC_Get_Source_Cap): + State::DrGetSourceCap(mode) => { + let result = match mode { + Mode::Spr => self.get_source_capabilities().await, + Mode::Epr => self.get_epr_source_capabilities().await, + }; + + let caps = match result { + Ok(caps) => Some(caps), + Err(err) => match err { + Error::Protocol(ProtocolError::RxError(RxError::ReceiveTimeout)) => None, + _ => return Err(err), + } + }; + + self.device_policy_manager.inform(Info::RemoteSourceCapabilities(caps)).await; + State::Ready + } + // 8.3.3.19.8 + State::DrGiveSinkCap(mode) => { + let sink_caps = self.device_policy_manager.sink_capabilities().await; + match mode { + Mode::Spr => { + self.protocol_layer.transmit_sink_capabilities(sink_caps).await?; + } + Mode::Epr => { + self.protocol_layer.transmit_epr_sink_capabilities(sink_caps).await?; + } + } + State::Ready + } + + // Custom State - Exit source running and signal to program to begin Sink + State::PrSwapToSinkStartup => { + // TODO: Intentional exit due to power swap + Err(Error::SwapToSink)? + } + + // 8.3.3.20 Source Vconn Swap + // 8.3.3.20.1 (PE_VCS_Send_Swap): + State::VcsSendSwap(source) => { + self.protocol_layer.transmit_control_message(ControlMessageType::VconnSwap).await?; + + let message = self.protocol_layer.receive_message_type(&[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait), + MessageType::Control(ControlMessageType::NotSupported) + ], + TimerType::SenderResponse + ).await; + + if let Err(err) = message { + match err { + ProtocolError::RxError(RxError::ReceiveTimeout) => State::Ready, + _ => Err(err)? + } + } else { + let message = message.unwrap(); + match message.header.message_type() { + MessageType::Control(ControlMessageType::Accept) => match self.vconn_source { + true => State::VcsWaitForVconn(*source), + false => State::VcsTurnOnVconn(*source), + }, + MessageType::Control(ControlMessageType::Reject) | + MessageType::Control(ControlMessageType::Wait) => State::Ready, + // May also transition to ForceVconn if NotSupported message and port presently not vconn source + MessageType::Control(ControlMessageType::NotSupported) => State::NotSupportedReceived, + _ => unreachable!() + } + } + } + // 8.3.3.20.2 (PE_VCS_Evaluate_Swap): + State::VcsEvaluateSwap(source) => { + match self.device_policy_manager.evaluate_swap_request(SwapType::Vconn).await { + true => State::VcsAcceptSwap(*source), + false => State::VcsRejectSwap(*source), + } + } + // 8.3.3.20.3 (PE_VCS_Accept_Swap): + State::VcsAcceptSwap(source) => { + self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + match self.vconn_source { + true => State::VcsWaitForVconn(*source), + false => State::VcsTurnOnVconn(*source), + } + } + // 8.3.3.20.4 (PE_VCS_Reject_Swap): + State::VcsRejectSwap(source) => { + // FIXME: Wait Message logic + self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + match source { + VcsSwapSource::Message => State::Ready, + VcsSwapSource::Epr => State::EprModeDiscoverCable, + } + } + // 8.3.3.20.5 (PE_VCS_Wait_for_Vconn): + State::VcsWaitForVconn(source) => { + self.protocol_layer.receive_message_type( + &[MessageType::Control(ControlMessageType::PsRdy)], + TimerType::VCONNOn + ).await?; + + State::VcsTurnOffVconn(*source) + } + // 8.3.3.20.6 (PE_VCS_Turn_Off_Vconn): + State::VcsTurnOffVconn(source) => { + self.device_policy_manager.drive_vconn(false).await + .map_err(|_| Error::ReconnectionRequired)?; + match source { + VcsSwapSource::Message => State::Ready, + VcsSwapSource::Epr => State::EprModeDiscoverCable, + } + } + // 8.3.3.20.7 (PE_VCS_Turn_On_Vconn): + State::VcsTurnOnVconn(source) => { + self.device_policy_manager.drive_vconn(true).await + .map_err(|_| Error::ReconnectionRequired)?; + State::VcsSendPsRdy(*source) + } + // 8.3.3.20.8 (PE_VCS_Send_PS_Rdy): + State::VcsSendPsRdy(source) => { + self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + match source { + VcsSwapSource::Message => State::Ready, + VcsSwapSource::Epr => State::EprModeDiscoverCable, + } + } + // 8.3.3.20.9 (PE_VCS_Force_Vconn): + State::VcsForceVconn(source) => { + self.device_policy_manager.drive_vconn(true).await + .map_err(|_| Error::ReconnectionRequired)?; + match source { + VcsSwapSource::Message => State::Ready, + VcsSwapSource::Epr => State::EprModeDiscoverCable, + } + } + + // TODO: Source EPR + // 8.3.3.26.1.1 (PE_SRC_Evaluate_EPR_Mode_Entry): + State::EprModeEntry => { + match self.device_policy_manager.epr_capable() { + true => State::EprModeEntryAck, + false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceUnableToEnterEprMode.into()) + } + } + // 8.3.3.26.1.2 (PE_SRC_EPR_Mode_Entry_Ack): + State::EprModeEntryAck => { + self.protocol_layer.transmit_epr_mode(epr_mode::Action::EnterAcknowledged, 0).await?; + + match (self.device_policy_manager.epr_cable_good(), self.vconn_source) { + (false, true) => State::VcsSendSwap(VcsSwapSource::Epr), + (false, false) => State::EprModeDiscoverCable, + (true, _) => State::EprModeEvaluateCable + } + } + // 8.3.3.26.1.3 (PE_SRC_EPR_Mode_Discover_Cable): + State::EprModeDiscoverCable => { + match self.vconn_source { + // FIXME: Discovery is done implicitly through DPM right now, + // switch to using PE_INIT_PORT_VDM_Identity_Request + true => State::EprModeEvaluateCable, + false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceFailedToBecomeVconnSource.into()) + } + } + // 8.3.3.26.1.4 (PE_SRC_EPR_Mode_Evaluate_Cable_EPR): + State::EprModeEvaluateCable => { + match self.device_policy_manager.epr_cable_good() { + true => State::EprModeEntrySucceeded, + false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::CableNotEprCapable.into()), + } + } + // 8.3.3.26.1.5 (PE_SRC_EPR_Mode_Entry_Succeeded): + State::EprModeEntrySucceeded => { + self.protocol_layer.transmit_epr_mode(epr_mode::Action::EnterSucceeded, 0).await?; + // FIXME: Set EPR headers + self.mode = Mode::Epr; + State::SendCapabilities + } + // 8.3.3.26.1.6 (PE_SRC_EPR_Mode_Entry_Failed): + State::EprModeEntryFailed(data) => { + self.protocol_layer + .transmit_epr_mode(epr_mode::Action::EnterFailed, *data) + .await?; + State::Ready + } + // 8.3.3.26.3.1 (PE_SRC_Send_EPR_Mode_Exit): + State::EprModeSendExit => { + self.protocol_layer.transmit_epr_mode(epr_mode::Action::Exit, 0).await?; + // FIXME: Clear EPR headers + self.mode = Mode::Spr; + State::SendCapabilities + } + // 8.3.3.26.3.2 (PE_SRC_EPR_Mode_Exit_Received): + State::EprModeExitReceived => { + self.mode = Mode::Spr; + // FIXME: Clear EPR headers + match self.contract { + Contract::Explicit(power_source) => { + let epr_capabilities = self.device_policy_manager.epr_source_capabilities(); + + if epr_capabilities.has_epr_pdo_in_spr_positions() { + State::HardReset + } else if epr_capabilities.spr_pdos().any(|(pos, _pdo)| pos == power_source.object_position()) { + State::SendCapabilities + } else { + State::HardReset + } + }, + Contract::Safe5V | Contract::Implicit => State::Ready, + _ => State::HardReset + } + } + + // 8.3.3.28.1 + State::ErrorRecovery => { + error!("Entered Error Recovery state! Reconnection required."); + Err(Error::ReconnectionRequired)? + } + }; + + self.state = new_state; + + Ok(()) + } + + /// 8.3.3.19.1 DFP to UFP Data Role Swap, 8.3.3.19.2 UFP to DFP Data Role Swap + async fn execute_data_role_swap_state(&mut self, state: DataRoleSwap) -> Result { + match state { + // 8.3.3.19.1.2 (PE_DRS_DFP_UFP_Evaluate_Swap, PE_DRS_UFP_DFP_Evaluate_Swap): + DataRoleSwap::Evaluate => { + match self.device_policy_manager.evaluate_swap_request(SwapType::Data).await { + true => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Accept))), + false => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Reject))), + } + }, + // 8.3.3.19.1.3 (PE_DRS_DFP_UFP_Accept_Swap, PE_DRS_UFP_DFP_Accept_Swap): + DataRoleSwap::Accept => { + self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))) + }, + // 8.3.3.19.1.4 (PE_DRS_DFP_UFP_Change_to_UFP_Swap, PE_DRS_UFP_DFP_Change_to_DFP_Swap): + DataRoleSwap::Change => { + let new_role = DataRole::from(!bool::from(self.protocol_layer.header().port_data_role())); + self.device_policy_manager.swap_data_role(new_role).await; + Ok(State::Ready) + }, + // 8.3.3.19.1.5 (PE_DRS_DFP_UFP_Send_Swap, PE_DRS_UFP_DFP_Send_Swap): + DataRoleSwap::Send => { + self.protocol_layer.transmit_control_message(ControlMessageType::DrSwap).await?; + + let message = self.protocol_layer.receive_message_type(&[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait)], + TimerType::SenderResponse + ).await?; + + match message.header.message_type() { + MessageType::Control(ControlMessageType::Accept) => + Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))), + + MessageType::Control(ControlMessageType::Reject) | + MessageType::Control(ControlMessageType::Wait) => + Ok(State::Ready), + + _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)) + } + }, + // 8.3.3.19.1.6 (PE_DRS_DFP_UFP_Reject_Swap, PE_DRS_UFP_DFP_Reject_Swap): + DataRoleSwap::Reject => { + // FIXME: Wait Message logic + self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + Ok(State::Ready) + }, + } + } + + /// 8.3.3.19.3 Source to Sink Power Role Swap + async fn execute_power_role_swap_state(&mut self, state: PowerRoleSwap) -> Result { + match state { + // 8.3.3.19.3.2 (PE_PRS_SRC_SNK_Evaluate_Swap): + PowerRoleSwap::Evaluate => { + match self.device_policy_manager.evaluate_swap_request(SwapType::Power).await { + true => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Accept))), + false => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Reject))), + } + }, + // 8.3.3.19.3.3 (PE_PRS_SRC_SNK_Accept_Swap): + PowerRoleSwap::Accept => { + self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) + }, + // 8.3.3.19.3.4 (PE_PRS_SRC_SNK_Transition_to_off): + PowerRoleSwap::TransitionToOff => { + self.device_policy_manager.disable_source().await; + Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::AssertRd))) + }, + // 8.3.3.19.3.5 (PE_PRS_SRC_SNK_Assert_Rd): + PowerRoleSwap::AssertRd => { + self.device_policy_manager.cc_sink().await; + Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn))) + }, + // 8.3.3.19.3.6 (PE_PRS_SRC_SNK_Wait_Source_on): + PowerRoleSwap::WaitSourceOn => { + self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + + self.protocol_layer.receive_message_type( + &[MessageType::Control(ControlMessageType::PsRdy)], + TimerType::PSSourceOnSpr + ).await?; + + Ok(State::PrSwapToSinkStartup) + }, + // 8.3.3.19.3.7 (PE_PRS_SRC_SNK_Send_Swap): + PowerRoleSwap::Send => { + self.protocol_layer.transmit_control_message(ControlMessageType::PrSwap).await?; + + let message = self.protocol_layer.receive_message_type(&[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait)], + TimerType::SenderResponse + ).await?; + + match message.header.message_type() { + MessageType::Control(ControlMessageType::Accept) => + Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))), + + MessageType::Control(ControlMessageType::Reject) | + MessageType::Control(ControlMessageType::Wait) => + Ok(State::Ready), + + _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)) + } + }, + // 8.3.3.18.3.8 (PE_PRS_SRC_SNK_Reject_Swap): + PowerRoleSwap::Reject => { + // FIXME: Wait Message logic + self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + Ok(State::Ready) + }, + } + } + + /// 8.3.3.19.5 Source to Sink Fast Role Swap + async fn execute_fast_power_role_swap_state(&mut self, state: FastPowerRoleSwap) -> Result { + match state { + // 8.3.3.19.5.2 (PE_FRS_SRC_SNK_Evaluate_Swap): + FastPowerRoleSwap::Evaluate => { + match self.device_policy_manager.fr_swap_signaled().await { + true => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Accept))), + false => Ok(State::HardReset), + } + }, + // 8.3.3.19.5.3 (PE_FRS_SRC_Accept): + FastPowerRoleSwap::Accept => { + match self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await { + Ok(_) => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff))), + _ => Ok(State::HardReset), // Soft Reset shall **not** be initiated in this case + } + }, + // 8.3.3.19.5.4 (PE_FRS_SRC_Transition_to_off): + FastPowerRoleSwap::TransitionToOff => { + self.device_policy_manager.discharge_vbus().await; + Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd))) + }, + // 8.3.3.19.5.5 (PE_FRS_SRC_Assert_Rd): + FastPowerRoleSwap::AssertRd => { + self.device_policy_manager.cc_sink().await; + Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn))) + }, + // 8.3.3.19.5.6 (PE_FRS_SRC_Wait_Source_on): + FastPowerRoleSwap::WaitSourceOn => { + self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + + self.protocol_layer.receive_message_type( + &[MessageType::Control(ControlMessageType::PsRdy)], + TimerType::PSSourceOnSpr + ).await?; + + Ok(State::PrSwapToSinkStartup) + }, + } + } + + async fn match_message_to_state(&mut self, message: Message) -> Result { + let state = match message.header.message_type() { + MessageType::Data(DataMessageType::Request) => { + if self.mode != Mode::Spr { + return Err(Error::Protocol(ProtocolError::RxError(RxError::HardReset))); + } + + let request = match message.payload { + Some(Payload::Data(Data::Request(power_source))) => power_source, + _ => unreachable!(), + }; + + State::NegotiateCapability(request) + } + + MessageType::Data(DataMessageType::EprRequest) => { + if self.mode != Mode::Epr { + return Err(Error::Protocol(ProtocolError::RxError(RxError::HardReset))); + } + + let request = match message.payload { + Some(Payload::Data(Data::Request(power_source))) => power_source, + _ => unreachable!(), + }; + + State::NegotiateCapability(request) + } + + MessageType::Data(DataMessageType::EprMode) => { + if let Some(Payload::Data(Data::EprMode(epr_data))) = message.payload { + match epr_data.action() { + epr_mode::Action::Enter => match self.mode { + Mode::Spr => State::EprModeEntry, + Mode::Epr => State::HardReset, + } + epr_mode::Action::Exit => match self.mode { + Mode::Spr => State::HardReset, + Mode::Epr => State::EprModeExitReceived, + } + _ => State::SendNotSupported, + } + } else { + State::SendNotSupported + } + } + + MessageType::Control(ControlMessageType::GetSourceCap) => match self.mode { + Mode::Spr => State::SendCapabilities, + Mode::Epr => State::GiveSourceCap, + }, + + MessageType::Control(ControlMessageType::VconnSwap) => { + State::VcsEvaluateSwap(VcsSwapSource::Message) + } + + MessageType::Control(ControlMessageType::DrSwap) => { + if !self.dual_role { + State::SendNotSupported + } else if self.mode == Mode::Epr { + State::HardReset + } else { + State::DrSwap(SwapState::Data(DataRoleSwap::Evaluate)) + } + } + + // 8.3.3.19.3.1 + MessageType::Control(ControlMessageType::PrSwap) => { + match self.dual_role { + true => State::DrSwap(SwapState::Power(PowerRoleSwap::Evaluate)), + false => State::SendNotSupported + } + } + + // 8.3.3.19.5.1 + MessageType::Control(ControlMessageType::FrSwap) => { + match self.dual_role { + true => State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), + false => State::SendNotSupported + } + } + + MessageType::Control(ControlMessageType::GetSinkCap) => { + match self.dual_role { + true => State::DrGiveSinkCap(Mode::Spr), + false => State::SendNotSupported, + } + } + + MessageType::Control(ControlMessageType::NotSupported) => { + State::NotSupportedReceived + } + + MessageType::Extended(ExtendedMessageType::ExtendedControl) => { + if let Some(Payload::Extended(Extended::ExtendedControl(ctrl))) = &message.payload { + match ctrl.message_type() { + ExtendedControlMessageType::EprGetSourceCap => match self.mode { + Mode::Spr => State::GiveSourceCap, + Mode::Epr => State::SendCapabilities, + }, + ExtendedControlMessageType::EprGetSinkCap => match self.dual_role { + true => State::DrGiveSinkCap(Mode::Epr), + false => State::SendNotSupported, + }, + ExtendedControlMessageType::EprKeepAlive => State::EprKeepAlive, + ExtendedControlMessageType::EprKeepAliveAck => State::SendNotSupported, // TODO + } + } else { + State::SendNotSupported + } + } + + _ => State::SendNotSupported, + }; + + Ok(state) + } + + /// Wait for the sink to request a capability after the source has published all capabilities. + /// Handles both SPR & EPR Modes. + async fn wait_for_sink_request(&mut self) -> Result { + // Receive a Request or EPR Request, depending on the current mode + let message = match self.mode { + Mode::Spr => { + let message = self.protocol_layer.wait_for_request().await?; + trace!("Sink Request {:?}", message); + + if message.header.message_type() != MessageType::Data(DataMessageType::Request) { + unreachable!() + } + + message + } + Mode::Epr => { + let message = self.protocol_layer.wait_for_epr_request().await?; + trace!("Sink EPR Request {:?}", message); + + if message.header.message_type() != MessageType::Data(DataMessageType::EprRequest) { + unreachable!() + } + + message + } + }; + + // Extract the power source from the request + let request = match message.payload { + Some(Payload::Data(Data::Request(power_source))) => power_source, + _ => unreachable!(), + }; + + Ok(request) + } + + /// GetSourceCap + async fn get_source_capabilities(&mut self) -> Result { + self.protocol_layer + .transmit_control_message(ControlMessageType::GetSourceCap) + .await?; + + let message = self.protocol_layer.receive_message_type( + &[MessageType::Data(DataMessageType::SourceCapabilities)], + TimerType::SenderResponse + ).await?; + + match message.payload { + Some(Payload::Data(Data::SourceCapabilities(caps))) => Ok(caps), + _ => unreachable!() + } + } + + /// GetSourceCap + async fn get_epr_source_capabilities(&mut self) -> Result { + self.protocol_layer + .transmit_extended_control_message(ExtendedControlMessageType::EprGetSourceCap) + .await?; + + let message = self.protocol_layer.receive_message_type( + &[MessageType::Extended(ExtendedMessageType::EprSourceCapabilities)], + TimerType::SenderResponse + ).await?; + + match message.payload { + Some(Payload::Extended(Extended::EprSourceCapabilities(cap))) => Ok(SourceCapabilities(cap)), + _ => unreachable!() + } + } +} + +#[cfg(test)] +mod tests { + use super::Source; + + // TODO +} From cabd266b7fca2ab15bec01b2809553ec7e274767 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Wed, 18 Feb 2026 16:16:14 -0500 Subject: [PATCH 2/8] feat: source integration tests --- usbpd/src/dummy.rs | 64 +++- usbpd/src/lib.rs | 1 - usbpd/src/source/device_policy_manager.rs | 42 +-- .../mod.rs} | 41 ++- usbpd/src/source/policy_engine/tests.rs | 292 ++++++++++++++++++ 5 files changed, 377 insertions(+), 63 deletions(-) rename usbpd/src/source/{policy_engine.rs => policy_engine/mod.rs} (98%) create mode 100644 usbpd/src/source/policy_engine/tests.rs diff --git a/usbpd/src/dummy.rs b/usbpd/src/dummy.rs index f43b91f..17ab077 100644 --- a/usbpd/src/dummy.rs +++ b/usbpd/src/dummy.rs @@ -5,11 +5,12 @@ use std::vec::Vec; use uom::si::power::watt; use usbpd_traits::Driver; -use crate::protocol_layer::message::data::request::EprRequestDataObject; +use crate::protocol_layer::message::data::request::{self, EprRequestDataObject}; use crate::protocol_layer::message::data::source_capabilities::{ - Augmented, FixedSupply, PowerDataObject, SprProgrammablePowerSupply, + Augmented, FixedSupply, PowerDataObject, SourceCapabilities, SprProgrammablePowerSupply }; use crate::sink::device_policy_manager::DevicePolicyManager as SinkDevicePolicyManager; +use crate::source::device_policy_manager::DevicePolicyManager as SourceDevicePolicyManager; use crate::timers::Timer; use crate::units::Power; @@ -120,13 +121,57 @@ impl SinkDevicePolicyManager for DummySinkEprDevice { } } +pub struct DummySourceDevice { + +} + +impl SourceDevicePolicyManager for DummySourceDevice { + + fn evaluate_request( + &mut self, + request: &crate::protocol_layer::message::data::request::PowerSource, + ) -> impl Future { + async { + if request.object_position() < 8 { + crate::source::device_policy_manager::CapabilityResponse::Accept + } else { + crate::source::device_policy_manager::CapabilityResponse::Reject + } + } + } + + fn source_capabilities(&mut self) -> SourceCapabilities { + SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()) + } + +} + +pub struct DummyDualRoleDevice { + +} + +impl SourceDevicePolicyManager for DummyDualRoleDevice { + +} + +impl SinkDevicePolicyManager for DummyDualRoleDevice { + +} + /// A dummy timer for testing. pub struct DummyTimer {} impl Timer for DummyTimer { - async fn after_millis(_milliseconds: u64) { - // Never time out - pending().await + async fn after_millis(_milliseconds: u64) { + // Should work OK since msgs should always send and arrive instantly in tests, + // such that timeouts never occur if a messaging sequence was pre-defined by the + // dummy endpoint before running `policy_engine.run_step().await.unwrap()` + embassy_futures::yield_now().await; + embassy_futures::yield_now().await; + embassy_futures::yield_now().await; + embassy_futures::yield_now().await; + embassy_futures::yield_now().await; + embassy_futures::yield_now().await; } } @@ -245,6 +290,15 @@ pub const DUMMY_CAPABILITIES: [u8; 30] = [ 0xC9, // + ]; + +pub fn get_source_capability_request() -> request::PowerSource { + request::PowerSource::new_fixed( + request::CurrentRequest::Highest, + request::VoltageRequest::Safe5V, + &crate::protocol_layer::message::data::source_capabilities::SourceCapabilities(heapless::Vec::from_slice(&get_dummy_source_capabilities()).unwrap()) + ).unwrap() +} + /// Get dummy source capabilities for testing. /// /// Corresponds to the `DUMMY_CAPABILITIES` above. diff --git a/usbpd/src/lib.rs b/usbpd/src/lib.rs index 06a14aa..daac207 100644 --- a/usbpd/src/lib.rs +++ b/usbpd/src/lib.rs @@ -102,7 +102,6 @@ use core::fmt::Debug; #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum PowerRole { /// The port is a source. - /// FIXME: Implement Source, /// The port is a sink. Sink, diff --git a/usbpd/src/source/device_policy_manager.rs b/usbpd/src/source/device_policy_manager.rs index bd5e85e..7c23dfc 100644 --- a/usbpd/src/source/device_policy_manager.rs +++ b/usbpd/src/source/device_policy_manager.rs @@ -69,7 +69,8 @@ pub enum SwapType { } /// Trait for the device policy manager. -/// Functions labeled **DRP** do not need to be implemented on single-role devices. +/// Functions labeled **EPR** do not need to be implemented on non-epr sources. +/// Functions labeled **DRP** do not need to be implemented on source-only devices. /// /// This entity commands the policy engine and enforces device policy. pub trait DevicePolicyManager { @@ -186,38 +187,7 @@ pub trait DevicePolicyManager { } } - -// TODO: Split DPM traits between base & -// pub trait DualRoleDevicePolicyManager { -// /// **DRP** Respond to the Policy Engine's request for this port's current sink capabilities -// /// -// /// Defaults to only default usb capability (5v @ 3A) -// fn sink_capabilities(&mut self) -> impl Future { -// async { SinkCapabilities::new_vsafe5v_only(3 * 100) } -// } - -// /// **DRP** Discharge the VBUS to vSafe5V -// fn discharge_vbus(&mut self) -> impl Future { async {} } - -// /// **DRP** Turn the Source off. -// /// -// /// This will be requested before a Role Swap to Sink -// fn disable_source(&mut self) -> impl Future { async {} } - -// /// **DRP** Set the CC lines to sink configuration -// /// -// /// This will be requested before a Role Swap to Sink -// fn cc_sink(&mut self) -> impl Future { async {} } - -// /// **DRP** Detect whether a fast role swap is signaled on the cc lines -// /// -// /// Table 1.4 - Fast Role Swap Request: -// /// -// /// An indication from an Initial Source to the Initial Sink that a -// /// Fast Role Swap is needed. The Fast Role Swap Request is indicated by -// /// driving the CC line to ground for a short period. -// fn fr_swap_signaled(&mut self) -> impl Future { -// async { true } -// } - -// } +// FIXME: Split DPM traits between base, epr, and dual roles +// pub trait DevicePolicyManager {} +// pub trait EprDevicePolicyManager {} +// pub trait DualRoleDevicePolicyManager {} diff --git a/usbpd/src/source/policy_engine.rs b/usbpd/src/source/policy_engine/mod.rs similarity index 98% rename from usbpd/src/source/policy_engine.rs rename to usbpd/src/source/policy_engine/mod.rs index 936fee5..a70d974 100644 --- a/usbpd/src/source/policy_engine.rs +++ b/usbpd/src/source/policy_engine/mod.rs @@ -26,6 +26,9 @@ use crate::source::device_policy_manager::{CapabilityResponse, Info}; use crate::timers::{Timer, TimerType}; use crate::{DataRole, PowerRole}; +#[cfg(test)] +mod tests; + /// Source capability #[derive(Debug, Clone, Copy, Default, PartialEq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -210,7 +213,7 @@ impl Source Self { Self { device_policy_manager, @@ -218,11 +221,8 @@ impl Source State::SendCapabilities, - false => State::Startup { role_swap: true } - }, - contract: match role_swapped { + state: State::Startup { role_swap }, + contract: match role_swap { true => Contract::Implicit, false => Contract::default(), }, @@ -336,7 +336,7 @@ impl Source { error!("Protocol error {:?} in source state transition", error); - None + return Err(Error::Protocol(error)); } }; @@ -374,7 +374,7 @@ impl Source(TimerType::SwapSourceStart).await; } - // TODO: Sources shall remain in the Startup state until a plug is Attached + // FIXME: Sources shall remain in the Startup state until a plug is Attached // For now, assume that a Source driver will only be ran after an attach occurs State::SendCapabilities @@ -462,11 +462,12 @@ impl Source Source State::EprModeSendExit, Event::RequestVconnSwap => State::VcsSendSwap(VcsSwapSource::Message), Event::RequestDataRoleSwap => State::DrSwap(SwapState::Data(DataRoleSwap::Send)), - Event::RequestPowerRoleSwap => State::DrSwap(SwapState::Power(PowerRoleSwap::Send)), }, + Event::RequestPowerRoleSwap => State::DrSwap(SwapState::Power(PowerRoleSwap::Send)), + }, Either3::Third(timeout_source) => match timeout_source { // DiscoverIdentity Timeout @@ -534,7 +536,7 @@ impl Source { - // This **SHOULD** put the device in a Safe5V default power mode + // This **SHOULD** put the device in a vSafe5V default power mode let source_capabilities = self.device_policy_manager.source_capabilities(); self.device_policy_manager .transition_power( @@ -814,7 +816,7 @@ impl Source { - // TODO: Intentional exit due to power swap + // FIXME: Switch to sink policy manager due to power swap Err(Error::SwapToSink)? } @@ -918,7 +920,7 @@ impl Source { match self.device_policy_manager.epr_capable() { @@ -1212,6 +1214,10 @@ impl Source { + State::SoftReset + } + MessageType::Control(ControlMessageType::GetSourceCap) => match self.mode { Mode::Spr => State::SendCapabilities, Mode::Epr => State::GiveSourceCap, @@ -1270,7 +1276,7 @@ impl Source State::SendNotSupported, }, ExtendedControlMessageType::EprKeepAlive => State::EprKeepAlive, - ExtendedControlMessageType::EprKeepAliveAck => State::SendNotSupported, // TODO + ExtendedControlMessageType::EprKeepAliveAck => State::SendNotSupported, // FIXME: Source EPR } } else { State::SendNotSupported @@ -1353,10 +1359,3 @@ impl Source Source, DummyTimer, DummySourceDevice> { + Source::new(DummyDriver::new(), DummySourceDevice {}, false) +} + +fn simulate_sink_control_message( + policy_engine: &mut Source, DummyTimer, DPM>, + control_message_type: ControlMessageType, + message_id: u8 +) { + let header = *policy_engine.protocol_layer.header(); + let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; + + Message::new(Header::new_control( + get_sink_header_template(), + Counter::new_from_value(CounterType::MessageId, message_id), + control_message_type, + )) + .to_bytes(&mut buf); + policy_engine.protocol_layer.driver().inject_received_data(&buf); +} + +/// Get a header template for simulating sink messages (Sink/Ufp roles). +/// This flips the roles from the source's perspective to simulate messages from the sink. +fn get_sink_header_template() -> Header { + use crate::protocol_layer::message::header::SpecificationRevision; + use crate::{DataRole, PowerRole}; + + // Non-DRP Sink messages have Sink/Ufp roles (opposite of source's Source/Dfp) + Header::new_template(DataRole::Ufp, PowerRole::Sink, SpecificationRevision::R3_X) +} + +fn request_capability_message(message_id: u8, highest_power: bool) -> Message { + let source_capabilities = SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()); + let header = Header::new_data( + get_sink_header_template(), + Counter::new_from_value(CounterType::MessageId, message_id), + DataMessageType::Request, + source_capabilities.0.len() as u8 + ); + + let data = match highest_power { + true => Data::Request(PowerSource::new_fixed( + CurrentRequest::Highest, + VoltageRequest::Highest, + &source_capabilities + ).unwrap()), + false => Data::Request(PowerSource::new_fixed( + CurrentRequest::Highest, + VoltageRequest::Safe5V, + &source_capabilities + ).unwrap()), + }; + + Message::new_with_data(header, data) +} + +fn simulate_sink_request ( + policy_engine: &mut Source, DummyTimer, DPM>, + message_id: u8, + highest_power: bool +) { + let mut message = request_capability_message(message_id, highest_power); + let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; + let len = message.to_bytes(&mut buf); + + policy_engine.protocol_layer.driver().inject_received_data(&buf[..len]); +} + +#[tokio::test] +async fn test_negotiation() { + let mut policy_engine = get_policy_engine(); + + eprintln!("\n<== Starting initial source SPR negotiation test! ==>\n"); { + // Instantiated in `SendCapabilities` state (not started from role-swap) + eprintln!("- Ran Step 0: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); + + // `Startup` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); + + // Request vSafe5V @ highest current + simulate_sink_request(&mut policy_engine, 1, false); + + // `SendCapabilities` -> Get Request -> `NegotiateCapability` + policy_engine.run_step().await.unwrap(); { + + let sent_capabilities = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + assert_eq!( + sent_capabilities.header.message_type(), + MessageType::Data(DataMessageType::SourceCapabilities) + ); + + let good_crc = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + assert_eq!( + good_crc.header.message_type(), + MessageType::Control(ControlMessageType::GoodCRC) + ); + + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + } + + // `NegotiateCapability` -> DPM: Accept -> `Transition Supply` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::TransitionSupply(_))); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); + + // `TransitionSupply` -> Accept -> PsRdy -> `Ready` + policy_engine.run_step().await.unwrap(); { + + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- TransitionSupply message: {:?}", msg.header.message_type()); + } + + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); + } + + } eprintln!("\n==> Finished initial source SPR negotiation test! <==\n"); + + eprintln!("\n<== Starting source SPR re-negotiation test! ==>\n"); { + + // Simulate `GoodCrc` to SourceCapabilities + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GetSourceCap, 3); + + // `Ready` -> GetSourceCap Received -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); + + // Request highest power + simulate_sink_request(&mut policy_engine, 4, true); + + // `SendCapabilities` -> Get Request -> `NegotiateCapability` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + + // `NegotiateCapability` -> Request Can Be Met -> `TransitionSupply` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::TransitionSupply(_))); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 4); + + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 5); + + // `TransitionSupply` -> Accept -> PsRdy -> `Ready` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); + + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Re-negotiation test message: {:?}", msg.header.message_type()); + } + + } eprintln!("\n==> Finished source SPR re-negotiation test! <==\n"); + + eprintln!("\n==> Starting source SPR soft reset - reject test! <==\n"); { + + // Simulate `SoftReset` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::SoftReset, 6); + + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SoftReset)); + + // `GoodCrc` for `Accept` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); + + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); + + // `GoodCrc` for `SourceCapabilities` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + + // Make an invalid request + let data = Data::Request(PowerSource::FixedVariableSupply(FixedVariableSupply(0xA000_0000))); + let header = Header::new_data( + get_sink_header_template(), + Counter::new_from_value(CounterType::MessageId, 2), + DataMessageType::Request, + 7 + ); + let message = Message::new_with_data(header, data); + let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; + let len = message.to_bytes(&mut buf); + policy_engine.protocol_layer.driver().inject_received_data(&buf[..len]); + + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::CapabilityResponse(CapabilityResponse::Reject))); + + // `GoodCrc` for `Reject` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); + + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 5: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); + + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- SoftReset-Reject message: {:?}", msg.header.message_type()); + } + + } eprintln!("\n==> Finished source SPR soft reset - reject test! <==\n"); + +} + +#[tokio::test] +async fn test_discovery() { + const MAX_DISCOVERY_ITERS: usize = 52; + let mut policy_engine = get_policy_engine(); + // HardReset -> Discovery -> Disabled + eprintln!("\n==> Starting source SPR discovery test! <==\n"); { + + eprintln!("- Ran Step 0: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); + + // `Startup` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); + + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); + + // `SendCapabilities` -> Capability Send Failure -> `Discovery` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Discovery)); + + // `Discovery` -> + let mut discovery_iterations = 0; + loop { + if !matches!(policy_engine.state, State::Discovery) { break; } + + // `Discovery` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + discovery_iterations += 1; + + if discovery_iterations > MAX_DISCOVERY_ITERS { break; } + if !matches!(policy_engine.state, State::SendCapabilities) { break; } + + // `SendCapabilities` -> `Discovery` + policy_engine.run_step().await.unwrap(); + } + eprintln!("- Ran Step 3: {0:?}, discovery state iterations = {1:?}", policy_engine.state, discovery_iterations); + assert!(matches!(policy_engine.state, State::Disabled)); + + } eprintln!("\n==> Finished source SPR discovery test! <==\n"); + +} + + From 3730ced1cc9cc435626879de5fef4df7413f0fe3 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Wed, 18 Feb 2026 16:25:50 -0500 Subject: [PATCH 3/8] fix: apply rustfmt to source additions --- usbpd/src/dummy.rs | 36 +- usbpd/src/source/device_policy_manager.rs | 65 +-- usbpd/src/source/policy_engine/mod.rs | 481 ++++++++++++---------- usbpd/src/source/policy_engine/tests.rs | 371 +++++++++-------- 4 files changed, 495 insertions(+), 458 deletions(-) diff --git a/usbpd/src/dummy.rs b/usbpd/src/dummy.rs index 17ab077..5b5c674 100644 --- a/usbpd/src/dummy.rs +++ b/usbpd/src/dummy.rs @@ -7,7 +7,7 @@ use usbpd_traits::Driver; use crate::protocol_layer::message::data::request::{self, EprRequestDataObject}; use crate::protocol_layer::message::data::source_capabilities::{ - Augmented, FixedSupply, PowerDataObject, SourceCapabilities, SprProgrammablePowerSupply + Augmented, FixedSupply, PowerDataObject, SourceCapabilities, SprProgrammablePowerSupply, }; use crate::sink::device_policy_manager::DevicePolicyManager as SinkDevicePolicyManager; use crate::source::device_policy_manager::DevicePolicyManager as SourceDevicePolicyManager; @@ -121,19 +121,16 @@ impl SinkDevicePolicyManager for DummySinkEprDevice { } } -pub struct DummySourceDevice { - -} +pub struct DummySourceDevice {} impl SourceDevicePolicyManager for DummySourceDevice { - fn evaluate_request( &mut self, request: &crate::protocol_layer::message::data::request::PowerSource, ) -> impl Future { - async { - if request.object_position() < 8 { - crate::source::device_policy_manager::CapabilityResponse::Accept + async { + if request.object_position() < 8 { + crate::source::device_policy_manager::CapabilityResponse::Accept } else { crate::source::device_policy_manager::CapabilityResponse::Reject } @@ -143,26 +140,19 @@ impl SourceDevicePolicyManager for DummySourceDevice { fn source_capabilities(&mut self) -> SourceCapabilities { SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()) } - -} - -pub struct DummyDualRoleDevice { - } -impl SourceDevicePolicyManager for DummyDualRoleDevice { - -} +pub struct DummyDualRoleDevice {} -impl SinkDevicePolicyManager for DummyDualRoleDevice { +impl SourceDevicePolicyManager for DummyDualRoleDevice {} -} +impl SinkDevicePolicyManager for DummyDualRoleDevice {} /// A dummy timer for testing. pub struct DummyTimer {} impl Timer for DummyTimer { - async fn after_millis(_milliseconds: u64) { + async fn after_millis(_milliseconds: u64) { // Should work OK since msgs should always send and arrive instantly in tests, // such that timeouts never occur if a messaging sequence was pre-defined by the // dummy endpoint before running `policy_engine.run_step().await.unwrap()` @@ -290,13 +280,15 @@ pub const DUMMY_CAPABILITIES: [u8; 30] = [ 0xC9, // + ]; - pub fn get_source_capability_request() -> request::PowerSource { request::PowerSource::new_fixed( request::CurrentRequest::Highest, request::VoltageRequest::Safe5V, - &crate::protocol_layer::message::data::source_capabilities::SourceCapabilities(heapless::Vec::from_slice(&get_dummy_source_capabilities()).unwrap()) - ).unwrap() + &crate::protocol_layer::message::data::source_capabilities::SourceCapabilities( + heapless::Vec::from_slice(&get_dummy_source_capabilities()).unwrap(), + ), + ) + .unwrap() } /// Get dummy source capabilities for testing. diff --git a/usbpd/src/source/device_policy_manager.rs b/usbpd/src/source/device_policy_manager.rs index 7c23dfc..69ca206 100644 --- a/usbpd/src/source/device_policy_manager.rs +++ b/usbpd/src/source/device_policy_manager.rs @@ -4,11 +4,10 @@ //! or renegotiate the power contract. use core::future::Future; -use crate::{DataRole, protocol_layer::message::data::{ - request, - sink_capabilities::SinkCapabilities, - source_capabilities::SourceCapabilities, -}}; +use crate::DataRole; +use crate::protocol_layer::message::data::request; +use crate::protocol_layer::message::data::sink_capabilities::SinkCapabilities; +use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; /// Events that the device policy manager can send to the policy engine. #[derive(Debug, Clone, Copy, PartialEq)] @@ -68,25 +67,21 @@ pub enum SwapType { Power, } -/// Trait for the device policy manager. +/// Trait for the device policy manager. /// Functions labeled **EPR** do not need to be implemented on non-epr sources. /// Functions labeled **DRP** do not need to be implemented on source-only devices. /// /// This entity commands the policy engine and enforces device policy. pub trait DevicePolicyManager { - /// Evaluate a request from the Sink - /// + /// /// The policy engine will use this evaluation to determine PD control flow - fn evaluate_request( - &mut self, - _request: &request::PowerSource, - ) -> impl Future { + fn evaluate_request(&mut self, _request: &request::PowerSource) -> impl Future { async { CapabilityResponse::Reject } } /// Evaluate a swap request: - /// - Vconn, + /// - Vconn, /// - **DRP**: Data, Power, Fast Power fn evaluate_swap_request(&mut self, _swap_request: SwapType) -> impl Future { async { false } @@ -107,7 +102,7 @@ pub trait DevicePolicyManager { } /// Hard reset power supply to vSafe5V via vSafe0V - /// + /// /// A Hard Reset shall not cause any change to either the Rp/Rd resistor being asserted fn hard_reset(&mut self) -> impl Future> { async { Ok(()) } @@ -119,12 +114,16 @@ pub trait DevicePolicyManager { } /// **EPR** Return `true` if device is EPR capable. - /// + /// /// Also possible to dynamically assess EPR capability - fn epr_capable(&mut self) -> bool { false } + fn epr_capable(&mut self) -> bool { + false + } /// **EPR** Return `true` if the `Cable Plug` supports EPR - fn epr_cable_good(&mut self) -> bool { false } + fn epr_cable_good(&mut self) -> bool { + false + } /// **EPR** Return device's EPR capabilities fn epr_source_capabilities(&mut self) -> SourceCapabilities { @@ -132,38 +131,46 @@ pub trait DevicePolicyManager { } /// **DRP** Respond to the Policy Engine's request for this port's current sink capabilities - /// + /// /// Defaults to only default usb capability (5v @ 3A) fn sink_capabilities(&mut self) -> impl Future { async { SinkCapabilities::new_vsafe5v_only(3 * 100) } } /// **DRP** Discharge the VBUS to vSafe5V - fn discharge_vbus(&mut self) -> impl Future { async {} } + fn discharge_vbus(&mut self) -> impl Future { + async {} + } /// **DRP** Turn the Source off. - /// + /// /// This will be requested before a Role Swap to Sink - fn disable_source(&mut self) -> impl Future { async {} } + fn disable_source(&mut self) -> impl Future { + async {} + } /// **DRP** Set the CC lines to sink configuration - /// + /// /// This will be requested before a Role Swap to Sink - fn cc_sink(&mut self) -> impl Future { async {} } + fn cc_sink(&mut self) -> impl Future { + async {} + } /// **DRP** Detect whether a fast role swap is signaled on the cc lines - /// - /// Table 1.4 - Fast Role Swap Request: - /// - /// An indication from an Initial Source to the Initial Sink that a - /// Fast Role Swap is needed. The Fast Role Swap Request is indicated by + /// + /// Table 1.4 - Fast Role Swap Request: + /// + /// An indication from an Initial Source to the Initial Sink that a + /// Fast Role Swap is needed. The Fast Role Swap Request is indicated by /// driving the CC line to ground for a short period. fn fr_swap_signaled(&mut self) -> impl Future { async { true } } /// **DRP** Swap data role - fn swap_data_role(&mut self, _role: DataRole) -> impl Future { async {} } + fn swap_data_role(&mut self, _role: DataRole) -> impl Future { + async {} + } /// The policy engine gets and evaluates device policy events when ready. /// diff --git a/usbpd/src/source/policy_engine/mod.rs b/usbpd/src/source/policy_engine/mod.rs index a70d974..ce0a84a 100644 --- a/usbpd/src/source/policy_engine/mod.rs +++ b/usbpd/src/source/policy_engine/mod.rs @@ -1,28 +1,23 @@ //! Policy engine for the implementation of a sink. use core::marker::PhantomData; -use embassy_futures::select::{Either, select}; -use embassy_futures::select::{Either3, select3}; +use embassy_futures::select::{Either, Either3, select, select3}; use usbpd_traits::Driver; -use super::device_policy_manager::DevicePolicyManager; -use super::device_policy_manager::Event; +use super::device_policy_manager::{DevicePolicyManager, Event}; use crate::counters::Counter; -use crate::protocol_layer::message::Message; -use crate::protocol_layer::message::Payload; use crate::protocol_layer::message::data::request::PowerSource; use crate::protocol_layer::message::data::sink_capabilities::SinkCapabilities; -use crate::protocol_layer::message::data::source_capabilities::Kind; -use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; +use crate::protocol_layer::message::data::source_capabilities::{Kind, SourceCapabilities}; use crate::protocol_layer::message::data::{Data, PdoKind, epr_mode, request}; use crate::protocol_layer::message::extended::Extended; use crate::protocol_layer::message::extended::extended_control::ExtendedControlMessageType; use crate::protocol_layer::message::header::{ ControlMessageType, DataMessageType, ExtendedMessageType, Header, MessageType, SpecificationRevision, }; +use crate::protocol_layer::message::{Message, Payload}; use crate::protocol_layer::{ProtocolError, ProtocolLayer, RxError, TxError}; -use crate::source::device_policy_manager::SwapType; -use crate::source::device_policy_manager::{CapabilityResponse, Info}; +use crate::source::device_policy_manager::{CapabilityResponse, Info, SwapType}; use crate::timers::{Timer, TimerType}; use crate::{DataRole, PowerRole}; @@ -56,9 +51,7 @@ enum Contract { enum State { // States of the policy engine as given by specification. // 8.3.3.2 Policy Engine Source Port State Diagram - Startup { - role_swap: bool, - }, + Startup { role_swap: bool }, Discovery, SendCapabilities, NegotiateCapability(PowerSource), @@ -118,7 +111,7 @@ enum State { #[cfg_attr(feature = "defmt", derive(defmt::Format))] enum VcsSwapSource { Message, - Epr + Epr, } #[derive(Debug, Clone, Copy)] @@ -210,11 +203,7 @@ impl Source Self { + pub fn new(driver: DRIVER, device_policy_manager: DPM, role_swap: bool) -> Self { Self { device_policy_manager, protocol_layer: Self::new_protocol_layer(driver), @@ -234,14 +223,10 @@ impl Source Self { + pub fn new_dual_role(driver: DRIVER, device_policy_manager: DPM, role_swapped: bool) -> Self { Self { device_policy_manager, protocol_layer: Self::new_protocol_layer(driver), @@ -250,7 +235,7 @@ impl Source State::SendCapabilities, - false => State::Startup { role_swap: true } + false => State::Startup { role_swap: true }, }, contract: match role_swapped { true => Contract::Implicit, @@ -308,16 +293,13 @@ impl Source - { - Some(State::ErrorRecovery) - } - + ( + _, + State::DrSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)) + | State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn)), + ProtocolError::RxError(RxError::ReceiveTimeout) | ProtocolError::TransmitRetriesExceeded(_), + ) => Some(State::ErrorRecovery), + // Per spec 8.3.3.2.5: When any Protocol Error occurs, transition to Hard Reset (_, State::TransitionSupply(_), _) => Some(State::HardReset), @@ -417,17 +399,19 @@ impl Source { - // FIXME: This should be done in the protocol layer // If the request is Unknown, attempt to match to its PDO to determine the Kind & re-type the request let power_request = match *power_request { PowerSource::Unknown(u) => { - match self.device_policy_manager + match self + .device_policy_manager .source_capabilities() - .at_object_position(u.object_position()) + .at_object_position(u.object_position()) { Some(q) => match q { - Kind::FixedSupply | Kind::VariableSupply => request::PowerSource::FixedVariableSupply(request::FixedVariableSupply(u.0)), + Kind::FixedSupply | Kind::VariableSupply => { + request::PowerSource::FixedVariableSupply(request::FixedVariableSupply(u.0)) + } Kind::Battery => request::PowerSource::Battery(request::Battery(u.0)), Kind::Pps => request::PowerSource::Pps(request::Pps(u.0)), Kind::Avs => request::PowerSource::Avs(request::Avs(u.0)), @@ -435,16 +419,13 @@ impl Source { trace!("Could not match Unknown Power Request to a PDO!"); *power_request - }, + } } } - _ => *power_request + _ => *power_request, }; - let response = self - .device_policy_manager - .evaluate_request(&power_request) - .await; + let response = self.device_policy_manager.evaluate_request(&power_request).await; match response { CapabilityResponse::Accept => State::TransitionSupply(power_request), @@ -505,8 +486,10 @@ impl Source { let message = message?; self.match_message_to_state(message).await? @@ -568,12 +551,13 @@ impl Source + Contract::Invalid => { if message_type == ControlMessageType::Reject { State::HardReset } else { State::WaitNewCapabilities } + } Contract::Explicit(_) => State::Ready, _ => State::WaitNewCapabilities, } @@ -690,15 +674,19 @@ impl Source if event == Event::UpdatedSourceCapabilities { break }, + match select(self.device_policy_manager.get_event(), TIMER::after_millis(5000)).await { + Either::First(event) => { + if event == Event::UpdatedSourceCapabilities { + break; + } + } Either::Second(_timeout) => { wait_time += 5; - warn!("{} seconds have passed waiting for updated source capabilities!", wait_time); - }, + warn!( + "{} seconds have passed waiting for updated source capabilities!", + wait_time + ); + } } } State::SendCapabilities @@ -772,32 +760,29 @@ impl Source { - match swap_state { - SwapState::Data(dr) => - self.execute_data_role_swap_state(*dr).await?, - SwapState::Power(pr) => - self.execute_power_role_swap_state(*pr).await?, - SwapState::FastPower(fpr) => - self.execute_fast_power_role_swap_state(*fpr).await? - } - } + State::DrSwap(swap_state) => match swap_state { + SwapState::Data(dr) => self.execute_data_role_swap_state(*dr).await?, + SwapState::Power(pr) => self.execute_power_role_swap_state(*pr).await?, + SwapState::FastPower(fpr) => self.execute_fast_power_role_swap_state(*fpr).await?, + }, // 8.3.3.19.7.1 (PE_DR_SRC_Get_Source_Cap): State::DrGetSourceCap(mode) => { let result = match mode { Mode::Spr => self.get_source_capabilities().await, Mode::Epr => self.get_epr_source_capabilities().await, }; - + let caps = match result { Ok(caps) => Some(caps), Err(err) => match err { Error::Protocol(ProtocolError::RxError(RxError::ReceiveTimeout)) => None, _ => return Err(err), - } + }, }; - self.device_policy_manager.inform(Info::RemoteSourceCapabilities(caps)).await; + self.device_policy_manager + .inform(Info::RemoteSourceCapabilities(caps)) + .await; State::Ready } // 8.3.3.19.8 @@ -816,28 +801,34 @@ impl Source { - // FIXME: Switch to sink policy manager due to power swap + // FIXME: Switch to sink policy manager due to power swap Err(Error::SwapToSink)? } // 8.3.3.20 Source Vconn Swap // 8.3.3.20.1 (PE_VCS_Send_Swap): State::VcsSendSwap(source) => { - self.protocol_layer.transmit_control_message(ControlMessageType::VconnSwap).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::VconnSwap) + .await?; - let message = self.protocol_layer.receive_message_type(&[ - MessageType::Control(ControlMessageType::Accept), - MessageType::Control(ControlMessageType::Reject), - MessageType::Control(ControlMessageType::Wait), - MessageType::Control(ControlMessageType::NotSupported) - ], - TimerType::SenderResponse - ).await; + let message = self + .protocol_layer + .receive_message_type( + &[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait), + MessageType::Control(ControlMessageType::NotSupported), + ], + TimerType::SenderResponse, + ) + .await; if let Err(err) = message { match err { ProtocolError::RxError(RxError::ReceiveTimeout) => State::Ready, - _ => Err(err)? + _ => Err(err)?, } } else { let message = message.unwrap(); @@ -846,11 +837,11 @@ impl Source State::VcsWaitForVconn(*source), false => State::VcsTurnOnVconn(*source), }, - MessageType::Control(ControlMessageType::Reject) | - MessageType::Control(ControlMessageType::Wait) => State::Ready, + MessageType::Control(ControlMessageType::Reject) + | MessageType::Control(ControlMessageType::Wait) => State::Ready, // May also transition to ForceVconn if NotSupported message and port presently not vconn source MessageType::Control(ControlMessageType::NotSupported) => State::NotSupportedReceived, - _ => unreachable!() + _ => unreachable!(), } } } @@ -863,7 +854,9 @@ impl Source { - self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await?; match self.vconn_source { true => State::VcsWaitForVconn(*source), false => State::VcsTurnOnVconn(*source), @@ -872,7 +865,9 @@ impl Source { // FIXME: Wait Message logic - self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Reject) + .await?; match source { VcsSwapSource::Message => State::Ready, VcsSwapSource::Epr => State::EprModeDiscoverCable, @@ -880,16 +875,17 @@ impl Source { - self.protocol_layer.receive_message_type( - &[MessageType::Control(ControlMessageType::PsRdy)], - TimerType::VCONNOn - ).await?; + self.protocol_layer + .receive_message_type(&[MessageType::Control(ControlMessageType::PsRdy)], TimerType::VCONNOn) + .await?; State::VcsTurnOffVconn(*source) } // 8.3.3.20.6 (PE_VCS_Turn_Off_Vconn): State::VcsTurnOffVconn(source) => { - self.device_policy_manager.drive_vconn(false).await + self.device_policy_manager + .drive_vconn(false) + .await .map_err(|_| Error::ReconnectionRequired)?; match source { VcsSwapSource::Message => State::Ready, @@ -898,13 +894,17 @@ impl Source { - self.device_policy_manager.drive_vconn(true).await + self.device_policy_manager + .drive_vconn(true) + .await .map_err(|_| Error::ReconnectionRequired)?; State::VcsSendPsRdy(*source) } // 8.3.3.20.8 (PE_VCS_Send_PS_Rdy): State::VcsSendPsRdy(source) => { - self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::PsRdy) + .await?; match source { VcsSwapSource::Message => State::Ready, VcsSwapSource::Epr => State::EprModeDiscoverCable, @@ -912,30 +912,32 @@ impl Source { - self.device_policy_manager.drive_vconn(true).await + self.device_policy_manager + .drive_vconn(true) + .await .map_err(|_| Error::ReconnectionRequired)?; match source { VcsSwapSource::Message => State::Ready, VcsSwapSource::Epr => State::EprModeDiscoverCable, } - } + } // FIXME: Source EPR // 8.3.3.26.1.1 (PE_SRC_Evaluate_EPR_Mode_Entry): - State::EprModeEntry => { - match self.device_policy_manager.epr_capable() { - true => State::EprModeEntryAck, - false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceUnableToEnterEprMode.into()) - } - } + State::EprModeEntry => match self.device_policy_manager.epr_capable() { + true => State::EprModeEntryAck, + false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceUnableToEnterEprMode.into()), + }, // 8.3.3.26.1.2 (PE_SRC_EPR_Mode_Entry_Ack): State::EprModeEntryAck => { - self.protocol_layer.transmit_epr_mode(epr_mode::Action::EnterAcknowledged, 0).await?; + self.protocol_layer + .transmit_epr_mode(epr_mode::Action::EnterAcknowledged, 0) + .await?; match (self.device_policy_manager.epr_cable_good(), self.vconn_source) { (false, true) => State::VcsSendSwap(VcsSwapSource::Epr), (false, false) => State::EprModeDiscoverCable, - (true, _) => State::EprModeEvaluateCable + (true, _) => State::EprModeEvaluateCable, } } // 8.3.3.26.1.3 (PE_SRC_EPR_Mode_Discover_Cable): @@ -944,19 +946,21 @@ impl Source State::EprModeEvaluateCable, - false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceFailedToBecomeVconnSource.into()) + false => { + State::EprModeEntryFailed(epr_mode::DataEnterFailed::SourceFailedToBecomeVconnSource.into()) + } } } // 8.3.3.26.1.4 (PE_SRC_EPR_Mode_Evaluate_Cable_EPR): - State::EprModeEvaluateCable => { - match self.device_policy_manager.epr_cable_good() { - true => State::EprModeEntrySucceeded, - false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::CableNotEprCapable.into()), - } - } + State::EprModeEvaluateCable => match self.device_policy_manager.epr_cable_good() { + true => State::EprModeEntrySucceeded, + false => State::EprModeEntryFailed(epr_mode::DataEnterFailed::CableNotEprCapable.into()), + }, // 8.3.3.26.1.5 (PE_SRC_EPR_Mode_Entry_Succeeded): State::EprModeEntrySucceeded => { - self.protocol_layer.transmit_epr_mode(epr_mode::Action::EnterSucceeded, 0).await?; + self.protocol_layer + .transmit_epr_mode(epr_mode::Action::EnterSucceeded, 0) + .await?; // FIXME: Set EPR headers self.mode = Mode::Epr; State::SendCapabilities @@ -985,14 +989,17 @@ impl Source State::Ready, - _ => State::HardReset + _ => State::HardReset, } } @@ -1007,56 +1014,65 @@ impl Source Result { match state { // 8.3.3.19.1.2 (PE_DRS_DFP_UFP_Evaluate_Swap, PE_DRS_UFP_DFP_Evaluate_Swap): - DataRoleSwap::Evaluate => { - match self.device_policy_manager.evaluate_swap_request(SwapType::Data).await { - true => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Accept))), - false => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Reject))), - } + DataRoleSwap::Evaluate => match self.device_policy_manager.evaluate_swap_request(SwapType::Data).await { + true => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Accept))), + false => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Reject))), }, // 8.3.3.19.1.3 (PE_DRS_DFP_UFP_Accept_Swap, PE_DRS_UFP_DFP_Accept_Swap): DataRoleSwap::Accept => { - self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await?; Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))) - }, + } // 8.3.3.19.1.4 (PE_DRS_DFP_UFP_Change_to_UFP_Swap, PE_DRS_UFP_DFP_Change_to_DFP_Swap): DataRoleSwap::Change => { let new_role = DataRole::from(!bool::from(self.protocol_layer.header().port_data_role())); self.device_policy_manager.swap_data_role(new_role).await; Ok(State::Ready) - }, + } // 8.3.3.19.1.5 (PE_DRS_DFP_UFP_Send_Swap, PE_DRS_UFP_DFP_Send_Swap): DataRoleSwap::Send => { - self.protocol_layer.transmit_control_message(ControlMessageType::DrSwap).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::DrSwap) + .await?; - let message = self.protocol_layer.receive_message_type(&[ - MessageType::Control(ControlMessageType::Accept), - MessageType::Control(ControlMessageType::Reject), - MessageType::Control(ControlMessageType::Wait)], - TimerType::SenderResponse - ).await?; + let message = self + .protocol_layer + .receive_message_type( + &[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait), + ], + TimerType::SenderResponse, + ) + .await?; match message.header.message_type() { - MessageType::Control(ControlMessageType::Accept) => - Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))), + MessageType::Control(ControlMessageType::Accept) => { + Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))) + } - MessageType::Control(ControlMessageType::Reject) | - MessageType::Control(ControlMessageType::Wait) => - Ok(State::Ready), + MessageType::Control(ControlMessageType::Reject) + | MessageType::Control(ControlMessageType::Wait) => Ok(State::Ready), - _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)) + _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)), } - }, + } // 8.3.3.19.1.6 (PE_DRS_DFP_UFP_Reject_Swap, PE_DRS_UFP_DFP_Reject_Swap): DataRoleSwap::Reject => { // FIXME: Wait Message logic - self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Reject) + .await?; Ok(State::Ready) - }, + } } } @@ -1064,66 +1080,79 @@ impl Source Result { match state { // 8.3.3.19.3.2 (PE_PRS_SRC_SNK_Evaluate_Swap): - PowerRoleSwap::Evaluate => { - match self.device_policy_manager.evaluate_swap_request(SwapType::Power).await { - true => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Accept))), - false => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Reject))), - } + PowerRoleSwap::Evaluate => match self.device_policy_manager.evaluate_swap_request(SwapType::Power).await { + true => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Accept))), + false => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Reject))), }, // 8.3.3.19.3.3 (PE_PRS_SRC_SNK_Accept_Swap): PowerRoleSwap::Accept => { - self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await?; Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) - }, + } // 8.3.3.19.3.4 (PE_PRS_SRC_SNK_Transition_to_off): PowerRoleSwap::TransitionToOff => { self.device_policy_manager.disable_source().await; Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::AssertRd))) - }, + } // 8.3.3.19.3.5 (PE_PRS_SRC_SNK_Assert_Rd): PowerRoleSwap::AssertRd => { self.device_policy_manager.cc_sink().await; Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn))) - }, + } // 8.3.3.19.3.6 (PE_PRS_SRC_SNK_Wait_Source_on): PowerRoleSwap::WaitSourceOn => { - self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::PsRdy) + .await?; + + self.protocol_layer + .receive_message_type( + &[MessageType::Control(ControlMessageType::PsRdy)], + TimerType::PSSourceOnSpr, + ) + .await?; - self.protocol_layer.receive_message_type( - &[MessageType::Control(ControlMessageType::PsRdy)], - TimerType::PSSourceOnSpr - ).await?; - Ok(State::PrSwapToSinkStartup) - }, + } // 8.3.3.19.3.7 (PE_PRS_SRC_SNK_Send_Swap): PowerRoleSwap::Send => { - self.protocol_layer.transmit_control_message(ControlMessageType::PrSwap).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::PrSwap) + .await?; - let message = self.protocol_layer.receive_message_type(&[ - MessageType::Control(ControlMessageType::Accept), - MessageType::Control(ControlMessageType::Reject), - MessageType::Control(ControlMessageType::Wait)], - TimerType::SenderResponse - ).await?; + let message = self + .protocol_layer + .receive_message_type( + &[ + MessageType::Control(ControlMessageType::Accept), + MessageType::Control(ControlMessageType::Reject), + MessageType::Control(ControlMessageType::Wait), + ], + TimerType::SenderResponse, + ) + .await?; match message.header.message_type() { - MessageType::Control(ControlMessageType::Accept) => - Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))), + MessageType::Control(ControlMessageType::Accept) => { + Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) + } - MessageType::Control(ControlMessageType::Reject) | - MessageType::Control(ControlMessageType::Wait) => - Ok(State::Ready), + MessageType::Control(ControlMessageType::Reject) + | MessageType::Control(ControlMessageType::Wait) => Ok(State::Ready), - _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)) + _ => Err(Error::Protocol(ProtocolError::UnexpectedMessage)), } - }, + } // 8.3.3.18.3.8 (PE_PRS_SRC_SNK_Reject_Swap): PowerRoleSwap::Reject => { // FIXME: Wait Message logic - self.protocol_layer.transmit_control_message(ControlMessageType::Reject).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::Reject) + .await?; Ok(State::Ready) - }, + } } } @@ -1131,40 +1160,46 @@ impl Source Result { match state { // 8.3.3.19.5.2 (PE_FRS_SRC_SNK_Evaluate_Swap): - FastPowerRoleSwap::Evaluate => { - match self.device_policy_manager.fr_swap_signaled().await { - true => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Accept))), - false => Ok(State::HardReset), - } + FastPowerRoleSwap::Evaluate => match self.device_policy_manager.fr_swap_signaled().await { + true => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Accept))), + false => Ok(State::HardReset), }, // 8.3.3.19.5.3 (PE_FRS_SRC_Accept): FastPowerRoleSwap::Accept => { - match self.protocol_layer.transmit_control_message(ControlMessageType::Accept).await { + match self + .protocol_layer + .transmit_control_message(ControlMessageType::Accept) + .await + { Ok(_) => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff))), - _ => Ok(State::HardReset), // Soft Reset shall **not** be initiated in this case + _ => Ok(State::HardReset), // Soft Reset shall **not** be initiated in this case } - }, + } // 8.3.3.19.5.4 (PE_FRS_SRC_Transition_to_off): FastPowerRoleSwap::TransitionToOff => { self.device_policy_manager.discharge_vbus().await; Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd))) - }, + } // 8.3.3.19.5.5 (PE_FRS_SRC_Assert_Rd): FastPowerRoleSwap::AssertRd => { self.device_policy_manager.cc_sink().await; Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn))) - }, + } // 8.3.3.19.5.6 (PE_FRS_SRC_Wait_Source_on): FastPowerRoleSwap::WaitSourceOn => { - self.protocol_layer.transmit_control_message(ControlMessageType::PsRdy).await?; + self.protocol_layer + .transmit_control_message(ControlMessageType::PsRdy) + .await?; + + self.protocol_layer + .receive_message_type( + &[MessageType::Control(ControlMessageType::PsRdy)], + TimerType::PSSourceOnSpr, + ) + .await?; - self.protocol_layer.receive_message_type( - &[MessageType::Control(ControlMessageType::PsRdy)], - TimerType::PSSourceOnSpr - ).await?; - Ok(State::PrSwapToSinkStartup) - }, + } } } @@ -1202,11 +1237,11 @@ impl Source match self.mode { Mode::Spr => State::EprModeEntry, Mode::Epr => State::HardReset, - } + }, epr_mode::Action::Exit => match self.mode { Mode::Spr => State::HardReset, Mode::Epr => State::EprModeExitReceived, - } + }, _ => State::SendNotSupported, } } else { @@ -1214,18 +1249,14 @@ impl Source { - State::SoftReset - } + MessageType::Control(ControlMessageType::SoftReset) => State::SoftReset, MessageType::Control(ControlMessageType::GetSourceCap) => match self.mode { Mode::Spr => State::SendCapabilities, Mode::Epr => State::GiveSourceCap, }, - MessageType::Control(ControlMessageType::VconnSwap) => { - State::VcsEvaluateSwap(VcsSwapSource::Message) - } + MessageType::Control(ControlMessageType::VconnSwap) => State::VcsEvaluateSwap(VcsSwapSource::Message), MessageType::Control(ControlMessageType::DrSwap) => { if !self.dual_role { @@ -1238,31 +1269,23 @@ impl Source { - match self.dual_role { - true => State::DrSwap(SwapState::Power(PowerRoleSwap::Evaluate)), - false => State::SendNotSupported - } - } + MessageType::Control(ControlMessageType::PrSwap) => match self.dual_role { + true => State::DrSwap(SwapState::Power(PowerRoleSwap::Evaluate)), + false => State::SendNotSupported, + }, // 8.3.3.19.5.1 - MessageType::Control(ControlMessageType::FrSwap) => { - match self.dual_role { - true => State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), - false => State::SendNotSupported - } - } + MessageType::Control(ControlMessageType::FrSwap) => match self.dual_role { + true => State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), + false => State::SendNotSupported, + }, - MessageType::Control(ControlMessageType::GetSinkCap) => { - match self.dual_role { - true => State::DrGiveSinkCap(Mode::Spr), - false => State::SendNotSupported, - } - } + MessageType::Control(ControlMessageType::GetSinkCap) => match self.dual_role { + true => State::DrGiveSinkCap(Mode::Spr), + false => State::SendNotSupported, + }, - MessageType::Control(ControlMessageType::NotSupported) => { - State::NotSupportedReceived - } + MessageType::Control(ControlMessageType::NotSupported) => State::NotSupportedReceived, MessageType::Extended(ExtendedMessageType::ExtendedControl) => { if let Some(Payload::Extended(Extended::ExtendedControl(ctrl))) = &message.payload { @@ -1331,14 +1354,17 @@ impl Source Ok(caps), - _ => unreachable!() + _ => unreachable!(), } } @@ -1348,14 +1374,17 @@ impl Source Ok(SourceCapabilities(cap)), - _ => unreachable!() + _ => unreachable!(), } } } diff --git a/usbpd/src/source/policy_engine/tests.rs b/usbpd/src/source/policy_engine/tests.rs index 82bf510..48aaf53 100644 --- a/usbpd/src/source/policy_engine/tests.rs +++ b/usbpd/src/source/policy_engine/tests.rs @@ -1,17 +1,18 @@ //! Tests for the policy engine. use super::Source; -use crate::dummy::{get_dummy_source_capabilities, get_source_capability_request}; +use crate::counters::{Counter, CounterType}; +use crate::dummy::{ + DummyDriver, DummySourceDevice, DummyTimer, MAX_DATA_MESSAGE_SIZE, get_dummy_source_capabilities, + get_source_capability_request, +}; use crate::protocol_layer::message::Message; use crate::protocol_layer::message::data::Data; use crate::protocol_layer::message::data::request::{CurrentRequest, FixedVariableSupply, PowerSource, VoltageRequest}; use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; -use crate::protocol_layer::message::header::{DataMessageType, MessageType}; +use crate::protocol_layer::message::header::{ControlMessageType, DataMessageType, Header, MessageType}; use crate::source::device_policy_manager::CapabilityResponse; use crate::source::policy_engine::State; -use crate::{counters::{Counter, CounterType}, dummy::{DummyDriver, DummySourceDevice, DummyTimer, MAX_DATA_MESSAGE_SIZE}, protocol_layer::message::header::{ControlMessageType, Header}}; - - fn get_policy_engine() -> Source, DummyTimer, DummySourceDevice> { Source::new(DummyDriver::new(), DummySourceDevice {}, false) @@ -20,7 +21,7 @@ fn get_policy_engine() -> Source, DummyTimer, fn simulate_sink_control_message( policy_engine: &mut Source, DummyTimer, DPM>, control_message_type: ControlMessageType, - message_id: u8 + message_id: u8, ) { let header = *policy_engine.protocol_layer.header(); let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; @@ -45,34 +46,31 @@ fn get_sink_header_template() -> Header { } fn request_capability_message(message_id: u8, highest_power: bool) -> Message { - let source_capabilities = SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()); + let source_capabilities = + SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()); let header = Header::new_data( get_sink_header_template(), Counter::new_from_value(CounterType::MessageId, message_id), DataMessageType::Request, - source_capabilities.0.len() as u8 + source_capabilities.0.len() as u8, ); let data = match highest_power { - true => Data::Request(PowerSource::new_fixed( - CurrentRequest::Highest, - VoltageRequest::Highest, - &source_capabilities - ).unwrap()), - false => Data::Request(PowerSource::new_fixed( - CurrentRequest::Highest, - VoltageRequest::Safe5V, - &source_capabilities - ).unwrap()), + true => Data::Request( + PowerSource::new_fixed(CurrentRequest::Highest, VoltageRequest::Highest, &source_capabilities).unwrap(), + ), + false => Data::Request( + PowerSource::new_fixed(CurrentRequest::Highest, VoltageRequest::Safe5V, &source_capabilities).unwrap(), + ), }; Message::new_with_data(header, data) } -fn simulate_sink_request ( +fn simulate_sink_request( policy_engine: &mut Source, DummyTimer, DPM>, message_id: u8, - highest_power: bool + highest_power: bool, ) { let mut message = request_capability_message(message_id, highest_power); let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; @@ -85,164 +83,169 @@ fn simulate_sink_request\n"); { - // Instantiated in `SendCapabilities` state (not started from role-swap) - eprintln!("- Ran Step 0: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); - - // `Startup` -> `SendCapabilities` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 1: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::SendCapabilities)); - - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); - - // Request vSafe5V @ highest current - simulate_sink_request(&mut policy_engine, 1, false); - - // `SendCapabilities` -> Get Request -> `NegotiateCapability` - policy_engine.run_step().await.unwrap(); { - - let sent_capabilities = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); - assert_eq!( - sent_capabilities.header.message_type(), - MessageType::Data(DataMessageType::SourceCapabilities) - ); + eprintln!("\n<== Starting initial source SPR negotiation test! ==>\n"); + { + // Instantiated in `SendCapabilities` state (not started from role-swap) + eprintln!("- Ran Step 0: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); - let good_crc = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); - assert_eq!( - good_crc.header.message_type(), - MessageType::Control(ControlMessageType::GoodCRC) - ); + // `Startup` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); - eprintln!("- Ran Step 2: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); - } + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); - // `NegotiateCapability` -> DPM: Accept -> `Transition Supply` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 3: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::TransitionSupply(_))); + // Request vSafe5V @ highest current + simulate_sink_request(&mut policy_engine, 1, false); - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + // `SendCapabilities` -> Get Request -> `NegotiateCapability` + policy_engine.run_step().await.unwrap(); + { + let sent_capabilities = + Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + assert_eq!( + sent_capabilities.header.message_type(), + MessageType::Data(DataMessageType::SourceCapabilities) + ); + + let good_crc = + Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + assert_eq!( + good_crc.header.message_type(), + MessageType::Control(ControlMessageType::GoodCRC) + ); + + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + } - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); + // `NegotiateCapability` -> DPM: Accept -> `Transition Supply` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::TransitionSupply(_))); - // `TransitionSupply` -> Accept -> PsRdy -> `Ready` - policy_engine.run_step().await.unwrap(); { + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); - while policy_engine.protocol_layer.driver().has_transmitted_data() { - let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); - eprintln!("- TransitionSupply message: {:?}", msg.header.message_type()); - } + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); - eprintln!("- Ran Step 4: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Ready)); + // `TransitionSupply` -> Accept -> PsRdy -> `Ready` + policy_engine.run_step().await.unwrap(); + { + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- TransitionSupply message: {:?}", msg.header.message_type()); + } + + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); + } } + eprintln!("\n==> Finished initial source SPR negotiation test! <==\n"); - } eprintln!("\n==> Finished initial source SPR negotiation test! <==\n"); - - eprintln!("\n<== Starting source SPR re-negotiation test! ==>\n"); { - - // Simulate `GoodCrc` to SourceCapabilities - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GetSourceCap, 3); + eprintln!("\n<== Starting source SPR re-negotiation test! ==>\n"); + { + // Simulate `GoodCrc` to SourceCapabilities + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GetSourceCap, 3); - // `Ready` -> GetSourceCap Received -> `SendCapabilities` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 1: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::SendCapabilities)); + // `Ready` -> GetSourceCap Received -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); - // Request highest power - simulate_sink_request(&mut policy_engine, 4, true); + // Request highest power + simulate_sink_request(&mut policy_engine, 4, true); - // `SendCapabilities` -> Get Request -> `NegotiateCapability` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 2: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + // `SendCapabilities` -> Get Request -> `NegotiateCapability` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); - // `NegotiateCapability` -> Request Can Be Met -> `TransitionSupply` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 3: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::TransitionSupply(_))); + // `NegotiateCapability` -> Request Can Be Met -> `TransitionSupply` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::TransitionSupply(_))); - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 4); + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 4); - // Simulate `GoodCrc` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 5); + // Simulate `GoodCrc` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 5); - // `TransitionSupply` -> Accept -> PsRdy -> `Ready` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 4: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Ready)); + // `TransitionSupply` -> Accept -> PsRdy -> `Ready` + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); - while policy_engine.protocol_layer.driver().has_transmitted_data() { - let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); - eprintln!("- Re-negotiation test message: {:?}", msg.header.message_type()); + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Re-negotiation test message: {:?}", msg.header.message_type()); + } } + eprintln!("\n==> Finished source SPR re-negotiation test! <==\n"); - } eprintln!("\n==> Finished source SPR re-negotiation test! <==\n"); - - eprintln!("\n==> Starting source SPR soft reset - reject test! <==\n"); { - - // Simulate `SoftReset` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::SoftReset, 6); - - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 1: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::SoftReset)); + eprintln!("\n==> Starting source SPR soft reset - reject test! <==\n"); + { + // Simulate `SoftReset` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::SoftReset, 6); - // `GoodCrc` for `Accept` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); - - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 2: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::SendCapabilities)); + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SoftReset)); - // `GoodCrc` for `SourceCapabilities` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + // `GoodCrc` for `Accept` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); - // Make an invalid request - let data = Data::Request(PowerSource::FixedVariableSupply(FixedVariableSupply(0xA000_0000))); - let header = Header::new_data( - get_sink_header_template(), - Counter::new_from_value(CounterType::MessageId, 2), - DataMessageType::Request, - 7 - ); - let message = Message::new_with_data(header, data); - let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; - let len = message.to_bytes(&mut buf); - policy_engine.protocol_layer.driver().inject_received_data(&buf[..len]); + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); + + // `GoodCrc` for `SourceCapabilities` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + + // Make an invalid request + let data = Data::Request(PowerSource::FixedVariableSupply(FixedVariableSupply(0xA000_0000))); + let header = Header::new_data( + get_sink_header_template(), + Counter::new_from_value(CounterType::MessageId, 2), + DataMessageType::Request, + 7, + ); + let message = Message::new_with_data(header, data); + let mut buf = [0u8; MAX_DATA_MESSAGE_SIZE]; + let len = message.to_bytes(&mut buf); + policy_engine.protocol_layer.driver().inject_received_data(&buf[..len]); - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 3: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 3: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::NegotiateCapability(_))); - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 4: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::CapabilityResponse(CapabilityResponse::Reject))); + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 4: {0:?}", policy_engine.state); + assert!(matches!( + policy_engine.state, + State::CapabilityResponse(CapabilityResponse::Reject) + )); - // `GoodCrc` for `Reject` - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); + // `GoodCrc` for `Reject` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 5: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Ready)); + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 5: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Ready)); - while policy_engine.protocol_layer.driver().has_transmitted_data() { - let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); - eprintln!("- SoftReset-Reject message: {:?}", msg.header.message_type()); + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- SoftReset-Reject message: {:?}", msg.header.message_type()); + } } - - } eprintln!("\n==> Finished source SPR soft reset - reject test! <==\n"); - + eprintln!("\n==> Finished source SPR soft reset - reject test! <==\n"); } #[tokio::test] @@ -250,43 +253,49 @@ async fn test_discovery() { const MAX_DISCOVERY_ITERS: usize = 52; let mut policy_engine = get_policy_engine(); // HardReset -> Discovery -> Disabled - eprintln!("\n==> Starting source SPR discovery test! <==\n"); { - - eprintln!("- Ran Step 0: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); - - // `Startup` -> `SendCapabilities` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 1: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::SendCapabilities)); - - simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); + eprintln!("\n==> Starting source SPR discovery test! <==\n"); + { + eprintln!("- Ran Step 0: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); - // `SendCapabilities` -> Capability Send Failure -> `Discovery` - policy_engine.run_step().await.unwrap(); - eprintln!("- Ran Step 2: {0:?}", policy_engine.state); - assert!(matches!(policy_engine.state, State::Discovery)); - - // `Discovery` -> - let mut discovery_iterations = 0; - loop { - if !matches!(policy_engine.state, State::Discovery) { break; } - - // `Discovery` -> `SendCapabilities` + // `Startup` -> `SendCapabilities` policy_engine.run_step().await.unwrap(); - discovery_iterations += 1; + eprintln!("- Ran Step 1: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::SendCapabilities)); - if discovery_iterations > MAX_DISCOVERY_ITERS { break; } - if !matches!(policy_engine.state, State::SendCapabilities) { break; } + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); - // `SendCapabilities` -> `Discovery` + // `SendCapabilities` -> Capability Send Failure -> `Discovery` policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step 2: {0:?}", policy_engine.state); + assert!(matches!(policy_engine.state, State::Discovery)); + + // `Discovery` -> + let mut discovery_iterations = 0; + loop { + if !matches!(policy_engine.state, State::Discovery) { + break; + } + + // `Discovery` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); + discovery_iterations += 1; + + if discovery_iterations > MAX_DISCOVERY_ITERS { + break; + } + if !matches!(policy_engine.state, State::SendCapabilities) { + break; + } + + // `SendCapabilities` -> `Discovery` + policy_engine.run_step().await.unwrap(); + } + eprintln!( + "- Ran Step 3: {0:?}, discovery state iterations = {1:?}", + policy_engine.state, discovery_iterations + ); + assert!(matches!(policy_engine.state, State::Disabled)); } - eprintln!("- Ran Step 3: {0:?}, discovery state iterations = {1:?}", policy_engine.state, discovery_iterations); - assert!(matches!(policy_engine.state, State::Disabled)); - - } eprintln!("\n==> Finished source SPR discovery test! <==\n"); - + eprintln!("\n==> Finished source SPR discovery test! <==\n"); } - - From 8d5bc5933fcb4ca85bc47796104edd9ddec93c39 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Fri, 20 Feb 2026 10:31:29 -0500 Subject: [PATCH 4/8] fix: DrSwap->DrpSwap to avoid confusion of "dual role port" and "data role" --- usbpd/src/source/policy_engine/mod.rs | 64 +++++++++++++-------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/usbpd/src/source/policy_engine/mod.rs b/usbpd/src/source/policy_engine/mod.rs index ce0a84a..42ebf61 100644 --- a/usbpd/src/source/policy_engine/mod.rs +++ b/usbpd/src/source/policy_engine/mod.rs @@ -75,10 +75,10 @@ enum State { SendNotSupported, NotSupportedReceived, - // 8.3.3.19 Dual-Role Port - DrSwap(SwapState), - DrGetSourceCap(Mode), - DrGiveSinkCap(Mode), + // 8.3.3.19 Dual-Role Port (DRP) States + DrpSwap(SwapState), + DrpGetSourceCap(Mode), + DrpGiveSinkCap(Mode), // Custom state to signal exit out of source to sink from a power swap PrSwapToSinkStartup, @@ -295,8 +295,8 @@ impl Source Some(State::ErrorRecovery), @@ -499,12 +499,12 @@ impl Source State::Ready, Event::UpdatedSourceCapabilities => State::SendCapabilities, Event::RequestSinkCapabilities => State::GetSinkCap, - Event::RequestSourceCapabilities => State::DrGetSourceCap(Mode::Spr), - Event::RequestEprSourceCapabilities => State::DrGetSourceCap(Mode::Epr), + Event::RequestSourceCapabilities => State::DrpGetSourceCap(Mode::Spr), + Event::RequestEprSourceCapabilities => State::DrpGetSourceCap(Mode::Epr), Event::ExitEprMode => State::EprModeSendExit, Event::RequestVconnSwap => State::VcsSendSwap(VcsSwapSource::Message), - Event::RequestDataRoleSwap => State::DrSwap(SwapState::Data(DataRoleSwap::Send)), - Event::RequestPowerRoleSwap => State::DrSwap(SwapState::Power(PowerRoleSwap::Send)), + Event::RequestDataRoleSwap => State::DrpSwap(SwapState::Data(DataRoleSwap::Send)), + Event::RequestPowerRoleSwap => State::DrpSwap(SwapState::Power(PowerRoleSwap::Send)), }, Either3::Third(timeout_source) => match timeout_source { @@ -760,13 +760,13 @@ impl Source match swap_state { + State::DrpSwap(swap_state) => match swap_state { SwapState::Data(dr) => self.execute_data_role_swap_state(*dr).await?, SwapState::Power(pr) => self.execute_power_role_swap_state(*pr).await?, SwapState::FastPower(fpr) => self.execute_fast_power_role_swap_state(*fpr).await?, }, // 8.3.3.19.7.1 (PE_DR_SRC_Get_Source_Cap): - State::DrGetSourceCap(mode) => { + State::DrpGetSourceCap(mode) => { let result = match mode { Mode::Spr => self.get_source_capabilities().await, Mode::Epr => self.get_epr_source_capabilities().await, @@ -786,7 +786,7 @@ impl Source { + State::DrpGiveSinkCap(mode) => { let sink_caps = self.device_policy_manager.sink_capabilities().await; match mode { Mode::Spr => { @@ -1020,15 +1020,15 @@ impl Source match self.device_policy_manager.evaluate_swap_request(SwapType::Data).await { - true => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Accept))), - false => Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Reject))), + true => Ok(State::DrpSwap(SwapState::Data(DataRoleSwap::Accept))), + false => Ok(State::DrpSwap(SwapState::Data(DataRoleSwap::Reject))), }, // 8.3.3.19.1.3 (PE_DRS_DFP_UFP_Accept_Swap, PE_DRS_UFP_DFP_Accept_Swap): DataRoleSwap::Accept => { self.protocol_layer .transmit_control_message(ControlMessageType::Accept) .await?; - Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))) + Ok(State::DrpSwap(SwapState::Data(DataRoleSwap::Change))) } // 8.3.3.19.1.4 (PE_DRS_DFP_UFP_Change_to_UFP_Swap, PE_DRS_UFP_DFP_Change_to_DFP_Swap): DataRoleSwap::Change => { @@ -1056,7 +1056,7 @@ impl Source { - Ok(State::DrSwap(SwapState::Data(DataRoleSwap::Change))) + Ok(State::DrpSwap(SwapState::Data(DataRoleSwap::Change))) } MessageType::Control(ControlMessageType::Reject) @@ -1081,25 +1081,25 @@ impl Source match self.device_policy_manager.evaluate_swap_request(SwapType::Power).await { - true => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Accept))), - false => Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::Reject))), + true => Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::Accept))), + false => Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::Reject))), }, // 8.3.3.19.3.3 (PE_PRS_SRC_SNK_Accept_Swap): PowerRoleSwap::Accept => { self.protocol_layer .transmit_control_message(ControlMessageType::Accept) .await?; - Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) + Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) } // 8.3.3.19.3.4 (PE_PRS_SRC_SNK_Transition_to_off): PowerRoleSwap::TransitionToOff => { self.device_policy_manager.disable_source().await; - Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::AssertRd))) + Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::AssertRd))) } // 8.3.3.19.3.5 (PE_PRS_SRC_SNK_Assert_Rd): PowerRoleSwap::AssertRd => { self.device_policy_manager.cc_sink().await; - Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn))) + Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn))) } // 8.3.3.19.3.6 (PE_PRS_SRC_SNK_Wait_Source_on): PowerRoleSwap::WaitSourceOn => { @@ -1136,7 +1136,7 @@ impl Source { - Ok(State::DrSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) + Ok(State::DrpSwap(SwapState::Power(PowerRoleSwap::TransitionToOff))) } MessageType::Control(ControlMessageType::Reject) @@ -1161,7 +1161,7 @@ impl Source match self.device_policy_manager.fr_swap_signaled().await { - true => Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Accept))), + true => Ok(State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Accept))), false => Ok(State::HardReset), }, // 8.3.3.19.5.3 (PE_FRS_SRC_Accept): @@ -1171,19 +1171,19 @@ impl Source Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff))), + Ok(_) => Ok(State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff))), _ => Ok(State::HardReset), // Soft Reset shall **not** be initiated in this case } } // 8.3.3.19.5.4 (PE_FRS_SRC_Transition_to_off): FastPowerRoleSwap::TransitionToOff => { self.device_policy_manager.discharge_vbus().await; - Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd))) + Ok(State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd))) } // 8.3.3.19.5.5 (PE_FRS_SRC_Assert_Rd): FastPowerRoleSwap::AssertRd => { self.device_policy_manager.cc_sink().await; - Ok(State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn))) + Ok(State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn))) } // 8.3.3.19.5.6 (PE_FRS_SRC_Wait_Source_on): FastPowerRoleSwap::WaitSourceOn => { @@ -1264,24 +1264,24 @@ impl Source match self.dual_role { - true => State::DrSwap(SwapState::Power(PowerRoleSwap::Evaluate)), + true => State::DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate)), false => State::SendNotSupported, }, // 8.3.3.19.5.1 MessageType::Control(ControlMessageType::FrSwap) => match self.dual_role { - true => State::DrSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), + true => State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), false => State::SendNotSupported, }, MessageType::Control(ControlMessageType::GetSinkCap) => match self.dual_role { - true => State::DrGiveSinkCap(Mode::Spr), + true => State::DrpGiveSinkCap(Mode::Spr), false => State::SendNotSupported, }, @@ -1295,7 +1295,7 @@ impl Source State::SendCapabilities, }, ExtendedControlMessageType::EprGetSinkCap => match self.dual_role { - true => State::DrGiveSinkCap(Mode::Epr), + true => State::DrpGiveSinkCap(Mode::Epr), false => State::SendNotSupported, }, ExtendedControlMessageType::EprKeepAlive => State::EprKeepAlive, From cb4b78f15e4ed0f88f7733be606888527564d14e Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Fri, 20 Feb 2026 15:20:58 -0500 Subject: [PATCH 5/8] feat: test for power, fast power, and data role swaps --- usbpd/src/dummy.rs | 17 +++ usbpd/src/source/policy_engine/tests.rs | 153 ++++++++++++++++++++++-- 2 files changed, 162 insertions(+), 8 deletions(-) diff --git a/usbpd/src/dummy.rs b/usbpd/src/dummy.rs index 5b5c674..7b8d846 100644 --- a/usbpd/src/dummy.rs +++ b/usbpd/src/dummy.rs @@ -140,6 +140,23 @@ impl SourceDevicePolicyManager for DummySourceDevice { fn source_capabilities(&mut self) -> SourceCapabilities { SourceCapabilities(heapless::Vec::from_slice(get_dummy_source_capabilities().as_slice()).unwrap()) } + + async fn evaluate_swap_request(&mut self, swap_request: crate::source::device_policy_manager::SwapType) -> bool { + match swap_request { + crate::source::device_policy_manager::SwapType::Vconn => true, + crate::source::device_policy_manager::SwapType::Data => true, + crate::source::device_policy_manager::SwapType::Power => true, + } + } + + async fn fr_swap_signaled(&mut self) -> bool { + true + } + + async fn disable_source(&mut self) { + // Dummy doesn't need to do anything to simulate this! + } + } pub struct DummyDualRoleDevice {} diff --git a/usbpd/src/source/policy_engine/tests.rs b/usbpd/src/source/policy_engine/tests.rs index 48aaf53..a37d1a0 100644 --- a/usbpd/src/source/policy_engine/tests.rs +++ b/usbpd/src/source/policy_engine/tests.rs @@ -12,7 +12,7 @@ use crate::protocol_layer::message::data::request::{CurrentRequest, FixedVariabl use crate::protocol_layer::message::data::source_capabilities::SourceCapabilities; use crate::protocol_layer::message::header::{ControlMessageType, DataMessageType, Header, MessageType}; use crate::source::device_policy_manager::CapabilityResponse; -use crate::source::policy_engine::State; +use crate::source::policy_engine::{DataRoleSwap, FastPowerRoleSwap, PowerRoleSwap, State, SwapState}; fn get_policy_engine() -> Source, DummyTimer, DummySourceDevice> { Source::new(DummyDriver::new(), DummySourceDevice {}, false) @@ -23,7 +23,6 @@ fn simulate_sink_control_message Finished source SPR re-negotiation test! <==\n"); - eprintln!("\n==> Starting source SPR soft reset - reject test! <==\n"); + eprintln!("\n==> Starting source soft reset - reject test! <==\n"); { // Simulate `SoftReset` simulate_sink_control_message(&mut policy_engine, ControlMessageType::SoftReset, 6); @@ -245,7 +244,7 @@ async fn test_negotiation() { eprintln!("- SoftReset-Reject message: {:?}", msg.header.message_type()); } } - eprintln!("\n==> Finished source SPR soft reset - reject test! <==\n"); + eprintln!("\n==> Finished source soft reset - reject test! <==\n"); } #[tokio::test] @@ -253,7 +252,7 @@ async fn test_discovery() { const MAX_DISCOVERY_ITERS: usize = 52; let mut policy_engine = get_policy_engine(); // HardReset -> Discovery -> Disabled - eprintln!("\n==> Starting source SPR discovery test! <==\n"); + eprintln!("\n<== Starting source discovery test! ==>\n"); { eprintln!("- Ran Step 0: {0:?}", policy_engine.state); assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); @@ -270,7 +269,6 @@ async fn test_discovery() { eprintln!("- Ran Step 2: {0:?}", policy_engine.state); assert!(matches!(policy_engine.state, State::Discovery)); - // `Discovery` -> let mut discovery_iterations = 0; loop { if !matches!(policy_engine.state, State::Discovery) { @@ -297,5 +295,144 @@ async fn test_discovery() { ); assert!(matches!(policy_engine.state, State::Disabled)); } - eprintln!("\n==> Finished source SPR discovery test! <==\n"); + eprintln!("\n==> Finished source discovery test! <==\n"); +} + +/// Return a policy engine that has already undergone initial power contract negotiation +async fn get_drp_policy_engine_at_ready() -> Source, DummyTimer, DummySourceDevice> { + let mut policy_engine = Source::new_dual_role(DummyDriver::new(), DummySourceDevice {}, false); + assert!(matches!(policy_engine.state, State::Startup { role_swap: false })); + + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 0); + simulate_sink_request(&mut policy_engine, 1, false); + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 1); + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 2); + + policy_engine.run_step().await.unwrap(); // `Startup` -> `SendCapabilities` + policy_engine.run_step().await.unwrap(); // `SendCapabilities` -> Get Request -> `NegotiateCapability` + policy_engine.run_step().await.unwrap(); // `NegotiateCapability` -> DPM: Accept -> `Transition Supply` + policy_engine.run_step().await.unwrap(); // `TransitionSupply` -> Accept -> PsRdy -> `Ready` + + // Flush all messages + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Initial negotiation: {:?}", msg.header.message_type()); + } + + assert!(matches!(policy_engine.state, State::Ready)); + + policy_engine +} + +#[tokio::test] +async fn test_role_swapping() { + + eprintln!("\n<== Starting source power role swap test! ==>\n"); + { + let mut policy_engine = get_drp_policy_engine_at_ready().await; + + simulate_sink_control_message(&mut policy_engine, ControlMessageType::PrSwap, 2); + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); // `GoodCrc` for `Accept` + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 4); // `GoodCrc` for `PS_RDY` 3 OR 4?? + simulate_sink_control_message(&mut policy_engine, ControlMessageType::PsRdy, 4); + + // `Ready` -> PR_Swap Message Received -> `DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate))` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate)), 1).await; + + /// `PowerRoleSwap::Evaluate` -> DRP Evaluates to `true` -> `PowerRoleSwap::Accept` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::Accept)), 2).await; + + // `PowerRoleSwap::Accept` -> `Accept` Sent -> `PowerRoleSwap::TransitionToOff` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::TransitionToOff)), 3).await; + + // `PowerRoleSwap::TransitionToOff` -> DPM turned source off -> `PowerRoleSwap::AssertRd` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::AssertRd)), 4).await; + + // `PowerRoleSwap::AssertRd` -> DPM asserted Rd -> `PowerRoleSwap::WaitSourceOn` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)), 5).await; + + // `PowerRoleSwap::WaitSourceOn` -> `PS_RDY` Sent, `PS_RDY` Received -> `PrSwapToSinkStartup` + run_test_step(&mut policy_engine, &State::PrSwapToSinkStartup, 6).await; + + // Flush all messages + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Power Role Swap Message: {:?}", msg.header.message_type()); + } + } + eprintln!("\n==> Finished source power role swap test! <==\n"); + + eprintln!("\n<== Starting source fast power role swap test! ==>\n"); + { + let mut policy_engine = get_drp_policy_engine_at_ready().await; + + simulate_sink_control_message(&mut policy_engine, ControlMessageType::FrSwap, 2); + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); // `GoodCrc` for `Accept` 3 OR 4?? + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 4); // `GoodCrc` for `PS_RDY` 3 OR 4?? + simulate_sink_control_message(&mut policy_engine, ControlMessageType::PsRdy, 4); + + // `Ready` -> FR_Swap Message Received -> `FastPowerRole::Evaluate` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), 1).await; + + // `FastPowerRole::Evaluate` -> DPM: Fast Role Swap Signaled -> `FastPowerRole::Accept` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Accept)), 2).await; + + // `FastPowerRole::Accept` -> `Accept` sent -> `FastPowerRole::TransitionToOff` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff)), 3).await; + + // `PowerRoleSwap::TransitionToOff` -> DPM turned source off -> `PowerRoleSwap::AssertRd` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd)), 4).await; + + // `PowerRoleSwap::AssertRd` -> DPM asserted Rd -> `PowerRoleSwap::WaitSourceOn` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn)), 5).await; + + // `PowerRoleSwap::WaitSourceOn` -> `PS_RDY` Sent, `PS_RDY` Received -> `PrSwapToSinkStartup` + run_test_step(&mut policy_engine, &State::PrSwapToSinkStartup, 6); + + // Flush all messages + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Fast Power Role Swap Message: {:?}", msg.header.message_type()); + } + } + + eprintln!("\n<== Starting source data role swap test! ==>\n"); + { + let mut policy_engine = get_drp_policy_engine_at_ready().await; + + simulate_sink_control_message(&mut policy_engine, ControlMessageType::DrSwap, 2); + simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); // `GoodCrc` for `Accept` + + // `Ready` -> DR_Swap Message Received -> `DataRoleSwap::Evaluate` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Evaluate)), 1).await; + + // `DataRoleSwap::Evaluate` -> DRP: Data Role Swap ok -> `DataRoleSwap::Accept` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Accept)), 2).await; + + // `DataRoleSwap::Accept` -> `Accept` sent -> `DataRoleSwap::Change` + run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Change)), 3).await; + + // `DataRoleSwap::Change` -> DPM: Port changed data role -> `Ready` + run_test_step(&mut policy_engine, &State::Ready, 4).await; + + // Flush all messages + while policy_engine.protocol_layer.driver().has_transmitted_data() { + let msg = Message::from_bytes(&policy_engine.protocol_layer.driver().probe_transmitted_data()).unwrap(); + eprintln!("- Data Role Swap Message: {:?}", msg.header.message_type()); + } + } + eprintln!("\n==> Finished source data role swap test! <==\n"); + +} + +async fn run_test_step( + policy_engine: &mut Source, DummyTimer, DPM>, + new_state: &State, + step_number: usize, +) { + policy_engine.run_step().await.unwrap(); + eprintln!("- Ran Step {step_number}: {0:?}", policy_engine.state); + if !matches!(&policy_engine.state, new_state) { + error!("Policy Engine State {0:?} did not match assumed new state {1:?}", policy_engine.state, new_state); + } } From e556f6956d86604a93d813febef4e0f4976a2ca7 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Fri, 20 Feb 2026 15:21:37 -0500 Subject: [PATCH 6/8] fix: new dual role source initial state fix --- usbpd/src/source/policy_engine/mod.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/usbpd/src/source/policy_engine/mod.rs b/usbpd/src/source/policy_engine/mod.rs index 42ebf61..59a9b7d 100644 --- a/usbpd/src/source/policy_engine/mod.rs +++ b/usbpd/src/source/policy_engine/mod.rs @@ -235,7 +235,7 @@ impl Source State::SendCapabilities, - false => State::Startup { role_swap: true }, + false => State::Startup { role_swap: false }, }, contract: match role_swapped { true => Contract::Implicit, From e9accccd68333121ddce2156a3d349150fa40d67 Mon Sep 17 00:00:00 2001 From: Matthew Schawe Date: Fri, 20 Feb 2026 15:23:57 -0500 Subject: [PATCH 7/8] fix: apply rustfmt to test addition --- usbpd/src/dummy.rs | 1 - usbpd/src/source/policy_engine/tests.rs | 98 +++++++++++++++++++++---- 2 files changed, 82 insertions(+), 17 deletions(-) diff --git a/usbpd/src/dummy.rs b/usbpd/src/dummy.rs index 7b8d846..0a52ea6 100644 --- a/usbpd/src/dummy.rs +++ b/usbpd/src/dummy.rs @@ -156,7 +156,6 @@ impl SourceDevicePolicyManager for DummySourceDevice { async fn disable_source(&mut self) { // Dummy doesn't need to do anything to simulate this! } - } pub struct DummyDualRoleDevice {} diff --git a/usbpd/src/source/policy_engine/tests.rs b/usbpd/src/source/policy_engine/tests.rs index a37d1a0..3f52f69 100644 --- a/usbpd/src/source/policy_engine/tests.rs +++ b/usbpd/src/source/policy_engine/tests.rs @@ -326,7 +326,6 @@ async fn get_drp_policy_engine_at_ready() -> Source\n"); { let mut policy_engine = get_drp_policy_engine_at_ready().await; @@ -337,19 +336,44 @@ async fn test_role_swapping() { simulate_sink_control_message(&mut policy_engine, ControlMessageType::PsRdy, 4); // `Ready` -> PR_Swap Message Received -> `DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate))` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate)), 1).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Power(PowerRoleSwap::Evaluate)), + 1, + ) + .await; /// `PowerRoleSwap::Evaluate` -> DRP Evaluates to `true` -> `PowerRoleSwap::Accept` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::Accept)), 2).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Power(PowerRoleSwap::Accept)), + 2, + ) + .await; // `PowerRoleSwap::Accept` -> `Accept` Sent -> `PowerRoleSwap::TransitionToOff` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::TransitionToOff)), 3).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Power(PowerRoleSwap::TransitionToOff)), + 3, + ) + .await; // `PowerRoleSwap::TransitionToOff` -> DPM turned source off -> `PowerRoleSwap::AssertRd` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::AssertRd)), 4).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Power(PowerRoleSwap::AssertRd)), + 4, + ) + .await; // `PowerRoleSwap::AssertRd` -> DPM asserted Rd -> `PowerRoleSwap::WaitSourceOn` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)), 5).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)), + 5, + ) + .await; // `PowerRoleSwap::WaitSourceOn` -> `PS_RDY` Sent, `PS_RDY` Received -> `PrSwapToSinkStartup` run_test_step(&mut policy_engine, &State::PrSwapToSinkStartup, 6).await; @@ -372,19 +396,44 @@ async fn test_role_swapping() { simulate_sink_control_message(&mut policy_engine, ControlMessageType::PsRdy, 4); // `Ready` -> FR_Swap Message Received -> `FastPowerRole::Evaluate` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), 1).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), + 1, + ) + .await; // `FastPowerRole::Evaluate` -> DPM: Fast Role Swap Signaled -> `FastPowerRole::Accept` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Accept)), 2).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Accept)), + 2, + ) + .await; // `FastPowerRole::Accept` -> `Accept` sent -> `FastPowerRole::TransitionToOff` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff)), 3).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::TransitionToOff)), + 3, + ) + .await; // `PowerRoleSwap::TransitionToOff` -> DPM turned source off -> `PowerRoleSwap::AssertRd` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd)), 4).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::AssertRd)), + 4, + ) + .await; // `PowerRoleSwap::AssertRd` -> DPM asserted Rd -> `PowerRoleSwap::WaitSourceOn` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn)), 5).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::WaitSourceOn)), + 5, + ) + .await; // `PowerRoleSwap::WaitSourceOn` -> `PS_RDY` Sent, `PS_RDY` Received -> `PrSwapToSinkStartup` run_test_step(&mut policy_engine, &State::PrSwapToSinkStartup, 6); @@ -404,13 +453,28 @@ async fn test_role_swapping() { simulate_sink_control_message(&mut policy_engine, ControlMessageType::GoodCRC, 3); // `GoodCrc` for `Accept` // `Ready` -> DR_Swap Message Received -> `DataRoleSwap::Evaluate` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Evaluate)), 1).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Data(DataRoleSwap::Evaluate)), + 1, + ) + .await; // `DataRoleSwap::Evaluate` -> DRP: Data Role Swap ok -> `DataRoleSwap::Accept` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Accept)), 2).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Data(DataRoleSwap::Accept)), + 2, + ) + .await; // `DataRoleSwap::Accept` -> `Accept` sent -> `DataRoleSwap::Change` - run_test_step(&mut policy_engine, &State::DrpSwap(SwapState::Data(DataRoleSwap::Change)), 3).await; + run_test_step( + &mut policy_engine, + &State::DrpSwap(SwapState::Data(DataRoleSwap::Change)), + 3, + ) + .await; // `DataRoleSwap::Change` -> DPM: Port changed data role -> `Ready` run_test_step(&mut policy_engine, &State::Ready, 4).await; @@ -422,7 +486,6 @@ async fn test_role_swapping() { } } eprintln!("\n==> Finished source data role swap test! <==\n"); - } async fn run_test_step( @@ -433,6 +496,9 @@ async fn run_test_step Date: Fri, 20 Feb 2026 15:28:51 -0500 Subject: [PATCH 8/8] fix: address cargo clippy --- usbpd/src/protocol_layer/message/data/source_capabilities.rs | 4 ++-- usbpd/src/protocol_layer/mod.rs | 2 ++ usbpd/src/source/policy_engine/mod.rs | 5 +++++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/usbpd/src/protocol_layer/message/data/source_capabilities.rs b/usbpd/src/protocol_layer/message/data/source_capabilities.rs index 428c0f3..2e81c13 100644 --- a/usbpd/src/protocol_layer/message/data/source_capabilities.rs +++ b/usbpd/src/protocol_layer/message/data/source_capabilities.rs @@ -129,7 +129,7 @@ impl FixedSupply { ElectricCurrent::new::(self.raw_max_current().into()) } - pub fn vSafe5V(max_current_10ma: u16) -> Self { + pub fn v_safe_5v(max_current_10ma: u16) -> Self { FixedSupply::default() .with_raw_voltage(100) // V = 5v = 100_u16 * 50mv .with_raw_max_current(max_current_10ma) @@ -303,7 +303,7 @@ impl SourceCapabilities { pub fn new_vsafe5v_only(maximum_current_10ma: u16) -> Self { let mut inner = Vec::new(); inner - .push(PowerDataObject::FixedSupply(FixedSupply::vSafe5V(maximum_current_10ma))) + .push(PowerDataObject::FixedSupply(FixedSupply::v_safe_5v(maximum_current_10ma))) .ok(); Self(inner) } diff --git a/usbpd/src/protocol_layer/mod.rs b/usbpd/src/protocol_layer/mod.rs index 0511495..ef811d7 100644 --- a/usbpd/src/protocol_layer/mod.rs +++ b/usbpd/src/protocol_layer/mod.rs @@ -162,6 +162,8 @@ impl ProtocolLayer { } /// Change the header's data role after a data role swap + /// FIXME: Use this after a data role swap + #[allow(unused)] pub fn set_header_data_role(&mut self, role: crate::DataRole) { self.default_header.set_port_data_role(role); } diff --git a/usbpd/src/source/policy_engine/mod.rs b/usbpd/src/source/policy_engine/mod.rs index 59a9b7d..45a9dc5 100644 --- a/usbpd/src/source/policy_engine/mod.rs +++ b/usbpd/src/source/policy_engine/mod.rs @@ -42,6 +42,9 @@ enum Contract { Implicit, // Only present after fast role swap. Limited to max. type C current. TransitionToExplicit, Explicit(PowerSource), + + // FIXME: Source EPR support may use this enum + #[allow(unused)] Invalid, } @@ -92,6 +95,8 @@ enum State { VcsTurnOffVconn(VcsSwapSource), VcsTurnOnVconn(VcsSwapSource), VcsSendPsRdy(VcsSwapSource), + // FIXME: For now, forcing a different state traversal, resulting in this being unused + #[allow(unused)] VcsForceVconn(VcsSwapSource), // 8.3.3.26 EPR States