From d2d16bb433c0c7b0a028343f04863a3f73b2719c Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Wed, 6 Oct 2021 16:07:11 +0200 Subject: [PATCH] Various improvements following industrial usage (#17) Fix: handle timeout by polling socket instead of delegating to Linux kernel (kernel precision is too low for a realtime behavior @ 1 kHz) Update: error handler now takes the error reason in argument to let the user takes the right decision (a lost datagram is different from a non responsive slave) Various cleanup and fixes (gcc warnings, ASAN and UBSAN feedback). --- example/easycat_example.cc | 14 ++- include/kickcat/AbstractSocket.h | 1 + include/kickcat/Bus.h | 38 ++++--- include/kickcat/Link.h | 22 ++-- include/kickcat/LinuxSocket.h | 5 +- include/kickcat/Slave.h | 15 +++ src/Bus.cc | 185 ++++++++++++++++++------------- src/CoE.cc | 2 +- src/Frame.cc | 14 ++- src/Link.cc | 14 +-- src/LinuxSocket.cc | 52 +++++---- src/Mailbox.cc | 2 +- src/Slave.cc | 125 ++++++++++++++++----- unit/Mocks.h | 1 + unit/bus-t.cc | 22 ++-- unit/frame-t.cc | 9 +- unit/link-t.cc | 28 +++-- 17 files changed, 351 insertions(+), 198 deletions(-) diff --git a/example/easycat_example.cc b/example/easycat_example.cc index 5a99d215..7381750f 100644 --- a/example/easycat_example.cc +++ b/example/easycat_example.cc @@ -31,7 +31,7 @@ int main(int argc, char* argv[]) uint8_t io_buffer[2048]; try { - socket->open(argv[1], 100us); + socket->open(argv[1], 2ms); bus.init(); print_current_state(); @@ -53,7 +53,7 @@ int main(int argc, char* argv[]) return 1; } - auto callback_error = [](){ THROW_ERROR("something bad happened"); }; + auto callback_error = [](DatagramState const&){ THROW_ERROR("something bad happened"); }; try { @@ -82,6 +82,8 @@ int main(int argc, char* argv[]) return 1; } + socket->setTimeout(500us); + constexpr int64_t LOOP_NUMBER = 12 * 3600 * 1000; // 12h FILE* stat_file = fopen("stats.csv", "w"); fwrite("latency\n", 1, 8, stat_file); @@ -90,17 +92,19 @@ int main(int argc, char* argv[]) int64_t last_error = 0; for (int64_t i = 0; i < LOOP_NUMBER; ++i) { - sleep(20ms); + sleep(1ms); try { nanoseconds t1 = since_epoch(); bus.sendLogicalRead(callback_error); bus.sendLogicalWrite(callback_error); - bus.sendrefreshErrorCounters(callback_error); - bus.sendMailboxesChecks(callback_error); + bus.sendRefreshErrorCounters(callback_error); + bus.sendMailboxesReadChecks(callback_error); + bus.sendMailboxesWriteChecks(callback_error); bus.sendReadMessages(callback_error); bus.sendWriteMessages(callback_error); + bus.finalizeDatagrams(); nanoseconds t2 = since_epoch(); diff --git a/include/kickcat/AbstractSocket.h b/include/kickcat/AbstractSocket.h index 29d5b4b7..fb59a827 100644 --- a/include/kickcat/AbstractSocket.h +++ b/include/kickcat/AbstractSocket.h @@ -17,6 +17,7 @@ namespace kickcat virtual ~AbstractSocket() = default; virtual void open(std::string const& interface, microseconds timeout) = 0; + virtual void setTimeout(microseconds timeout) = 0; virtual void close() noexcept = 0; virtual int32_t read(uint8_t* frame, int32_t frame_size) = 0; virtual int32_t write(uint8_t const* frame, int32_t frame_size) = 0; diff --git a/include/kickcat/Bus.h b/include/kickcat/Bus.h index 06377bab..4f1af1fb 100644 --- a/include/kickcat/Bus.h +++ b/include/kickcat/Bus.h @@ -50,27 +50,29 @@ namespace kickcat // asynchrone read/write/mailbox/state methods // It enable users to do one or multiple operations in a row, process something, and process all awaiting frames. - void sendGetALStatus(Slave& slave, std::function const& error); - - void sendLogicalRead(std::function const& error); - void sendLogicalWrite(std::function const& error); - void sendLogicalReadWrite(std::function const& error); - void sendMailboxesChecks(std::function const& error); // Fetch in/out mailboxes states (full/empty) of compatible slaves - void sendNop(std::function const& error); // Send a NOP datagram + void sendGetALStatus(Slave& slave, std::function const& error); + + void sendLogicalRead(std::function const& error); + void sendLogicalWrite(std::function const& error); + void sendLogicalReadWrite(std::function const& error); + void sendMailboxesReadChecks (std::function const& error); // Fetch in mailboxes states (full/empty) of compatible slaves + void sendMailboxesWriteChecks(std::function const& error); // Fetch out mailboxes states (full/empty) of compatible slaves + void sendNop(std::function const& error); // Send a NOP datagram void processAwaitingFrames(); // Process messages (read or write slave mailbox) - one at once per slave. - void sendReadMessages(std::function const& error); - void sendWriteMessages(std::function const& error); - void sendrefreshErrorCounters(std::function const& error); - - // helpers around start/finalize oeprations - void processDataRead(std::function const& error); - void processDataWrite(std::function const& error); - void processDataReadWrite(std::function const& error); - - void checkMailboxes( std::function const& error); - void processMessages(std::function const& error); + void sendReadMessages(std::function const& error); + void sendWriteMessages(std::function const& error); + void sendRefreshErrorCounters(std::function const& error); + + // helpers around start/finalize operations + void finalizeDatagrams(); // send a frame if there is awaiting datagram inside + void processDataRead(std::function const& error); + void processDataWrite(std::function const& error); + void processDataReadWrite(std::function const& error); + + void checkMailboxes( std::function const& error); + void processMessages(std::function const& error); enum Access { diff --git a/include/kickcat/Link.h b/include/kickcat/Link.h index 075c8872..e053ad0a 100644 --- a/include/kickcat/Link.h +++ b/include/kickcat/Link.h @@ -11,6 +11,14 @@ namespace kickcat { class AbstractSocket; + enum class DatagramState + { + LOST, + INVALID_WKC, + NO_HANDLER, + OK + }; + /// \brief Handle link layer /// \details This class is responsible to handle frames and datagrams on the link layers: /// - associate an id to each datagram to call the associate callback later without depending on the read order @@ -25,12 +33,12 @@ namespace kickcat void writeThenRead(Frame& frame); void addDatagram(enum Command command, uint32_t address, void const* data, uint16_t data_size, - std::function const& process, - std::function const& error); + std::function const& process, + std::function const& error); template void addDatagram(enum Command command, uint32_t address, T const& data, - std::function const& process, - std::function const& error) + std::function const& process, + std::function const& error) { addDatagram(command, address, &data, sizeof(data), process, error); } @@ -49,9 +57,9 @@ namespace kickcat struct Callbacks { - bool in_error{false}; - std::function process; - std::function error; + DatagramState status{DatagramState::LOST}; + std::function process; + std::function error; }; std::array callbacks_{}; }; diff --git a/include/kickcat/LinuxSocket.h b/include/kickcat/LinuxSocket.h index 251856eb..70128779 100644 --- a/include/kickcat/LinuxSocket.h +++ b/include/kickcat/LinuxSocket.h @@ -8,13 +8,14 @@ namespace kickcat class LinuxSocket : public AbstractSocket { public: - LinuxSocket(microseconds rx_coalescing = -1us); + LinuxSocket(microseconds rx_coalescing = -1us, microseconds polling_period = 20us); virtual ~LinuxSocket() { close(); } void open(std::string const& interface, microseconds timeout) override; + void setTimeout(microseconds timeout) override; void close() noexcept override; int32_t read(uint8_t* frame, int32_t frame_size) override; int32_t write(uint8_t const* frame, int32_t frame_size) override; @@ -22,6 +23,8 @@ namespace kickcat private: int fd_{-1}; microseconds rx_coalescing_; + microseconds timeout_; + microseconds polling_period_; }; } diff --git a/include/kickcat/Slave.h b/include/kickcat/Slave.h index 61e5719a..8b1ff76b 100644 --- a/include/kickcat/Slave.h +++ b/include/kickcat/Slave.h @@ -3,6 +3,7 @@ #include #include +#include #include "protocol.h" #include "Mailbox.h" @@ -14,8 +15,21 @@ namespace kickcat void parseSII(); void printInfo() const; + std::string getInfo() const; + void printPDOs() const; + std::string getPDOs() const; + void printErrorCounters() const; + std::string getErrorCounters() const; + int computeErrorCounters() const; + + /// \return the number of new errors since last call. + int computeRelativeErrorCounters(); + + /// \brief Check the total number of errors since start of the slave + /// \return True if too many errors detected since start of the slave. Return false otherwise. + bool checkAbsoluteErrorCounters(int max_absolute_errors); uint16_t address; uint8_t al_status{State::INVALID}; @@ -62,6 +76,7 @@ namespace kickcat PIMapping output; ErrorCounters error_counters; + int previous_errors_sum{0}; private: void parseStrings(uint8_t const* section_start); diff --git a/src/Bus.cc b/src/Bus.cc index 4a750a35..8737fa45 100644 --- a/src/Bus.cc +++ b/src/Bus.cc @@ -13,7 +13,7 @@ namespace kickcat int32_t Bus::detectedSlaves() const { - return slaves_.size(); + return static_cast(slaves_.size()); } @@ -69,7 +69,7 @@ namespace kickcat waitForState(State::PRE_OP, 3000ms); // clear mailboxes - auto error_callback = [](){ THROW_ERROR("init error while cleaning slaves mailboxes"); }; + auto error_callback = [](DatagramState const&){ THROW_ERROR("init error while cleaning slaves mailboxes"); }; checkMailboxes(error_callback); processMessages(error_callback); @@ -96,19 +96,19 @@ namespace kickcat } - void Bus::sendGetALStatus(Slave& slave, std::function const& error) + void Bus::sendGetALStatus(Slave& slave, std::function const& error) { slave.al_status = State::INVALID; auto process = [&slave](DatagramHeader const*, uint8_t const* data, uint16_t wkc) { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } slave.al_status = data[0]; slave.al_status_code = *reinterpret_cast(data + 4); - return false; + return DatagramState::OK; }; link_.addDatagram(Command::FPRD, createAddress(slave.address, reg::AL_STATUS), nullptr, 6, process, error); @@ -117,7 +117,7 @@ namespace kickcat State Bus::getCurrentState(Slave& slave) { - auto error = []() + auto error = [](DatagramState const&) { DEBUG_PRINT("Error while trying to get slave state."); }; @@ -205,18 +205,19 @@ namespace kickcat { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } - return false; + return DatagramState::OK; }; - auto error = []() + auto error = [](DatagramState const&) { THROW_ERROR("Invalid working counter"); }; slaves_[i].address = static_cast(i); - link_.addDatagram(Command::APWR, createAddress(0 - i, reg::STATION_ADDR), slaves_[i].address, process, error); + link_.addDatagram(Command::APWR, createAddress(0 - slaves_[i].address, reg::STATION_ADDR), + slaves_[i].address, process, error); } link_.processDatagrams(); @@ -229,12 +230,12 @@ namespace kickcat { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } - return false; + return DatagramState::OK; }; - auto error = []() + auto error = [](DatagramState const&) { THROW_ERROR("Invalid working counter"); }; @@ -301,7 +302,7 @@ namespace kickcat uint16_t mapped_index[128]; uint32_t map_size = sizeof(mapped_index); - readSDO(slave, CoE::SM_CHANNEL + i, 1, Access::EMULATE_COMPLETE, mapped_index, &map_size); + readSDO(slave, CoE::SM_CHANNEL + static_cast(i), 1, Access::EMULATE_COMPLETE, mapped_index, &map_size); for (uint32_t j = 0; j < (map_size / 2); ++j) { @@ -332,7 +333,6 @@ namespace kickcat mapping = &slave.input; mapping->sync_manager = 1; - mapping->size = 0; for (auto const& pdo : slave.sii.TxPDO) { mapping->size += pdo->bitlen; @@ -366,7 +366,7 @@ namespace kickcat pi_frames_.back().size = address - pi_frames_.back().address; // frame size = current address - frame address // current size will overflow the frame at the current offset: set in on the next frame - address = pi_frames_.size() * MAX_ETHERCAT_PAYLOAD_SIZE; + address = static_cast(pi_frames_.size()) * MAX_ETHERCAT_PAYLOAD_SIZE; pi_frames_.push_back({address, 0, {}, {}}); } @@ -412,7 +412,7 @@ namespace kickcat } - void Bus::sendLogicalRead(std::function const& error) + void Bus::sendLogicalRead(std::function const& error) { for (auto const& pi_frame : pi_frames_) { @@ -421,30 +421,29 @@ namespace kickcat if (wkc != pi_frame.inputs.size()) { DEBUG_PRINT("Invalid working counter\n"); - return true; + return DatagramState::INVALID_WKC; } for (auto& input : pi_frame.inputs) { std::memcpy(input.iomap, data + input.offset, input.size); } - return false; + return DatagramState::OK; }; - link_.addDatagram(Command::LRD, pi_frame.address, nullptr, pi_frame.size, process, error); + link_.addDatagram(Command::LRD, pi_frame.address, nullptr, static_cast(pi_frame.size), process, error); } - link_.finalizeDatagrams(); } - void Bus::processDataRead(std::function const& error) + void Bus::processDataRead(std::function const& error) { sendLogicalRead(error); link_.processDatagrams(); } - void Bus::sendLogicalWrite(std::function const& error) + void Bus::sendLogicalWrite(std::function const& error) { for (auto const& pi_frame : pi_frames_) { @@ -459,24 +458,23 @@ namespace kickcat if (wkc != pi_frame.outputs.size()) { DEBUG_PRINT("Invalid working counter\n"); - return true; + return DatagramState::INVALID_WKC; } - return false; + return DatagramState::OK; }; - link_.addDatagram(Command::LWR, pi_frame.address, buffer, pi_frame.size, process, error); + link_.addDatagram(Command::LWR, pi_frame.address, buffer, static_cast(pi_frame.size), process, error); } - link_.finalizeDatagrams(); } - void Bus::processDataWrite(std::function const& error) + void Bus::processDataWrite(std::function const& error) { sendLogicalWrite(error); link_.processDatagrams(); } - void Bus::sendLogicalReadWrite(std::function const& error) + void Bus::sendLogicalReadWrite(std::function const& error) { for (auto const& pi_frame : pi_frames_) { @@ -491,22 +489,21 @@ namespace kickcat if (wkc != pi_frame.inputs.size()) { DEBUG_PRINT("Invalid working counter\n"); - return true; + return DatagramState::INVALID_WKC; } for (auto& input : pi_frame.inputs) { std::memcpy(input.iomap, data + input.offset, input.size); } - return false; + return DatagramState::OK; }; - link_.addDatagram(Command::LRW, pi_frame.address, buffer, pi_frame.size, process, error); + link_.addDatagram(Command::LRW, pi_frame.address, buffer, static_cast(pi_frame.size), process, error); } - link_.finalizeDatagrams(); } - void Bus::processDataReadWrite(std::function const& error) + void Bus::processDataReadWrite(std::function const& error) { sendLogicalReadWrite(error); link_.processDatagrams(); @@ -523,7 +520,7 @@ namespace kickcat return; } - auto error = []() + auto error = [](DatagramState const&) { THROW_ERROR("Invalid working counter"); }; @@ -532,9 +529,9 @@ namespace kickcat { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } - return false; + return DatagramState::OK; }; // Get SyncManager configuration from SII @@ -556,15 +553,17 @@ namespace kickcat } sm.start_address = sii_sm->start_adress; - sm.length = mapping.bsize; + sm.length = static_cast(mapping.bsize); sm.status = 0x00; // RO register sm.activate = 0x01; // Sync Manager enable sm.pdi_control = 0x00; // RO register - link_.addDatagram(Command::FPWR, createAddress(slave.address, reg::SYNC_MANAGER + mapping.sync_manager * 8), sm, process, error); + link_.addDatagram(Command::FPWR, + createAddress(slave.address, reg::SYNC_MANAGER + static_cast(mapping.sync_manager * 8)), + sm, process, error); DEBUG_PRINT("SM[%d] type %d - start address 0x%04x - length %d - flags: 0x%02x\n", mapping.sync_manager, type, sm.start_address, sm.length, sm.control); fmmu.logical_address = mapping.address; - fmmu.length = mapping.bsize; + fmmu.length = static_cast(mapping.bsize); fmmu.logical_start_bit = 0; // we map every bits fmmu.logical_stop_bit = 0x7; // we map every bits fmmu.physical_address = sii_sm->start_adress; @@ -591,17 +590,17 @@ namespace kickcat { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } uint16_t answer = *reinterpret_cast(data); if (answer & 0x8000) { ready = false; } - return false; + return DatagramState::OK; }; - auto error = []() + auto error = [](DatagramState const&) { THROW_ERROR("Error while fetching eeprom state"); }; @@ -663,7 +662,7 @@ namespace kickcat } // Read result - auto error = []() + auto error = [](DatagramState const&) { THROW_ERROR("Invalid working counter"); }; @@ -674,11 +673,12 @@ namespace kickcat { if (wkc != 1) { - return true; + return DatagramState::INVALID_WKC; } - uint32_t answer = *reinterpret_cast(data); + uint32_t answer; + std::memcpy(&answer, data, sizeof(uint32_t)); apply(*slave, answer); - return false; + return DatagramState::OK; }; link_.addDatagram(Command::FPRD, createAddress(slave->address, reg::EEPROM_DATA), nullptr, 4, process, error); @@ -703,9 +703,11 @@ namespace kickcat // Mailbox info readEeprom(eeprom::STANDARD_MAILBOX + eeprom::RECV_MBO_OFFSET, slaves, - [](Slave& s, uint32_t word) { s.mailbox.recv_offset = word; s.mailbox.recv_size = word >> 16; } ); + [](Slave& s, uint32_t word) { s.mailbox.recv_offset = static_cast(word); + s.mailbox.recv_size = static_cast(word >> 16); } ); readEeprom(eeprom::STANDARD_MAILBOX + eeprom::SEND_MBO_OFFSET, slaves, - [](Slave& s, uint32_t word) { s.mailbox.send_offset = word; s.mailbox.send_size = word >> 16; } ); + [](Slave& s, uint32_t word) { s.mailbox.send_offset = static_cast(word); + s.mailbox.send_size = static_cast(word >> 16); } ); readEeprom(eeprom::MAILBOX_PROTOCOL, slaves, [](Slave& s, uint32_t word) { s.supported_mailbox = static_cast(word); }); @@ -715,14 +717,14 @@ namespace kickcat { s.eeprom_size = (word & 0xFF) + 1; // 0 means 1024 bits s.eeprom_size *= 128; // Kibit to bytes - s.eeprom_version = word >> 16; + s.eeprom_version = static_cast(word >> 16); }); // Get SII section int32_t pos = 0; while (not slaves.empty()) { - readEeprom(eeprom::START_CATEGORY + pos, slaves, + readEeprom(eeprom::START_CATEGORY + static_cast(pos), slaves, [](Slave& s, uint32_t word) { s.sii.buffer.push_back(word); @@ -746,7 +748,7 @@ namespace kickcat } - void Bus::sendMailboxesChecks(std::function const& error) + void Bus::sendMailboxesReadChecks(std::function const& error) { auto isFull = [](uint8_t state, uint16_t wkc, bool stable_value) { @@ -760,16 +762,38 @@ namespace kickcat for (auto& slave : slaves_) { - auto process_write = [&slave, isFull](DatagramHeader const*, uint8_t const* state, uint16_t wkc) + auto process_read = [&slave, isFull](DatagramHeader const*, uint8_t const* state, uint16_t wkc) { - slave.mailbox.can_write = not isFull(*state, wkc, true); - return false; + slave.mailbox.can_read = isFull(*state, wkc, false); + return DatagramState::OK; }; - auto process_read = [&slave, isFull](DatagramHeader const*, uint8_t const* state, uint16_t wkc) + if (slave.supported_mailbox == 0) { - slave.mailbox.can_read = isFull(*state, wkc, false); - return false; + continue; + } + link_.addDatagram(Command::FPRD, createAddress(slave.address, reg::SYNC_MANAGER_1 + reg::SM_STATS), nullptr, 1, process_read, error); + } + } + + void Bus::sendMailboxesWriteChecks(std::function const& error) + { + auto isFull = [](uint8_t state, uint16_t wkc, bool stable_value) + { + if (wkc != 1) + { + DEBUG_PRINT("Invalid working counter\n"); + return stable_value; + } + return ((state & 0x08) == 0x08); + }; + + for (auto& slave : slaves_) + { + auto process_write = [&slave, isFull](DatagramHeader const*, uint8_t const* state, uint16_t wkc) + { + slave.mailbox.can_write = not isFull(*state, wkc, true); + return DatagramState::OK; }; if (slave.supported_mailbox == 0) @@ -777,28 +801,27 @@ namespace kickcat continue; } link_.addDatagram(Command::FPRD, createAddress(slave.address, reg::SYNC_MANAGER_0 + reg::SM_STATS), nullptr, 1, process_write, error); - link_.addDatagram(Command::FPRD, createAddress(slave.address, reg::SYNC_MANAGER_1 + reg::SM_STATS), nullptr, 1, process_read, error); } - link_.finalizeDatagrams(); } - void Bus::checkMailboxes(std::function const& error) + void Bus::checkMailboxes(std::function const& error) { - sendMailboxesChecks(error); + sendMailboxesWriteChecks(error); + sendMailboxesReadChecks(error); link_.processDatagrams(); } - void Bus::sendWriteMessages(std::function const& error) + void Bus::sendWriteMessages(std::function const& error) { auto process = [](DatagramHeader const*, uint8_t const*, uint16_t wkc) { if (wkc != 1) { DEBUG_PRINT("Invalid working counter\n"); - return true; + return DatagramState::INVALID_WKC; } - return false; + return DatagramState::OK; }; for (auto& slave : slaves_) @@ -807,13 +830,13 @@ namespace kickcat { // send one waiting message auto message = slave.mailbox.send(); - link_.addDatagram(Command::FPWR, createAddress(slave.address, slave.mailbox.recv_offset), message->data(), message->size(), process, error); + link_.addDatagram(Command::FPWR, createAddress(slave.address, slave.mailbox.recv_offset), message->data(), + static_cast(message->size()), process, error); } } - link_.finalizeDatagrams(); } - void Bus::sendReadMessages(std::function const& error) + void Bus::sendReadMessages(std::function const& error) { Frame frame; for (auto& slave : slaves_) @@ -823,16 +846,16 @@ namespace kickcat if (wkc != 1) { DEBUG_PRINT("Invalid working counter for slave %d\n", slave.address); - return true; + return DatagramState::INVALID_WKC; } if (not slave.mailbox.receive(data)) { DEBUG_PRINT("Slave %d: receive a message but didn't process it\n", slave.address); - return true; + return DatagramState::NO_HANDLER; } - return false; + return DatagramState::OK; }; if (slave.mailbox.can_read) @@ -841,11 +864,10 @@ namespace kickcat link_.addDatagram(Command::FPRD, createAddress(slave.address, slave.mailbox.send_offset), nullptr, slave.mailbox.send_size, process, error); } } - link_.finalizeDatagrams(); } - void Bus::processMessages(std::function const& error) + void Bus::processMessages(std::function const& error) { sendWriteMessages(error); sendReadMessages(error); @@ -853,9 +875,9 @@ namespace kickcat } - void Bus::sendNop(std::function const& error) + void Bus::sendNop(std::function const& error) { - auto process = [](DatagramHeader const*, uint8_t const*, uint16_t) { return false; }; + auto process = [](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::OK; }; link_.addDatagram(Command::NOP, 0, nullptr, 1, process, error); } @@ -866,6 +888,12 @@ namespace kickcat } + void Bus::finalizeDatagrams() + { + link_.finalizeDatagrams(); + } + + void Bus::clearErrorCounters() { uint16_t clear_param[20]; // Note: value is not taken into account by the slave and result will always be zero @@ -876,7 +904,7 @@ namespace kickcat } } - void Bus::sendrefreshErrorCounters(std::function const& error) + void Bus::sendRefreshErrorCounters(std::function const& error) { for (auto& slave : slaves_) { @@ -885,15 +913,14 @@ namespace kickcat if (wkc != 1) { DEBUG_PRINT("Invalid working counter for slave %d\n", slave.address); - return true; + return DatagramState::INVALID_WKC; } std::memcpy(&slave.error_counters, data, sizeof(ErrorCounters)); - return false; + return DatagramState::OK; }; link_.addDatagram(Command::FPRD, createAddress(slave.address, reg::ERROR_COUNTERS), nullptr, sizeof(ErrorCounters), process, error); } - link_.finalizeDatagrams(); } } diff --git a/src/CoE.cc b/src/CoE.cc index 382989e9..38390162 100644 --- a/src/CoE.cc +++ b/src/CoE.cc @@ -6,7 +6,7 @@ namespace kickcat { void Bus::waitForMessage(std::shared_ptr message, nanoseconds timeout) { - auto error_callback = [](){ THROW_ERROR("error while checking mailboxes"); }; + auto error_callback = [](DatagramState const&){ THROW_ERROR("error while checking mailboxes"); }; nanoseconds now = since_epoch(); while (message->status() == MessageStatus::RUNNING) diff --git a/src/Frame.cc b/src/Frame.cc index 6d46cb2b..894f4d28 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -61,7 +61,7 @@ namespace kickcat int32_t Frame::freeSpace() const { - int32_t size = ETH_MTU_SIZE - sizeof(EthercatHeader) - (next_datagram_ - first_datagram_); + int32_t size = static_cast(ETH_MTU_SIZE - sizeof(EthercatHeader) - (next_datagram_ - first_datagram_)); return size; } @@ -126,7 +126,7 @@ namespace kickcat pos += 2; header_->len += sizeof(DatagramHeader) + data_size + 2; // +2 for wkc - last_datagram_ = reinterpret_cast(header); // save last datagram header to finalize frame when ready + last_datagram_ = next_datagram_; // save last datagram header to finalize frame when ready next_datagram_ = pos; // set next datagram ++datagram_counter_; // one more datagram in the frame to be sent } @@ -165,8 +165,10 @@ namespace kickcat { DatagramHeader const* header = reinterpret_cast(next_datagram_); uint8_t* data = next_datagram_ + sizeof(DatagramHeader); - uint16_t* wkc = reinterpret_cast(data + header->len); - next_datagram_ = reinterpret_cast(wkc) + sizeof(uint16_t); + uint8_t* wkc_addr = data + header->len; + uint16_t wkc; + std::memcpy(&wkc, wkc_addr, sizeof(uint16_t)); + next_datagram_ = wkc_addr + sizeof(uint16_t); if (header->multiple == 0) { @@ -174,13 +176,13 @@ namespace kickcat clear(); } - return std::make_tuple(header, data, *wkc); + return std::make_tuple(header, data, wkc); } void Frame::read(std::shared_ptr socket) { - int32_t read = socket->read(frame_.data(), frame_.size()); + int32_t read = socket->read(frame_.data(), static_cast(frame_.size())); if (read < 0) { THROW_SYSTEM_ERROR("read()"); diff --git a/src/Link.cc b/src/Link.cc index 644c92a9..2de62dd3 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -26,8 +26,8 @@ namespace kickcat void Link::addDatagram(enum Command command, uint32_t address, void const* data, uint16_t data_size, - std::function const& process, - std::function const& error) + std::function const& process, + std::function const& error) { if (index_queue_ == static_cast(index_head_ + 1)) { @@ -43,7 +43,7 @@ namespace kickcat frame_.addDatagram(index_head_, command, address, data, data_size); callbacks_[index_head_].process = process; callbacks_[index_head_].error = error; - callbacks_[index_head_].in_error = true; + callbacks_[index_head_].status = DatagramState::LOST; ++index_head_; if (frame_.isFull()) @@ -77,7 +77,7 @@ namespace kickcat while (frame_.isDatagramAvailable()) { auto [header, data, wkc] = frame_.nextDatagram(); - callbacks_[header->index].in_error = callbacks_[header->index].process(header, data, wkc); + callbacks_[header->index].status = callbacks_[header->index].process(header, data, wkc); } } catch (std::exception const& e) @@ -89,12 +89,12 @@ namespace kickcat std::exception_ptr client_exception; for (uint8_t i = index_queue_; i != index_head_; ++i) { - if (callbacks_[i].in_error) + if (callbacks_[i].status != DatagramState::OK) { // Datagram was either lost or processing it encountered an error. try { - callbacks_[i].error(); + callbacks_[i].error(callbacks_[i].status); } catch (...) { @@ -104,7 +104,7 @@ namespace kickcat // Attach a callback to handle not THAT lost frames. // -> if a frame suspected to be lost was in fact in the pipe, it is needed to pop it - callbacks_[i].process = [&](DatagramHeader const*, uint8_t const*, uint16_t){ frame_.read(socket_); return false; }; + callbacks_[i].process = [&](DatagramHeader const*, uint8_t const*, uint16_t){ frame_.read(socket_); return DatagramState::OK; }; } index_queue_ = index_head_; diff --git a/src/LinuxSocket.cc b/src/LinuxSocket.cc index e496959d..01e71531 100644 --- a/src/LinuxSocket.cc +++ b/src/LinuxSocket.cc @@ -15,16 +15,19 @@ namespace kickcat { - LinuxSocket::LinuxSocket(microseconds rx_coalescing) + LinuxSocket::LinuxSocket(microseconds rx_coalescing, microseconds polling_period) : AbstractSocket() , fd_{-1} , rx_coalescing_{rx_coalescing} + , polling_period_(polling_period) { } - void LinuxSocket::open(std::string const& interface, microseconds requested_timeout) + void LinuxSocket::open(std::string const& interface, microseconds timeout) { + timeout_ = timeout; + // RAW socket with EtherCAT type fd_ = socket(PF_PACKET, SOCK_RAW, ETH_ETHERCAT_TYPE); if (fd_ < 0) @@ -32,24 +35,8 @@ namespace kickcat THROW_SYSTEM_ERROR("socket()"); } - // Non-blocking mode - struct timeval timeout{0, requested_timeout.count()}; - - int rc = setsockopt(fd_, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - if (rc < 0) - { - THROW_SYSTEM_ERROR("setsockopt(SO_RCVTIMEO)"); - } - - rc = setsockopt(fd_, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); - if (rc < 0) - { - THROW_SYSTEM_ERROR("setsockopt(SO_SNDTIMEO)"); - } - - constexpr int buffer_size = ETH_MAX_SIZE * 256; // max 256 frames on the wire - rc = setsockopt(fd_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)); + int rc = setsockopt(fd_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)); if (rc < 0) { THROW_SYSTEM_ERROR("setsockopt(SO_SNDBUF)"); @@ -128,6 +115,11 @@ namespace kickcat } } + void LinuxSocket::setTimeout(microseconds timeout) + { + timeout_ = timeout; + } + void LinuxSocket::close() noexcept { if (fd_ == -1) @@ -145,11 +137,29 @@ namespace kickcat int32_t LinuxSocket::read(uint8_t* frame, int32_t frame_size) { - return ::recv(fd_, frame, frame_size, 0); + nanoseconds deadline = since_epoch() + timeout_; + + while (since_epoch() < deadline) + { + ssize_t read_size = ::recv(fd_, frame, frame_size, MSG_DONTWAIT); + if (read_size < 0) + { + if (errno == EAGAIN) + { + sleep(polling_period_); + continue; + } + } + + return static_cast(read_size); + } + + errno = ETIMEDOUT; + return -1; } int32_t LinuxSocket::write(uint8_t const* frame, int32_t frame_size) { - return ::send(fd_, frame, frame_size, 0); + return static_cast(::send(fd_, frame, frame_size, MSG_DONTWAIT)); } } diff --git a/src/Mailbox.cc b/src/Mailbox.cc index 9107289a..96ef55ac 100644 --- a/src/Mailbox.cc +++ b/src/Mailbox.cc @@ -146,7 +146,7 @@ namespace kickcat } else { - header_->len += size; + header_->len += static_cast(size); std::memcpy(payload_, &size, sizeof(uint32_t)); std::memcpy(payload_ + sizeof(uint32_t), data, size); } diff --git a/src/Slave.cc b/src/Slave.cc index 53707516..03778c65 100644 --- a/src/Slave.cc +++ b/src/Slave.cc @@ -1,6 +1,8 @@ #include "Slave.h" #include "Error.h" +#include + namespace kickcat { void Slave::parseStrings(uint8_t const* section_start) @@ -73,7 +75,7 @@ namespace kickcat void Slave::parseSII() { uint8_t const* pos = reinterpret_cast(sii.buffer.data()); - uint8_t const* const max_pos = reinterpret_cast(sii.buffer.data() + sii.buffer.size()); + uint8_t const* const max_pos = reinterpret_cast(sii.buffer.data() + sii.buffer.size() - 4); while (pos < max_pos) { @@ -139,70 +141,139 @@ namespace kickcat void Slave::printInfo() const { - printf("-*-*-*-*- slave 0x%04x -*-*-*-*-\n", address); - printf("Vendor ID: 0x%08x\n", vendor_id); - printf("Product code: 0x%08x\n", product_code); - printf("Revision number: 0x%08x\n", revision_number); - printf("Serial number: 0x%08x\n", serial_number); - printf("mailbox in: size %d - offset 0x%04x\n", mailbox.recv_size, mailbox.recv_offset); - printf("mailbox out: size %d - offset 0x%04x\n", mailbox.send_size, mailbox.send_offset); - printf("supported mailbox protocol: 0x%02x\n", supported_mailbox); - printf("EEPROM: size: %d - version 0x%02x\n", eeprom_size, eeprom_version); - printf("\nSII size: %lu\n", sii.buffer.size() * sizeof(uint32_t)); + printf("%s", getInfo().c_str()); + } + + + std::string Slave::getInfo() const + { + std::stringstream os; + os << "\n -*-*-*-*- slave " << std::to_string(address) << " -*-*-*-*-\n"; + os << "Vendor ID: " << "0x" << std::setfill('0') << std::setw(8) << std::hex << vendor_id << "\n"; + os << "Product code: " << "0x" << std::setfill('0') << std::setw(8) << std::hex << product_code << "\n"; + os << "Revision number: " << "0x" << std::setfill('0') << std::setw(8) << std::hex << revision_number << "\n"; + os << "Serial number: " << "0x" << std::setfill('0') << std::setw(8) << std::hex << serial_number << "\n"; + os << "mailbox in: size " << std::dec << mailbox.recv_size << " - offset " << "0x" << std::setfill('0') + << std::setw(4) << std::hex << mailbox.recv_offset << "\n"; + + os << "mailbox out: size " << std::dec << mailbox.send_size << " - offset " << "0x" << std::setfill('0') + << std::setw(4) << std::hex << mailbox.send_offset << "\n"; + + os << "supported mailbox protocol: " << "0x" << std::setfill('0') << std::setw(2) + << std::hex << supported_mailbox << "\n"; + + os << "EEPROM: size: " << std::dec << eeprom_size << " - version "<< "0x" << std::setfill('0') + << std::setw(2) << std::hex << eeprom_version << "\n"; + + os << "\nSII size: " << std::dec << sii.buffer.size() * sizeof(uint32_t) << "\n"; for (size_t i = 0; i < sii.fmmus_.size(); ++i) { - printf("FMMU[%lu] %d\n", i, sii.fmmus_[i]); + os << "FMMU[" << std::to_string(i) << "] " << std::to_string(sii.fmmus_[i]) << "\n"; } for (size_t i = 0; i < sii.syncManagers_.size(); ++i) { auto const& sm = sii.syncManagers_[i]; - printf("SM[%lu] config\n", i); - printf(" physical address: %x\n", sm->start_adress); - printf(" length: %d\n", sm->length); - printf(" type: %d\n", sm->type); + os << "SM[" << std::dec << i << "] config\n"; + os << " physical address: " << "0x" << std::hex << sm->start_adress << "\n"; + os << " length: " << std::dec << sm->length << "\n"; + os << " type: " << std::dec << sm->type << "\n"; } + + return os.str(); } void Slave::printPDOs() const { + printf("%s", getPDOs().c_str()); + } + + + std::string Slave::getPDOs() const + { + std::stringstream os; if (not sii.RxPDO.empty()) { - printf("RxPDO\n"); + os <<"RxPDO\n"; for (size_t i = 0; i < sii.RxPDO.size(); ++i) { auto const& pdo = sii.RxPDO[i]; auto const& name = sii.strings[pdo->name]; - printf(" (0x%04x ; 0x%02x) - %d bit(s) - %.*s\n", pdo->index, pdo->subindex, pdo->bitlen, static_cast(name.size()), name.data()); + os << " (0x" << std::setfill('0') << std::setw(4) << std::hex << pdo->index << + " ; 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(pdo->subindex) << + ") - " << std::to_string(pdo->bitlen) << " bit(s) - " << std::string(name) << "\n"; } } if (not sii.TxPDO.empty()) { - printf("TxPDO\n"); + os << "TxPDO\n"; for (size_t i = 0; i < sii.TxPDO.size(); ++i) { auto const& pdo = sii.TxPDO[i]; auto const& name = sii.strings[pdo->name]; - printf(" (0x%04x ; 0x%02x) - %d bit(s) - %.*s\n", pdo->index, pdo->subindex, pdo->bitlen, static_cast(name.size()), name.data()); + os << " (0x" << std::setfill('0') << std::setw(4) << std::hex << pdo->index << + " ; 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(pdo->subindex) << + ") - " << std::to_string(pdo->bitlen) << " bit(s) - " << std::string(name) << "\n"; } } + + return os.str(); } void Slave::printErrorCounters() const { - printf("-*-*-*-*- slave 0x%04x -*-*-*-*-\n", address); + printf("%s", getErrorCounters().c_str()); + } + + + std::string Slave::getErrorCounters() const + { + std::stringstream os; + os << "\n -*-*-*-*- slave " << std::to_string(address) << " -*-*-*-*-\n"; for (int32_t i = 0; i < 4; ++i) { - printf("Port %d\n", i); - printf(" invalid frame: %d\n", error_counters.rx[i].invalid_frame); - printf(" physical layer: %d\n", error_counters.rx[i].physical_layer); - printf(" forwarded: %d\n", error_counters.forwarded[i]); - printf(" lost link: %d\n", error_counters.lost_link[i]); + os << "Port " << std::to_string(i) << " \n"; + os << " invalid frame: " << std::to_string(error_counters.rx[i].invalid_frame) << " \n"; + os << " physical layer: " << std::to_string(error_counters.rx[i].physical_layer) << " \n"; + os << " forwarded: " << std::to_string(error_counters.forwarded[i]) << " \n"; + os << " lost link: " << std::to_string(error_counters.lost_link[i]) << " \n"; } - printf("\n"); + os << " \n"; + + return os.str(); + } + + + int Slave::computeRelativeErrorCounters() + { + int current_error_sum = computeErrorCounters(); + int delta_error_sum = current_error_sum - previous_errors_sum; + + previous_errors_sum = current_error_sum; + return delta_error_sum; + } + + + bool Slave::checkAbsoluteErrorCounters(int max_absolute_errors) + { + return computeErrorCounters() > max_absolute_errors; + } + + + int Slave::computeErrorCounters() const + { + int sum = 0; + for (int32_t i = 0; i < 4; ++i) + { + sum += error_counters.rx[i].invalid_frame; + sum += error_counters.rx[i].physical_layer; + sum += error_counters.lost_link[i]; + } + + return sum; } } diff --git a/unit/Mocks.h b/unit/Mocks.h index e25eb335..05b1005c 100644 --- a/unit/Mocks.h +++ b/unit/Mocks.h @@ -8,6 +8,7 @@ namespace kickcat { public: MOCK_METHOD(void, open, (std::string const& interface, microseconds timeout), (override)); + MOCK_METHOD(void, setTimeout, (microseconds timeout), (override)); MOCK_METHOD(void, close, (), (noexcept override)); MOCK_METHOD(int32_t, read, (uint8_t* frame, int32_t frame_size), (override)); MOCK_METHOD(int32_t, write, (uint8_t const* frame, int32_t frame_size), (override)); diff --git a/unit/bus-t.cc b/unit/bus-t.cc index bea02156..bb70f826 100644 --- a/unit/bus-t.cc +++ b/unit/bus-t.cc @@ -227,7 +227,7 @@ class BusTest : public testing::Test checkSendFrame(Command::FPRD); handleReply({0, 0x08});// can write, something to read - answer.sdo.subindex = i; + answer.sdo.subindex = static_cast(i); std::memcpy(answer.payload, &data_to_reply[i], sizeof(T)); checkSendFrame(Command::FPRD); handleReply({answer}); // read answer @@ -249,7 +249,7 @@ TEST_F(BusTest, nop) checkSendFrame(Command::NOP); handleReply(); - bus.sendNop([](){}); + bus.sendNop([](DatagramState const&){}); bus.processAwaitingFrames(); } @@ -266,7 +266,7 @@ TEST_F(BusTest, error_counters) checkSendFrame(Command::FPRD); handleReply({counters}); - bus.sendrefreshErrorCounters([](){}); + bus.sendRefreshErrorCounters([](DatagramState const&){}); bus.processAwaitingFrames(); auto const& slave = bus.slaves().at(0); @@ -300,7 +300,7 @@ TEST_F(BusTest, logical_cmd) int64_t logical_read = 0x0001020304050607; checkSendFrame(Command::LRD); handleReply({logical_read}); - bus.processDataRead([](){}); + bus.processDataRead([](DatagramState const&){}); for (int i = 0; i < 8; ++i) { @@ -311,7 +311,7 @@ TEST_F(BusTest, logical_cmd) std::memcpy(slave.output.data, &logical_write, sizeof(int64_t)); checkSendFrame(Command::LWR, logical_write); handleReply({logical_write}); - bus.processDataWrite([](){}); + bus.processDataWrite([](DatagramState const&){}); for (int i = 0; i < 8; ++i) { @@ -323,7 +323,7 @@ TEST_F(BusTest, logical_cmd) std::memcpy(slave.output.data, &logical_write, sizeof(int64_t)); checkSendFrame(Command::LRW, logical_write); handleReply({logical_read}); - bus.processDataReadWrite([](){}); + bus.processDataReadWrite([](DatagramState const&){}); for (int i = 0; i < 8; ++i) { @@ -333,15 +333,15 @@ TEST_F(BusTest, logical_cmd) // check error callbacks checkSendFrame(Command::LRD); handleReply(0); - ASSERT_THROW(bus.processDataRead([](){ throw std::out_of_range(""); }), std::out_of_range); + ASSERT_THROW(bus.processDataRead([](DatagramState const&){ throw std::out_of_range(""); }), std::out_of_range); checkSendFrame(Command::LWR); handleReply(0); - ASSERT_THROW(bus.processDataWrite([](){ throw std::logic_error(""); }), std::logic_error); + ASSERT_THROW(bus.processDataWrite([](DatagramState const&){ throw std::logic_error(""); }), std::logic_error); checkSendFrame(Command::LRW); handleReply(0); - ASSERT_THROW(bus.processDataReadWrite([](){ throw std::overflow_error(""); }), std::overflow_error); + ASSERT_THROW(bus.processDataReadWrite([](DatagramState const&){ throw std::overflow_error(""); }), std::overflow_error); } @@ -396,12 +396,12 @@ TEST_F(BusTest, messages_errors) checkSendFrame(Command::FPRD); handleReply(0); - bus.sendReadMessages([](){ throw std::logic_error(""); }); + bus.sendReadMessages([](DatagramState const&){ throw std::logic_error(""); }); ASSERT_THROW(bus.processAwaitingFrames(), std::logic_error); checkSendFrame(Command::FPRD); handleReply(); - bus.sendReadMessages([](){ throw std::out_of_range(""); }); + bus.sendReadMessages([](DatagramState const&){ throw std::out_of_range(""); }); ASSERT_THROW(bus.processAwaitingFrames(), std::out_of_range); } diff --git a/unit/frame-t.cc b/unit/frame-t.cc index 7f14d518..d597bfcf 100644 --- a/unit/frame-t.cc +++ b/unit/frame-t.cc @@ -55,7 +55,7 @@ TEST(Frame, write_multiples_datagrams) uint8_t buffer[PAYLOAD_SIZE]; for (int32_t i = 0; i < PAYLOAD_SIZE; ++i) { - buffer[i] = i*i; + buffer[i] = static_cast(i * i); } frame.addDatagram(17, Command::FPRD, 20, nullptr, PAYLOAD_SIZE); @@ -126,6 +126,7 @@ TEST(Frame, write_multiples_datagrams) } break; } + default: {} } } @@ -142,7 +143,7 @@ TEST(Frame, nextDatagram) uint8_t buffer[PAYLOAD_SIZE]; for (int32_t i = 0; i < PAYLOAD_SIZE; ++i) { - buffer[i] = i*i; + buffer[i] = static_cast(i * i); } frame.addDatagram(17, Command::FPRD, 20, nullptr, PAYLOAD_SIZE); @@ -190,6 +191,10 @@ TEST(Frame, nextDatagram) } break; } + default: + { + break; + } } } } diff --git a/unit/link-t.cc b/unit/link-t.cc index 4642040b..74d75d5a 100644 --- a/unit/link-t.cc +++ b/unit/link-t.cc @@ -38,9 +38,13 @@ class LinkTest : public testing::Test [&, error](DatagramHeader const*, uint8_t const*, uint16_t) { process_callback_counter++; - return error; + if (error) + { + return DatagramState::INVALID_WKC; + } + return DatagramState::OK; }, - [&]() + [&](DatagramState const&) { error_callback_counter++; }); @@ -252,24 +256,24 @@ TEST_F(LinkTest, process_datagrams_error_rethrow) uint8_t payload; link.addDatagram(Command::BRD, 0, payload, - [&](DatagramHeader const*, uint8_t const*, uint16_t) { return true; }, - [&](){ throw std::runtime_error("A"); } + [&](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::INVALID_WKC; }, + [&](DatagramState const&){ throw std::runtime_error("A"); } ); link.addDatagram(Command::BRD, 0, payload, - [&](DatagramHeader const*, uint8_t const*, uint16_t) { return true; }, - [&](){ throw std::out_of_range("B"); } + [&](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::INVALID_WKC; }, + [&](DatagramState const&){ throw std::out_of_range("B"); } ); link.addDatagram(Command::BRD, 0, payload, - [&](DatagramHeader const*, uint8_t const*, uint16_t) { return true; }, - [&](){ throw std::logic_error("C"); } + [&](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::INVALID_WKC; }, + [&](DatagramState const&){ throw std::logic_error("C"); } ); link.addDatagram(Command::BRD, 0, payload, - [&](DatagramHeader const*, uint8_t const*, uint16_t) { return true; }, - [&](){ throw std::overflow_error("D"); } + [&](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::INVALID_WKC; }, + [&](DatagramState const&){ throw std::overflow_error("D"); } ); link.addDatagram(Command::BRD, 0, payload, - [&](DatagramHeader const*, uint8_t const*, uint16_t) { return false; }, - [&](){ throw std::underflow_error("E"); } + [&](DatagramHeader const*, uint8_t const*, uint16_t) { return DatagramState::OK; }, + [&](DatagramState const&){ throw std::underflow_error("E"); } ); EXPECT_CALL(*io, read(_,_))