Merge pull request #14 from sjoerdsimons/pdo-utils

Add utility functions to access various part of source capabilities

pd-interceptor/src/main.rs

@@ -103,29 +103,30 @@ fn handle_event(event: Event) -> Option<Request> {
 
             // Take maximum voltage
             let (index, supply) = caps
+                .pdos()
                 .iter()
                 .enumerate()
                 .filter_map(|(i, cap)| {
                     if let PowerDataObject::FixedSupply(supply) = cap {
                         debug!(
                             "supply @ {}: {}mV {}mA",
                             i,
-                            supply.voltage() * 50,
-                            supply.max_current() * 10
+                            supply.voltage_mv(),
+                            supply.max_current_ma()
                         );
                         Some((i, supply))
                     } else {
                         None
                     }
                 })
-                .max_by(|(_, x), (_, y)| x.voltage().cmp(&y.voltage()))
+                .max_by(|(_, x), (_, y)| x.raw_voltage().cmp(&y.raw_voltage()))
                 .unwrap();
 
             info!("requesting supply {:?}@{}", supply, index);
 
             return Some(Request::RequestPower {
                 index,
-                current: supply.max_current() * 10,
+                current: supply.max_current_ma(),
             });
         }
         Event::PowerReady => info!("power ready"),

usb-pd/src/messages/mod.rs

@@ -9,7 +9,7 @@ use {
     pdo::{
         AugmentedPowerDataObject, AugmentedPowerDataObjectRaw, Battery, EPRAdjustableVoltageSupply,
         FixedSupply, PowerDataObject, PowerDataObjectRaw, SPRProgrammablePowerSupply,
-        VariableSupply,
+        SourceCapabilities, VariableSupply,
     },
     vdo::{VDMHeader, VDMHeaderRaw, VDMHeaderStructured, VDMHeaderUnstructured, VDMType},
 };
