Merge pull request #19 from sjoerdsimons/parse-all-control-messages

Parse all control messages
fmckeogh · Jun 16, 2024 · ad7d271 · ad7d271
2 parents 010a6c2 + f87522a
commit ad7d271
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 20 deletions.
diff --git a/usb-pd/src/lib.rs b/usb-pd/src/lib.rs
@@ -1,4 +1,5 @@
 #![no_std]
+use defmt::Format;
 
 #[macro_use]
 extern crate uom;
@@ -10,7 +11,7 @@ pub mod source;
 pub mod timers;
 pub mod token;
 
-#[derive(Clone, Copy, PartialEq, Eq)]
+#[derive(Clone, Copy, PartialEq, Eq, Debug, Format)]
 pub enum CcPin {
     CC1,
     CC2,
@@ -27,7 +28,7 @@ impl core::ops::Not for CcPin {
     }
 }
 
-#[derive(Clone, Copy, Debug)]
+#[derive(Clone, Copy, Debug, Format)]
 pub enum PowerRole {
     Source,
     Sink,
@@ -51,7 +52,7 @@ impl From<PowerRole> for bool {
     }
 }
 
-#[derive(Clone, Copy, Debug)]
+#[derive(Clone, Copy, Debug, Format)]
 pub enum DataRole {
     Ufp,
     Dfp,

diff --git a/usb-pd/src/messages/mod.rs b/usb-pd/src/messages/mod.rs
@@ -30,23 +30,17 @@ impl PdoState for () {
 
 #[derive(Debug, Clone, Format)]
 pub enum Message {
-    Accept,
-    Reject,
-    Ready,
+    Control(ControlMessageType),
     SourceCapabilities(SourceCapabilities),
     Request(Request),
     VendorDefined((VDMHeader, Vec<u32, 7>)), // TODO: Incomplete
-    SoftReset,
     Unknown,
 }
 
 impl Message {
     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,
-            MessageType::Control(ControlMessageType::PsRdy) => Message::Ready,
-            MessageType::Control(ControlMessageType::SoftReset) => Message::SoftReset,
+            MessageType::Control(c) => Message::Control(c),
             MessageType::Data(DataMessageType::SourceCapabilities) => {
                 Message::SourceCapabilities(SourceCapabilities(
                     payload
@@ -105,6 +99,9 @@ impl Message {
             MessageType::Data(DataMessageType::VendorDefined) => {
                 // Keep for now...
                 let len = payload.len();
+                if len < 4 {
+                    return Message::Unknown;
+                }
                 let num_obj = header.num_objects();
                 trace!("VENDOR: {:?}, {:?}, {:x}", len, num_obj, payload);
 
@@ -138,7 +135,7 @@ impl Message {
 
                 Message::VendorDefined((header, data))
             }
-            _ => {
+            MessageType::Data(_) => {
                 warn!("unknown message type");
                 Message::Unknown
             }

diff --git a/usb-pd/src/sink.rs b/usb-pd/src/sink.rs
@@ -1,6 +1,6 @@
 use {
     crate::{
-        header::{DataMessageType, Header, SpecificationRevision},
+        header::{ControlMessageType, DataMessageType, Header, SpecificationRevision},
         messages::{
             pdo::{FixedVariableRequestDataObject, PPSRequestDataObject, SourceCapabilities},
             vdo::{
@@ -293,20 +293,26 @@ impl<DRIVER: Driver> Sink<DRIVER> {
     }
 
     fn handle_msg(&mut self, message: Message) -> Option<Event> {
+        use ControlMessageType::*;
         match message {
-            Message::Accept => Some(Event::PowerAccepted),
-            Message::Reject => {
+            Message::Control(Accept) => Some(Event::PowerAccepted),
+            Message::Control(Reject) => {
                 self.requested_voltage = 0;
                 self.requested_max_current = 0;
                 Some(Event::PowerRejected)
             }
-            Message::Ready => {
+            Message::Control(PsRdy) => {
                 self.active_voltage = self.requested_voltage;
                 self.active_max_current = self.requested_max_current;
                 self.requested_voltage = 0;
                 self.requested_max_current = 0;
                 Some(Event::PowerReady)
             }
+            Message::Control(SoftReset) => {
+                warn!("UNHANDLED: Soft RESET request.");
+                None
+            }
+            Message::Control(_) => None,
             Message::SourceCapabilities(caps) => Some(Event::SourceCapabilitiesChanged(caps)),
             Message::VendorDefined((hdr, data)) => {
                 match hdr {
@@ -329,10 +335,6 @@ impl<DRIVER: Driver> Sink<DRIVER> {
                 }
                 Some(Event::VDMReceived((hdr, data)))
             }
-            Message::SoftReset => {
-                warn!("UNHANDLED: Soft RESET request.");
-                None
-            }
             Message::Request(_) => None,
             Message::Unknown => unimplemented!(),
         }