Merge pull request #18 from sjoerdsimons/parse-requests

Add support to parse request data objects

usb-pd/src/messages/mod.rs

@@ -2,6 +2,10 @@ pub mod pdo;
 pub mod vdo;
 
 use {
+    self::pdo::{
+        AVSRequestDataObject, BatteryRequestDataObject, FixedVariableRequestDataObject,
+        PPSRequestDataObject, PowerDataObjectType, RawRequestDataObject, Request,
+    },
     crate::header::{ControlMessageType, DataMessageType, Header, MessageType},
     byteorder::{ByteOrder, LittleEndian},
     defmt::{trace, warn, Format},
@@ -14,19 +18,30 @@ use {
     vdo::{VDMHeader, VDMHeaderRaw, VDMHeaderStructured, VDMHeaderUnstructured, VDMType},
 };
 
+pub trait PdoState {
+    fn pdo_at_object_position(&self, position: u8) -> Option<PowerDataObjectType>;
+}
+
+impl PdoState for () {
+    fn pdo_at_object_position(&self, _position: u8) -> Option<PowerDataObjectType> {
+        None
+    }
+}
+
 #[derive(Debug, Clone, Format)]
 pub enum Message {
     Accept,
     Reject,
     Ready,
     SourceCapabilities(SourceCapabilities),
+    Request(Request),
     VendorDefined((VDMHeader, Vec<u32, 7>)), // TODO: Incomplete
     SoftReset,
     Unknown,
 }
 
 impl Message {
-    pub fn parse(header: Header, payload: &[u8]) -> Self {
+    pub fn parse_with_state<P: PdoState>(header: Header, payload: &[u8], state: &P) -> Self {
         match header.message_type() {
             MessageType::Control(ControlMessageType::Accept) => Message::Accept,
             MessageType::Control(ControlMessageType::Reject) => Message::Reject,
@@ -64,6 +79,29 @@ impl Message {
                         .collect(),
                 ))
             }
+            MessageType::Data(DataMessageType::Request) => {
+                if payload.len() != 4 {
+                    return Message::Unknown;
+                }
+                let raw = RawRequestDataObject(LittleEndian::read_u32(payload));
+                if let Some(t) = state.pdo_at_object_position(raw.object_position()) {
+                    Message::Request(match t {
+                        PowerDataObjectType::FixedSupply => {
+                            Request::FixedSupply(FixedVariableRequestDataObject(raw.0))
+                        }
+                        PowerDataObjectType::Battery => {
+                            Request::Battery(BatteryRequestDataObject(raw.0))
+                        }
+                        PowerDataObjectType::VariableSupply => {
+                            Request::VariableSupply(FixedVariableRequestDataObject(raw.0))
+                        }
+                        PowerDataObjectType::PPS => Request::PPS(PPSRequestDataObject(raw.0)),
+                        PowerDataObjectType::AVS => Request::AVS(AVSRequestDataObject(raw.0)),
+                    })
+                } else {
+                    Message::Request(Request::Unknown(raw))
+                }
+            }
             MessageType::Data(DataMessageType::VendorDefined) => {
                 // Keep for now...
                 let len = payload.len();
@@ -106,4 +144,8 @@ impl Message {
             }
         }
     }
+
+    pub fn parse(header: Header, payload: &[u8]) -> Self {
+        Self::parse_with_state(header, payload, &())
+    }
 }

usb-pd/src/messages/pdo.rs

@@ -1,4 +1,6 @@
 use {
+    super::PdoState,
+    _20millivolts_mod::_20millivolts,
     _250milliwatts_mod::_250milliwatts,
     _50milliamperes_mod::_50milliamperes,
     _50millivolts_mod::_50millivolts,
@@ -9,6 +11,15 @@ use {
     uom::si::{self, electric_current::centiampere, electric_potential::decivolt, power::watt},
 };
 
+mod _20millivolts_mod {
+    unit! {
+        system: uom::si;
+        quantity: uom::si::electric_potential;
+
+        @_20millivolts: 0.02; "_20mV", "_20millivolts", "_20millivolts";
+    }
+}
+
 mod _50milliamperes_mod {
     unit! {
         system: uom::si;
@@ -36,6 +47,15 @@ mod _250milliwatts_mod {
     }
 }
 
+#[derive(Clone, Copy, Debug, Format)]
+pub enum PowerDataObjectType {
+    FixedSupply,
+    Battery,
+    VariableSupply,
+    PPS,
+    AVS,
+}
+
 #[derive(Clone, Copy, Debug, Format)]
 pub enum PowerDataObject {
     FixedSupply(FixedSupply),
@@ -225,6 +245,14 @@ impl EPRAdjustableVoltageSupply {
     }
 }
 
+bitfield! {
+    #[derive(Clone, Copy, PartialEq, Eq, Format)]
+    pub struct RawRequestDataObject(pub u32): Debug, FromRaw, IntoRaw {
+        /// Valid range 1..=14
+        pub object_position: u8 @ 28..=31,
+    }
+}
+
 bitfield! {
     #[derive(Clone, Copy, PartialEq, Eq, Format)]
     pub struct FixedVariableRequestDataObject(pub u32): Debug, FromRaw, IntoRaw {
@@ -236,20 +264,28 @@ bitfield! {
         pub no_usb_suspend: bool @ 24,
         pub unchunked_extended_messages_supported: bool @ 23,
         pub epr_mode_capable: bool @ 22,
-        pub operating_current: u16 @ 10..=19,
-        pub max_operating_current: u16 @ 0..=9,
+        pub raw_operating_current: u16 @ 10..=19,
+        pub raw_max_operating_current: u16 @ 0..=9,
     }
 }
 
 impl FixedVariableRequestDataObject {
     pub fn to_bytes(&self, buf: &mut [u8]) {
         LittleEndian::write_u32(buf, self.0);
     }
+
+    pub fn operating_current(&self) -> si::u16::ElectricCurrent {
+        si::u16::ElectricCurrent::new::<centiampere>(self.raw_operating_current())
+    }
+
+    pub fn max_operating_current(&self) -> si::u16::ElectricCurrent {
+        si::u16::ElectricCurrent::new::<centiampere>(self.raw_max_operating_current())
+    }
 }
 
 bitfield! {
     #[derive(Clone, Copy, PartialEq, Eq, Format)]
-    pub struct BatteryRequestDataObject(pub u32): FromRaw, IntoRaw {
+    pub struct BatteryRequestDataObject(pub u32): Debug, FromRaw, IntoRaw {
         /// Object position (0000b and 1110b…1111b are Reserved and Shall Not be used)
         pub object_position: u8 @ 28..=31,
         /// GiveBackFlag = 0
@@ -265,21 +301,29 @@ bitfield! {
         /// EPR mode capable
         pub epr_mode_capable: bool @ 22,
         /// Operating power in 250mW units
-        pub operating_power: u16 @ 10..=19,
+        pub raw_operating_power: u16 @ 10..=19,
         /// Maximum operating power in 250mW units
-        pub max_operating_power: u16 @ 0..=9,
+        pub raw_max_operating_power: u16 @ 0..=9,
     }
 }
 
 impl BatteryRequestDataObject {
     pub fn to_bytes(&self, buf: &mut [u8]) {
         LittleEndian::write_u32(buf, self.0);
     }
+
+    pub fn operating_power(&self) -> si::u32::Power {
+        si::u32::Power::new::<_250milliwatts>(self.raw_operating_power().into())
+    }
+
+    pub fn max_operating_power(&self) -> si::u32::Power {
+        si::u32::Power::new::<_250milliwatts>(self.raw_max_operating_power().into())
+    }
 }
 
 bitfield!(
     #[derive(Clone, Copy, PartialEq, Eq, Format)]
-    pub struct PPSRequestDataObject(pub u32): FromRaw, IntoRaw {
+    pub struct PPSRequestDataObject(pub u32): Debug, FromRaw, IntoRaw {
         /// Object position (0000b and 1110b…1111b are Reserved and Shall Not be used)
         pub object_position: u8 @ 28..=31,
         /// Capability mismatch
@@ -293,16 +337,83 @@ bitfield!(
         /// EPR mode capable
         pub epr_mode_capable: bool @ 22,
         /// Output voltage in 20mV units
-        pub output_voltage: u16 @ 9..=20,
+        pub raw_output_voltage: u16 @ 9..=20,
         /// Operating current in 50mA units
-        pub operating_current: u16 @ 0..=6,
+        pub raw_operating_current: u16 @ 0..=6,
     }
 );
 
 impl PPSRequestDataObject {
     pub fn to_bytes(&self, buf: &mut [u8]) {
         LittleEndian::write_u32(buf, self.0);
     }
+
+    pub fn output_voltage(&self) -> si::u16::ElectricPotential {
+        si::u16::ElectricPotential::new::<_20millivolts>(self.raw_output_voltage())
+    }
+
+    pub fn operating_current(&self) -> si::u16::ElectricCurrent {
+        si::u16::ElectricCurrent::new::<_50milliamperes>(self.raw_operating_current())
+    }
+}
+
+bitfield!(
+    #[derive(Clone, Copy, PartialEq, Eq, Format)]
+    pub struct AVSRequestDataObject(pub u32): Debug, FromRaw, IntoRaw {
+        /// Object position (0000b and 1110b…1111b are Reserved and Shall Not be used)
+        pub object_position: u8 @ 28..=31,
+        /// Capability mismatch
+        pub capability_mismatch: bool @ 26,
+        /// USB communications capable
+        pub usb_communications_capable: bool @ 25,
+        /// No USB Suspend
+        pub no_usb_suspend: bool @ 24,
+        /// Unchunked extended messages supported
+        pub unchunked_extended_messages_supported: bool @ 23,
+        /// EPR mode capable
+        pub epr_mode_capable: bool @ 22,
+        /// Output voltage in 20mV units
+        pub raw_output_voltage: u16 @ 9..=20,
+        /// Operating current in 50mA units
+        pub raw_operating_current: u16 @ 0..=6,
+    }
+);
+
+impl AVSRequestDataObject {
+    pub fn to_bytes(&self, buf: &mut [u8]) {
+        LittleEndian::write_u32(buf, self.0);
+    }
+
+    pub fn output_voltage(&self) -> si::u16::ElectricPotential {
+        si::u16::ElectricPotential::new::<_20millivolts>(self.raw_output_voltage())
+    }
+
+    pub fn operating_current(&self) -> si::u16::ElectricCurrent {
+        si::u16::ElectricCurrent::new::<_50milliamperes>(self.raw_operating_current())
+    }
+}
+
+#[derive(Debug, Clone, Format)]
+pub enum Request {
+    FixedSupply(FixedVariableRequestDataObject),
+    VariableSupply(FixedVariableRequestDataObject),
+    Battery(BatteryRequestDataObject),
+    PPS(PPSRequestDataObject),
+    AVS(AVSRequestDataObject),
+    Unknown(RawRequestDataObject),
+}
+
+impl Request {
+    pub fn object_position(&self) -> u8 {
+        match self {
+            Request::FixedSupply(p) => p.object_position(),
+            Request::VariableSupply(p) => p.object_position(),
+            Request::Battery(p) => p.object_position(),
+            Request::PPS(p) => p.object_position(),
+            Request::AVS(p) => p.object_position(),
+            Request::Unknown(p) => p.object_position(),
+        }
+    }
 }
 
 #[derive(Debug, Clone, Format)]
@@ -359,3 +470,33 @@ impl SourceCapabilities {
         &self.0
     }
 }
+
+impl PdoState for SourceCapabilities {
+    fn pdo_at_object_position(&self, position: u8) -> Option<PowerDataObjectType> {
+        self.pdos()
+            .get(position.saturating_sub(1) as usize)
+            .and_then(|pdo| match pdo {
+                PowerDataObject::FixedSupply(_) => Some(PowerDataObjectType::FixedSupply),
+                PowerDataObject::Battery(_) => Some(PowerDataObjectType::Battery),
+                PowerDataObject::VariableSupply(_) => Some(PowerDataObjectType::VariableSupply),
+                PowerDataObject::AugmentedPowerDataObject(augmented) => match augmented {
+                    AugmentedPowerDataObject::SPR(_) => Some(PowerDataObjectType::PPS),
+                    AugmentedPowerDataObject::EPR(_) => Some(PowerDataObjectType::AVS),
+                    AugmentedPowerDataObject::Unknown(_) => None,
+                },
+                PowerDataObject::Unknown(_) => None,
+            })
+    }
+}
+
+impl PdoState for Option<SourceCapabilities> {
+    fn pdo_at_object_position(&self, position: u8) -> Option<PowerDataObjectType> {
+        self.as_ref().pdo_at_object_position(position)
+    }
+}
+
+impl PdoState for Option<&SourceCapabilities> {
+    fn pdo_at_object_position(&self, position: u8) -> Option<PowerDataObjectType> {
+        self.and_then(|s| s.pdo_at_object_position(position))
+    }
+}

usb-pd/src/sink.rs

@@ -166,8 +166,8 @@ impl<DRIVER: Driver> Sink<DRIVER> {
                 // Create PPS request data object
                 let pps = PPSRequestDataObject(0)
                     .with_object_position(obj_pos as u8)
-                    .with_operating_current(current / 50) // Convert current from millis to 50ma units
-                    .with_output_voltage(voltage / 20) // Convert voltage from millis to 20mv units
+                    .with_raw_operating_current(current / 50) // Convert current from millis to 50ma units
+                    .with_raw_output_voltage(voltage / 20) // Convert voltage from millis to 20mv units
                     .with_capability_mismatch(false)
                     .with_epr_mode_capable(false)
                     .with_usb_communications_capable(true);
@@ -333,6 +333,7 @@ impl<DRIVER: Driver> Sink<DRIVER> {
                 warn!("UNHANDLED: Soft RESET request.");
                 None
             }
+            Message::Request(_) => None,
             Message::Unknown => unimplemented!(),
         }
     }
@@ -364,8 +365,8 @@ impl<DRIVER: Driver> Sink<DRIVER> {
         assert!(obj_pos > 0b0000 && obj_pos <= 0b1110);
 
         FixedVariableRequestDataObject(0)
-            .with_operating_current(current)
-            .with_max_operating_current(current)
+            .with_raw_operating_current(current)
+            .with_raw_max_operating_current(current)
             .with_object_position(obj_pos)
             .with_no_usb_suspend(true)
             .with_usb_communications_capable(true)