Add bit-aligned / sub-byte PDO entry support to the emulator

lib/include/kickcat/CoE/OD.h

@@ -215,6 +215,9 @@ namespace kickcat::CoE
         void* data{nullptr};
         bool is_mapped{false};
 
+        // Bit position of the value inside `data`. Nonzero only for entries aliased into a PDO buffer.
+        uint8_t data_bit_offset{0};
+
         /// Called before access
         std::vector<std::function<void(uint16_t access, Entry*)>> before_access;
 
@@ -243,18 +246,27 @@ namespace kickcat::CoE
     {
         object.entries.emplace_back(subindex, bitlen, bitoff, access, type, description);
         auto& alloc = object.entries.back().data;
-        std::size_t size = bitlen / 8;
-        alloc = std::malloc(size);
+        std::size_t alloc_size = (bitlen + 7) / 8;
+        std::size_t copy_size  = bitlen / 8;
+        alloc = std::malloc(alloc_size);
+        std::memset(alloc, 0, alloc_size);
 
         if constexpr(std::is_same_v<const char*, T>)
         {
-            std::memcpy(alloc, data, size);
+            std::memcpy(alloc, data, copy_size);
         }
         else
         {
-            std::memcpy(alloc, &data, size);
+            if (bitlen < 8)
+            {
+                uint8_t mask = static_cast<uint8_t>((1u << bitlen) - 1);
+                *static_cast<uint8_t*>(alloc) = static_cast<uint8_t>(data) & mask;
+            }
+            else
+            {
+                std::memcpy(alloc, &data, copy_size);
+            }
         }
-
     }
 
     inline void addEntry(Object &object, uint8_t subindex, uint16_t bitlen, uint16_t bitoff,
@@ -263,6 +275,14 @@ namespace kickcat::CoE
         object.entries.emplace_back(subindex, bitlen, bitoff, access, type, description);
     }
 
+    void readEntryBits (Entry const* entry, uint8_t* dst, uint32_t dst_bit_offset);
+    void writeEntryBits(Entry*       entry, uint8_t const* src, uint32_t src_bit_offset);
+
+    // LSB-first per byte. RMW: bits in dst outside [dst_bit_offset, +n_bits) are preserved.
+    void copyBits(uint8_t const* src, uint32_t src_bit_offset,
+                  uint8_t* dst,       uint32_t dst_bit_offset,
+                  uint32_t n_bits);
+
     Dictionary createOD();
     Dictionary& dictionary();
 }

lib/slave/include/kickcat/ESC/EmulatedESC.h

@@ -174,10 +174,16 @@ namespace kickcat
             uint32_t logical_address;
             uint8_t* physical_address;
             uint16_t size;
+            uint8_t  logical_start_bit;
+            uint8_t  logical_stop_bit;
+            uint8_t  physical_start_bit;
         };
         std::vector<PDO> rx_pdos_;
         std::vector<PDO> tx_pdos_;
 
+        static bool isByteAligned(PDO const& pdo);
+        static uint32_t totalMappedBits(PDO const& pdo);
+
         void loadEeprom();
 
         void processEcatRequest(DatagramHeader* header, void* data, uint16_t* wkc);
@@ -191,6 +197,7 @@ namespace kickcat
         void processLWR(DatagramHeader* header, void* data, uint16_t* wkc);
         void processLRW(DatagramHeader* header, void* data, uint16_t* wkc);
         uint16_t processPDO(std::vector<PDO> const& pdos, bool read, DatagramHeader* header, void* data);
+        bool processBitAlignedPDO(DatagramHeader const* header, void* data, PDO const& pdo, bool read);
 
         void configureSMs();
         void configurePDOs();

lib/slave/include/kickcat/PDO.h

@@ -30,7 +30,7 @@ namespace kickcat
 
         std::vector<uint16_t> parseAssignment(CoE::Dictionary& dict, uint16_t assign_idx);
 
-        bool parsePdoMap(CoE::Dictionary& dict, uint16_t pdo_idx, void* buffer, uint16_t& bit_offset, uint32_t max_size);
+        bool parsePdoMap(CoE::Dictionary& dict, uint16_t pdo_idx, void* buffer, uint32_t& bit_offset, uint32_t max_size);
 
         AbstractESC* esc_;
         void* input_                = {nullptr};

lib/slave/src/ESC/EmulatedESC.cc

@@ -207,27 +207,115 @@ namespace kickcat
     }
 
 
