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

lib/include/kickcat/CoE/OD.h

@@ -204,7 +204,7 @@ namespace kickcat::CoE
 
         uint8_t      subindex;
         uint16_t     bitlen;    // For PDO, shall be < 11888
-        uint16_t     bitoff;    // offset in bit
+        uint16_t     bitoff;    // offset in bit, inside the OD object (used by SDO complete access for payload layout)
         uint16_t     access{0};
         DataType     type;
         // default value
@@ -215,6 +215,14 @@ namespace kickcat::CoE
         void* data{nullptr};
         bool is_mapped{false};
 
+        // Bit position within the byte at `data` where the entry's value starts.
+        // - When the entry owns its storage (is_mapped == false), always 0:
+        //   the value lives in bits [0..bitlen-1] of the allocated buffer (LSB-first).
+        // - When the entry is aliased to a PDO image byte (is_mapped == true), set
+        //   by PDO::parsePdoMap to bit_offset % 8 in the PDO buffer; the value lives
+        //   at [data_bit_offset .. data_bit_offset + bitlen - 1] starting at `data`.
+        uint8_t data_bit_offset{0};
+
         /// Called before access
         std::vector<std::function<void(uint16_t access, Entry*)>> before_access;
 
@@ -243,18 +251,28 @@ 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);
+            // Sub-byte entries (bitlen < 8) take the low `bitlen` bits of the source value.
+            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 +281,23 @@ namespace kickcat::CoE
         object.entries.emplace_back(subindex, bitlen, bitoff, access, type, description);
     }
 
+    // Read entry->bitlen bits, starting at entry->data + entry->data_bit_offset, and
+    // place them in dst at dst_bit_offset (LSB-first). Bits in dst outside the written
+    // range are preserved (read-modify-write).
+    void readEntryBits (Entry const* entry, uint8_t* dst, uint32_t dst_bit_offset);
+
+    // Write entry->bitlen bits from src + src_bit_offset (LSB-first) into entry storage,
+    // starting at entry->data + entry->data_bit_offset. Bits in entry storage outside
+    // the written range are preserved (read-modify-write).
+    void writeEntryBits(Entry*       entry, uint8_t const* src, uint32_t src_bit_offset);
+
+    // Generic bit-buffer copy used internally by the helpers above. Copies n_bits bits
+    // from src (starting at src_bit_offset) into dst (starting at dst_bit_offset),
+    // LSB-first within each byte. Surrounding bits in dst 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

@@ -6,6 +6,12 @@
 
 namespace kickcat
 {
+    // EmulatedESC is NOT thread-safe. processDatagram (called from the
+    // network-facing side) and the PDI read/write methods (called from the
+    // application/slave side) share memory_ without any synchronization.
+    // Callers must drive both from the same thread, or arrange external
+    // mutual exclusion. A real ESC chip provides this via the SyncManager
+    // buffering hardware; the emulator does not model that mechanism.
     class EmulatedESC final : public AbstractESC
     {
         // ESC access type
@@ -173,11 +179,17 @@ namespace kickcat
         {
             uint32_t logical_address;
             uint8_t* physical_address;
-            uint16_t size;
+            uint16_t size;                  // Byte span in both logical and physical address range
+            uint8_t  logical_start_bit;     // First mapped bit in the first logical byte (0-7)
+            uint8_t  logical_stop_bit;      // Last mapped bit in the last logical byte (0-7)
+            uint8_t  physical_start_bit;    // First mapped bit in the first physical byte (0-7)
         };
         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 +203,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,120 @@ 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: total bits = length * 8 - logical_start_bit - (7 - logical_stop_bit)
+        // Compute in signed arithmetic so a malformed FMMU (e.g. stop < start - 7 + 8*size)
+        // is clamped to 0 rather than wrapping to ~4 billion and stalling the per-bit loop.
+        int64_t bits = int64_t(pdo.size) * 8
+                     + int64_t(pdo.logical_stop_bit)
+                     - int64_t(pdo.logical_start_bit)
+                     - 7;
+        if (bits <= 0)
         {
-            auto[frame, internal, to_copy] = computeLogicalIntersection(header, data, pdo);
-            if (to_copy == 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;
+
+        // Early-out: skip the per-bit loop entirely when the frame and the PDO's
+        // logical byte range cannot overlap. The byte-aligned path gets this for
+        // free via computeLogicalIntersection; the bit path needs an explicit check
+        // so a non-matching FMMU doesn't iterate every bit just to `continue`.
+        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)
+        {
+            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
+            {
+                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
             {
-                std::memcpy(internal, frame, to_copy);
-                lastLogicalWrite_ = since_epoch();   // update watchdog
+                if (processBitAlignedPDO(header, data, pdo, read))
+                {
+                    if (not read)
+                    {
+                        lastLogicalWrite_ = since_epoch();   // update watchdog
+                    }
+                    ++wkc;
+                }
             }
-            ++wkc;
         }
         return wkc;
     }
@@ -535,8 +628,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

@@ -134,7 +134,7 @@ 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)
@@ -173,15 +173,21 @@ 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);
+                // Migrate previous content into the aliased bit slot. Use the
+                // generic bit-copy so sub-byte entries land at the correct bit
+                // position; for byte-aligned entries this collapses to memcpy.
+                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 +204,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 +217,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)

lib/src/CoE/EsiParser.cc

@@ -196,7 +196,7 @@ namespace kickcat::CoE
                 data = loadHexBinary(node_default_data);
             }
 
-            if (data.size() != (entry.bitlen / 8))
+            if (data.size() != static_cast<std::size_t>((entry.bitlen + 7) / 8))
             {
                 esi_warning("Cannot load default data for 0x%04x.%d, expected size mismatch.\n"
                         "-> Got %ld bits, expected: %d bit\n"
@@ -205,7 +205,7 @@ namespace kickcat::CoE
                     data.size() * 8, entry.bitlen);
                 return;
             }
-            entry.data = malloc(entry.bitlen / 8);
+            entry.data = malloc((entry.bitlen + 7) / 8);
             std::memcpy(entry.data, data.data(), data.size());
             return;
         }
@@ -225,9 +225,21 @@ namespace kickcat::CoE
                 value = std::stoll(text, nullptr, 10);
             }
 
-            uint32_t size = entry.bitlen / 8;
-            entry.data = malloc(size);
-            std::memcpy(entry.data, &value, size);
+            uint32_t alloc_size = (entry.bitlen + 7) / 8;
+            uint32_t copy_size  = entry.bitlen / 8;
+            entry.data = malloc(alloc_size);
+            std::memset(entry.data, 0, alloc_size);
+
+            if (entry.bitlen < 8)
+            {
+                // Sub-byte entry: pack the low `bitlen` bits of the parsed value.
+                uint8_t mask = static_cast<uint8_t>((1u << entry.bitlen) - 1);
+                *static_cast<uint8_t*>(entry.data) = static_cast<uint8_t>(value) & mask;
+            }
+            else
+            {
+                std::memcpy(entry.data, &value, copy_size);
+            }
         }
     }