diff --git a/usbpd/src/dummy.rs b/usbpd/src/dummy.rs index f43b91f..0a52ea6 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,63 @@ 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()) + } + + 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 {} + +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 + // 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 +296,17 @@ 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 ab0207b..daac207 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)] @@ -101,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/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..2e81c13 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 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) + .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::v_safe_5v(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..ef811d7 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,18 @@ 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 + /// 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); + } + fn get_message_buffer() -> [u8; MAX_MESSAGE_SIZE] { [0u8; MAX_MESSAGE_SIZE] } @@ -696,6 +703,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 +837,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..69ca206 --- /dev/null +++ b/usbpd/src/source/device_policy_manager.rs @@ -0,0 +1,200 @@ +//! 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; +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)] +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 **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 { + 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 {} + } +} + +// 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/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/mod.rs b/usbpd/src/source/policy_engine/mod.rs new file mode 100644 index 0000000..45a9dc5 --- /dev/null +++ b/usbpd/src/source/policy_engine/mod.rs @@ -0,0 +1,1395 @@ +//! Policy engine for the implementation of a sink. +use core::marker::PhantomData; + +use embassy_futures::select::{Either, Either3, select, select3}; +use usbpd_traits::Driver; + +use super::device_policy_manager::{DevicePolicyManager, Event}; +use crate::counters::Counter; +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, 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::{CapabilityResponse, Info, SwapType}; +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))] +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), + + // FIXME: Source EPR support may use this enum + #[allow(unused)] + 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 (DRP) States + DrpSwap(SwapState), + DrpGetSourceCap(Mode), + DrpGiveSinkCap(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), + // FIXME: For now, forcing a different state traversal, resulting in this being unused + #[allow(unused)] + 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_swap: 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: State::Startup { role_swap }, + contract: match role_swap { + 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: false }, + }, + 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::DrpSwap(SwapState::Power(PowerRoleSwap::WaitSourceOn)) + | State::DrpSwap(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); + return Err(Error::Protocol(error)); + } + }; + + 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; + } + + // 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 + } + // 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.contract = Contract::Explicit(*power_request); + + self.protocol_layer + .transmit_control_message(ControlMessageType::PsRdy) + .await?; + + 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::DrpGetSourceCap(Mode::Spr), + Event::RequestEprSourceCapabilities => State::DrpGetSourceCap(Mode::Epr), + Event::ExitEprMode => State::EprModeSendExit, + Event::RequestVconnSwap => State::VcsSendSwap(VcsSwapSource::Message), + Event::RequestDataRoleSwap => State::DrpSwap(SwapState::Data(DataRoleSwap::Send)), + Event::RequestPowerRoleSwap => State::DrpSwap(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 vSafe5V 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::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::DrpGetSourceCap(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::DrpGiveSinkCap(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 => { + // 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?; + + 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, + } + } + + // 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()), + }, + // 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::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::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 => { + 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::DrpSwap(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::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::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::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::DrpSwap(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::DrpSwap(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::DrpSwap(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::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::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::DrpSwap(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::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::DrSwap) => { + if !self.dual_role { + State::SendNotSupported + } else if self.mode == Mode::Epr { + State::HardReset + } else { + State::DrpSwap(SwapState::Data(DataRoleSwap::Evaluate)) + } + } + + // 8.3.3.19.3.1 + MessageType::Control(ControlMessageType::PrSwap) => match self.dual_role { + 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::DrpSwap(SwapState::FastPower(FastPowerRoleSwap::Evaluate)), + false => State::SendNotSupported, + }, + + MessageType::Control(ControlMessageType::GetSinkCap) => match self.dual_role { + true => State::DrpGiveSinkCap(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::DrpGiveSinkCap(Mode::Epr), + false => State::SendNotSupported, + }, + ExtendedControlMessageType::EprKeepAlive => State::EprKeepAlive, + ExtendedControlMessageType::EprKeepAliveAck => State::SendNotSupported, // FIXME: Source EPR + } + } 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!(), + } + } +} diff --git a/usbpd/src/source/policy_engine/tests.rs b/usbpd/src/source/policy_engine/tests.rs new file mode 100644 index 0000000..3f52f69 --- /dev/null +++ b/usbpd/src/source/policy_engine/tests.rs @@ -0,0 +1,504 @@ +//! Tests for the policy engine. + +use super::Source; +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::{ControlMessageType, DataMessageType, Header, MessageType}; +use crate::source::device_policy_manager::CapabilityResponse; +use crate::source::policy_engine::{DataRoleSwap, FastPowerRoleSwap, PowerRoleSwap, State, SwapState}; + +fn get_policy_engine() -> 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 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 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 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 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 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)); + + 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 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 + ); + } +}