-    uint16_t EmulatedESC::processPDO(std::vector<PDO> const& pdos, bool read, DatagramHeader* header, void* data)
+    bool EmulatedESC::isByteAligned(PDO const& pdo)
     {
-        int wkc = 0;
-        for (auto const& pdo : pdos)
+        return (pdo.logical_start_bit  == 0)
+           and (pdo.logical_stop_bit   == 7)
+           and (pdo.physical_start_bit == 0);
+    }
+
+
+    uint32_t EmulatedESC::totalMappedBits(PDO const& pdo)
+    {
+        // ETG1000.4: length * 8 - logical_start_bit - (7 - logical_stop_bit).
+        // Signed: malformed FMMU clamps to 0 instead of unsigned-wrapping to ~4B.
+        int64_t bits = int64_t(pdo.size) * 8
+                     + int64_t(pdo.logical_stop_bit)
+                     - int64_t(pdo.logical_start_bit)
+                     - 7;
+        if (bits <= 0)
+        {
+            return 0;
+        }
+        return static_cast<uint32_t>(bits);
+    }
+
+
+    bool EmulatedESC::processBitAlignedPDO(DatagramHeader const* header, void* data, PDO const& pdo, bool read)
+    {
+        uint32_t total_bits = totalMappedBits(pdo);
+        uint32_t frame_start = header->address;
+        uint32_t frame_end   = header->address + header->len;
+
+        uint32_t pdo_logical_start = pdo.logical_address;
+        uint32_t pdo_logical_end   = pdo.logical_address + pdo.size;
+        if ((frame_end <= pdo_logical_start) or (frame_start >= pdo_logical_end))
+        {
+            return false;
+        }
+
+        bool touched = false;
+        for (uint32_t bit = 0; bit < total_bits; ++bit)
         {
-            auto[frame, internal, to_copy] = computeLogicalIntersection(header, data, pdo);
-            if (to_copy == 0)
+            uint32_t logical_bit  = bit + pdo.logical_start_bit;
+            uint32_t physical_bit = bit + pdo.physical_start_bit;
+
+            uint32_t logical_addr  = pdo.logical_address + logical_bit / 8;
+            uint8_t  logical_bpos  = logical_bit % 8;
+            uint32_t physical_off  = physical_bit / 8;
+            uint8_t  physical_bpos = physical_bit % 8;
+
+            if ((logical_addr < frame_start) or (logical_addr >= frame_end))
             {
                 continue;
             }
+            touched = true;
+
+            uint8_t* frame_byte = static_cast<uint8_t*>(data) + (logical_addr - frame_start);
+            uint8_t* phys_byte  = pdo.physical_address + physical_off;
 
             if (read)
             {
-                std::memcpy(frame, internal, to_copy);
+                uint8_t value = (*phys_byte >> physical_bpos) & 0x1;
+                *frame_byte = static_cast<uint8_t>((*frame_byte & ~(1u << logical_bpos))
+                                                 | (value << logical_bpos));
             }
             else
             {
-                std::memcpy(internal, frame, to_copy);
-                lastLogicalWrite_ = since_epoch();   // update watchdog
+                uint8_t value = (*frame_byte >> logical_bpos) & 0x1;
+                *phys_byte = static_cast<uint8_t>((*phys_byte & ~(1u << physical_bpos))
+                                                | (value << physical_bpos));
+            }
+        }
+        return touched;
+    }
+
+
+    uint16_t EmulatedESC::processPDO(std::vector<PDO> const& pdos, bool read, DatagramHeader* header, void* data)
+    {
+        int wkc = 0;
+        for (auto const& pdo : pdos)
+        {
+            if (isByteAligned(pdo))
+            {
+                auto[frame, internal, to_copy] = computeLogicalIntersection(header, data, pdo);
+                if (to_copy == 0)
+                {
+                    continue;
+                }
+
+                if (read)
+                {
+                    std::memcpy(frame, internal, to_copy);
+                }
+                else
+                {
+                    std::memcpy(internal, frame, to_copy);
+                    lastLogicalWrite_ = since_epoch();   // update watchdog
+                }
+                ++wkc;
+            }
+            else
+            {
+                if (processBitAlignedPDO(header, data, pdo, read))
+                {
+                    if (not read)
+                    {
+                        lastLogicalWrite_ = since_epoch();   // update watchdog
+                    }
+                    ++wkc;
+                }
             }
-            ++wkc;
         }
         return wkc;
     }
