diff --git a/embedded-service/src/type_c/controller.rs b/embedded-service/src/type_c/controller.rs index 83cafced..4d3b5587 100644 --- a/embedded-service/src/type_c/controller.rs +++ b/embedded-service/src/type_c/controller.rs @@ -475,11 +475,7 @@ impl<'a> Device<'a> { /// Covert a local port ID to a global port ID pub fn lookup_global_port(&self, port: LocalPortId) -> Result { - if port.0 >= self.num_ports as u8 { - return Err(PdError::InvalidParams); - } - - Ok(self.ports[port.0 as usize]) + Ok(*self.ports.get(port.0 as usize).ok_or(PdError::InvalidParams)?) } /// Convert a global port ID to a local port ID diff --git a/embedded-service/src/type_c/event.rs b/embedded-service/src/type_c/event.rs index 4748bc06..aef14e02 100644 --- a/embedded-service/src/type_c/event.rs +++ b/embedded-service/src/type_c/event.rs @@ -4,6 +4,7 @@ //! Processing these events typically requires acessing similar registers so they are grouped together. //! [`PortNotification`] contains events that are typically more message-like (PD alerts, VDMs, etc) and can be processed independently. //! Consequently [`PortNotification`] implements iterator traits to allow for processing these events as a stream. +use super::error; use bitfield::bitfield; use bitvec::BitArr; @@ -33,6 +34,14 @@ bitfield! { pub u8, pd_hard_reset, set_pd_hard_reset: 8, 8; } +/// Port pending errors +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum PortPendingError { + /// Invalid port + InvalidPort(usize), +} + /// Port status change events /// This is a type-safe wrapper around the raw bitfield /// These events are related to the overall port state and typically need to be considered together. @@ -405,25 +414,37 @@ impl PortPending { } /// Marks the given port as pending - pub fn pend_port(&mut self, port: usize) { + pub fn pend_port(&mut self, port: usize) -> Result<(), PortPendingError> { + if port >= self.0.len() { + return Err(PortPendingError::InvalidPort(port)); + } self.0.set(port, true); + + Ok(()) } /// Marks the indexes given by the iterator as pending pub fn pend_ports>(&mut self, iter: I) { for port in iter { - self.pend_port(port); + if self.pend_port(port).is_err() { + error!("Error pending port {}", port); + } } } /// Clears the pending status of the given port - pub fn clear_port(&mut self, port: usize) { + pub fn clear_port(&mut self, port: usize) -> Result<(), PortPendingError> { + if port >= self.0.len() { + return Err(PortPendingError::InvalidPort(port)); + } + self.0.set(port, false); + Ok(()) } /// Returns true if the given port is pending - pub fn is_pending(&self, port: usize) -> bool { - self.0[port] + pub fn is_pending(&self, port: usize) -> Result { + Ok(*self.0.get(port).ok_or(PortPendingError::InvalidPort(port))?) } /// Returns a combination of the current pending ports and other @@ -474,9 +495,12 @@ impl Iterator for PortPendingIter { while self.index < self.flags.len() { let port_index = self.index; self.index += 1; - if self.flags.is_pending(port_index) { - self.flags.clear_port(port_index); - return Some(port_index); + if self.flags.is_pending(port_index).unwrap_or(false) { + if self.flags.clear_port(port_index).is_ok() { + return Some(port_index); + } else { + continue; + } } } None @@ -500,12 +524,16 @@ mod tests { fn test_port_event_flags_iter() { let mut pending = PortPending::none(); - pending.pend_port(0); - pending.pend_port(1); - pending.pend_port(2); - pending.pend_port(10); - pending.pend_port(23); - pending.pend_port(31); + pending.pend_port(0).unwrap(); + pending.pend_port(1).unwrap(); + pending.pend_port(2).unwrap(); + pending.pend_port(10).unwrap(); + pending.pend_port(23).unwrap(); + pending.pend_port(31).unwrap(); + + let result = pending.pend_port(32); + let expected = Err(PortPendingError::InvalidPort(32)); + assert_eq!(expected, result); let mut iter = pending.into_iter(); assert_eq!(iter.next(), Some(0)); diff --git a/embedded-service/src/type_c/mod.rs b/embedded-service/src/type_c/mod.rs index 8169b055..2943acdd 100644 --- a/embedded-service/src/type_c/mod.rs +++ b/embedded-service/src/type_c/mod.rs @@ -2,6 +2,7 @@ use embedded_usb_pd::pdo::{Common, Contract}; use embedded_usb_pd::type_c; +use crate::error; use crate::power::policy; pub mod comms; diff --git a/type-c-service/src/lib.rs b/type-c-service/src/lib.rs index 99e9e2a5..4dfea19f 100644 --- a/type-c-service/src/lib.rs +++ b/type-c-service/src/lib.rs @@ -148,9 +148,9 @@ mod tests { #[tokio::test] async fn test_port_status_changed() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); - pending_ports.pend_port(2); - pending_ports.pend_port(3); + pending_ports.pend_port(0).unwrap(); + pending_ports.pend_port(2).unwrap(); + pending_ports.pend_port(3).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); @@ -197,7 +197,7 @@ mod tests { #[tokio::test] async fn test_port_notification() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); + pending_ports.pend_port(0).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); let event = streamer @@ -229,7 +229,7 @@ mod tests { #[tokio::test] async fn test_last_notifications() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); + pending_ports.pend_port(0).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); @@ -252,8 +252,8 @@ mod tests { #[tokio::test] async fn test_port_event() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); - pending_ports.pend_port(6); + pending_ports.pend_port(0).unwrap(); + pending_ports.pend_port(6).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); @@ -321,7 +321,7 @@ mod tests { #[tokio::test] async fn test_empty_event() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); + pending_ports.pend_port(0).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); let event = streamer.next::<(), _, _>(async |_| Ok(PortEvent::none())).await; @@ -332,8 +332,8 @@ mod tests { #[tokio::test] async fn test_skip_no_pending() { let mut pending_ports = PortPending::none(); - pending_ports.pend_port(0); - pending_ports.pend_port(1); + pending_ports.pend_port(0).unwrap(); + pending_ports.pend_port(1).unwrap(); let mut streamer = PortEventStreamer::new(pending_ports.into_iter()); let event = streamer diff --git a/type-c-service/src/wrapper/mod.rs b/type-c-service/src/wrapper/mod.rs index 37f60649..38dcb170 100644 --- a/type-c-service/src/wrapper/mod.rs +++ b/type-c-service/src/wrapper/mod.rs @@ -299,7 +299,9 @@ where if events != PortEvent::none() { let mut pending = PortPending::none(); - pending.pend_port(global_port_id.0 as usize); + pending + .pend_port(global_port_id.0 as usize) + .map_err(|_| Error::Pd(PdError::InvalidPort))?; self.registration.pd_controller.notify_ports(pending); trace!("P{}: Notified service for events: {:#?}", global_port_id.0, events); } @@ -333,7 +335,9 @@ where // Pend this port let mut pending = PortPending::none(); - pending.pend_port(global_port_id.0 as usize); + pending + .pend_port(global_port_id.0 as usize) + .map_err(|_| Error::Pd(PdError::InvalidPort))?; self.registration.pd_controller.notify_ports(pending); Ok(()) } diff --git a/type-c-service/src/wrapper/vdm.rs b/type-c-service/src/wrapper/vdm.rs index d0968608..0eb01fa5 100644 --- a/type-c-service/src/wrapper/vdm.rs +++ b/type-c-service/src/wrapper/vdm.rs @@ -55,7 +55,9 @@ where } let mut pending = PortPending::none(); - pending.pend_port(global_port_id.0 as usize); + pending + .pend_port(global_port_id.0 as usize) + .map_err(|_| PdError::InvalidPort)?; self.registration.pd_controller.notify_ports(pending); Ok(()) }