Add first-class PDO discovery and manipulation

Cargo.toml

@@ -37,6 +37,8 @@ smlang = "0.8.0"
 ethercrab-wire = { version = "0.2.0", path = "./ethercrab-wire" }
 spin = { version = "0.10.0", default-features = false, features = ["rwlock"] }
 crc = { version = "3.2.1", default-features = false }
+indexmap = { version = "2.2", optional = true, default-features = false }
+fnv = { version = "1.0.7", optional = true, default-features = false }
 
 [target.'cfg(target_os = "windows")'.dependencies]
 pnet_datalink = { version = "0.35.0", features = ["std"], optional = true }
@@ -107,6 +109,7 @@ std = [
 ]
 xdp = ["dep:xsk-rs"]
 serde = ["dep:serde", "bitflags/serde"]
+alloc = ["dep:indexmap", "dep:fnv"]
 
 # [[example]]
 # name = "akd"

src/eeprom/types.rs

@@ -1,6 +1,7 @@
 //! SubDevice Information Interface (SII).
 
 use crate::{
+    base_data_types::PrimitiveDataType,
     coe::SdoExpedited,
     sync_manager_channel::{self, Direction, OperationMode},
 };
@@ -566,7 +567,7 @@ pub enum SyncManagerType {
 impl SdoExpedited for SyncManagerType {}
 
 /// Defined in ETG2010 Table 14 – Structure Category TXPDO and RXPDO for each PDO
-#[derive(Debug, Copy, Clone, PartialEq, ethercrab_wire::EtherCrabWireRead)]
+#[derive(Debug, Clone, PartialEq, ethercrab_wire::EtherCrabWireRead)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[wire(bytes = 8)]
 pub struct Pdo {
@@ -583,12 +584,9 @@ pub struct Pdo {
     // pub(crate) name_string_idx: u8,
     // #[wire(bytes = 2)]
     // pub(crate) flags: PdoFlags,
-
-    // NOTE: Field is only used to sum up `bit_len`, so we don't need to read or store it.
-    // Definition is left here in case we need it later.
-    // // NOTE: This field is skipped during parsing from the wire and is populated later.
-    // #[wire(skip)]
-    // pub(crate) entries: heapless::Vec<PdoEntry, 16>,
+    // NOTE: This field is skipped during parsing from the wire and is populated later.
+    #[wire(skip)]
+    pub entries: heapless::Vec<PdoEntry, 16>,
 
     // NOTE: This field is skipped during parsing from the wire and is populated from all the
     // `PdoEntry`s later.
@@ -626,19 +624,19 @@ pub struct Pdo {
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[wire(bytes = 8)]
 pub struct PdoEntry {
-    // #[wire(bytes = 2)]
-    // pub(crate) index: u16,
-    // #[wire(bytes = 1)]
-    // pub(crate) sub_index: u8,
-    // #[wire(bytes = 1)]
-    // pub(crate) name_string_idx: u8,
-    // // See page 103 of ETG2000
-    // #[wire(bytes = 1)]
-    // pub(crate) data_type: PrimitiveDataType,
-    #[wire(bytes = 1, pre_skip_bytes = 5, post_skip_bytes = 2)]
+    #[wire(bytes = 2)]
+    pub(crate) index: u16,
+    #[wire(bytes = 1)]
+    pub(crate) sub_index: u8,
+    #[wire(bytes = 1)]
+    pub(crate) name_string_idx: u8,
+    // See page 103 of ETG2000
+    #[wire(bytes = 1)]
+    pub(crate) data_type: PrimitiveDataType,
+    #[wire(bytes = 1)]
     pub(crate) data_length_bits: u8,
-    // #[wire(bytes = 2)]
-    // pub(crate) flags: u16,
+    #[wire(bytes = 2)]
+    pub(crate) flags: u16,
 }
 
 // impl core::fmt::Debug for PdoEntry {

src/pdi.rs

@@ -103,6 +103,13 @@ impl core::fmt::Display for PdiSegment {
 //     }
 // }
 
+/// Type alias for a mapping of PDOs to their ranges in the PDI (relative to the start of SM FMMU)
+#[cfg(feature = "alloc")]
+pub type PdoMapping = indexmap::IndexMap<(u16, u8), (u16, u8), fnv::FnvBuildHasher>;
+
+#[cfg(not(feature = "alloc"))]
+pub type PdoMapping = heapless::LinearMap<(u16, u8), (u16, u8), 64>;
+
 #[cfg(test)]
 mod tests {
     use super::*;

src/subdevice/configuration.rs

@@ -8,7 +8,7 @@ use crate::{
     error::{Error, IgnoreNoCategory, Item},
     fmmu::Fmmu,
     fmt,
-    pdi::{PdiOffset, PdiSegment},
+    pdi::{PdiOffset, PdiSegment, PdoMapping},
     register::RegisterAddress,
     subdevice::types::{Mailbox, MailboxConfig},
     subdevice_state::SubDeviceState,
@@ -138,7 +138,7 @@ where
             has_coe
         );
 
-        let range = if has_coe {
+        let (range, pdos) = if has_coe {
             self.configure_pdos_coe(&sync_managers, &fmmu_usage, direction, &mut global_offset)
                 .await?
         } else {
@@ -152,12 +152,14 @@ where
                     bytes: (range.bytes.start - group_start_address as usize)
                         ..(range.bytes.end - group_start_address as usize),
                 };
+                self.state.config.io.tx_pdos = pdos;
             }
             PdoDirection::MasterWrite => {
                 self.state.config.io.output = PdiSegment {
                     bytes: (range.bytes.start - group_start_address as usize)
                         ..(range.bytes.end - group_start_address as usize),
                 };
+                self.state.config.io.rx_pdos = pdos;
             }
         };
 
@@ -316,7 +318,7 @@ where
         fmmu_usage: &[FmmuUsage],
         direction: PdoDirection,
         global_offset: &mut PdiOffset,
-    ) -> Result<PdiSegment, Error> {
+    ) -> Result<(PdiSegment, PdoMapping), Error> {
         if !self.state.config.mailbox.has_coe {
             fmt::warn!("Invariant: attempting to configure PDOs from COE with no SOE support");
         }
@@ -334,6 +336,7 @@ where
 
         let start_offset = *global_offset;
         // let mut total_bit_len = 0;
+        let mut pdo_mappings = PdoMapping::default();
 
         for (sync_manager_index, (sm_type, sync_manager)) in self
             .state
@@ -415,6 +418,21 @@ where
                         mapping_bit_len,
                     );
 
+                    #[allow(unused_variables)]
+                    let err = pdo_mappings.insert(
+                        (index, sub_index),
+                        (sm_bit_len.div_ceil(8), mapping_bit_len.div_ceil(8)),
+                    );
+
+                    #[cfg(not(feature = "alloc"))]
+                    err.map_err(|_| {
+                        fmt::error!(
+                            "Too many PDO entries for PDO, max {}",
+                            pdo_mappings.capacity()
+                        );
+                        Error::Capacity(Item::PdoEntry)
+                    })?;
+
                     sm_bit_len += u16::from(mapping_bit_len);
                 }
             }
@@ -451,10 +469,13 @@ where
             // total_bit_len += sm_bit_len;
         }
 
-        Ok(PdiSegment {
-            // bit_len: total_bit_len.into(),
-            bytes: start_offset.up_to(*global_offset),
-        })
+        Ok((
+            PdiSegment {
+                // bit_len: total_bit_len.into(),
+                bytes: start_offset.up_to(*global_offset),
+            },
+            pdo_mappings,
+        ))
     }
 
     async fn write_fmmu_config(
@@ -515,7 +536,7 @@ where
         sync_managers: &[SyncManager],
         direction: PdoDirection,
         offset: &mut PdiOffset,
-    ) -> Result<PdiSegment, Error> {
+    ) -> Result<(PdiSegment, PdoMapping), Error> {
         let eeprom = self.eeprom();
 
         let pdos = match direction {
@@ -539,6 +560,7 @@ where
 
         let start_offset = *offset;
         // let mut total_bit_len = 0;
+        let mut pdo_mappings = PdoMapping::default();
 
         let (sm_type, _fmmu_type) = direction.filter_terms();
 
@@ -549,11 +571,30 @@ where
         {
             let sync_manager_index = sync_manager_index as u8;
 
-            let bit_len = pdos
+            let mut bit_len = 0u16;
+            for pdo_entry in pdos
                 .iter()
                 .filter(|pdo| pdo.sync_manager == sync_manager_index)
-                .map(|pdo| pdo.bit_len)
-                .sum();
+                .flat_map(|pdo| pdo.entries.iter())
+            {
+                {
+                    #[allow(unused_variables)]
+                    let err = pdo_mappings.insert(
+                        (pdo_entry.index, pdo_entry.sub_index),
+                        (bit_len.div_ceil(8), pdo_entry.data_length_bits.div_ceil(8)),
+                    );
+
+                    #[cfg(not(feature = "alloc"))]
+                    err.map_err(|_| {
+                        fmt::error!(
+                            "Too many PDO entries for PDO, max {}",
+                            pdo_mappings.capacity()
+                        );
+                        Error::Capacity(Item::PdoEntry)
+                    })?;
+                }
+                bit_len += u16::from(pdo_entry.data_length_bits);
+            }
 
             // total_bit_len += bit_len;
 
@@ -587,10 +628,13 @@ where
             .await?;
         }
 
-        Ok(PdiSegment {
-            // bit_len: total_bit_len.into(),
-            bytes: start_offset.up_to(*offset),
-        })
+        Ok((
+            PdiSegment {
+                // bit_len: total_bit_len.into(),
+                bytes: start_offset.up_to(*offset),
+            },
+            pdo_mappings,
+        ))
     }
 }
 