@@ -19,7 +19,7 @@ pub enum Message {
     Accept,
     Reject,
     Ready,
-    SourceCapabilities(Vec<PowerDataObject, 8>),
+    SourceCapabilities(SourceCapabilities),
     VendorDefined((VDMHeader, Vec<u32, 7>)), // TODO: Incomplete
     SoftReset,
     Unknown,
@@ -32,36 +32,38 @@ impl Message {
             MessageType::Control(ControlMessageType::Reject) => Message::Reject,
             MessageType::Control(ControlMessageType::PsRdy) => Message::Ready,
             MessageType::Control(ControlMessageType::SoftReset) => Message::SoftReset,
-            MessageType::Data(DataMessageType::SourceCapabilities) => Message::SourceCapabilities(
-                payload
-                    .chunks_exact(4)
-                    .take(header.num_objects())
-                    .map(|buf| PowerDataObjectRaw(LittleEndian::read_u32(buf)))
-                    .map(|pdo| match pdo.kind() {
-                        0b00 => PowerDataObject::FixedSupply(FixedSupply(pdo.0)),
-                        0b01 => PowerDataObject::Battery(Battery(pdo.0)),
-                        0b10 => PowerDataObject::VariableSupply(VariableSupply(pdo.0)),
-                        0b11 => PowerDataObject::AugmentedPowerDataObject({
-                            match AugmentedPowerDataObjectRaw(pdo.0).supply() {
-                                0b00 => {
-                                    AugmentedPowerDataObject::SPR(SPRProgrammablePowerSupply(pdo.0))
-                                }
-                                0b01 => {
-                                    AugmentedPowerDataObject::EPR(EPRAdjustableVoltageSupply(pdo.0))
-                                }
-                                _ => {
-                                    warn!("Unknown AugmentedPowerDataObject supply");
-                                    AugmentedPowerDataObject::Unknown(pdo.0)
+            MessageType::Data(DataMessageType::SourceCapabilities) => {
+                Message::SourceCapabilities(SourceCapabilities(
+                    payload
+                        .chunks_exact(4)
+                        .take(header.num_objects())
+                        .map(|buf| PowerDataObjectRaw(LittleEndian::read_u32(buf)))
+                        .map(|pdo| match pdo.kind() {
+                            0b00 => PowerDataObject::FixedSupply(FixedSupply(pdo.0)),
+                            0b01 => PowerDataObject::Battery(Battery(pdo.0)),
+                            0b10 => PowerDataObject::VariableSupply(VariableSupply(pdo.0)),
+                            0b11 => PowerDataObject::AugmentedPowerDataObject({
+                                match AugmentedPowerDataObjectRaw(pdo.0).supply() {
+                                    0b00 => AugmentedPowerDataObject::SPR(
+                                        SPRProgrammablePowerSupply(pdo.0),
+                                    ),
+                                    0b01 => AugmentedPowerDataObject::EPR(
+                                        EPRAdjustableVoltageSupply(pdo.0),
+                                    ),
+                                    _ => {
+                                        warn!("Unknown AugmentedPowerDataObject supply");
+                                        AugmentedPowerDataObject::Unknown(pdo.0)
+                                    }
                                 }
+                            }),
+                            _ => {
+                                warn!("Unknown PowerDataObject kind");
+                                PowerDataObject::Unknown(pdo)
                             }
-                        }),
-                        _ => {
-                            warn!("Unknown PowerDataObject kind");
-                            PowerDataObject::Unknown(pdo)
-                        }
-                    })
-                    .collect(),
-            ),
+                        })
+                        .collect(),
+                ))
+            }
             MessageType::Data(DataMessageType::VendorDefined) => {
                 // Keep for now...
                 let len = payload.len();

usb-pd/src/messages/pdo.rs

@@ -1,6 +1,7 @@
 use {
     byteorder::{ByteOrder, LittleEndian},
     defmt::Format,
+    heapless::Vec,
     proc_bitfield::bitfield,
 };
 
@@ -42,9 +43,19 @@ bitfield! {
         /// Peak current
         pub peak_current: u8 @ 20..=21,
         /// Voltage in 50mV units
-        pub voltage: u16 @ 10..=19,
+        pub raw_voltage: u16 @ 10..=19,
         /// Maximum current in 10mA units
-        pub max_current: u16 @ 0..=9,
+        pub raw_max_current: u16 @ 0..=9,
+    }
+}
+
+impl FixedSupply {
+    pub fn voltage_mv(&self) -> u16 {
+        self.raw_voltage() * 50
+    }
+
+    pub fn max_current_ma(&self) -> u16 {
+        self.raw_max_current() * 10
     }
 }
 
@@ -54,11 +65,25 @@ bitfield! {
         /// Battery
         pub kind: u8 @ 30..=31,
         /// Maximum Voltage in 50mV units
-        pub max_voltage: u16 @ 20..=29,
+        pub raw_max_voltage: u16 @ 20..=29,
         /// Minimum Voltage in 50mV units
-        pub min_voltage: u16 @ 10..=19,
+        pub raw_min_voltage: u16 @ 10..=19,
         /// Maximum Allowable Power in 250mW units
-        pub max_power: u16 @ 0..=9,
+        pub raw_max_power: u16 @ 0..=9,
+    }
+}
+
+impl Battery {
+    pub fn max_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_max_voltage()) * 50
+    }
+
+    pub fn min_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_min_voltage()) * 50
+    }
+
+    pub fn max_power_mw(&self) -> u32 {
+        u32::from(self.raw_max_power()) * 250
     }
 }
 
@@ -68,11 +93,25 @@ bitfield! {
         /// Variable supply (non-battery)
         pub kind: u8 @ 30..=31,
         /// Maximum Voltage in 50mV units
-        pub max_voltage: u16 @ 20..=29,
+        pub raw_max_voltage: u16 @ 20..=29,
         /// Minimum Voltage in 50mV units
-        pub min_voltage: u16 @ 10..=19,
+        pub raw_min_voltage: u16 @ 10..=19,
         /// Maximum current in 10mA units
-        pub max_current: u16 @ 0..=9,
+        pub raw_max_current: u16 @ 0..=9,
+    }
+}
+
+impl VariableSupply {
+    pub fn max_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_max_voltage()) * 50
+    }
+
+    pub fn min_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_min_voltage()) * 50
+    }
+
+    pub fn max_current_ma(&self) -> u16 {
+        u16::from(self.raw_max_current()) * 10
     }
 }
 
@@ -102,11 +141,25 @@ bitfield! {
         pub supply: u8 @ 28..=29,
         pub pps_power_limited: bool @ 27,
         /// Maximum voltage in 100mV increments
-        pub max_voltage: u8 @ 17..=24,
+        pub raw_max_voltage: u8 @ 17..=24,
         /// Minimum Voltage in 100mV increments
-        pub min_voltage: u8 @ 8..=15,
+        pub raw_min_voltage: u8 @ 8..=15,
         /// Maximum Current in 50mA increments
-        pub maximum_current: u8 @ 0..=6,
+        pub raw_max_current: u8 @ 0..=6,
+    }
+}
+
+impl SPRProgrammablePowerSupply {
+    pub fn max_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_max_voltage()) * 100
+    }
+
+    pub fn min_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_min_voltage()) * 100
+    }
+
+    pub fn max_current_ma(&self) -> u16 {
+        u16::from(self.raw_max_current()) * 50
     }
 }
 