@@ -535,8 +623,11 @@ namespace kickcat
 
             PDO pdo;
             pdo.size = fmmu.length;
-            pdo.logical_address = fmmu.logical_address;
-            pdo.physical_address = memory_.process_data_ram + (fmmu.physical_address - 0x1000);
+            pdo.logical_address    = fmmu.logical_address;
+            pdo.physical_address   = reinterpret_cast<uint8_t*>(&memory_) + fmmu.physical_address;
+            pdo.logical_start_bit  = fmmu.logical_start_bit;
+            pdo.logical_stop_bit   = fmmu.logical_stop_bit;
+            pdo.physical_start_bit = fmmu.physical_start_bit;
 
             if (fmmu.type == 1)
             {

lib/slave/src/PDO.cc

@@ -117,14 +117,14 @@ namespace kickcat
         std::vector<uint16_t> pdo_indices;
 
         auto [obj0, entry0] = CoE::findObject(dict, assign_idx, 0);
-        if (entry0)
+        if (entry0 and entry0->data)
         {
             uint8_t count = *static_cast<uint8_t*>(entry0->data);
 
             for (uint8_t i = 1; i <= count; ++i)
             {
                 auto [obj, entry] = CoE::findObject(dict, assign_idx, i);
-                if (entry)
+                if (entry and entry->data)
                 {
                     pdo_indices.push_back(*static_cast<uint16_t*>(entry->data));
                 }
@@ -134,10 +134,10 @@ namespace kickcat
         return pdo_indices;
     }
 
-    bool PDO::parsePdoMap(CoE::Dictionary& dict, uint16_t pdo_idx, void* buffer, uint16_t& bit_offset, uint32_t max_size)
+    bool PDO::parsePdoMap(CoE::Dictionary& dict, uint16_t pdo_idx, void* buffer, uint32_t& bit_offset, uint32_t max_size)
     {
         auto [obj0, entry0] = CoE::findObject(dict, pdo_idx, 0);
-        if (not entry0)
+        if (not entry0 or entry0->data == nullptr)
         {
             return false;
         }
@@ -147,7 +147,7 @@ namespace kickcat
         for (uint8_t i = 1; i <= count; ++i)
         {
             auto [obj, entry] = CoE::findObject(dict, pdo_idx, i);
-            if (not entry)
+            if (not entry or entry->data == nullptr)
             {
                 return false;
             }
@@ -164,6 +164,13 @@ namespace kickcat
                 return false;
             }
 
+            // ETG1000.6 PDO mapping: Index=0 is a padding gap, not an OD entry.
+            if (index == 0)
+            {
+                bit_offset += bits;
+                continue;
+            }
+
             auto [od_obj, od_entry] = CoE::findObject(dict, index, sub);
             if (not od_entry)
             {
@@ -173,15 +180,18 @@ namespace kickcat
             // Aliasing logic
             void* old_data = od_entry->data;
             bool old_is_mapped = od_entry->is_mapped;
+            uint8_t old_data_bit_offset = od_entry->data_bit_offset;
 
             uint8_t* new_ptr = static_cast<uint8_t*>(buffer) + (bit_offset / 8);
 
-            od_entry->data = new_ptr;
-            od_entry->is_mapped = true; // data has been remapped/aliased
+            od_entry->data            = new_ptr;
+            od_entry->data_bit_offset = static_cast<uint8_t>(bit_offset % 8);
+            od_entry->is_mapped       = true; // data has been remapped/aliased
 
             if (old_data)
             {
-                std::memcpy(new_ptr, old_data, bits / 8);
+                CoE::copyBits(static_cast<uint8_t const*>(old_data), old_data_bit_offset,
+                              new_ptr, od_entry->data_bit_offset, bits);
 
                 if (not old_is_mapped) // if the old data was not mapped, we allocated it, so free it
                 {
@@ -198,7 +208,7 @@ namespace kickcat
     StatusCode PDO::configureMapping(CoE::Dictionary& dict)
     {
         {
-            uint16_t bit_offset = 0;
+            uint32_t bit_offset = 0;
             std::vector<uint16_t> pdo_indices = parseAssignment(dict, 0x1C13);
 
             for (auto pdo : pdo_indices)
@@ -211,7 +221,7 @@ namespace kickcat
         }
 
         {
-            uint16_t bit_offset = 0;
+            uint32_t bit_offset = 0;
             std::vector<uint16_t> pdo_indices = parseAssignment(dict, 0x1C12);
 
             for (auto pdo : pdo_indices)