diff --git a/lib/include/kickcat/CoE/OD.h b/lib/include/kickcat/CoE/OD.h index 18d89ba8..1c3e32b5 100644 --- a/lib/include/kickcat/CoE/OD.h +++ b/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> 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) { - 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((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 +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(); } diff --git a/lib/slave/include/kickcat/ESC/EmulatedESC.h b/lib/slave/include/kickcat/ESC/EmulatedESC.h index d3afe6d1..022165f8 100644 --- a/lib/slave/include/kickcat/ESC/EmulatedESC.h +++ b/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 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 +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 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..16060ccf 100644 --- a/lib/slave/src/ESC/EmulatedESC.cc +++ b/lib/slave/src/ESC/EmulatedESC.cc @@ -207,27 +207,120 @@ 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: 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(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(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 + { + 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 { - 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(&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..ad8a0c14 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) @@ -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(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); + // 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(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 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 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..6f9c04dd 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,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((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..8f312d4d 100644 --- a/lib/src/CoE/OD.cc +++ b/lib/src/CoE/OD.cc @@ -266,23 +266,66 @@ 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; + } + + // Fast path: byte-aligned source, destination and length + 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 a6457dbf..3baaf9a0 100644 --- a/lib/src/CoE/mailbox/response.cc +++ b/lib/src/CoE/mailbox/response.cc @@ -136,7 +136,9 @@ namespace kickcat::mailbox::response beforeHooks(CoE::Access::READ, entry); - uint32_t size = entry->bitlen / 8; + // Round up: sub-byte entries (BOOL, BIT2..BIT7) are sent as 1 octet on the wire + // per ETG1000.5 §5.3.1.1-2. Unused high bits must be zero (ETG1000.6 §5.6.2.1). + 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 +156,17 @@ 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 + { + // Sub-byte / bit-aligned entry: pack into LSB of payload, zero unused high bits. + 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 +181,44 @@ 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; + uint32_t number_of_entries = *static_cast(object->entries.at(0).data); + // Defensive cap: SI 0's claimed count may exceed the populated entries vector. + if (number_of_entries >= object->entries.size()) + { + number_of_entries = static_cast(object->entries.size() - 1); + } + + // Bit-accurate layout: payload positions are derived from each entry's + // bitoff (offset inside the OD object), relative to the starting subindex. + // Required for objects that pack multiple sub-byte entries in one byte. + uint32_t skip_bit_offset = object->entries.at(sdo_->subindex).bitoff; + uint32_t end_bit_offset = 0; // last bit position written (exclusive) + + // Degenerate: count says there is nothing to read past the requested subindex + // (e.g. SI 0 count is 0, or CA from SI 1 on a count==0 object). Reply empty + // to avoid an underflow in the pre-zero arithmetic below. + if (number_of_entries < sdo_->subindex) + { + std::memcpy(payload_, &end_bit_offset, 4); // size = 0 + 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; + } + + // Pre-zero the payload so unused / padding bits are 0 (ETG1000.6 §5.6.2.1). + { + auto const& last = object->entries.at(number_of_entries); + uint32_t last_end = uint32_t(last.bitoff) + last.bitlen; + // Sanity: a well-formed object has entries in increasing bitoff order, so + // last_end > skip_bit_offset. Guard the unsigned subtraction defensively. + 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) { @@ -184,14 +231,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; // round up to byte boundary std::memcpy(payload_, &size, 4); header_->len = sizeof(mailbox::Header) + sizeof(CoE::ServiceData) + size; @@ -222,13 +269,21 @@ 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 + { + // Sub-byte / bit-aligned entry: RMW the targeted bits, preserve neighbours. + CoE::writeEntryBits(entry, payload_, 0); + } coe_->service = CoE::Service::SDO_RESPONSE; sdo_->command = CoE::SDO::response::DOWNLOAD; @@ -244,23 +299,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; + + // Bit-accurate layout: payload positions are derived from each entry's + // bitoff (offset inside the OD object), relative to the starting subindex. + 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; + // SI 0 is always the count byte (UINT8 at bitoff 0), so byte-aligned + // memcpy is equivalent to writeEntryBits and avoids the bit-loop. + 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()) { @@ -268,6 +326,21 @@ namespace kickcat::mailbox::response } auto* entry = &object->entries.at(subindex); + // Defensive: a well-formed object has entries in increasing bitoff + // order, so entry->bitoff >= skip_bit_offset. Guard the unsigned + // subtraction so a malformed OD can't drive writeEntryBits out of + // bounds (mirrors the guard in 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); @@ -276,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..78ab0763 100644 --- a/unit/src/CoE/EsiParser-t.cc +++ b/unit/src/CoE/EsiParser-t.cc @@ -317,26 +317,34 @@ TEST(EsiParser, load_basic_with_bit_types_and_no_info) ASSERT_EQ(e1->bitlen, 16); ASSERT_EQ(e1->bitoff, 16); - // SubIndex 2: BOOL (DigitalIn) + // SubIndex 2: BOOL (DigitalIn) — sub-byte entry, must have at least 1 byte allocated auto [obj2, e2] = findObject(dictionary, 0x6000, 2); ASSERT_NE(e2, nullptr); 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) + // SubIndex 3: BIT6 (Padding) — sub-byte, must be allocated 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); // SubIndex 4: BOOL (StatusBit) + // Note: the ESI test fixture only declares DefaultData for 4 SubItems, + // which the parser matches positionally to entries[0..3]; SI 4 has no + // default value, so its data pointer is not allocated. 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..9c686804 100644 --- a/unit/src/CoE/OD-t.cc +++ b/unit/src/CoE/OD-t.cc @@ -164,6 +164,122 @@ TEST(OD, entry_data_to_string) } } +TEST(OD, addEntry_sub_byte_allocates_one_byte) +{ + // BOOL / BIT2..BIT7 had bitlen / 8 == 0 → malloc(0) (UB). + // Verify the allocation now rounds up and the low bits hold the value. + 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}); // 6-bit pattern + + 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); // 6 low bits of 0x2A == 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; // bit 0 = 1 + uint8_t dst = 0xA5; // 1010 0101 + + 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) +{ + // Copy 4 bits starting at src bit 6 → dst bit 5 + // src bits {6,7,8,9} (= byte0 high 2 bits + byte1 low 2 bits) + uint8_t src[2] = {0xC0, 0x03}; // bits 6,7 of byte0 set; bits 0,1 of byte1 set + uint8_t dst[2] = {0x00, 0x00}; + + copyBits(src, 6, dst, 5, 4); + // dst bits 5,6,7 of byte0 set; bit 0 of byte1 set + 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) +{ + // Copy 16 bits starting mid-byte: src bit 6 spans bytes 0, 1 and 2. + // Source bit pattern (LSB-first within each byte): + // byte 0 = 0xC0 → bit6=1 bit7=1 + // byte 1 = 0xAA → b0..b7 = 0,1,0,1,0,1,0,1 + // byte 2 = 0x03 → b0=1 b1=1 b2..b7=0 + // 16 mapped bits packed into dst starting at bit 0: + // dst bits 0..1 from byte0 bits 6..7 → 1,1 + // dst bits 2..9 from byte1 bits 0..7 → 0,1,0,1,0,1,0,1 + // dst bits 10..15 from byte2 bits 0..5 → 1,1,0,0,0,0 + // dst[0] LSB→MSB = 1,1,0,1,0,1,0,1 → 0xAB + // dst[1] LSB→MSB = 0,1,1,1,0,0,0,0 → 0x0E + uint8_t src[3] = {0xC0, 0xAA, 0x03}; + uint8_t dst[2] = {0xFF, 0xFF}; // pre-filled to verify clean overwrite + + copyBits(src, 6, dst, 0, 16); + + EXPECT_EQ(dst[0], 0xAB); + EXPECT_EQ(dst[1], 0x0E); +} + +TEST(OD, readEntryBits_writeEntryBits_roundtrip) +{ + // Mimic an aliased BOOL entry living 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); // bit 3 of 0xA5 = 0 + + wire = 0x01; // master sends BOOL=true + 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 the alias before Entry destructor would free our stack byte + 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..89d1ea67 100644 --- a/unit/src/EmulatedESC-t.cc +++ b/unit/src/EmulatedESC-t.cc @@ -284,6 +284,224 @@ TEST(EmulatedESC, ecat_PDOs) ASSERT_EQ(read_test, payload); } +TEST(EmulatedESC, ecat_PDOs_bit_aligned_single_bit) +{ + // Map a single physical bit to a single logical bit through an FMMU. + // This reproduces the "mailbox status check" pattern used by the master. + 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); + + // Read-access FMMU: physical bit 3 of byte 0x300A -> logical bit 5 of byte 0x2003. + 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)); + + // Trigger PDO configuration on PRE_OP -> SAFE_OP transition + 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); + + // Set physical bit 3 at 0x300A, other bits off + uint8_t phys = (1 << 3); + esc.write(0x300A, &phys, 1); + + // LRD of byte 0x2003: we expect only bit 5 to be set (bit 3 of physical -> bit 5 of logical) + uint8_t frame = 0xAA; // non-trivial initial content to verify masking + 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)); // bit 5 is set + EXPECT_EQ(frame & ~uint8_t(1 << 5), 0xAA & ~uint8_t(1 << 5)); // other bits untouched + + // Clear physical bit, re-read, bit 5 must now be 0 and others untouched + 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) +{ + // Master -> slave bit mapping: writing a specific logical bit only updates + // the mapped physical bit and leaves everything else in the physical byte intact. + 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; // write access (master -> slave) + 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 FMMU (byte-aligned read-back) so the PDI side is allowed to pre-fill + // the physical byte at 0x3000 and to read it back after the LWR. + memset(&fmmu, 0, sizeof(FMMU)); + fmmu.type = 1; // read access (slave -> master) + 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)); + + // Trigger PDO configuration + 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); + + // Pre-fill physical byte with a known pattern to verify only one bit is modified + uint8_t phys = 0xA5; + ASSERT_EQ(esc.write(0x3000, &phys, 1), 1); + + // LWR byte with logical bit 2 set + 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)); // mapped physical bit set + EXPECT_EQ(phys & ~uint8_t(1 << 6), 0xA5 & ~uint8_t(1 << 6)); // other bits preserved + + // LWR byte with logical bit 2 cleared + 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 to the next byte on both sides: + // logical bits: byte 0x2000 bit 6, bit 7; byte 0x2001 bit 0, bit 1 + // physical bits: byte 0x3000 bit 5, bit 6, bit 7; byte 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; // read access (slave -> master) + fmmu.logical_address = 0x2000; + fmmu.length = 2; // wraps to second byte since stop < start + 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); + + // Set 4 consecutive physical bits starting at byte 0x3000 bit 5 + uint8_t phys[2] = { uint8_t(0xE0), uint8_t(0x01) }; // bits 5,6,7 of byte 0; bit 0 of byte 1 + esc.write(0x3000, phys, 2); + + // LRD two bytes starting at 0x2000; logical bits 6,7 of byte 0 and 0,1 of byte 1 must all be 1 + 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); // bits 6,7 + EXPECT_EQ(frame[0] & 0x3F, 0x00); // untouched low bits + EXPECT_EQ(frame[1] & 0x03, 0x03); // bits 0,1 + EXPECT_EQ(frame[1] & 0xFC, 0x00); // untouched high bits +} + +TEST(EmulatedESC, ecat_PDOs_malformed_fmmu_does_not_hang) +{ + // Regression: a malformed FMMU (stop_bit < start_bit and length too small to wrap) + // would yield a negative bit count; if computed in unsigned arithmetic that wraps + // to ~4 billion and processBitAlignedPDO never returns. Verify the slave just + // produces no work for such an FMMU. + 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; // start > stop within a single-byte FMMU + fmmu.logical_stop_bit = 0; // → degenerate, total_bits would underflow + 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); // PRE_OP -> SAFE_OP triggers configurePDOs + + // LRD on the FMMU's logical range must complete in finite time and write nothing. + 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); // no bits were transferred + EXPECT_EQ(frame, 0xAA); // frame untouched +} + 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..de0866eb 100644 --- a/unit/src/mailbox/CoE/response-t.cc +++ b/unit/src/mailbox/CoE/response-t.cc @@ -996,3 +996,209 @@ 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 --- + +// Build a dictionary with a 0x6000 RECORD object holding 4 BOOL entries packed in one byte. +// SI 0 = count (8 bits at bitoff 0), SI 1..4 = BOOL (1 bit each at bitoffs 8..11). +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()); + + // GPIO1 default value = 1 → expect 1 octet payload with bit 0 set + 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); // expedited + ASSERT_EQ(3, sdo->block_size); // 4 - 1 byte + ASSERT_EQ(0x01, *payload & 0x01); // BOOL = true + ASSERT_EQ(0x00, *payload & 0xFE); // unused bits zeroed +} + +TEST(CoE_Response_Bits, SDO_download_bool_writes_only_target_bit) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + // Pre-set neighbours so we can check they survive: GPIO2 = 1 (bit 1 of stored byte? no: + // each entry has its own owned byte before mapping). For SDO download here the BOOL + // entry has its own storage with data_bit_offset=0; the test verifies the value lands + // in the low bit of that byte. + uint32_t value_to_download = 0x01; // BOOL true + uint32_t size = 1; // 1 octet on the wire + 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); + + // GPIO2 stored byte must now have bit 0 (its data_bit_offset) set + 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()); + + // Send 4 bytes for a 1-bit BOOL entry → expect 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) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + // Complete-access read of object 0x6000 starting at SI 1 (skip count byte). + // Expected wire payload: 1 octet packed as bit0=GPIO1=1, bit1=GPIO2=0, bit2=GPIO3=1, bit3=GPIO4=0 → 0x05. + 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); // 4 bits → 1 byte + ASSERT_EQ(0x05, payload[0] & 0x0F); // GPIO1 + GPIO3 set + ASSERT_EQ(0x00, payload[0] & 0xF0); // padding bits zeroed +} + +TEST(CoE_Response_Bits, SDO_upload_complete_from_SI0_includes_count_byte) +{ + // CA upload starting at SI 0: payload must include the count byte (8 bits at wire + // bit 0), then the 4 BOOLs at wire bits 8..11. Total = 12 bits → 2 bytes wire. + 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); // 12 bits → 2 bytes + EXPECT_EQ(payload[0], 4); // count byte + EXPECT_EQ(payload[1] & 0x0F, 0x05); // GPIO1 + GPIO3 set + EXPECT_EQ(payload[1] & 0xF0, 0x00); // padding zeroed +} + +TEST(CoE_Response_Bits, SDO_upload_complete_count_zero_replies_empty) +{ + // Regression: an object with SI 0 count == 0 must not underflow the pre-zero + // arithmetic when CA starts at SI 1. Reply should be size = 0, no crash. + 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}); + // A populated entry exists in the vector but the count says 0 → CA should ignore it. + 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_download_complete_unpacks_bits) +{ + MockESC esc; + Mailbox mbx{&esc, TEST_MAILBOX_SIZE, 1}; + mbx.enableCoE(createBitObjectDictionary()); + + // Send a wire payload of 5 bytes (forces Normal-Download framing instead of expedited, + // which is what downloadComplete handles): byte 0 packs the 4 BOOLs + // (bits 1 and 3 set → GPIO2=1, GPIO4=1, GPIO1=GPIO3=0); bytes 1..4 are unused 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); // GPIO1 + EXPECT_EQ(*static_cast(entries.at(2).data) & 0x01, 1); // GPIO2 + EXPECT_EQ(*static_cast(entries.at(3).data) & 0x01, 0); // GPIO3 + EXPECT_EQ(*static_cast(entries.at(4).data) & 0x01, 1); // GPIO4 +} diff --git a/unit/src/slave/PDO-t.cc b/unit/src/slave/PDO-t.cc index a0d9f5a1..c8593b67 100644 --- a/unit/src/slave/PDO-t.cc +++ b/unit/src/slave/PDO-t.cc @@ -466,6 +466,85 @@ 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 (BitLen=1) packed in one PDO byte. parsePdoMap must + // record each entry's intra-byte bit position via Entry::data_bit_offset + // so SDO upload/download can pick the right bit later. + CoE::Dictionary dict; + + { + CoE::Object obj{0x6000, CoE::ObjectCode::RECORD, "GPIOs", {}}; + // SubIndex 0 (count) lives in its own owned byte; not mapped through PDO. + 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)); + + // All 4 BOOL entries must alias to input_[0], at successive bit positions. + 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); + } + + // Round-trip via the bit helpers: writing each entry independently must not + // disturb its neighbours. + auto [_, gpio1] = CoE::findObject(dict, 0x6000, 1); + auto [__, gpio3] = CoE::findObject(dict, 0x6000, 3); + + uint8_t one = 0x01; + CoE::writeEntryBits(gpio1, &one, 0); // GPIO1 = 1 + CoE::writeEntryBits(gpio3, &one, 0); // GPIO3 = 1 + + EXPECT_EQ(input_[0] & 0x0F, 0x05); // bits 0 and 2 set, bits 1 and 3 clear + + 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_null_old_data_no_memcpy) { // Entry with data=nullptr — parsePdoMap should skip the memcpy