@@ -119,11 +172,21 @@ bitfield! {
         pub supply: u8 @ 28..=29,
         pub peak_current: u8 @ 26..=27,
         /// Maximum voltage in 100mV increments
-        pub max_voltage: u16 @ 17..=25,
+        pub raw_max_voltage: u16 @ 17..=25,
         /// Minimum Voltage in 100mV increments
-        pub min_voltage: u8 @ 8..=15,
+        pub raw_min_voltage: u8 @ 8..=15,
         /// PDP in 1W increments
-        pub maximum_current: u8 @ 0..=7,
+        pub pd_power: u8 @ 0..=7,
+    }
+}
+
+impl EPRAdjustableVoltageSupply {
+    pub fn max_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_max_voltage()) * 100
+    }
+
+    pub fn min_voltage_mv(&self) -> u16 {
+        u16::from(self.raw_min_voltage()) * 100
     }
 }
 
@@ -139,7 +202,7 @@ bitfield! {
         pub unchunked_extended_messages_supported: bool @ 23,
         pub epr_mode_capable: bool @ 22,
         pub operating_current: u16 @ 10..=19,
-        pub maximum_operating_current: u16 @ 0..=9,
+        pub max_operating_current: u16 @ 0..=9,
     }
 }
 
@@ -169,7 +232,7 @@ bitfield! {
         /// Operating power in 250mW units
         pub operating_power: u16 @ 10..=19,
         /// Maximum operating power in 250mW units
-        pub maximum_operating_power: u16 @ 0..=9,
+        pub max_operating_power: u16 @ 0..=9,
     }
 }
 
@@ -206,3 +269,58 @@ impl PPSRequestDataObject {
         LittleEndian::write_u32(buf, self.0);
     }
 }
+
+#[derive(Debug, Clone, Format)]
+pub struct SourceCapabilities(pub(crate) Vec<PowerDataObject, 8>);
+
+impl SourceCapabilities {
+    pub fn vsafe_5v(&self) -> Option<&FixedSupply> {
+        self.0.first().and_then(|supply| {
+            if let PowerDataObject::FixedSupply(supply) = supply {
+                Some(supply)
+            } else {
+                None
+            }
+        })
+    }
+
+    pub fn dual_role_power(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::dual_role_power)
+            .unwrap_or_default()
+    }
+
+    pub fn usb_suspend_supported(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::usb_suspend_supported)
+            .unwrap_or_default()
+    }
+
+    pub fn unconstrained_power(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::unconstrained_power)
+            .unwrap_or_default()
+    }
+
+    pub fn dual_role_data(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::dual_role_data)
+            .unwrap_or_default()
+    }
+
+    pub fn unchunked_extended_messages_supported(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::unchunked_extended_messages_supported)
+            .unwrap_or_default()
+    }
+
+    pub fn epr_mode_capable(&self) -> bool {
+        self.vsafe_5v()
+            .map(FixedSupply::epr_mode_capable)
+            .unwrap_or_default()
+    }
+
+    pub fn pdos(&self) -> &[PowerDataObject] {
+        &self.0
+    }
+}

usb-pd/src/sink.rs

@@ -2,7 +2,7 @@ use {
     crate::{
         header::{DataMessageType, Header, SpecificationRevision},
         messages::{
-            pdo::{FixedVariableRequestDataObject, PPSRequestDataObject, PowerDataObject},
+            pdo::{FixedVariableRequestDataObject, PPSRequestDataObject, SourceCapabilities},
             vdo::{
                 CertStatVDO, ProductVDO, UFPTypeVDO, VDMCommand, VDMCommandType, VDMHeader,
                 VDMHeaderStructured, VDMIdentityHeader, VDMType, VDMVersionMajor, VDMVersionMinor,
@@ -37,7 +37,7 @@ pub enum Event {
     /// Power delivery protocol has changed
     ProtocolChanged,
     /// Source capabilities have changed (immediately request power)
-    SourceCapabilitiesChanged(Vec<PowerDataObject, 8>),
+    SourceCapabilitiesChanged(SourceCapabilities),
     /// Requested power has been accepted (but not ready yet)
     PowerAccepted,
     /// Requested power has been rejected
@@ -365,7 +365,7 @@ impl<DRIVER: Driver> Sink<DRIVER> {
 
         FixedVariableRequestDataObject(0)
             .with_operating_current(current)
-            .with_maximum_operating_current(current)
+            .with_max_operating_current(current)
             .with_object_position(obj_pos)
             .with_no_usb_suspend(true)
             .with_usb_communications_capable(true)