WIP: Add Source Support

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<Output = crate::source::device_policy_manager::CapabilityResponse> {
+        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.

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,

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),

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::<centiampere>(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<PowerDataObject, 16>);
 
 impl SourceCapabilities {
+    pub fn new_with_pdos(pdos: Vec<PowerDataObject, 16>) -> 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 {

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<DRIVER: Driver, TIMER: Timer> ProtocolLayer<DRIVER, TIMER> {
         &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<DRIVER: Driver, TIMER: Timer> ProtocolLayer<DRIVER, TIMER> {
         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<Message, ProtocolError> {
+        // 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<Message, ProtocolError> {
+        // 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<DRIVER: Driver, TIMER: Timer> ProtocolLayer<DRIVER, TIMER> {
 
         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)]