From 211c4484d97117c7da6d06487c1adb5515ad8420 Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Wed, 15 Apr 2026 11:30:44 +0200 Subject: [PATCH 1/2] Add bit-aligned / sub-byte PDO entry support to the emulator MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Lets the emulator host slaves that declare sub-byte PDO entries (e.g. 4 BOOL GPIOs at 1): - EmulatedESC honors FMMU logical/physical bit offsets via a per-bit slow path; byte-aligned mappings keep the memcpy fast path. FMMUs targeting addresses < 0x1000 (e.g. mailbox-status) now work. - CoE::Entry gains data_bit_offset; addEntry/EsiParser allocate (bitlen+7)/8 bytes (was 0 for BOOL/BIT2..7). New copyBits / read/writeEntryBits helpers. - SDO upload/download/complete-access switched to bit-accurate payload positioning. Sub-byte entries travel as 1 octet per ETG1000.5 §5.3.1; CA packs entries by bitoff per ETG2000. --- lib/include/kickcat/CoE/OD.h | 30 ++- lib/slave/include/kickcat/ESC/EmulatedESC.h | 7 + lib/slave/include/kickcat/PDO.h | 2 +- lib/slave/src/ESC/EmulatedESC.cc | 113 ++++++++- lib/slave/src/PDO.cc | 22 +- lib/src/CoE/EsiParser.cc | 21 +- lib/src/CoE/OD.cc | 64 ++++- lib/src/CoE/mailbox/response.cc | 111 +++++++-- unit/src/CoE/EsiParser-t.cc | 8 +- unit/src/CoE/OD-t.cc | 102 ++++++++ unit/src/EmulatedESC-t.cc | 203 ++++++++++++++++ unit/src/mailbox/CoE/response-t.cc | 256 ++++++++++++++++++++ unit/src/slave/PDO-t.cc | 127 ++++++++++ 13 files changed, 1001 insertions(+), 65 deletions(-) diff --git a/lib/include/kickcat/CoE/OD.h b/lib/include/kickcat/CoE/OD.h index 18d89ba8..def4b39a 100644 --- a/lib/include/kickcat/CoE/OD.h +++ b/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> 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) { - 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((1u << bitlen) - 1); + *static_cast(alloc) = static_cast(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(); } diff --git a/lib/slave/include/kickcat/ESC/EmulatedESC.h b/lib/slave/include/kickcat/ESC/EmulatedESC.h index d3afe6d1..80729656 100644 --- a/lib/slave/include/kickcat/ESC/EmulatedESC.h +++ b/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 rx_pdos_; std::vector 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 const& pdos, bool read, DatagramHeader* header, void* data); + bool processBitAlignedPDO(DatagramHeader const* header, void* data, PDO const& pdo, bool read); void configureSMs(); void configurePDOs(); diff --git a/lib/slave/include/kickcat/PDO.h b/lib/slave/include/kickcat/PDO.h index 876a1763..4f35d521 100644 --- a/lib/slave/include/kickcat/PDO.h +++ b/lib/slave/include/kickcat/PDO.h @@ -30,7 +30,7 @@ namespace kickcat std::vector 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}; diff --git a/lib/slave/src/ESC/EmulatedESC.cc b/lib/slave/src/ESC/EmulatedESC.cc index e91983d7..ed1dfc1a 100644 --- a/lib/slave/src/ESC/EmulatedESC.cc +++ b/lib/slave/src/ESC/EmulatedESC.cc @@ -207,27 +207,115 @@ namespace kickcat } - uint16_t EmulatedESC::processPDO(std::vector 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(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(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((*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((*phys_byte & ~(1u << physical_bpos)) + | (value << physical_bpos)); + } + } + return touched; + } + + + uint16_t EmulatedESC::processPDO(std::vector 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(&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) { diff --git a/lib/slave/src/PDO.cc b/lib/slave/src/PDO.cc index f288654e..439b7fea 100644 --- a/lib/slave/src/PDO.cc +++ b/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) @@ -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(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(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(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 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 pdo_indices = parseAssignment(dict, 0x1C12); for (auto pdo : pdo_indices) diff --git a/lib/src/CoE/EsiParser.cc b/lib/src/CoE/EsiParser.cc index 04e062b3..500a43dc 100644 --- a/lib/src/CoE/EsiParser.cc +++ b/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((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,20 @@ 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) + { + uint8_t mask = static_cast((1u << entry.bitlen) - 1); + *static_cast(entry.data) = static_cast(value) & mask; + } + else + { + std::memcpy(entry.data, &value, copy_size); + } } } diff --git a/lib/src/CoE/OD.cc b/lib/src/CoE/OD.cc index a141850f..48660fa6 100644 --- a/lib/src/CoE/OD.cc +++ b/lib/src/CoE/OD.cc @@ -266,23 +266,65 @@ namespace kickcat::CoE std::free(data); } - subindex = other.subindex; - bitlen = other.bitlen; - bitoff = other.bitoff; - access = other.access; - type = other.type; - description = std::move(other.description); - data = other.data; - is_mapped = other.is_mapped; - - other.data = nullptr; - other.is_mapped = false; + subindex = other.subindex; + bitlen = other.bitlen; + bitoff = other.bitoff; + access = other.access; + type = other.type; + description = std::move(other.description); + data = other.data; + is_mapped = other.is_mapped; + data_bit_offset = other.data_bit_offset; + + other.data = nullptr; + other.is_mapped = false; + other.data_bit_offset = 0; } return *this; } + void copyBits(uint8_t const* src, uint32_t src_bit_offset, + uint8_t* dst, uint32_t dst_bit_offset, + uint32_t n_bits) + { + if (n_bits == 0) + { + return; + } + + if ((src_bit_offset % 8 == 0) and (dst_bit_offset % 8 == 0) and (n_bits % 8 == 0)) + { + std::memcpy(dst + dst_bit_offset / 8, src + src_bit_offset / 8, n_bits / 8); + return; + } + + for (uint32_t i = 0; i < n_bits; ++i) + { + uint32_t s = src_bit_offset + i; + uint32_t d = dst_bit_offset + i; + uint8_t bit = static_cast((src[s / 8] >> (s % 8)) & 0x1); + uint8_t mask = static_cast(1u << (d % 8)); + dst[d / 8] = static_cast((dst[d / 8] & ~mask) | (bit << (d % 8))); + } + } + + + void readEntryBits(Entry const* entry, uint8_t* dst, uint32_t dst_bit_offset) + { + copyBits(static_cast(entry->data), entry->data_bit_offset, + dst, dst_bit_offset, entry->bitlen); + } + + + void writeEntryBits(Entry* entry, uint8_t const* src, uint32_t src_bit_offset) + { + copyBits(src, src_bit_offset, + static_cast(entry->data), entry->data_bit_offset, entry->bitlen); + } + + std::tuple findObject(Dictionary& dict, uint16_t index, uint8_t subindex) { auto object_it = std::find_if(dict.begin(), dict.end(), [index](Object const& object) diff --git a/lib/src/CoE/mailbox/response.cc b/lib/src/CoE/mailbox/response.cc index c1021cd0..63947073 100644 --- a/lib/src/CoE/mailbox/response.cc +++ b/lib/src/CoE/mailbox/response.cc @@ -136,7 +136,8 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::READ, entry); - uint32_t size = entry->bitlen / 8; + // ETG1000.5: sub-byte types travel as 1 octet with unused high bits = 0. + uint32_t size = (entry->bitlen + 7) / 8; header_->len = sizeof(mailbox::Header) + sizeof(CoE::ServiceData); sdo_->size_indicator = 1; // always 1 on upload response @@ -154,7 +155,16 @@ namespace kickcat::mailbox::response header_->len += size; } - std::memcpy(payload_, entry->data, size); + if ((entry->bitlen % 8 == 0) and (entry->data_bit_offset == 0)) + { + std::memcpy(payload_, entry->data, size); + } + else + { + std::memset(payload_, 0, size); + CoE::readEntryBits(entry, payload_, 0); + } + coe_->service = CoE::Service::SDO_RESPONSE; sdo_->command = CoE::SDO::response::UPLOAD; reply(std::move(data_)); @@ -169,9 +179,43 @@ namespace kickcat::mailbox::response sdo_->size_indicator = 1; // always 1 on upload response sdo_->transfer_type = 0; // complete access -> not expedited - uint32_t size = 0; - uint8_t number_of_entries = *(uint8_t*)object->entries.at(0).data; - uint16_t skip_offset = object->entries.at(sdo_->subindex).bitoff / 8; + // SI 0 may have no default data if the ESI omits . + if (object->entries.at(0).data == nullptr) + { + abort(CoE::SDO::abort::NO_DATA_AVAILABLE); + return ProcessingResult::FINALIZE; + } + + uint32_t number_of_entries = *static_cast(object->entries.at(0).data); + if (number_of_entries >= object->entries.size()) + { + number_of_entries = static_cast(object->entries.size() - 1); + } + + uint32_t skip_bit_offset = object->entries.at(sdo_->subindex).bitoff; + uint32_t end_bit_offset = 0; + + // No entries past sdo_->subindex: reply empty, skip the pre-zero underflow. + if (number_of_entries < sdo_->subindex) + { + std::memcpy(payload_, &end_bit_offset, 4); + header_->len = sizeof(mailbox::Header) + sizeof(CoE::ServiceData); + coe_->service = CoE::Service::SDO_RESPONSE; + sdo_->command = CoE::SDO::response::UPLOAD; + reply(std::move(data_)); + return ProcessingResult::FINALIZE; + } + + // ETG1000.6: padding bits must be 0. + { + auto const& last = object->entries.at(number_of_entries); + uint32_t last_end = uint32_t(last.bitoff) + last.bitlen; + if (last_end > skip_bit_offset) + { + uint32_t total_bits = last_end - skip_bit_offset; + std::memset(payload_ + 4, 0, (total_bits + 7) / 8); + } + } for (uint32_t i = sdo_->subindex; i <= number_of_entries; ++i) { @@ -190,14 +234,14 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::READ, entry); - uint16_t entry_size = entry->bitlen / 8; - uint16_t entry_off = entry->bitoff / 8 - skip_offset; - std::memcpy(payload_ + 4 + entry_off, entry->data, entry_size); - size = entry_size + entry_off; // only record the last position + last entry size + uint32_t wire_bit_offset = entry->bitoff - skip_bit_offset; + CoE::readEntryBits(entry, payload_ + 4, wire_bit_offset); + end_bit_offset = wire_bit_offset + entry->bitlen; afterHooks(CoE::Access::READ, entry); } + uint32_t size = (end_bit_offset + 7) / 8; std::memcpy(payload_, &size, 4); header_->len = sizeof(mailbox::Header) + sizeof(CoE::ServiceData) + size; @@ -228,13 +272,20 @@ namespace kickcat::mailbox::response payload_ += 4; } - if (size != (entry->bitlen / 8)) + if (size != static_cast((entry->bitlen + 7) / 8)) { abort(CoE::SDO::abort::DATA_TYPE_LENGTH_MISMATCH); return ProcessingResult::FINALIZE; } - std::memcpy(entry->data, payload_, size); + if ((entry->bitlen % 8 == 0) and (entry->data_bit_offset == 0)) + { + std::memcpy(entry->data, payload_, size); + } + else + { + CoE::writeEntryBits(entry, payload_, 0); + } coe_->service = CoE::Service::SDO_RESPONSE; sdo_->command = CoE::SDO::response::DOWNLOAD; @@ -250,23 +301,26 @@ namespace kickcat::mailbox::response uint32_t msg_size; std::memcpy(&msg_size, payload_, 4); - uint8_t* start_offset = payload_ + 4; - uint8_t* current_offset = start_offset; - uint16_t skip_offset = object->entries.at(sdo_->subindex).bitoff / 8; + uint8_t const* start_offset = payload_ + 4; + + uint32_t skip_bit_offset = object->entries.at(sdo_->subindex).bitoff; if (sdo_->subindex == 0) { auto* entry = &object->entries.at(0); - beforeHooks(CoE::Access::WRITE, entry); - - std::memcpy(entry->data, payload_ + 4, 1); - current_offset += 1; + if (entry->data == nullptr) + { + abort(CoE::SDO::abort::NO_DATA_AVAILABLE); + return ProcessingResult::FINALIZE; + } + beforeHooks(CoE::Access::WRITE, entry); + std::memcpy(entry->data, start_offset, 1); afterHooks(CoE::Access::WRITE, entry); } uint16_t subindex = 1; - while ((current_offset - start_offset) < msg_size) + while (true) { if (subindex >= object->entries.size()) { @@ -275,6 +329,18 @@ namespace kickcat::mailbox::response } auto* entry = &object->entries.at(subindex); + // Guard unsigned subtraction; mirrors uploadComplete. + if (entry->bitoff < skip_bit_offset) + { + break; + } + uint32_t wire_bit_offset = entry->bitoff - skip_bit_offset; + uint32_t entry_end_bit = wire_bit_offset + entry->bitlen; + if (((entry_end_bit + 7) / 8) > msg_size) + { + break; + } + if (not isDownloadAuthorized(entry)) { abort(CoE::SDO::abort::WRITE_READ_ONLY_ACCESS); @@ -283,12 +349,7 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::WRITE, entry); - uint32_t entry_size = entry->bitlen / 8; - uint32_t entry_off = entry->bitoff / 8; - current_offset = start_offset + entry_off - skip_offset; - - std::memcpy(entry->data, current_offset, entry_size); - current_offset += entry_size; + CoE::writeEntryBits(entry, start_offset, wire_bit_offset); subindex++; afterHooks(CoE::Access::WRITE, entry); diff --git a/unit/src/CoE/EsiParser-t.cc b/unit/src/CoE/EsiParser-t.cc index e2df4229..6b7b237c 100644 --- a/unit/src/CoE/EsiParser-t.cc +++ b/unit/src/CoE/EsiParser-t.cc @@ -323,6 +323,8 @@ TEST(EsiParser, load_basic_with_bit_types_and_no_info) ASSERT_EQ(e2->type, CoE::DataType::BOOLEAN); ASSERT_EQ(e2->bitlen, 1); ASSERT_EQ(e2->bitoff, 32); + ASSERT_EQ(e2->data_bit_offset, 0); + ASSERT_NE(e2->data, nullptr); // SubIndex 3: BIT6 (Padding) auto [obj3, e3] = findObject(dictionary, 0x6000, 3); @@ -330,13 +332,17 @@ TEST(EsiParser, load_basic_with_bit_types_and_no_info) ASSERT_EQ(e3->type, CoE::DataType::BIT6); ASSERT_EQ(e3->bitlen, 6); ASSERT_EQ(e3->bitoff, 33); + ASSERT_EQ(e3->data_bit_offset, 0); + ASSERT_NE(e3->data, nullptr); - // SubIndex 4: BOOL (StatusBit) + // SubIndex 4: BOOL (StatusBit). Fixture declares DefaultData for only + // 4 SubItems, matched positionally to entries[0..3]; SI 4 stays null. auto [obj4, e4] = findObject(dictionary, 0x6000, 4); ASSERT_NE(e4, nullptr); ASSERT_EQ(e4->type, CoE::DataType::BOOLEAN); ASSERT_EQ(e4->bitlen, 1); ASSERT_EQ(e4->bitoff, 39); + ASSERT_EQ(e4->data_bit_offset, 0); } // 0x7010 - RECORD with BIT6 SubItem (outputs) diff --git a/unit/src/CoE/OD-t.cc b/unit/src/CoE/OD-t.cc index 4cb9fce4..86116a11 100644 --- a/unit/src/CoE/OD-t.cc +++ b/unit/src/CoE/OD-t.cc @@ -164,6 +164,108 @@ TEST(OD, entry_data_to_string) } } +TEST(OD, addEntry_sub_byte_allocates_one_byte) +{ + Object object{0x6000, ObjectCode::VAR, "GPIOs", {}}; + + addEntry(object, 1, 1, 0, Access::READ, DataType::BOOLEAN, "GPIO 1", uint8_t{1}); + addEntry(object, 2, 1, 1, Access::READ, DataType::BOOLEAN, "GPIO 2", uint8_t{0}); + addEntry(object, 3, 1, 2, Access::READ, DataType::BOOLEAN, "GPIO 3", uint8_t{1}); + addEntry(object, 4, 6, 3, Access::READ, DataType::BIT6, "BIT6", uint8_t{0x2A}); + + ASSERT_EQ(object.entries.size(), 4); + for (auto const& entry : object.entries) + { + ASSERT_NE(entry.data, nullptr); + ASSERT_EQ(entry.data_bit_offset, 0); + } + + EXPECT_EQ(*static_cast(object.entries[0].data), 0x01); + EXPECT_EQ(*static_cast(object.entries[1].data), 0x00); + EXPECT_EQ(*static_cast(object.entries[2].data), 0x01); + EXPECT_EQ(*static_cast(object.entries[3].data), 0x2A); +} + +TEST(OD, copyBits_byte_aligned_is_memcpy) +{ + uint8_t src[4] = {0x11, 0x22, 0x33, 0x44}; + uint8_t dst[4] = {0xAA, 0xBB, 0xCC, 0xDD}; + + copyBits(src, 0, dst, 0, 32); + + EXPECT_EQ(dst[0], 0x11); + EXPECT_EQ(dst[1], 0x22); + EXPECT_EQ(dst[2], 0x33); + EXPECT_EQ(dst[3], 0x44); +} + +TEST(OD, copyBits_single_bit_preserves_neighbours) +{ + uint8_t src = 0x01; + uint8_t dst = 0xA5; + + copyBits(&src, 0, &dst, 5, 1); + EXPECT_EQ(dst & (1 << 5), (1 << 5)); + EXPECT_EQ(dst & ~uint8_t(1 << 5), 0xA5 & ~uint8_t(1 << 5)); + + src = 0x00; + dst = 0xFF; + copyBits(&src, 0, &dst, 3, 1); + EXPECT_EQ(dst & (1 << 3), 0); + EXPECT_EQ(dst | uint8_t(1 << 3), 0xFF); +} + +TEST(OD, copyBits_cross_byte) +{ + // 4 bits src[bit 6..9] → dst[bit 5..8] + uint8_t src[2] = {0xC0, 0x03}; + uint8_t dst[2] = {0x00, 0x00}; + + copyBits(src, 6, dst, 5, 4); + EXPECT_EQ(dst[0] & 0xE0, 0xE0); + EXPECT_EQ(dst[0] & 0x1F, 0x00); + EXPECT_EQ(dst[1] & 0x01, 0x01); + EXPECT_EQ(dst[1] & 0xFE, 0x00); +} + +TEST(OD, copyBits_spans_three_source_bytes) +{ + // 16 bits src[bit 6..21] → dst[bit 0..15]; src spans 3 bytes, dst 2. LSB-first. + uint8_t src[3] = {0xC0, 0xAA, 0x03}; + uint8_t dst[2] = {0xFF, 0xFF}; + + copyBits(src, 6, dst, 0, 16); + + EXPECT_EQ(dst[0], 0xAB); + EXPECT_EQ(dst[1], 0x0E); +} + +TEST(OD, readEntryBits_writeEntryBits_roundtrip) +{ + // BOOL aliased at bit 3 of a buffer byte. + uint8_t aliased_buffer = 0xA5; + Entry entry; + entry.bitlen = 1; + entry.bitoff = 0; + entry.type = DataType::BOOLEAN; + entry.data = &aliased_buffer; + entry.is_mapped = true; + entry.data_bit_offset = 3; + + uint8_t wire = 0x00; + readEntryBits(&entry, &wire, 0); + EXPECT_EQ(wire & 0x01, (0xA5 >> 3) & 0x01); + + wire = 0x01; + writeEntryBits(&entry, &wire, 0); + EXPECT_EQ(aliased_buffer & (1 << 3), (1 << 3)); + EXPECT_EQ(aliased_buffer & ~uint8_t(1 << 3), 0xA5 & ~uint8_t(1 << 3)); + + // Defuse alias: Entry destructor would free our stack byte otherwise. + entry.data = nullptr; + entry.is_mapped = false; +} + TEST(OD, print_object_and_entries) { CoE::Object object diff --git a/unit/src/EmulatedESC-t.cc b/unit/src/EmulatedESC-t.cc index 2227ef74..c6c8e6ab 100644 --- a/unit/src/EmulatedESC-t.cc +++ b/unit/src/EmulatedESC-t.cc @@ -284,6 +284,209 @@ TEST(EmulatedESC, ecat_PDOs) ASSERT_EQ(read_test, payload); } +TEST(EmulatedESC, ecat_PDOs_bit_aligned_single_bit) +{ + // Physical bit 3 of byte 0x300A → logical bit 5 of byte 0x2003 + // (mailbox-status-check pattern). + EmulatedESC esc; + + uint8_t current = State::PRE_OP; + esc.write(reg::AL_STATUS, ¤t, 1); + + uint8_t next = State::SAFE_OP; + esc.write(reg::AL_CONTROL, &next, 1); + + FMMU fmmu; + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 1; + fmmu.logical_address = 0x2003; + fmmu.length = 1; + fmmu.logical_start_bit = 5; + fmmu.logical_stop_bit = 5; + fmmu.physical_address = 0x300A; + fmmu.physical_start_bit = 3; + fmmu.activate = 1; + esc.write(reg::FMMU + 0x00, &fmmu, sizeof(FMMU)); + + // PRE_OP → SAFE_OP triggers configurePDOs. + DatagramHeader header{Command::BRD, 0, 0, sizeof(uint64_t), 0, 0, 0, 0}; + uint64_t scratch = 0; + uint16_t wkc = 0; + esc.processDatagram(&header, &scratch, &wkc); + + uint8_t phys = (1 << 3); + esc.write(0x300A, &phys, 1); + + uint8_t frame = 0xAA; + header.command = Command::LRD; + header.address = 0x2003; + header.len = 1; + wkc = 0; + esc.processDatagram(&header, &frame, &wkc); + ASSERT_EQ(wkc, 1); + EXPECT_EQ(frame & (1 << 5), (1 << 5)); + EXPECT_EQ(frame & ~uint8_t(1 << 5), 0xAA & ~uint8_t(1 << 5)); + + phys = 0; + esc.write(0x300A, &phys, 1); + frame = 0xFF; + wkc = 0; + esc.processDatagram(&header, &frame, &wkc); + ASSERT_EQ(wkc, 1); + EXPECT_EQ(frame & (1 << 5), 0); + EXPECT_EQ(frame | (1 << 5), 0xFF); +} + +TEST(EmulatedESC, ecat_PDOs_bit_aligned_write) +{ + // LWR of logical bit 2 → physical bit 6; other 7 physical bits must survive. + EmulatedESC esc; + + uint8_t current = State::PRE_OP; + esc.write(reg::AL_STATUS, ¤t, 1); + + uint8_t next = State::SAFE_OP; + esc.write(reg::AL_CONTROL, &next, 1); + + FMMU fmmu; + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 2; + fmmu.logical_address = 0x2000; + fmmu.length = 1; + fmmu.logical_start_bit = 2; + fmmu.logical_stop_bit = 2; + fmmu.physical_address = 0x3000; + fmmu.physical_start_bit = 6; + fmmu.activate = 1; + esc.write(reg::FMMU + 0x00, &fmmu, sizeof(FMMU)); + + // Second (byte-aligned read) FMMU lets PDI pre-fill / read back byte 0x3000. + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 1; + fmmu.logical_address = 0x2100; + fmmu.length = 1; + fmmu.logical_start_bit = 0; + fmmu.logical_stop_bit = 7; + fmmu.physical_address = 0x3000; + fmmu.physical_start_bit = 0; + fmmu.activate = 1; + esc.write(reg::FMMU + 0x10, &fmmu, sizeof(FMMU)); + + DatagramHeader header{Command::BRD, 0, 0, sizeof(uint64_t), 0, 0, 0, 0}; + uint64_t scratch = 0; + uint16_t wkc = 0; + esc.processDatagram(&header, &scratch, &wkc); + + uint8_t phys = 0xA5; + ASSERT_EQ(esc.write(0x3000, &phys, 1), 1); + + uint8_t frame = (1 << 2); + header.command = Command::LWR; + header.address = 0x2000; + header.len = 1; + wkc = 0; + esc.processDatagram(&header, &frame, &wkc); + ASSERT_EQ(wkc, 1); + + esc.read(0x3000, &phys, 1); + EXPECT_EQ(phys & (1 << 6), (1 << 6)); + EXPECT_EQ(phys & ~uint8_t(1 << 6), 0xA5 & ~uint8_t(1 << 6)); + + frame = 0; + wkc = 0; + esc.processDatagram(&header, &frame, &wkc); + ASSERT_EQ(wkc, 1); + + esc.read(0x3000, &phys, 1); + EXPECT_EQ(phys & (1 << 6), 0); + EXPECT_EQ(phys | uint8_t(1 << 6), 0xA5 | uint8_t(1 << 6)); +} + +TEST(EmulatedESC, ecat_PDOs_bit_aligned_cross_byte) +{ + // 4-bit mapping wrapping both sides: + // logical 0x2000 bits 6,7 + 0x2001 bits 0,1 + // physical 0x3000 bits 5,6,7 + 0x3001 bit 0 + EmulatedESC esc; + + uint8_t current = State::PRE_OP; + esc.write(reg::AL_STATUS, ¤t, 1); + + uint8_t next = State::SAFE_OP; + esc.write(reg::AL_CONTROL, &next, 1); + + FMMU fmmu; + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 1; + fmmu.logical_address = 0x2000; + fmmu.length = 2; + fmmu.logical_start_bit = 6; + fmmu.logical_stop_bit = 1; + fmmu.physical_address = 0x3000; + fmmu.physical_start_bit = 5; + fmmu.activate = 1; + esc.write(reg::FMMU + 0x00, &fmmu, sizeof(FMMU)); + + DatagramHeader header{Command::BRD, 0, 0, sizeof(uint64_t), 0, 0, 0, 0}; + uint64_t scratch = 0; + uint16_t wkc = 0; + esc.processDatagram(&header, &scratch, &wkc); + + uint8_t phys[2] = { uint8_t(0xE0), uint8_t(0x01) }; + esc.write(0x3000, phys, 2); + + uint8_t frame[2] = { 0x00, 0x00 }; + header.command = Command::LRD; + header.address = 0x2000; + header.len = 2; + wkc = 0; + esc.processDatagram(&header, frame, &wkc); + ASSERT_EQ(wkc, 1); + EXPECT_EQ(frame[0] & 0xC0, 0xC0); + EXPECT_EQ(frame[0] & 0x3F, 0x00); + EXPECT_EQ(frame[1] & 0x03, 0x03); + EXPECT_EQ(frame[1] & 0xFC, 0x00); +} + +TEST(EmulatedESC, ecat_PDOs_malformed_fmmu_does_not_hang) +{ + // Regression: stop < start in a single-byte FMMU used to wrap total_bits + // to ~4B and stall processBitAlignedPDO. + EmulatedESC esc; + + uint8_t current = State::PRE_OP; + esc.write(reg::AL_STATUS, ¤t, 1); + + uint8_t next = State::SAFE_OP; + esc.write(reg::AL_CONTROL, &next, 1); + + FMMU fmmu; + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 1; + fmmu.logical_address = 0x2000; + fmmu.length = 1; + fmmu.logical_start_bit = 6; + fmmu.logical_stop_bit = 0; + fmmu.physical_address = 0x3000; + fmmu.physical_start_bit = 0; + fmmu.activate = 1; + esc.write(reg::FMMU + 0x00, &fmmu, sizeof(FMMU)); + + DatagramHeader header{Command::BRD, 0, 0, sizeof(uint64_t), 0, 0, 0, 0}; + uint64_t scratch = 0; + uint16_t wkc = 0; + esc.processDatagram(&header, &scratch, &wkc); + + uint8_t frame = 0xAA; + header.command = Command::LRD; + header.address = 0x2000; + header.len = 1; + wkc = 0; + esc.processDatagram(&header, &frame, &wkc); + EXPECT_EQ(wkc, 0); + EXPECT_EQ(frame, 0xAA); +} + TEST(EmulatedESC, watchdog) { EmulatedESC esc; diff --git a/unit/src/mailbox/CoE/response-t.cc b/unit/src/mailbox/CoE/response-t.cc index dec7c3b3..a9d11af3 100644 --- a/unit/src/mailbox/CoE/response-t.cc +++ b/unit/src/mailbox/CoE/response-t.cc @@ -996,3 +996,259 @@ TEST_F(CoE_Response, createSDOMessage_rxpdo_remote_request) auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); ASSERT_EQ(nullptr, response_msg); } + + +// --- Bit-level (sub-byte) PDO entries --- + +// 0x6000 RECORD with 4 BOOLs packed in one byte (bitoffs 8..11 after count). +static CoE::Dictionary createBitObjectDictionary() +{ + CoE::Dictionary dict; + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "GPIO Bits", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{4}); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::BOOLEAN, "GPIO1", uint8_t{1}); + CoE::addEntry(obj, 2, 1, 9, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::BOOLEAN, "GPIO2", uint8_t{0}); + CoE::addEntry(obj, 3, 1, 10, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::BOOLEAN, "GPIO3", uint8_t{1}); + CoE::addEntry(obj, 4, 1, 11, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::BOOLEAN, "GPIO4", uint8_t{0}); + dict.push_back(std::move(obj)); + return dict; +} + +TEST(CoE_Response_Bits, SDO_upload_bool_returns_one_octet) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + std::vector raw = createTestReadSDO(0x6000, 1); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + ASSERT_EQ(1, sdo->transfer_type); + ASSERT_EQ(3, sdo->block_size); + ASSERT_EQ(0x01, *payload & 0x01); + ASSERT_EQ(0x00, *payload & 0xFE); +} + +TEST(CoE_Response_Bits, SDO_download_bool_writes_only_target_bit) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + uint32_t value_to_download = 0x01; + uint32_t size = 1; + mailbox::request::SDOMessage req{TEST_MAILBOX_SIZE, 0x6000, 2, false, + CoE::SDO::request::DOWNLOAD, + &value_to_download, &size, 1ms}; + std::vector raw(req.data(), req.data() + TEST_MAILBOX_SIZE); + + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::DOWNLOAD, sdo->command); + + auto const& entries = mbx.getDictionary().at(0).entries; + ASSERT_EQ(0x01, *static_cast(entries.at(2).data) & 0x01); +} + +TEST(CoE_Response_Bits, SDO_download_bool_size_mismatch_aborts) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + // 4-byte download for a 1-bit BOOL → DATA_TYPE_LENGTH_MISMATCH + std::vector raw = createTestWriteSDO(0x6000, 1, 0xCAFEBABE); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + ASSERT_EQ(CoE::Service::SDO_REQUEST, coe->service); + ASSERT_EQ(CoE::SDO::request::ABORT, sdo->command); + ASSERT_EQ(CoE::SDO::abort::DATA_TYPE_LENGTH_MISMATCH, *payload); +} + +TEST(CoE_Response_Bits, SDO_upload_complete_packs_bits) +{ + // CA from SI 1: wire byte = GPIO1|GPIO2<<1|GPIO3<<2|GPIO4<<3 = 0x05. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + std::vector raw = createTestReadSDO(0x6000, 1, true); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto size = pointData(sdo); + auto payload = pointData(size); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + ASSERT_EQ(1u, *size); + ASSERT_EQ(0x05, payload[0] & 0x0F); + ASSERT_EQ(0x00, payload[0] & 0xF0); +} + +TEST(CoE_Response_Bits, SDO_upload_complete_from_SI0_includes_count_byte) +{ + // CA from SI 0: count byte at wire[0], packed BOOLs at wire[1] bits 0..3. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + std::vector raw = createTestReadSDO(0x6000, 0, true); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto size = pointData(sdo); + auto payload = pointData(size); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + ASSERT_EQ(2u, *size); + EXPECT_EQ(payload[0], 4); + EXPECT_EQ(payload[1] & 0x0F, 0x05); + EXPECT_EQ(payload[1] & 0xF0, 0x00); +} + +TEST(CoE_Response_Bits, SDO_upload_complete_count_zero_replies_empty) +{ + // Regression: SI 0 count == 0 with CA from SI 1 used to underflow pre-zero. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "Empty", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{0}); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ, CoE::DataType::BOOLEAN, "GPIO1", uint8_t{1}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + std::vector raw = createTestReadSDO(0x6000, 1, true); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto size = pointData(sdo); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + EXPECT_EQ(0u, *size); +} + +TEST(CoE_Response_Bits, SDO_upload_complete_null_si0_data_aborts) +{ + // Regression: SI 0 with data == nullptr used to crash uploadComplete. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "No default", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", nullptr); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ, CoE::DataType::BOOLEAN, "GPIO1", uint8_t{0}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + std::vector raw = createTestReadSDO(0x6000, 1, true); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + ASSERT_EQ(CoE::Service::SDO_REQUEST, coe->service); + ASSERT_EQ(CoE::SDO::request::ABORT, sdo->command); + ASSERT_EQ(CoE::SDO::abort::NO_DATA_AVAILABLE, *payload); +} + +TEST(CoE_Response_Bits, SDO_download_complete_null_si0_data_aborts) +{ + // Regression: same null-deref in downloadComplete when CA starts at SI 0. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x7000, CoE::ObjectCode::RECORD, "No default", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::UNSIGNED8, "Count", nullptr); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::BOOLEAN, "GPIO1", uint8_t{0}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + uint8_t value[5] = {0x01, 0, 0, 0, 0}; + uint32_t size = sizeof(value); + mailbox::request::SDOMessage req{TEST_MAILBOX_SIZE, 0x7000, 0, true, + CoE::SDO::request::DOWNLOAD, + value, &size, 1ms}; + std::vector raw(req.data(), req.data() + TEST_MAILBOX_SIZE); + + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + ASSERT_EQ(CoE::Service::SDO_REQUEST, coe->service); + ASSERT_EQ(CoE::SDO::request::ABORT, sdo->command); + ASSERT_EQ(CoE::SDO::abort::NO_DATA_AVAILABLE, *payload); +} + +TEST(CoE_Response_Bits, SDO_download_complete_unpacks_bits) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + // 5-byte payload forces Normal-Download framing (expedited is ≤4 bytes). + // Byte 0 = 0x0A → GPIO2=1, GPIO4=1; other bytes are padding. + uint8_t value[5] = {0x0A, 0x00, 0x00, 0x00, 0x00}; + uint32_t size = sizeof(value); + mailbox::request::SDOMessage req{TEST_MAILBOX_SIZE, 0x6000, 1, true, + CoE::SDO::request::DOWNLOAD, + value, &size, 1ms}; + std::vector raw(req.data(), req.data() + TEST_MAILBOX_SIZE); + + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& entries = mbx.getDictionary().at(0).entries; + EXPECT_EQ(*static_cast(entries.at(1).data) & 0x01, 0); + EXPECT_EQ(*static_cast(entries.at(2).data) & 0x01, 1); + EXPECT_EQ(*static_cast(entries.at(3).data) & 0x01, 0); + EXPECT_EQ(*static_cast(entries.at(4).data) & 0x01, 1); +} diff --git a/unit/src/slave/PDO-t.cc b/unit/src/slave/PDO-t.cc index a0d9f5a1..b2fbe479 100644 --- a/unit/src/slave/PDO-t.cc +++ b/unit/src/slave/PDO-t.cc @@ -466,6 +466,133 @@ TEST_F(PDOTest, configureMapping_already_mapped_entry_is_not_freed) ASSERT_EQ(static_cast(input_), entry->data); } +TEST_F(PDOTest, configureMapping_bit_entries_alias_with_data_bit_offset) +{ + // 4 BOOL entries packed into one PDO byte; each must get its own data_bit_offset. + CoE::Dictionary dict; + + { + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "GPIOs", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{4}); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "GPIO1", uint8_t{0}); + CoE::addEntry(obj, 2, 1, 9, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "GPIO2", uint8_t{0}); + CoE::addEntry(obj, 3, 1, 10, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "GPIO3", uint8_t{0}); + CoE::addEntry(obj, 4, 1, 11, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "GPIO4", uint8_t{0}); + dict.push_back(std::move(obj)); + } + + { + CoE::Object obj{0x1A00, CoE::ObjectCode::RECORD, "TxPDO map", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{4}); + CoE::addEntry(obj, 1, 32, 8, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M1", + makeMappingEntry(0x6000, 1, 1)); + CoE::addEntry(obj, 2, 32, 40, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M2", + makeMappingEntry(0x6000, 2, 1)); + CoE::addEntry(obj, 3, 32, 72, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M3", + makeMappingEntry(0x6000, 3, 1)); + CoE::addEntry(obj, 4, 32, 104, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M4", + makeMappingEntry(0x6000, 4, 1)); + dict.push_back(std::move(obj)); + } + + { + CoE::Object assign{0x1C13, CoE::ObjectCode::RECORD, "TxPDO assign", {}}; + CoE::addEntry(assign, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{1}); + CoE::addEntry(assign, 1, 16, 8, CoE::Access::READ, CoE::DataType::UNSIGNED16, "PDO 1", uint16_t{0x1A00}); + dict.push_back(std::move(assign)); + } + + ASSERT_EQ(StatusCode::NO_ERROR, pdo_.configureMapping(dict)); + + for (uint8_t sub = 1; sub <= 4; ++sub) + { + auto [obj, entry] = CoE::findObject(dict, 0x6000, sub); + ASSERT_NE(entry, nullptr) << "sub=" << int(sub); + ASSERT_TRUE(entry->is_mapped) << "sub=" << int(sub); + EXPECT_EQ(entry->data, static_cast(input_)) << "sub=" << int(sub); + EXPECT_EQ(entry->data_bit_offset, sub - 1) << "sub=" << int(sub); + } + + auto [_, gpio1] = CoE::findObject(dict, 0x6000, 1); + auto [__, gpio3] = CoE::findObject(dict, 0x6000, 3); + + uint8_t one = 0x01; + CoE::writeEntryBits(gpio1, &one, 0); + CoE::writeEntryBits(gpio3, &one, 0); + + EXPECT_EQ(input_[0] & 0x0F, 0x05); + + uint8_t bit = 0; + CoE::readEntryBits(gpio1, &bit, 0); + EXPECT_EQ(bit & 0x01, 1); + bit = 0; + CoE::readEntryBits(gpio3, &bit, 0); + EXPECT_EQ(bit & 0x01, 1); + + auto [___, gpio2] = CoE::findObject(dict, 0x6000, 2); + bit = 0; + CoE::readEntryBits(gpio2, &bit, 0); + EXPECT_EQ(bit & 0x01, 0); +} + +TEST_F(PDOTest, configureMapping_padding_entry_index_zero_is_skipped) +{ + // Repro sample_app_Digitalio: 4 × BOOL + 4-bit Index=0 gap rounded into one byte. + CoE::Dictionary dict; + + { + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "Inputs", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{4}); + CoE::addEntry(obj, 1, 1, 8, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "DigitalIN0", uint8_t{0}); + CoE::addEntry(obj, 2, 1, 9, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "DigitalIN1", uint8_t{0}); + CoE::addEntry(obj, 3, 1, 10, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "DigitalIN2", uint8_t{0}); + CoE::addEntry(obj, 4, 1, 11, CoE::Access::READ | CoE::Access::TxPDO, + CoE::DataType::BOOLEAN, "DigitalIN3", uint8_t{0}); + dict.push_back(std::move(obj)); + } + + { + CoE::Object obj{0x1A00, CoE::ObjectCode::RECORD, "TxPDO map", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{5}); + CoE::addEntry(obj, 1, 32, 8, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M1", + makeMappingEntry(0x6000, 1, 1)); + CoE::addEntry(obj, 2, 32, 40, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M2", + makeMappingEntry(0x6000, 2, 1)); + CoE::addEntry(obj, 3, 32, 72, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M3", + makeMappingEntry(0x6000, 3, 1)); + CoE::addEntry(obj, 4, 32, 104, CoE::Access::READ, CoE::DataType::UNSIGNED32, "M4", + makeMappingEntry(0x6000, 4, 1)); + CoE::addEntry(obj, 5, 32, 136, CoE::Access::READ, CoE::DataType::UNSIGNED32, "Padding", + makeMappingEntry(0x0000, 0, 4)); + dict.push_back(std::move(obj)); + } + + { + CoE::Object assign{0x1C13, CoE::ObjectCode::RECORD, "TxPDO assign", {}}; + CoE::addEntry(assign, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{1}); + CoE::addEntry(assign, 1, 16, 8, CoE::Access::READ, CoE::DataType::UNSIGNED16, "PDO 1", uint16_t{0x1A00}); + dict.push_back(std::move(assign)); + } + + ASSERT_EQ(StatusCode::NO_ERROR, pdo_.configureMapping(dict)); + + for (uint8_t sub = 1; sub <= 4; ++sub) + { + auto [obj, entry] = CoE::findObject(dict, 0x6000, sub); + ASSERT_NE(entry, nullptr); + ASSERT_TRUE(entry->is_mapped); + EXPECT_EQ(entry->data, static_cast(input_)) << "sub=" << int(sub); + EXPECT_EQ(entry->data_bit_offset, sub - 1) << "sub=" << int(sub); + } +} + TEST_F(PDOTest, configureMapping_null_old_data_no_memcpy) { // Entry with data=nullptr — parsePdoMap should skip the memcpy From f55d29f08c7539962c7bf291c77eb20a1f353fc4 Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Thu, 23 Apr 2026 20:11:34 +0200 Subject: [PATCH 2/2] =?UTF-8?q?Fix=20ESI=20Parser=20on=20sparse=20CoE=20sl?= =?UTF-8?q?aves:=20=20=20-=20EsiParser:=20match=20Object/Info/SubItem=20to?= =?UTF-8?q?=20DataType/SubItem=20by=20Name=20=20=20=20=20(ETG2000=204807).?= =?UTF-8?q?=20For=20ARRAY=20types,=20DataType=20elements=20have=20no=20per?= =?UTF-8?q?-item=20=20=20=20=20Name,=20so=20Info/SubItem/Name=20is=20"SubI?= =?UTF-8?q?ndex=20NNN"=20=E2=80=94=20fall=20back=20to=20parsing=20=20=20?= =?UTF-8?q?=20=20the=20trailing=20subindex.=20=20=20-=20PDO=20and=20SDO:?= =?UTF-8?q?=20null-guard=20entry->data.=20Missing=20=20is=20l?= =?UTF-8?q?egal=20(ETG2000=204797)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lib/slave/src/PDO.cc | 8 +- lib/src/CoE/EsiParser.cc | 39 ++++++--- lib/src/CoE/mailbox/response.cc | 32 ++++++-- unit/src/CoE/EsiParser-t.cc | 80 +++++++++++++++++- unit/src/CoE/OD-t.cc | 4 +- unit/src/EmulatedESC-t.cc | 7 +- unit/src/mailbox/CoE/response-t.cc | 126 ++++++++++++++++++++++++++++- unit/src/slave/PDO-t.cc | 8 +- 8 files changed, 262 insertions(+), 42 deletions(-) diff --git a/lib/slave/src/PDO.cc b/lib/slave/src/PDO.cc index 439b7fea..c57317af 100644 --- a/lib/slave/src/PDO.cc +++ b/lib/slave/src/PDO.cc @@ -117,14 +117,14 @@ namespace kickcat std::vector pdo_indices; auto [obj0, entry0] = CoE::findObject(dict, assign_idx, 0); - if (entry0) + if (entry0 and entry0->data) { uint8_t count = *static_cast(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(entry->data)); } @@ -137,7 +137,7 @@ namespace kickcat 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; } diff --git a/lib/src/CoE/EsiParser.cc b/lib/src/CoE/EsiParser.cc index 500a43dc..39a24398 100644 --- a/lib/src/CoE/EsiParser.cc +++ b/lib/src/CoE/EsiParser.cc @@ -506,30 +506,43 @@ namespace kickcat::CoE } - // Set default data value - // Update name if possible by using the object node + // ETG2000 4807: match Info/SubItem to DataType/SubItem by Name. + // ARRAY: DataType elements have no per-item Name, so Info/SubItem/Name + // is "SubIndex NNN" - fall back to parsing the trailing subindex. auto node_info = node->FirstChildElement("Info"); if (node_info == nullptr) { return object; } auto object_subitem = node_info->FirstChildElement("SubItem"); - auto entry = object.entries.begin(); - for (auto& object_entry : object.entries) + while (object_subitem) { - if (object_subitem) + auto object_subitem_name = object_subitem->FirstChildElement("Name"); + if (object_subitem_name) { - auto object_subitem_name = object_subitem->FirstChildElement("Name"); - if (object_subitem_name) + std::string const name = object_subitem_name->GetText(); + auto it = std::find_if(object.entries.begin(), object.entries.end(), + [&name](CoE::Entry const& e){ return e.description == name; }); + if (it == object.entries.end()) { - object_entry.description = object_subitem_name->GetText(); + auto space = name.find_last_of(' '); + if (space != std::string::npos) + { + try + { + uint8_t const subidx = static_cast(std::stoi(name.substr(space + 1))); + it = std::find_if(object.entries.begin(), object.entries.end(), + [subidx](CoE::Entry const& e){ return e.subindex == subidx; }); + } + catch (std::exception const&) {} + } + } + if (it != object.entries.end()) + { + loadDefaultData(object_subitem, object, *it); } - - loadDefaultData(object_subitem, object, *entry); - - entry++; - object_subitem = object_subitem->NextSiblingElement(); } + object_subitem = object_subitem->NextSiblingElement("SubItem"); } return object; diff --git a/lib/src/CoE/mailbox/response.cc b/lib/src/CoE/mailbox/response.cc index 63947073..cbea3dea 100644 --- a/lib/src/CoE/mailbox/response.cc +++ b/lib/src/CoE/mailbox/response.cc @@ -155,7 +155,12 @@ namespace kickcat::mailbox::response header_->len += size; } - if ((entry->bitlen % 8 == 0) and (entry->data_bit_offset == 0)) + // Entry with no storage (ESI without ): reply zeros. + if (entry->data == nullptr) + { + std::memset(payload_, 0, size); + } + else if ((entry->bitlen % 8 == 0) and (entry->data_bit_offset == 0)) { std::memcpy(payload_, entry->data, size); } @@ -219,11 +224,9 @@ namespace kickcat::mailbox::response for (uint32_t i = sdo_->subindex; i <= number_of_entries; ++i) { - // Sparse RECORD or sub-0 overreports: stop before reading past the dense vector. if (i >= object->entries.size()) { - abort(CoE::SDO::abort::UNSUPPORTED_ACCESS); - return ProcessingResult::FINALIZE; + break; } auto* entry = &object->entries.at(i); if (not isUploadAuthorized(entry)) @@ -235,7 +238,11 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::READ, entry); uint32_t wire_bit_offset = entry->bitoff - skip_bit_offset; - CoE::readEntryBits(entry, payload_ + 4, wire_bit_offset); + // Entry without storage: payload was pre-zeroed, so just skip it. + if (entry->data != nullptr) + { + CoE::readEntryBits(entry, payload_ + 4, wire_bit_offset); + } end_bit_offset = wire_bit_offset + entry->bitlen; afterHooks(CoE::Access::READ, entry); @@ -278,6 +285,12 @@ namespace kickcat::mailbox::response return ProcessingResult::FINALIZE; } + if (entry->data == nullptr) + { + abort(CoE::SDO::abort::NO_DATA_AVAILABLE); + return ProcessingResult::FINALIZE; + } + if ((entry->bitlen % 8 == 0) and (entry->data_bit_offset == 0)) { std::memcpy(entry->data, payload_, size); @@ -324,8 +337,7 @@ namespace kickcat::mailbox::response { if (subindex >= object->entries.size()) { - abort(CoE::SDO::abort::UNSUPPORTED_ACCESS); - return ProcessingResult::FINALIZE; + break; } auto* entry = &object->entries.at(subindex); @@ -349,7 +361,11 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::WRITE, entry); - CoE::writeEntryBits(entry, start_offset, wire_bit_offset); + // Entry without storage: skip silently (nothing to write into). + if (entry->data != nullptr) + { + CoE::writeEntryBits(entry, start_offset, wire_bit_offset); + } subindex++; afterHooks(CoE::Access::WRITE, entry); diff --git a/unit/src/CoE/EsiParser-t.cc b/unit/src/CoE/EsiParser-t.cc index 6b7b237c..9d6bd48e 100644 --- a/unit/src/CoE/EsiParser-t.cc +++ b/unit/src/CoE/EsiParser-t.cc @@ -326,23 +326,25 @@ TEST(EsiParser, load_basic_with_bit_types_and_no_info) ASSERT_EQ(e2->data_bit_offset, 0); ASSERT_NE(e2->data, nullptr); - // SubIndex 3: BIT6 (Padding) + // SubIndex 3: BIT6 (Padding). Fixture's Info block has no "Padding" + // SubItem, so per ETG2000 name-matching this entry gets no default. auto [obj3, e3] = findObject(dictionary, 0x6000, 3); ASSERT_NE(e3, nullptr); ASSERT_EQ(e3->type, CoE::DataType::BIT6); ASSERT_EQ(e3->bitlen, 6); ASSERT_EQ(e3->bitoff, 33); ASSERT_EQ(e3->data_bit_offset, 0); - ASSERT_NE(e3->data, nullptr); + ASSERT_EQ(e3->data, nullptr); - // SubIndex 4: BOOL (StatusBit). Fixture declares DefaultData for only - // 4 SubItems, matched positionally to entries[0..3]; SI 4 stays null. + // SubIndex 4: BOOL (StatusBit). Fixture has "StatusBit" Info SubItem + // with DefaultData, matched by name to this entry. auto [obj4, e4] = findObject(dictionary, 0x6000, 4); ASSERT_NE(e4, nullptr); ASSERT_EQ(e4->type, CoE::DataType::BOOLEAN); ASSERT_EQ(e4->bitlen, 1); ASSERT_EQ(e4->bitoff, 39); ASSERT_EQ(e4->data_bit_offset, 0); + ASSERT_NE(e4->data, nullptr); } // 0x7010 - RECORD with BIT6 SubItem (outputs) @@ -371,3 +373,73 @@ TEST(EsiParser, load_basic_with_bit_types_and_no_info) ASSERT_EQ(object->entries.size(), 5); } } + +// ETG2000 4807: Info/SubItem is matched to DataType/SubItem by Name. +// Fixture reorders them and skips one to verify spec-conforming matching. +TEST(EsiParser, info_subitems_matched_by_name_not_position) +{ + std::string xml = R"( + + #x1Test + + + + ByName + ByName + + 5001 + + + USINT8 + UINT16 + + DT600024 + 0SubIndex 000USINT80 + 1AlphaUINT168 + 2BravoUINT1624 + 3CharlieUINT1640 + + + + + #x6000DataDT600056 + + + CharlieCCCC + SubIndex 00003 + AlphaAAAA + + + + + + MBoxOut + + + + +)"; + + CoE::EsiParser parser; + auto dict = parser.loadString(xml); + + auto [obj0, si0] = CoE::findObject(dict, 0x6000, 0); + auto [obj1, si1] = CoE::findObject(dict, 0x6000, 1); + auto [obj2, si2] = CoE::findObject(dict, 0x6000, 2); + auto [obj3, si3] = CoE::findObject(dict, 0x6000, 3); + ASSERT_NE(si0, nullptr); + ASSERT_NE(si1, nullptr); + ASSERT_NE(si2, nullptr); + ASSERT_NE(si3, nullptr); + + ASSERT_NE(si0->data, nullptr); + EXPECT_EQ(*static_cast(si0->data), 0x03); + + ASSERT_NE(si1->data, nullptr); + EXPECT_EQ(*static_cast(si1->data), 0xAAAA); + + EXPECT_EQ(si2->data, nullptr); + + ASSERT_NE(si3->data, nullptr); + EXPECT_EQ(*static_cast(si3->data), 0xCCCC); +} diff --git a/unit/src/CoE/OD-t.cc b/unit/src/CoE/OD-t.cc index 86116a11..378343c0 100644 --- a/unit/src/CoE/OD-t.cc +++ b/unit/src/CoE/OD-t.cc @@ -217,7 +217,7 @@ TEST(OD, copyBits_single_bit_preserves_neighbours) TEST(OD, copyBits_cross_byte) { - // 4 bits src[bit 6..9] → dst[bit 5..8] + // 4 bits src[6..9] -> dst[5..8] uint8_t src[2] = {0xC0, 0x03}; uint8_t dst[2] = {0x00, 0x00}; @@ -230,7 +230,7 @@ TEST(OD, copyBits_cross_byte) TEST(OD, copyBits_spans_three_source_bytes) { - // 16 bits src[bit 6..21] → dst[bit 0..15]; src spans 3 bytes, dst 2. LSB-first. + // 16 bits src[6..21] -> dst[0..15], LSB-first. uint8_t src[3] = {0xC0, 0xAA, 0x03}; uint8_t dst[2] = {0xFF, 0xFF}; diff --git a/unit/src/EmulatedESC-t.cc b/unit/src/EmulatedESC-t.cc index c6c8e6ab..23adf9b7 100644 --- a/unit/src/EmulatedESC-t.cc +++ b/unit/src/EmulatedESC-t.cc @@ -286,8 +286,7 @@ TEST(EmulatedESC, ecat_PDOs) TEST(EmulatedESC, ecat_PDOs_bit_aligned_single_bit) { - // Physical bit 3 of byte 0x300A → logical bit 5 of byte 0x2003 - // (mailbox-status-check pattern). + // Phys bit 3 @ 0x300A maps to logical bit 5 @ 0x2003. EmulatedESC esc; uint8_t current = State::PRE_OP; @@ -308,7 +307,7 @@ TEST(EmulatedESC, ecat_PDOs_bit_aligned_single_bit) fmmu.activate = 1; esc.write(reg::FMMU + 0x00, &fmmu, sizeof(FMMU)); - // PRE_OP → SAFE_OP triggers configurePDOs. + // PRE_OP to SAFE_OP triggers configurePDOs. DatagramHeader header{Command::BRD, 0, 0, sizeof(uint64_t), 0, 0, 0, 0}; uint64_t scratch = 0; uint16_t wkc = 0; @@ -339,7 +338,7 @@ TEST(EmulatedESC, ecat_PDOs_bit_aligned_single_bit) TEST(EmulatedESC, ecat_PDOs_bit_aligned_write) { - // LWR of logical bit 2 → physical bit 6; other 7 physical bits must survive. + // LWR logical bit 2 -> phys bit 6; other 7 phys bits must survive. EmulatedESC esc; uint8_t current = State::PRE_OP; diff --git a/unit/src/mailbox/CoE/response-t.cc b/unit/src/mailbox/CoE/response-t.cc index a9d11af3..d1e4fadd 100644 --- a/unit/src/mailbox/CoE/response-t.cc +++ b/unit/src/mailbox/CoE/response-t.cc @@ -1071,7 +1071,7 @@ TEST(CoE_Response_Bits, SDO_download_bool_size_mismatch_aborts) Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; mbx.enableCoE(createBitObjectDictionary()); - // 4-byte download for a 1-bit BOOL → DATA_TYPE_LENGTH_MISMATCH + // 4-byte download of a 1-bit BOOL must abort DATA_TYPE_LENGTH_MISMATCH. std::vector raw = createTestWriteSDO(0x6000, 1, 0xCAFEBABE); auto response_msg = createSDOMessage(&mbx, std::move(raw)); ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); @@ -1228,14 +1228,134 @@ TEST(CoE_Response_Bits, SDO_download_complete_null_si0_data_aborts) ASSERT_EQ(CoE::SDO::abort::NO_DATA_AVAILABLE, *payload); } +TEST(CoE_Response_Bits, SDO_upload_null_entry_data_replies_zeros) +{ + // Repro 0x1C33 shape: ESI declares the object but some SubItems omit + // , so EsiParser leaves their entry->data as nullptr. + // Individual SDO upload of such an entry used to crash; must now reply zeros. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x1C33, CoE::ObjectCode::RECORD, "SM input parameter", {}}; + CoE::addEntry(obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{3}); + CoE::addEntry(obj, 1, 16, 8, CoE::Access::READ, CoE::DataType::UNSIGNED16, "Sync Type", uint16_t{0x0022}); + CoE::addEntry(obj, 2, 32, 24, CoE::Access::READ, CoE::DataType::UNSIGNED32, "Cycle Time", nullptr); + CoE::addEntry(obj, 3, 16, 56, CoE::Access::READ, CoE::DataType::UNSIGNED16, "Sync Modes", uint16_t{0x8007}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + std::vector raw = createTestReadSDO(0x1C33, 2); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + EXPECT_EQ(0u, *payload); +} + +TEST(CoE_Response_Bits, SDO_upload_complete_skips_null_entries) +{ + // Repro 0x1C33: CA upload over an object where some middle entries have + // no default data. Null entries must be skipped (payload slot stays 0); + // entries with data must round-trip correctly. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x1C33, CoE::ObjectCode::RECORD, "SM input parameter", {}}; + CoE::addEntry (obj, 0, 8, 0, CoE::Access::READ, CoE::DataType::UNSIGNED8, "Count", uint8_t{3}); + CoE::addEntry(obj, 1, 16, 8, CoE::Access::READ, CoE::DataType::UNSIGNED16, "Sync Type", uint16_t{0x0022}); + CoE::addEntry(obj, 2, 32, 24, CoE::Access::READ, CoE::DataType::UNSIGNED32, "Cycle Time", nullptr); + CoE::addEntry(obj, 3, 16, 56, CoE::Access::READ, CoE::DataType::UNSIGNED16, "Sync Modes", uint16_t{0x8007}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + std::vector raw = createTestReadSDO(0x1C33, 1, true); + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto header = pointData(msg.data()); + auto coe = pointData(header); + auto sdo = pointData(coe); + auto size = pointData(sdo); + auto payload = pointData(size); + + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + ASSERT_EQ(8u, *size); + + uint16_t sync_type; + uint32_t cycle_time; + uint16_t sync_modes; + std::memcpy(&sync_type, payload, 2); + std::memcpy(&cycle_time, payload + 2, 4); + std::memcpy(&sync_modes, payload + 6, 2); + + EXPECT_EQ(sync_type, 0x0022); + EXPECT_EQ(cycle_time, 0u); + EXPECT_EQ(sync_modes, 0x8007); +} + +TEST(CoE_Response_Bits, SDO_download_complete_skips_null_entries) +{ + // Mirror of the upload test for CA download: writes to a null-data entry + // in the middle of the object must be silently dropped, adjacent entries + // with storage must still be written correctly. + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + { + CoE::Dictionary dict; + CoE::Object obj{0x1C32, CoE::ObjectCode::RECORD, "SM output parameter", {}}; + CoE::addEntry (obj, 0, 8, 0, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::UNSIGNED8, "Count", uint8_t{3}); + CoE::addEntry(obj, 1, 16, 8, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::UNSIGNED16, "Sync Type", uint16_t{0}); + CoE::addEntry(obj, 2, 32, 24, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::UNSIGNED32, "Cycle Time", nullptr); + CoE::addEntry(obj, 3, 16, 56, CoE::Access::READ | CoE::Access::WRITE, CoE::DataType::UNSIGNED16, "Sync Modes", uint16_t{0}); + dict.push_back(std::move(obj)); + mbx.enableCoE(std::move(dict)); + } + + // Wire layout from SI 1: 2 bytes Sync Type + 4 bytes Cycle Time + 2 bytes Sync Modes = 8. + struct __attribute__((packed)) { uint16_t sync_type; uint32_t cycle_time; uint16_t sync_modes; } + payload{ 0x0001, 0x0000FA00, 0x8001 }; + uint32_t size = sizeof(payload); + mailbox::request::SDOMessage req{TEST_MAILBOX_SIZE, 0x1C32, 1, true, + CoE::SDO::request::DOWNLOAD, + &payload, &size, 1ms}; + std::vector raw(req.data(), req.data() + TEST_MAILBOX_SIZE); + + auto response_msg = createSDOMessage(&mbx, std::move(raw)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()); + + auto const& msg = mbx.readyToSend(); + auto coe = pointData(pointData(msg.data())); + auto sdo = pointData(coe); + ASSERT_EQ(CoE::Service::SDO_RESPONSE, coe->service); + ASSERT_EQ(CoE::SDO::response::DOWNLOAD, sdo->command); + + auto const& entries = mbx.getDictionary().at(0).entries; + EXPECT_EQ(*static_cast(entries.at(1).data), 0x0001); + EXPECT_EQ(entries.at(2).data, nullptr); + EXPECT_EQ(*static_cast(entries.at(3).data), 0x8001); +} + TEST(CoE_Response_Bits, SDO_download_complete_unpacks_bits) { MockESC esc; Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; mbx.enableCoE(createBitObjectDictionary()); - // 5-byte payload forces Normal-Download framing (expedited is ≤4 bytes). - // Byte 0 = 0x0A → GPIO2=1, GPIO4=1; other bytes are padding. + // 5-byte payload forces Normal-Download framing (expedited is <=4 bytes). + // Byte 0 = 0x0A sets GPIO2=1 and GPIO4=1. uint8_t value[5] = {0x0A, 0x00, 0x00, 0x00, 0x00}; uint32_t size = sizeof(value); mailbox::request::SDOMessage req{TEST_MAILBOX_SIZE, 0x6000, 1, true, diff --git a/unit/src/slave/PDO-t.cc b/unit/src/slave/PDO-t.cc index b2fbe479..6486f723 100644 --- a/unit/src/slave/PDO-t.cc +++ b/unit/src/slave/PDO-t.cc @@ -253,8 +253,8 @@ TEST_F(PDOTest, updateOutput_read_error_does_not_crash) // ---- configureMapping() ---- // Build a dictionary with optional TxPDO (input) and RxPDO (output) assignments. -// TxPDO: 0x1C13 → 0x1A00 → (0x6000, sub1=uint16_t 0x1234) -// RxPDO: 0x1C12 → 0x1600 → (0x7000, sub1=uint16_t 0x5678) +// TxPDO: 0x1C13 -> 0x1A00 -> (0x6000, sub1=uint16_t 0x1234) +// RxPDO: 0x1C12 -> 0x1600 -> (0x7000, sub1=uint16_t 0x5678) static CoE::Dictionary createMappingDict(bool with_input_assign, bool with_output_assign) { CoE::Dictionary dict; @@ -541,7 +541,7 @@ TEST_F(PDOTest, configureMapping_bit_entries_alias_with_data_bit_offset) TEST_F(PDOTest, configureMapping_padding_entry_index_zero_is_skipped) { - // Repro sample_app_Digitalio: 4 × BOOL + 4-bit Index=0 gap rounded into one byte. + // 4 BOOL + 4-bit Index=0 gap rounded into one byte. CoE::Dictionary dict; { @@ -595,7 +595,7 @@ TEST_F(PDOTest, configureMapping_padding_entry_index_zero_is_skipped) TEST_F(PDOTest, configureMapping_null_old_data_no_memcpy) { - // Entry with data=nullptr — parsePdoMap should skip the memcpy + // Entry with data=nullptr must skip the memcpy in parsePdoMap. CoE::Dictionary dict; CoE::Object data_obj{0x6000, CoE::ObjectCode::VAR, "Data", {}};