km003c_lib/
pd.rs

1use bytes::Bytes;
2use uom::si::electric_current::milliampere;
3use uom::si::electric_potential::millivolt;
4use uom::si::f64::{ElectricCurrent, ElectricPotential, Time};
5use uom::si::time::millisecond;
6use zerocopy::byteorder::little_endian::{I16, U16, U32};
7use zerocopy::{FromBytes, Immutable, IntoBytes, KnownLayout, Unaligned};
8
9use crate::constants::*;
10use crate::error::KMError;
11
12#[cfg(feature = "serde")]
13use serde::{Deserialize, Serialize};
14
15#[cfg(feature = "python")]
16use uom::si::{electric_current::ampere, electric_potential::volt};
17
18/// PD measurement block (12 bytes).
19///
20/// The device uses this exact layout both as a standalone/chained PD status
21/// payload and as the preamble of a PD event stream.
22#[derive(Debug, Clone, Copy, FromBytes, IntoBytes, KnownLayout, Immutable, Unaligned)]
23#[repr(C)]
24pub struct PdStatusRaw {
25    pub timestamp_ms: U32,
26    pub vbus_mv: U16,
27    pub ibus_ma: I16, // Signed! Negative = power flowing from male to female port
28    pub cc1_mv: U16,
29    pub cc2_mv: U16,
30}
31
32#[derive(Debug, Clone, Copy, PartialEq)]
33#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
34#[cfg_attr(feature = "python", pyo3::pyclass(skip_from_py_object))]
35pub struct PdStatus {
36    pub timestamp: Time,
37    pub vbus: ElectricPotential,
38    pub ibus: ElectricCurrent,
39    pub cc1: ElectricPotential,
40    pub cc2: ElectricPotential,
41}
42
43impl From<PdStatusRaw> for PdStatus {
44    fn from(raw: PdStatusRaw) -> Self {
45        Self {
46            timestamp: Time::new::<millisecond>(f64::from(raw.timestamp_ms.get())),
47            vbus: ElectricPotential::new::<millivolt>(f64::from(raw.vbus_mv.get())),
48            ibus: ElectricCurrent::new::<milliampere>(f64::from(raw.ibus_ma.get())),
49            cc1: ElectricPotential::new::<millivolt>(f64::from(raw.cc1_mv.get())),
50            cc2: ElectricPotential::new::<millivolt>(f64::from(raw.cc2_mv.get())),
51        }
52    }
53}
54
55impl From<PdStatus> for PdStatusRaw {
56    fn from(status: PdStatus) -> Self {
57        Self {
58            timestamp_ms: U32::new(status.timestamp.get::<millisecond>() as u32),
59            vbus_mv: U16::new(status.vbus.get::<millivolt>() as u16),
60            ibus_ma: I16::new(status.ibus.get::<milliampere>() as i16),
61            cc1_mv: U16::new(status.cc1.get::<millivolt>() as u16),
62            cc2_mv: U16::new(status.cc2.get::<millivolt>() as u16),
63        }
64    }
65}
66
67#[cfg(feature = "python")]
68#[pyo3::pymethods]
69impl PdStatus {
70    #[getter]
71    fn timestamp(&self) -> f64 {
72        self.timestamp.get::<millisecond>()
73    }
74    #[getter]
75    fn vbus_v(&self) -> f64 {
76        self.vbus.get::<volt>()
77    }
78    #[getter]
79    fn ibus_a(&self) -> f64 {
80        self.ibus.get::<ampere>()
81    }
82    #[getter]
83    fn cc1_v(&self) -> f64 {
84        self.cc1.get::<volt>()
85    }
86    #[getter]
87    fn cc2_v(&self) -> f64 {
88        self.cc2.get::<volt>()
89    }
90
91    fn __repr__(&self) -> String {
92        format!(
93            "PdStatus(timestamp={}ms, vbus={:.3}V, ibus={:.3}A)",
94            self.timestamp.get::<millisecond>(),
95            self.vbus.get::<volt>(),
96            self.ibus.get::<ampere>()
97        )
98    }
99
100    fn __str__(&self) -> String {
101        self.__repr__()
102    }
103}
104
105/// Event data types that can appear in PD stream
106#[derive(Debug, Clone, PartialEq)]
107#[cfg_attr(feature = "python", derive(pyo3::IntoPyObject))]
108pub enum PdEventData {
109    #[cfg_attr(feature = "python", pyo3(transparent))]
110    Connect(()),
111    #[cfg_attr(feature = "python", pyo3(transparent))]
112    Disconnect(()),
113    PdMessage {
114        sop: u8,
115        wire_data: Vec<u8>,
116    },
117}
118
119/// Timestamped PD event
120#[derive(Debug, Clone, PartialEq)]
121#[cfg_attr(feature = "python", pyo3::pyclass(skip_from_py_object, name = "PdEvent"))]
122pub struct PdEvent {
123    pub timestamp: Time,
124    pub data: PdEventData,
125}
126
127/// Complete PD event stream with preamble and events
128#[derive(Debug, Clone, PartialEq)]
129#[cfg_attr(feature = "python", pyo3::pyclass(skip_from_py_object, name = "PdEventStream"))]
130pub struct PdEventStream {
131    /// Measurements captured immediately before the event records.
132    pub preamble: PdStatus,
133    pub events: Vec<PdEvent>,
134}
135
136impl PdEventStream {
137    /// Parse PD event stream from bytes
138    /// Expected format: 12-byte preamble + repeated (6-byte header + wire data) events
139    pub fn from_bytes(bytes: Bytes) -> Result<Self, KMError> {
140        if bytes.len() < PD_STATUS_SIZE {
141            return Err(KMError::InvalidPacket(format!(
142                "PD event stream too short for preamble: need {}, got {}",
143                PD_STATUS_SIZE,
144                bytes.len()
145            )));
146        }
147
148        // Parse preamble
149        let preamble_raw = PdStatusRaw::ref_from_bytes(&bytes[..PD_STATUS_SIZE])
150            .map_err(|_| KMError::InvalidPacket("Failed to parse PD preamble".to_string()))?;
151        let preamble = PdStatus::from(*preamble_raw);
152
153        let mut events = Vec::new();
154        let mut offset = PD_STATUS_SIZE;
155
156        // Parse events
157        while offset < bytes.len() {
158            if bytes.len() - offset < PD_EVENT_HEADER_SIZE {
159                return Err(KMError::InvalidPacket(format!(
160                    "Incomplete PD event header at offset {}: need {}, got {}",
161                    offset,
162                    PD_EVENT_HEADER_SIZE,
163                    bytes.len() - offset
164                )));
165            }
166
167            let size_flag = bytes[offset];
168
169            // Connection/status event (0x45): 6-byte header with ts24 + code
170            if size_flag == PD_EVENT_TYPE_CONNECTION {
171                let ts24 = u32::from_le_bytes([bytes[offset + 1], bytes[offset + 2], bytes[offset + 3], 0]);
172                let event_code = bytes[offset + 5];
173                let data = match event_code {
174                    PD_CONNECTION_CONNECT | PD_CONNECTION_CONNECT_LEGACY => PdEventData::Connect(()),
175                    PD_CONNECTION_DISCONNECT | PD_CONNECTION_DISCONNECT_LEGACY => PdEventData::Disconnect(()),
176                    _ => PdEventData::PdMessage {
177                        sop: event_code,
178                        wire_data: Vec::new(),
179                    },
180                };
181                events.push(PdEvent {
182                    timestamp: Time::new::<millisecond>(f64::from(ts24)),
183                    data,
184                });
185                offset += PD_EVENT_HEADER_SIZE;
186                continue;
187            }
188
189            let timestamp = u32::from_le_bytes([
190                bytes[offset + 1],
191                bytes[offset + 2],
192                bytes[offset + 3],
193                bytes[offset + 4],
194            ]);
195            let sop = bytes[offset + 5];
196
197            // Calculate wire data length: wire_len = (size_flag & mask) - offset
198            let encoded_size = size_flag & PD_EVENT_SIZE_MASK;
199            let wire_len = encoded_size.checked_sub(PD_EVENT_SIZE_OFFSET).ok_or_else(|| {
200                KMError::InvalidPacket(format!(
201                    "Invalid PD event size at offset {}: encoded size {} is below {}",
202                    offset, encoded_size, PD_EVENT_SIZE_OFFSET
203                ))
204            })? as usize;
205
206            offset += PD_EVENT_HEADER_SIZE;
207
208            if bytes.len() - offset < wire_len {
209                return Err(KMError::InvalidPacket(format!(
210                    "Insufficient bytes for wire data: expected {}, got {}",
211                    wire_len,
212                    bytes.len() - offset
213                )));
214            }
215
216            let wire_data = if wire_len > 0 {
217                bytes.slice(offset..offset + wire_len).to_vec()
218            } else {
219                Vec::new()
220            };
221            offset += wire_len;
222
223            let data = PdEventData::PdMessage { sop, wire_data };
224
225            events.push(PdEvent {
226                timestamp: Time::new::<millisecond>(f64::from(timestamp)),
227                data,
228            });
229        }
230
231        Ok(Self { preamble, events })
232    }
233
234    /// Helper: get all PD messages, ignoring connection events
235    pub fn pd_messages(&self) -> impl Iterator<Item = (&Time, u8, &Vec<u8>)> {
236        self.events.iter().filter_map(|e| match &e.data {
237            PdEventData::PdMessage { sop, wire_data } => Some((&e.timestamp, *sop, wire_data)),
238            _ => None,
239        })
240    }
241
242    /// Helper: get connection state changes
243    pub fn connection_events(&self) -> impl Iterator<Item = (&Time, bool)> {
244        self.events.iter().filter_map(|e| match &e.data {
245            PdEventData::Connect(()) => Some((&e.timestamp, true)),
246            PdEventData::Disconnect(()) => Some((&e.timestamp, false)),
247            _ => None,
248        })
249    }
250}
251
252#[cfg(feature = "python")]
253#[pyo3::pymethods]
254impl PdEvent {
255    #[getter]
256    fn timestamp(&self) -> f64 {
257        self.timestamp.get::<millisecond>()
258    }
259
260    #[getter]
261    fn data(&self) -> PdEventData {
262        self.data.clone()
263    }
264
265    fn __repr__(&self) -> String {
266        match &self.data {
267            PdEventData::Connect(()) => format!(
268                "PdEvent(timestamp={}ms, type=connect)",
269                self.timestamp.get::<millisecond>()
270            ),
271            PdEventData::Disconnect(()) => format!(
272                "PdEvent(timestamp={}ms, type=disconnect)",
273                self.timestamp.get::<millisecond>()
274            ),
275            PdEventData::PdMessage { sop, wire_data } => format!(
276                "PdEvent(timestamp={}, type=pd_message, sop={}, {} bytes)",
277                self.timestamp.get::<millisecond>(),
278                sop,
279                wire_data.len()
280            ),
281        }
282    }
283
284    fn __str__(&self) -> String {
285        self.__repr__()
286    }
287}
288
289#[cfg(feature = "python")]
290#[pyo3::pymethods]
291impl PdEventStream {
292    #[getter]
293    fn preamble(&self) -> PdStatus {
294        self.preamble
295    }
296
297    #[getter]
298    fn events(&self) -> Vec<PdEvent> {
299        self.events.clone()
300    }
301
302    fn __repr__(&self) -> String {
303        format!(
304            "PdEventStream(preamble={}, {} events)",
305            self.preamble.__repr__(),
306            self.events.len()
307        )
308    }
309
310    fn __str__(&self) -> String {
311        self.__repr__()
312    }
313}
km003c_lib/pd.rs

km003c_lib/
pd.rs