src/subdevice/eeprom.rs

@@ -339,6 +339,13 @@ where
                 fmt::debug!("--> PDO entry:\n{:#?}", entry);
 
                 pdo.bit_len += u16::from(entry.data_length_bits);
+                pdo.entries.push(entry).map_err(|_| {
+                    fmt::error!(
+                        "Too many PDO entries for PDO, max {}",
+                        pdo.entries.capacity()
+                    );
+                    Error::Capacity(Item::PdoEntry)
+                })?;
             }
 
             pdos.push(pdo).map_err(|_| {
@@ -522,6 +529,7 @@ mod tests {
 
     use super::*;
     use crate::{
+        base_data_types::PrimitiveDataType,
         eeprom::{
             file_provider::EepromFile,
             types::{
@@ -780,15 +788,15 @@ mod tests {
             "../../dumps/eeprom/el2828.hex"
         )));
 
-        fn pdo(_index: u16, _name_string_idx: u8, _entry_idx: u16) -> Pdo {
-            // let entry_defaults = PdoEntry {
-            //     index: 0x7000,
-            //     sub_index: 1,
-            //     name_string_idx: 6,
-            //     data_type: PrimitiveDataType::Bool,
-            //     data_length_bits: 1,
-            //     flags: 0,
-            // };
+        fn pdo(_index: u16, _name_string_idx: u8, entry_idx: u16) -> Pdo {
+            let entry_defaults = PdoEntry {
+                index: 0x7000,
+                sub_index: 1,
+                name_string_idx: 6,
+                data_type: PrimitiveDataType::Bool,
+                data_length_bits: 1,
+                flags: 0,
+            };
 
             let pdo_defaults = Pdo {
                 // index: 0x1600,
@@ -798,11 +806,11 @@ mod tests {
                 // dc_sync: 0,
                 // flags: PdoFlags::PDO_MANDATORY | PdoFlags::PDO_FIXED_CONTENT,
                 bit_len: 1,
-                // entries: heapless::Vec::from_slice(&[PdoEntry {
-                //     index: 0x7000,
-                //     ..entry_defaults
-                // }])
-                // .unwrap(),
+                entries: heapless::Vec::from_slice(&[PdoEntry {
+                    index: entry_idx,
+                    ..entry_defaults
+                }])
+                .unwrap(),
             };
 
             Pdo {

src/subdevice/mod.rs

@@ -398,7 +398,8 @@ impl SubDevice {
         self.dc_support
     }
 
-    pub(crate) fn io_segments(&self) -> &IoRanges {
+    /// Get information about the PDI segments including PDO mappings
+    pub fn io_segments(&self) -> &IoRanges {
         &self.config.io
     }