diff --git a/CMakeLists.txt b/CMakeLists.txt index 36bb60e7..567e1b4d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,8 +46,10 @@ endif() if (ENABLE_UBSAN) # 'alignment' is excluded on purpose: the EtherCAT frame layer reads/writes packed buffers at # unaligned offsets (supported on the x86/Cortex-M targets). The other UB checks stay on and abort. - add_compile_options(-fsanitize=undefined -fno-sanitize=alignment -fno-sanitize-recover=undefined) - add_link_options(-fsanitize=undefined) + # float-cast-overflow is not in gcc's -fsanitize=undefined, and its no-recover needs naming too. + add_compile_options(-fsanitize=undefined,float-cast-overflow -fno-sanitize=alignment + -fno-sanitize-recover=undefined,float-cast-overflow) + add_link_options(-fsanitize=undefined,float-cast-overflow) endif() diff --git a/examples/master/gateway/emitter.cc b/examples/master/gateway/emitter.cc index 73f18abf..b5d717ba 100644 --- a/examples/master/gateway/emitter.cc +++ b/examples/master/gateway/emitter.cc @@ -62,6 +62,7 @@ int main(int argc, char* argv[]) // Local mailbox to generate and process messages mailbox::request::Mailbox mailbox; mailbox.recv_size = 128; + mailbox.send_size = 128; // Frame to send/rec on the UDP socket uint8_t frame[ETH_MTU_SIZE]; diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index 88067f63..10521009 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -7,6 +7,7 @@ set(KICKCAT_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/TapSocket.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/SIIParser.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Filesystem.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/SoftPll.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Time.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Timer.cc @@ -30,6 +31,7 @@ if (KICKOS) ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/KickOS/Mutex.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/KickOS/ConditionVariable.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/KickOS/SharedMemory.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/KickOS/Filesystem.cc ) set(OS_LIBRARIES ) # Selects the KickOS os_types branch in types.h, for the library and its consumers. @@ -38,6 +40,7 @@ elseif (NUTTX) # NuttX is POSIX enough to share the Unix time backend (clock_gettime/clock_nanosleep). set(OS_LIB_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Time.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Filesystem.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/NuttX/Socket.cc ) set(OS_LIBRARIES ) @@ -46,6 +49,7 @@ elseif(PIKEOS) ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/PikeOS/Socket.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/PikeOS/Time.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/PikeOS/ErrorCategory.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Filesystem.cc ) set(OS_LIBRARIES ) @@ -59,6 +63,7 @@ elseif (UNIX) ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/SharedMemory.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/ConditionVariable.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Thread.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Filesystem.cc ) set(OS_LIBRARIES pthread rt) elseif (WIN32) @@ -69,6 +74,7 @@ elseif (WIN32) ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Windows/Time.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/Mutex.cc ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Unix/ConditionVariable.cc + ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Windows/Filesystem.cc ) find_package(npcap CONFIG REQUIRED) diff --git a/lib/include/kickcat/CoE/mailbox/request.h b/lib/include/kickcat/CoE/mailbox/request.h index 5ea4ef1f..6cca1918 100644 --- a/lib/include/kickcat/CoE/mailbox/request.h +++ b/lib/include/kickcat/CoE/mailbox/request.h @@ -9,7 +9,7 @@ namespace kickcat::mailbox::request class SDOMessage final : public AbstractMessage { public: - SDOMessage(uint16_t mailbox_size, uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout); + SDOMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout); virtual ~SDOMessage() = default; ProcessingResult process(uint8_t const* received) override; @@ -33,7 +33,7 @@ namespace kickcat::mailbox::request class SDOInformationMessage final : public AbstractMessage { public: - SDOInformationMessage(uint16_t mailbox_size, uint8_t request, void* data, uint32_t* data_size, uint32_t request_payload_size, nanoseconds timeout); + SDOInformationMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint8_t request, void* data, uint32_t* data_size, uint32_t request_payload_size, nanoseconds timeout); virtual ~SDOInformationMessage() = default; ProcessingResult process(uint8_t const* received) override; diff --git a/lib/include/kickcat/CoE/protocol.h b/lib/include/kickcat/CoE/protocol.h index 95f4ba2b..2d6ca654 100644 --- a/lib/include/kickcat/CoE/protocol.h +++ b/lib/include/kickcat/CoE/protocol.h @@ -178,6 +178,22 @@ namespace kickcat::CoE uint16_t access; } __attribute__((__packed__)); std::string toString(EntryDescription const& entry_description); + + /// \brief Size of the CoE service a response to this opcode occupies, headers included. + /// \details A server builds its answer in the buffer the request arrived in, past the + /// request it parsed, so a request sized to its own length cannot hold it. The + /// mailbox header is the mailbox layer's own and is not counted here. + constexpr std::size_t responseSize(uint16_t opcode) + { + constexpr std::size_t headers = sizeof(Header) + sizeof(ServiceDataInfo); + switch (opcode) + { + case GET_OD_LIST_REQ: { return headers + sizeof(ListType) + 5 * sizeof(uint16_t); } + case GET_OD_REQ: { return headers + sizeof(ObjectDescription); } + case GET_ED_REQ: { return headers + sizeof(EntryDescription); } + default: { return headers + sizeof(uint32_t); } // abort code + } + } } namespace abort diff --git a/lib/include/kickcat/Mailbox.h b/lib/include/kickcat/Mailbox.h index 4e2e1a06..4a0b5ceb 100644 --- a/lib/include/kickcat/Mailbox.h +++ b/lib/include/kickcat/Mailbox.h @@ -49,8 +49,10 @@ namespace kickcat::mailbox::request class AbstractMessage { public: - /// \param mailbox_size Size of the mailbox the message is targeted to (required to adapt internal buffer) - AbstractMessage(uint16_t mailbox_size, nanoseconds timeout); + /// \param mbx_recv_size Slave receive mailbox size: sizes the internal buffer + /// \param mbx_send_size Slave send mailbox size. A mailbox may be asymmetric, so this is + /// not mbx_recv_size. + AbstractMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, nanoseconds timeout); virtual ~AbstractMessage() = default; // set message counter (aka session handle) @@ -78,6 +80,7 @@ namespace kickcat::mailbox::request std::vector data_; // data of the message (send and gateway rec) mailbox::Header* header_; // pointer on the mailbox header in data uint32_t status_; // message current status + uint16_t send_size_; // valid bytes in the buffer given to process() - not data_.size() private: nanoseconds timeout_; // Max time to handle the message. Relative time before sending, absolute time after. 0 means no timeout @@ -89,7 +92,7 @@ namespace kickcat::mailbox::request class GatewayMessage final : public AbstractMessage { public: - GatewayMessage(uint16_t mailbox_size, uint8_t const* raw_message, uint16_t gateway_index, nanoseconds timeout); + GatewayMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint8_t const* raw_message, uint16_t gateway_index, nanoseconds timeout); /// \brief Build a GatewayMessage that is already completed: the reply is already in hand, /// so there is no bus round-trip. Used by synchronous dispatch paths (e.g. the master OD, @@ -110,15 +113,15 @@ namespace kickcat::mailbox::request /// \brief Request mailbox - it orchestrates the emission and the processing of messages (for master) struct Mailbox { - uint16_t recv_offset; - uint16_t recv_size; - uint16_t send_offset; - uint16_t send_size; - - bool can_read; // data available on the slave - bool can_write; // free space for a new message on the slave - uint8_t counter{0}; // session handle, from 1 to 7 - bool toggle; // for SDO segmented transfer + uint16_t recv_offset{0}; + uint16_t recv_size{0}; // slave receive mailbox: bounds what the master writes + uint16_t send_offset{0}; + uint16_t send_size{0}; // slave send mailbox: bounds what a reply carries + + bool can_read{false}; // data available on the slave + bool can_write{false}; // free space for a new message on the slave + uint8_t counter{0}; // session handle, from 1 to 7 + bool toggle{false}; // for SDO segmented transfer // void generateSMConfig(SyncManager::Register SM[2]); diff --git a/lib/include/kickcat/OS/Filesystem.h b/lib/include/kickcat/OS/Filesystem.h new file mode 100644 index 00000000..db3b887c --- /dev/null +++ b/lib/include/kickcat/OS/Filesystem.h @@ -0,0 +1,71 @@ +#ifndef KICKCAT_OS_FILESYSTEM_H +#define KICKCAT_OS_FILESYSTEM_H + +#include +#include +#include +#include + +namespace kickcat::filesystem +{ + // Thin wrappers over the platform's own file API, deliberately not /: + // EmulatedESC is compiled for the embedded targets, whose C++ export has neither, and on MinGW + // those two headers bind the binary to libstdc++ symbols the runtime DLL it finds may not + // export (a load-time STATUS_ENTRYPOINT_NOT_FOUND, seen in CI). + // + // Paths are byte strings separated by '/', on every platform: the Win32 API accepts '/' too. + // Nothing here resolves symlinks, expands '..' or makes a path absolute. + + struct Entry + { + std::string name; // leaf name, not a path + bool is_directory; + }; + + /// \return true if the path exists, whatever its kind. + bool exists(std::string const& path); + + /// \return true if the path exists and is a directory. + bool isDirectory(std::string const& path); + + /// \brief Delete one file. + /// \return true if it was deleted, false if it was already absent. + bool removeFile(std::string const& path); + + /// \brief Create one directory. The parent must already exist. + /// \return true if it was created, false if it was already there. + bool createDirectory(std::string const& path); + + /// \brief Delete one directory, which must be empty. + /// \return true if it was deleted, false if it was already absent. + bool removeDirectory(std::string const& path); + + /// \brief One directory level, in whatever order the OS reports. '.' and '..' are not listed. + std::vector list(std::string const& path); + + /// \brief Every file below directory, as paths prefixed with directory. Directories themselves + /// are not listed, and the order is unspecified. + std::vector listFilesRecursive(std::string const& directory); + + /// \return everything before the last separator, empty if the path has none. + std::string parent(std::string const& path); + + /// \return the leaf name: everything after the last separator. + std::string filename(std::string const& path); + + /// \return the last '.' of the leaf name and what follows, empty if the leaf has none. + std::string extension(std::string const& path); + + /// \brief Append name to directory, inserting a separator only where one is missing. An empty + /// directory yields name unchanged, so joining onto parent() of a bare filename works. + std::string join(std::string const& directory, std::string const& name); + + /// \brief Read a whole file. + std::vector readFile(std::string const& path); + + /// \brief Create or truncate a file and write it whole. + void writeFile(std::string const& path, void const* data, std::size_t size); + void writeFile(std::string const& path, std::string const& content); +} + +#endif diff --git a/lib/include/kickcat/OS/math.h b/lib/include/kickcat/OS/math.h deleted file mode 100644 index a4ac3223..00000000 --- a/lib/include/kickcat/OS/math.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef KICKCAT_OS_MATH_H -#define KICKCAT_OS_MATH_H - -#include - -namespace kickcat -{ - // Freestanding min/max/abs/round helpers that pull no //. Those - // standard headers are broken or mutually conflicting on some embedded C++ exports (e.g. NuttX - // on arm-none-eabi, where cxx/cmath references an absent ::nextafterl and collides with - // the toolchain's stdlib.h on div_t) so code compiled for those targets uses these instead. - // Prefer the std equivalents (std::clamp / std::abs / std::llround) in any translation unit that - // already compiles with those headers. - - template - T clamp(T v, T lo, T hi) - { - if (v < lo) - { - return lo; - } - if (v > hi) - { - return hi; - } - return v; - } - - template - T abs_value(T v) - { - if (v < 0) - { - return -v; - } - return v; - } - - inline int64_t round_to_int(double v) - { - if (v < 0.0) - { - return static_cast(v - 0.5); - } - return static_cast(v + 0.5); - } -} - -#endif diff --git a/lib/include/kickcat/utils/math.h b/lib/include/kickcat/utils/math.h new file mode 100644 index 00000000..4fa963a3 --- /dev/null +++ b/lib/include/kickcat/utils/math.h @@ -0,0 +1,111 @@ +#ifndef KICKCAT_UTILS_MATH_H +#define KICKCAT_UTILS_MATH_H + +#include +#include +#include + +namespace kickcat +{ + // Freestanding min/max/abs/round/convert helpers that pull no //. + // In a subdirectory on purpose: lib/include/kickcat is itself on the include path, so a math.h + // sitting directly in it is what every translation unit here would get for #include . + // Those standard headers are broken or mutually conflicting on some embedded C++ exports (e.g. + // NuttX on arm-none-eabi, where cxx/cmath references an absent ::nextafterl and + // collides with the toolchain's stdlib.h on div_t) so code compiled for those targets uses these + // instead. is required of a freestanding implementation, so it is safe here. + + template + constexpr T clamp(T v, T lo, T hi) + { + if (v < lo) + { + return lo; + } + if (v > hi) + { + return hi; + } + return v; + } + + // Only NaN compares unequal to itself, and is one of the headers this file avoids. + constexpr bool is_nan(double v) + { + return v != v; + } + + template + constexpr T abs_value(T v) + { + if (v < 0) + { + return -v; + } + return v; + } + + /// \brief Convert a real value to T, clamping it into [min, max] reduced into T's range. + /// \details static_cast(double) is undefined behaviour outside T's range, for a narrower + /// floating type as much as for an integer. Bounds the destination cannot hold give a + /// meaningless result, never undefined behaviour. + template + constexpr T saturate(double v, double min, double max) + { + static_assert(std::is_arithmetic_v, "saturate() converts to an arithmetic type"); + + // lowest(), not min(): for a floating T the latter is the smallest positive normal. + // Beyond 32 bits these images are not exact - numeric_limits::max() rounds up to + // 2^63 - so they are thresholds to compare against, never values to convert back. + double const lowest = static_cast(std::numeric_limits::lowest()); + double const highest = static_cast(std::numeric_limits::max()); + double const first = clamp(min, lowest, highest); + double const second = clamp(max, lowest, highest); + + // Transposed bounds still describe the interval between them; rejecting them belongs to the + // caller-facing API (see Drive::setLimits). + double lo = first; + double hi = second; + if (second < first) + { + lo = second; + hi = first; + } + + if (v >= hi) + { + if (hi >= highest) + { + return std::numeric_limits::max(); + } + return static_cast(hi); + } + + if (v <= lo) + { + if (lo <= lowest) + { + return std::numeric_limits::lowest(); + } + return static_cast(lo); + } + + return static_cast(v); + } + + /// \brief Round to the nearest integer, saturating: the destination range is the only bound a + /// bare double to int64_t conversion has, and exceeding it would be undefined. + constexpr int64_t round_to_int(double v) + { + constexpr double lowest = static_cast(std::numeric_limits::lowest()); + constexpr double highest = static_cast(std::numeric_limits::max()); + + if (v < 0.0) + { + return saturate(v - 0.5, lowest, highest); + } + return saturate(v + 0.5, lowest, highest); + } +} + +#endif diff --git a/lib/master/include/kickcat/CoE/CiA/DS402/Drive.h b/lib/master/include/kickcat/CoE/CiA/DS402/Drive.h index 1e98feac..6dfddf98 100644 --- a/lib/master/include/kickcat/CoE/CiA/DS402/Drive.h +++ b/lib/master/include/kickcat/CoE/CiA/DS402/Drive.h @@ -2,6 +2,7 @@ #define KICKCAT_COE_CiA_DS402_DRIVE_H #include +#include #include "kickcat/CoE/CiA/DS402/StateMachine.h" @@ -112,6 +113,22 @@ namespace kickcat::CoE::CiA::DS402 // caches the conversion factors. Call before any SI accessor. void setUnits(UnitConfig const& units); + // Integration limits, output-shaft frame. These are the envelope the machine imposes, which + // is tighter than the joint's own capability and is not in the slave OD: commissioning a + // robot starts with a deliberately small range and opens it up. Defaults are wide open. + // A zero limit means zero, not "unlimited". + struct Limits + { + double min_position_rad = std::numeric_limits::lowest(); + double max_position_rad = std::numeric_limits::max(); + double max_velocity_rad_per_s = std::numeric_limits::max(); + double max_torque_Nm = std::numeric_limits::max(); + }; + + // Rejects min_position > max_position and negative magnitudes. + void setLimits(Limits const& limits); + Limits const& limits() const { return limits_; } + void update(); void enable() { sm_.enable(); } @@ -129,8 +146,8 @@ namespace kickcat::CoE::CiA::DS402 void setTargetVelocityRaw (int32_t ticks_per_s){ out_->target_velocity = ticks_per_s; } void setTargetTorqueRaw (int16_t per_mille) { out_->target_torque = per_mille; } - // SI setpoints. Output-shaft frame. Out-of-range values are clamped - // to the underlying int range, not silently wrapped. + // SI setpoints. Output-shaft frame. Values outside the limits are clamped into them, + // never wrapped. The Raw setters bypass the limits. void setTargetPosition(double rad); void setTargetVelocity(double rad_per_s); void setTargetTorque (double nm); @@ -152,6 +169,8 @@ namespace kickcat::CoE::CiA::DS402 UnitConfig units_{}; control::ControlMode mode_ = control::NO_MODE; + Limits limits_{}; + // Cached conversion factors, recomputed in setUnits(). double pos_ticks_per_rad_ = 0.0; double torque_per_mille_per_nm_ = 0.0; diff --git a/lib/master/src/CoE/CiA/DS402/Drive.cc b/lib/master/src/CoE/CiA/DS402/Drive.cc index 9413f018..d02c0c1b 100644 --- a/lib/master/src/CoE/CiA/DS402/Drive.cc +++ b/lib/master/src/CoE/CiA/DS402/Drive.cc @@ -1,11 +1,10 @@ -#include #include #include -#include #include "kickcat/CoE/CiA/DS402/Drive.h" #include "kickcat/Bus.h" #include "kickcat/Error.h" +#include "kickcat/utils/math.h" #include "kickcat/Slave.h" #include "kickcat/Units.h" @@ -31,16 +30,6 @@ namespace kickcat::CoE::CiA::DS402 0x60400010, 0x60600010, 0x607A0020, 0x60FF0020, 0x60710010 }; - - // Clamp before cast: static_cast(double) is UB when the - // truncated value falls outside the destination type's range. - template - T saturate(double v) - { - return static_cast(std::clamp(v, - static_cast(std::numeric_limits::min()), - static_cast(std::numeric_limits::max()))); - } } // Cross-check: the bit-lengths declared in the PDO mapping arrays must @@ -147,6 +136,14 @@ namespace kickcat::CoE::CiA::DS402 void Drive::setUnits(UnitConfig const& units) { + // NaN compares false against everything, so it has to be rejected on its own. + if (is_nan(units.encoder_ticks_per_rev) + or is_nan(units.gear_ratio) + or is_nan(units.rated_torque_Nm)) + { + THROW_ERROR("Drive::setUnits does not accept NaN"); + } + if (units.encoder_ticks_per_rev <= 0.0 or units.gear_ratio <= 0.0 or units.rated_torque_Nm <= 0.0) @@ -165,19 +162,43 @@ namespace kickcat::CoE::CiA::DS402 out_->control_word = sm_.controlWord(); } + void Drive::setLimits(Limits const& limits) + { + if (is_nan(limits.min_position_rad) or is_nan(limits.max_position_rad) + or is_nan(limits.max_velocity_rad_per_s) or is_nan(limits.max_torque_Nm)) + { + THROW_ERROR("Drive::setLimits does not accept NaN"); + } + + if (limits.min_position_rad > limits.max_position_rad) + { + THROW_ERROR("Drive::setLimits requires min_position_rad <= max_position_rad"); + } + if (limits.max_velocity_rad_per_s < 0.0 or limits.max_torque_Nm < 0.0) + { + THROW_ERROR("Drive::setLimits requires non-negative velocity and torque magnitudes"); + } + + limits_ = limits; + } + void Drive::setTargetPosition(double rad) { - out_->target_position = saturate(rad * pos_ticks_per_rad_); + out_->target_position = saturate(rad * pos_ticks_per_rad_, + limits_.min_position_rad * pos_ticks_per_rad_, + limits_.max_position_rad * pos_ticks_per_rad_); } void Drive::setTargetVelocity(double rad_per_s) { - out_->target_velocity = saturate(rad_per_s * pos_ticks_per_rad_); + double const magnitude = limits_.max_velocity_rad_per_s * pos_ticks_per_rad_; + out_->target_velocity = saturate(rad_per_s * pos_ticks_per_rad_, -magnitude, magnitude); } void Drive::setTargetTorque(double nm) { - out_->target_torque = saturate(nm * torque_per_mille_per_nm_); + double const magnitude = limits_.max_torque_Nm * torque_per_mille_per_nm_; + out_->target_torque = saturate(nm * torque_per_mille_per_nm_, -magnitude, magnitude); } double Drive::actualPosition() const diff --git a/lib/simulation/include/kickcat/simulation/SimulatedSlave.h b/lib/simulation/include/kickcat/simulation/SimulatedSlave.h index 4cda9a1b..9f203ab9 100644 --- a/lib/simulation/include/kickcat/simulation/SimulatedSlave.h +++ b/lib/simulation/include/kickcat/simulation/SimulatedSlave.h @@ -1,7 +1,6 @@ #ifndef KICKCAT_SIMULATION_SIMULATED_SLAVE_H #define KICKCAT_SIMULATION_SIMULATED_SLAVE_H -#include #include #include #include @@ -15,8 +14,6 @@ namespace kickcat::sim { - namespace fs = std::filesystem; - // One emulated slave. unique_ptr members (PDO/Slave/Mailbox hold raw pointers // into the ESC) and vectors (whose data() survives a move) make the aggregate // safe to hold in a std::vector. @@ -37,7 +34,7 @@ namespace kickcat::sim // Build one slave from its JSON config (ESI device or raw eeprom, optional CoE). // Throws std::runtime_error on any failure. - SimulatedSlave buildSlave(fs::path const& config_path); + SimulatedSlave buildSlave(std::string const& config_path); } #endif diff --git a/lib/simulation/include/kickcat/simulation/devices/Ds402Motor.h b/lib/simulation/include/kickcat/simulation/devices/Ds402Motor.h index 2c70cc99..931c0ac8 100644 --- a/lib/simulation/include/kickcat/simulation/devices/Ds402Motor.h +++ b/lib/simulation/include/kickcat/simulation/devices/Ds402Motor.h @@ -5,6 +5,7 @@ #include #include "kickcat/CoE/CiA/DS402/protocol.h" +#include "kickcat/utils/math.h" #include "kickcat/simulation/DeviceApp.h" #include "kickcat/slave/Slave.h" @@ -194,14 +195,14 @@ namespace kickcat::sim pos_ += vel_ * dt; } - if (actual_position_ != nullptr) { *actual_position_ = static_cast(pos_); } - if (actual_velocity_ != nullptr) { *actual_velocity_ = static_cast(vel_); } + // The emulated feedback registers are the only range the plant state has to fit. + if (actual_position_ != nullptr) { *actual_position_ = saturate(pos_, INT32_MIN, INT32_MAX); } + if (actual_velocity_ != nullptr) { *actual_velocity_ = saturate(vel_, INT32_MIN, INT32_MAX); } if (actual_torque_ != nullptr) { // Torque the motor experiences; for CST this equals the commanded torque. - double torque = params_.inertia * accel + params_.friction * vel_; - torque = std::clamp(torque, -32768.0, 32767.0); - *actual_torque_ = static_cast(torque); + *actual_torque_ = saturate(params_.inertia * accel + params_.friction * vel_, + INT16_MIN, INT16_MAX); } } diff --git a/lib/simulation/src/SimulatedSlave.cc b/lib/simulation/src/SimulatedSlave.cc index f1e9d436..faa3b69c 100644 --- a/lib/simulation/src/SimulatedSlave.cc +++ b/lib/simulation/src/SimulatedSlave.cc @@ -1,7 +1,5 @@ #include "kickcat/simulation/SimulatedSlave.h" -#include -#include #include #include #include @@ -12,12 +10,12 @@ #include "kickcat/ESC/EmulatedESC.h" #include "kickcat/ESI/Parser.h" #include "kickcat/ESI/SIIBuilder.h" +#include "kickcat/OS/Filesystem.h" #include "kickcat/SIIParser.h" namespace kickcat::sim { using json = nlohmann::json; - namespace fs = std::filesystem; void configureDeviceDictionary(SimulatedSlave& sim, ESI::Device& device) { @@ -38,23 +36,28 @@ namespace kickcat::sim } } - SimulatedSlave buildSlave(fs::path const& config_path) + SimulatedSlave buildSlave(std::string const& config_path) { - fs::path config_dir = config_path.parent_path(); + std::string config_dir = filesystem::parent(config_path); - std::ifstream f(config_path); - if (not f.is_open()) + std::vector raw_config; + try + { + raw_config = filesystem::readFile(config_path); + } + catch (std::exception const& e) { - throw std::runtime_error("Failed to open config file: " + config_path.string()); + throw std::runtime_error("Failed to open config file: " + config_path + ": " + e.what()); } + json config; try { - f >> config; + config = json::parse(raw_config); } catch (const json::parse_error& e) { - throw std::runtime_error("Failed to parse JSON in " + config_path.string() + ": " + e.what()); + throw std::runtime_error("Failed to parse JSON in " + config_path + ": " + e.what()); } SimulatedSlave sim; @@ -65,10 +68,10 @@ namespace kickcat::sim if (config.contains("esi")) { // Build the EEPROM image (and CoE dictionary) from a selected ESI device. - fs::path esi_full_path = config_dir / config["esi"].get(); - if (not fs::exists(esi_full_path)) + std::string esi_full_path = filesystem::join(config_dir, config["esi"].get()); + if (not filesystem::exists(esi_full_path)) { - throw std::runtime_error("ESI file not found: " + esi_full_path.string()); + throw std::runtime_error("ESI file not found: " + esi_full_path); } ESI::DeviceFilter filter; if (config.contains("device_type")) { filter.type = config["device_type"].get(); } @@ -78,32 +81,32 @@ namespace kickcat::sim try { ESI::Parser parser; - ESI::Device device = parser.loadDevice(esi_full_path.string(), filter); + ESI::Device device = parser.loadDevice(esi_full_path, filter); CoE::materializeStorage(device.dictionary); sim.esc->loadEeprom(ESI::buildEepromImage(device)); configureDeviceDictionary(sim, device); } catch (std::exception const& e) { - throw std::runtime_error("Failed to build EEPROM from ESI " + esi_full_path.string() + ": " + e.what()); + throw std::runtime_error("Failed to build EEPROM from ESI " + esi_full_path + ": " + e.what()); } } else if (config.contains("eeprom")) { - fs::path eeprom_full_path = config_dir / config["eeprom"].get(); - if (not fs::exists(eeprom_full_path)) + std::string eeprom_full_path = filesystem::join(config_dir, config["eeprom"].get()); + if (not filesystem::exists(eeprom_full_path)) { - throw std::runtime_error("EEPROM file not found: " + eeprom_full_path.string()); + throw std::runtime_error("EEPROM file not found: " + eeprom_full_path); } - std::vector eeprom_image = loadBinaryFile(eeprom_full_path); + std::vector eeprom_image = filesystem::readFile(eeprom_full_path); sim.esc->loadEeprom(eeprom_image); if (config.contains("coe_xml")) { - fs::path coe_xml_full_path = config_dir / config["coe_xml"].get(); - if (not fs::exists(coe_xml_full_path)) + std::string coe_xml_full_path = filesystem::join(config_dir, config["coe_xml"].get()); + if (not filesystem::exists(coe_xml_full_path)) { - throw std::runtime_error("CoE XML file not found: " + coe_xml_full_path.string()); + throw std::runtime_error("CoE XML file not found: " + coe_xml_full_path); } eeprom::SII sii; sii.parse(eeprom_image); @@ -114,14 +117,14 @@ namespace kickcat::sim filter.revision_no = revision_no; filter.product_code = product_code; ESI::Parser parser; - ESI::Device device = parser.loadDevice(coe_xml_full_path.string(), filter); + ESI::Device device = parser.loadDevice(coe_xml_full_path, filter); CoE::materializeStorage(device.dictionary); configureDeviceDictionary(sim, device); } } else { - throw std::runtime_error("Config file " + config_path.string() + " missing 'eeprom' or 'esi' field"); + throw std::runtime_error("Config file " + config_path + " missing 'eeprom' or 'esi' field"); } sim.input.resize(PDO_MAX_SIZE); diff --git a/lib/slave/include/kickcat/ESC/EmulatedESC.h b/lib/slave/include/kickcat/ESC/EmulatedESC.h index 4f680493..47479133 100644 --- a/lib/slave/include/kickcat/ESC/EmulatedESC.h +++ b/lib/slave/include/kickcat/ESC/EmulatedESC.h @@ -1,15 +1,13 @@ #ifndef KICKCAT_SLAVE_ESC_EMULATED_ESC_H #define KICKCAT_SLAVE_ESC_EMULATED_ESC_H -#include +#include #include "kickcat/protocol.h" #include "kickcat/AbstractESC.h" namespace kickcat { - namespace fs = std::filesystem; - class EmulatedESC final : public AbstractESC { // ESC access type @@ -23,11 +21,11 @@ namespace kickcat public: EmulatedESC(); - EmulatedESC(fs::path const& eeprom_path); + EmulatedESC(std::string const& eeprom_path); virtual ~EmulatedESC() = default; // Helpers to load the eeprom - void loadEeprom(fs::path const& eeprom_path); + void loadEeprom(std::string const& eeprom_path); void loadEeprom(std::vector const& eeprom_data); void loadEeprom(std::vector const& image); // word-addressed; odd trailing byte zero-filled @@ -274,7 +272,6 @@ namespace kickcat mutable uint64_t jitter_rng_{0x2545f4914f6cdd1dull}; }; - std::vector loadBinaryFile(fs::path const& path); } #endif diff --git a/lib/slave/src/ESC/EmulatedESC.cc b/lib/slave/src/ESC/EmulatedESC.cc index 7b870f93..329dac14 100644 --- a/lib/slave/src/ESC/EmulatedESC.cc +++ b/lib/slave/src/ESC/EmulatedESC.cc @@ -1,7 +1,7 @@ #include -#include #include "kickcat/ESC/EmulatedESC.h" +#include "kickcat/OS/Filesystem.h" #include "kickcat/OS/Time.h" #include "kickcat/debug.h" @@ -43,31 +43,15 @@ namespace kickcat std::memset(memory_.sync_manager, 0, sizeof(memory_.sync_manager)); } - EmulatedESC::EmulatedESC(fs::path const& path) + EmulatedESC::EmulatedESC(std::string const& path) : EmulatedESC() { loadEeprom(path); } - void EmulatedESC::loadEeprom(fs::path const& path) + void EmulatedESC::loadEeprom(std::string const& path) { - std::vector image = loadBinaryFile(path); - loadEeprom(image); - } - - std::vector loadBinaryFile(fs::path const& path) { - std::ifstream eeprom_file; - eeprom_file.open(path, std::ios::binary | std::ios::ate); - if (not eeprom_file.is_open()) - { - THROW_ERROR("Cannot load EEPROM"); - } - int size = eeprom_file.tellg(); - eeprom_file.seekg (0, std::ios::beg); - std::vector image(static_cast(size)); - eeprom_file.read(reinterpret_cast(image.data()), size); - eeprom_file.close(); - return image; + loadEeprom(filesystem::readFile(path)); } void EmulatedESC::loadEeprom(std::vector const& eeprom_data) diff --git a/lib/src/CoE/mailbox/request.cc b/lib/src/CoE/mailbox/request.cc index f1e420b2..cddd0392 100644 --- a/lib/src/CoE/mailbox/request.cc +++ b/lib/src/CoE/mailbox/request.cc @@ -1,17 +1,24 @@ #include #include +#include "Error.h" #include "debug.h" #include "kickcat/CoE/mailbox/request.h" namespace kickcat::mailbox::request { - SDOMessage::SDOMessage(uint16_t mailbox_size, uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout) - : AbstractMessage(mailbox_size, timeout) + SDOMessage::SDOMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout) + : AbstractMessage(mbx_recv_size, mbx_send_size, timeout) , client_data_(reinterpret_cast(data)) , client_data_size_(data_size) , client_buffer_size_(*data_size) { + // What this message writes below: headers, service data and the four expedited/size bytes. + if (data_.size() < (sizeof(mailbox::Header) + sizeof(CoE::Header) + sizeof(CoE::ServiceData) + sizeof(uint32_t))) + { + THROW_ERROR("Mailbox is too small to hold an SDO request"); + } + coe_ = pointData(header_); sdo_ = pointData(coe_); payload_ = pointData(sdo_); @@ -106,9 +113,9 @@ namespace kickcat::mailbox::request return ProcessingResult::FINALIZE; } - // the declared service-data length cannot exceed what the mailbox carries; otherwise the + // the declared service-data length cannot exceed what the reply carries; otherwise the // size/segment reads below would run past the end of the received frame - if ((sizeof(mailbox::Header) + header->len) > data_.size()) + if ((sizeof(mailbox::Header) + header->len) > send_size_) { status_ = MessageStatus::COE_WRONG_SERVICE; return ProcessingResult::FINALIZE; @@ -322,12 +329,18 @@ namespace kickcat::mailbox::request } - SDOInformationMessage::SDOInformationMessage(uint16_t mailbox_size, uint8_t request, void* data, uint32_t* data_size, + SDOInformationMessage::SDOInformationMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint8_t request, void* data, uint32_t* data_size, uint32_t request_payload_size, nanoseconds timeout) - : AbstractMessage(mailbox_size, timeout) + : AbstractMessage(mbx_recv_size, mbx_send_size, timeout) , client_data_(reinterpret_cast(data)) , client_data_size_(data_size) { + if (data_.size() < (sizeof(mailbox::Header) + sizeof(CoE::Header) + sizeof(CoE::ServiceDataInfo) + + request_payload_size)) + { + THROW_ERROR("Mailbox is too small to hold an SDO information request"); + } + coe_ = pointData(header_); sdo_ = pointData(coe_); payload_ = pointData(sdo_); @@ -409,6 +422,13 @@ namespace kickcat::mailbox::request return ProcessingResult::FINALIZE; } + // the fragment length is taken from the reply itself, so it must fit what the reply carries + if ((sizeof(mailbox::Header) + header->len) > send_size_) + { + status_ = MessageStatus::COE_WRONG_SERVICE; + return ProcessingResult::FINALIZE; + } + int32_t size = header->len - sizeof(CoE::ServiceDataInfo) - sizeof(CoE::Header); int32_t remaining_size = *client_data_size_ - already_received_size_; @@ -449,7 +469,7 @@ namespace kickcat::mailbox::request } EmergencyMessage::EmergencyMessage(Mailbox& mailbox) - : AbstractMessage(mailbox.recv_size, 0ns) + : AbstractMessage(mailbox.recv_size, mailbox.send_size, 0ns) , mailbox_{mailbox} { } @@ -474,7 +494,7 @@ namespace kickcat::mailbox::request } CheckMessage::CheckMessage(Mailbox& mailbox) - : AbstractMessage(mailbox.recv_size, 0ns) + : AbstractMessage(mailbox.recv_size, mailbox.send_size, 0ns) , mailbox_{mailbox} { } diff --git a/lib/src/CoE/mailbox/response.cc b/lib/src/CoE/mailbox/response.cc index e1c8f2a0..47954bc9 100644 --- a/lib/src/CoE/mailbox/response.cc +++ b/lib/src/CoE/mailbox/response.cc @@ -15,11 +15,25 @@ namespace kickcat::mailbox::response return nullptr; } + // The received buffer -- not the announced len -- is what bounds the reads below: a + // mailbox message shorter than the CoE header it claims to carry must be refused before + // the service field is read. + if (raw_message.size() < (sizeof(mailbox::Header) + sizeof(CoE::Header))) + { + return std::make_shared( + mbx, std::move(raw_message), mailbox::Error::SIZE_TOO_SHORT); + } + auto const* coe = pointData(header); switch (coe->service) { case CoE::Service::SDO_REQUEST: { + if (raw_message.size() < (sizeof(mailbox::Header) + sizeof(CoE::Header) + sizeof(CoE::ServiceData))) + { + return std::make_shared( + mbx, std::move(raw_message), mailbox::Error::SIZE_TOO_SHORT); + } return std::make_shared(mbx, std::move(raw_message)); } case CoE::Service::EMERGENCY: @@ -33,6 +47,11 @@ namespace kickcat::mailbox::response } case CoE::Service::SDO_INFORMATION: { + if (raw_message.size() < (sizeof(mailbox::Header) + sizeof(CoE::Header) + sizeof(CoE::ServiceDataInfo))) + { + return std::make_shared( + mbx, std::move(raw_message), mailbox::Error::SIZE_TOO_SHORT); + } return std::make_shared(mbx, std::move(raw_message)); } @@ -79,6 +98,24 @@ namespace kickcat::mailbox::response return ProcessingResult::FINALIZE; } + // ETG.1000.6 Table 109: the command specifier is checked on reception, before any object + // lookup, so the answer does not depend on the dictionary. + if (sdo_->command > CoE::SDO::request::UPLOAD_SEGMENTED) + { + // Above 3 is not a client service (4 is the server's own abort): invalid header. + replyError(std::move(data_), mailbox::Error::INVALID_HEADER); + return ProcessingResult::FINALIZE; + } + + if ((sdo_->command == CoE::SDO::request::DOWNLOAD_SEGMENTED) + or (sdo_->command == CoE::SDO::request::UPLOAD_SEGMENTED)) + { + // A segment with no transfer in progress: process(raw_message) serves the ones that + // belong to a transfer, so reaching here is out of sequence. + abort(CoE::SDO::abort::COMMAND_SPECIFIER_INVALID); + return ProcessingResult::FINALIZE; + } + auto [object, entry] = findObject(mailbox_->getDictionary(), sdo_->index, sdo_->subindex); if (object == nullptr) { @@ -123,11 +160,23 @@ namespace kickcat::mailbox::response return ProcessingResult::NOOP; // not serving a segmented upload } + // Both the buffer and the announced len must cover the CoE service data before coe/sdo + // are dereferenced: the segment payload below is located from len. + constexpr std::size_t MIN_SIZE = sizeof(mailbox::Header) + sizeof(CoE::Header) + sizeof(CoE::ServiceData); + if (raw_message.size() < MIN_SIZE) + { + return ProcessingResult::NOOP; + } + auto const* header = pointData(raw_message.data()); - auto const* coe = pointData(header); - auto const* sdo = pointData(coe); - if ((header->type != mailbox::Type::CoE) or (coe->service != CoE::Service::SDO_REQUEST) - or (header->len < 10)) + if ((header->len < 10) or (header->type != mailbox::Type::CoE)) + { + return ProcessingResult::NOOP; + } + + auto const* coe = pointData(header); + auto const* sdo = pointData(coe); + if (coe->service != CoE::Service::SDO_REQUEST) { return ProcessingResult::NOOP; } @@ -524,6 +573,12 @@ namespace kickcat::mailbox::response ProcessingResult SDOInformationMessage::process() { + if (data_.size() < (sizeof(mailbox::Header) + CoE::SDO::information::responseSize(sdo_->opcode))) + { + replyError(std::move(data_), mailbox::Error::SIZE_TOO_SHORT); + return ProcessingResult::FINALIZE; + } + switch (sdo_->opcode) { case CoE::SDO::information::GET_OD_LIST_REQ: { return processODList(); } @@ -756,7 +811,7 @@ namespace kickcat::mailbox::response ProcessingResult MailboxErrorMessage::process() { - replyError(std::move(data_), mailbox::Error::INVALID_HEADER); + replyError(std::move(data_), error_); return ProcessingResult::FINALIZE; } diff --git a/lib/src/ESI/Parser.cc b/lib/src/ESI/Parser.cc index 9e6972a0..b27aa8b9 100644 --- a/lib/src/ESI/Parser.cc +++ b/lib/src/ESI/Parser.cc @@ -1803,7 +1803,7 @@ uint16_t Parser::loadAccess(XMLNode* node) std::string restrictions{raw_restrictions}; std::transform(restrictions.begin(), restrictions.end(), restrictions.begin(), - [](char c){ return std::tolower(c); }); + [](char c){ return static_cast(std::tolower(static_cast(c))); }); uint16_t result = 0; if (restrictions.find("preop") != std::string::npos) { result |= CoE::Access::READ_PREOP; } @@ -1829,11 +1829,11 @@ uint16_t Parser::loadAccess(XMLNode* node) { for (char const* c = mapping; *c != '\0'; ++c) { - if (std::tolower(*c) == 'r') + if (std::tolower(static_cast(*c)) == 'r') { flags |= CoE::Access::RxPDO; } - if (std::tolower(*c) == 't') + if (std::tolower(static_cast(*c)) == 't') { flags |= CoE::Access::TxPDO; } diff --git a/lib/src/Mailbox.cc b/lib/src/Mailbox.cc index 576b4d26..1643c34f 100644 --- a/lib/src/Mailbox.cc +++ b/lib/src/Mailbox.cc @@ -56,7 +56,7 @@ namespace kickcat::mailbox::request { THROW_ERROR("This mailbox is inactive"); } - auto sdo = std::make_shared(recv_size, index, subindex, CA, request, data, data_size, timeout); + auto sdo = std::make_shared(recv_size, send_size, index, subindex, CA, request, data, data_size, timeout); sdo->setCounter(nextCounter()); to_send.push(sdo); return sdo; @@ -83,7 +83,7 @@ namespace kickcat::mailbox::request header->len, raw_message_size, gateway_index); return nullptr; } - auto msg = std::make_shared(recv_size, raw_message, gateway_index, timeout); + auto msg = std::make_shared(recv_size, send_size, raw_message, gateway_index, timeout); msg->setCounter(nextCounter()); to_send.push(msg); return msg; @@ -100,7 +100,7 @@ namespace kickcat::mailbox::request uint32_t request_payload_size = sizeof(type); std::memcpy(data, &type, request_payload_size); - auto sdo = std::make_shared(recv_size, CoE::SDO::information::GET_OD_LIST_REQ, data, data_size, request_payload_size, timeout); + auto sdo = std::make_shared(recv_size, send_size, CoE::SDO::information::GET_OD_LIST_REQ, data, data_size, request_payload_size, timeout); sdo->setCounter(nextCounter()); to_send.push(sdo); return sdo; @@ -116,7 +116,7 @@ namespace kickcat::mailbox::request uint32_t request_payload_size = sizeof(index); std::memcpy(data, &index, request_payload_size); - auto sdo = std::make_shared(recv_size, CoE::SDO::information::GET_OD_REQ, data, data_size, request_payload_size, timeout); + auto sdo = std::make_shared(recv_size, send_size, CoE::SDO::information::GET_OD_REQ, data, data_size, request_payload_size, timeout); sdo->setCounter(nextCounter()); to_send.push(sdo); return sdo; @@ -137,7 +137,7 @@ namespace kickcat::mailbox::request std::memcpy(static_cast(data) + sizeof(index), &subindex, sizeof(subindex)); std::memcpy(static_cast(data) + sizeof(index) + sizeof(subindex), &value_info, sizeof(value_info)); uint32_t request_payload_size = sizeof(index) + sizeof(subindex) + sizeof(value_info); - auto sdo = std::make_shared(recv_size, CoE::SDO::information::GET_ED_REQ, data, data_size, request_payload_size, timeout); + auto sdo = std::make_shared(recv_size, send_size, CoE::SDO::information::GET_ED_REQ, data, data_size, request_payload_size, timeout); sdo->setCounter(nextCounter()); to_send.push(sdo); return sdo; @@ -201,14 +201,15 @@ namespace kickcat::mailbox::request } - AbstractMessage::AbstractMessage(uint16_t mailbox_size, nanoseconds timeout) - : timeout_{timeout} + AbstractMessage::AbstractMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, nanoseconds timeout) + : send_size_{mbx_send_size} + , timeout_{timeout} { // A slave may advertise a mailbox protocol yet a zero (or sub-header) mailbox // size in its SII (seen in the wild, e.g. Beckhoff AMP8805-A000). The buffer // must still hold a header, otherwise data() is null and the writes below // dereference it. - data_.resize(std::max(mailbox_size, sizeof(mailbox::Header))); + data_.resize(std::max(mbx_recv_size, sizeof(mailbox::Header))); header_ = reinterpret_cast(data_.data()); header_->address = 0; // Default: local processing address status_ = MessageStatus::RUNNING; // Default mode is running to send the msg on the bus @@ -233,8 +234,8 @@ namespace kickcat::mailbox::request } - GatewayMessage::GatewayMessage(uint16_t mailbox_size, uint8_t const* raw_message, uint16_t gateway_index, nanoseconds timeout) - : AbstractMessage(mailbox_size, timeout) + GatewayMessage::GatewayMessage(uint16_t mbx_recv_size, uint16_t mbx_send_size, uint8_t const* raw_message, uint16_t gateway_index, nanoseconds timeout) + : AbstractMessage(mbx_recv_size, mbx_send_size, timeout) { auto const* header = pointData(raw_message); @@ -255,8 +256,9 @@ namespace kickcat::mailbox::request } + // send size 0: this message is already complete, no bus reply is processed for it. GatewayMessage::GatewayMessage(std::vector&& reply, uint16_t gateway_index) - : AbstractMessage(static_cast(reply.size()), 0ms) + : AbstractMessage(static_cast(reply.size()), 0, 0ms) { // SDO response preserves header->address, so no mask-tag + process() round-trip is needed. data_ = std::move(reply); @@ -279,15 +281,18 @@ namespace kickcat::mailbox::request // It is the reply to this request: store the result and set back the address field int32_t size = header->len + sizeof(mailbox::Header); - if (size > static_cast(data_.size())) + if (size > static_cast(send_size_)) { // oversized reply: drop rather than over-read 'received' (the message then times out) - gateway_error("Reply for gateway index %u claims %d bytes, exceeds mailbox size %zu; dropping it\n", - gateway_index_, size, data_.size()); + gateway_error("Reply for gateway index %u claims %d bytes, exceeds send mailbox size %u; dropping it\n", + gateway_index_, size, send_size_); return ProcessingResult::NOOP; } + // On an asymmetric mailbox the reply can be larger than the request buffer, so this resize + // may reallocate: header_ has to follow the new storage. data_.resize(size); std::memcpy(data_.data(), received, size); + header_ = pointData(data_.data()); header_->address = address_; @@ -382,6 +387,23 @@ namespace kickcat::mailbox::response void Mailbox::handleMessage(std::vector&& raw_message) { + // The buffer is sized by the mailbox SyncManager (or by the gateway request) while len + // comes from the wire: handlers below locate their payload from len, so an incoherent pair + // must be rejected here rather than dereferenced. + if (raw_message.size() < sizeof(mailbox::Header)) + { + // replyError zero-pads up to the error reply, so an answer is still possible. + replyError(std::move(raw_message), mailbox::Error::SIZE_TOO_SHORT); + return; + } + + auto const* header = pointData(raw_message.data()); + if ((sizeof(mailbox::Header) + header->len) > raw_message.size()) + { + replyError(std::move(raw_message), mailbox::Error::INVALID_SIZE); + return; + } + for (auto it = to_process_.begin(); it != to_process_.end(); ++it) { ProcessingResult state = (*it)->process(raw_message); diff --git a/lib/src/OS/Filesystem.cc b/lib/src/OS/Filesystem.cc new file mode 100644 index 00000000..5bfa3eab --- /dev/null +++ b/lib/src/OS/Filesystem.cc @@ -0,0 +1,77 @@ +// \brief OS agnostic filesystem helpers - pure path string handling, no syscall +#include "kickcat/OS/Filesystem.h" + +namespace kickcat::filesystem +{ + std::string parent(std::string const& path) + { + std::size_t separator = path.find_last_of('/'); + if (separator == std::string::npos) + { + return {}; + } + // Keep the root itself for "/name": trimming to "" would turn it into a relative path. + if (separator == 0) + { + return "/"; + } + return path.substr(0, separator); + } + + std::string filename(std::string const& path) + { + std::size_t separator = path.find_last_of('/'); + if (separator == std::string::npos) + { + return path; + } + return path.substr(separator + 1); + } + + std::string extension(std::string const& path) + { + std::string leaf = filename(path); + std::size_t dot = leaf.find_last_of('.'); + // A leading dot names a hidden file, it does not start an extension. + if ((dot == std::string::npos) or (dot == 0)) + { + return {}; + } + return leaf.substr(dot); + } + + std::vector listFilesRecursive(std::string const& directory) + { + std::vector files; + for (auto const& entry : list(directory)) + { + std::string path = join(directory, entry.name); + if (entry.is_directory) + { + std::vector nested = listFilesRecursive(path); + files.insert(files.end(), nested.begin(), nested.end()); + continue; + } + files.push_back(path); + } + return files; + } + + std::string join(std::string const& directory, std::string const& name) + { + if (directory.empty()) + { + return name; + } + if (directory.back() == '/') + { + return directory + name; + } + return directory + "/" + name; + } + + void writeFile(std::string const& path, std::string const& content) + { + writeFile(path, content.data(), content.size()); + } +} diff --git a/lib/src/OS/KickOS/Filesystem.cc b/lib/src/OS/KickOS/Filesystem.cc new file mode 100644 index 00000000..eff19a50 --- /dev/null +++ b/lib/src/OS/KickOS/Filesystem.cc @@ -0,0 +1,51 @@ +// KickOS filesystem backend: not implemented yet. Everything throws so the full library links for +// KickOS; nothing on that target reads files today (EmulatedESC only needs it for the host-side +// EEPROM loaders). +#include "Error.h" +#include "OS/Filesystem.h" + +// Throw-only placeholders: -Wmissing-noreturn is expected until a real backend lands. +#pragma GCC diagnostic ignored "-Wmissing-noreturn" + +namespace kickcat::filesystem +{ + bool exists(std::string const&) + { + THROW_ERROR("filesystem::exists() not implemented on KickOS"); + } + + bool isDirectory(std::string const&) + { + THROW_ERROR("filesystem::isDirectory() not implemented on KickOS"); + } + + bool createDirectory(std::string const&) + { + THROW_ERROR("filesystem::createDirectory() not implemented on KickOS"); + } + + bool removeFile(std::string const&) + { + THROW_ERROR("filesystem::removeFile() not implemented on KickOS"); + } + + bool removeDirectory(std::string const&) + { + THROW_ERROR("filesystem::removeDirectory() not implemented on KickOS"); + } + + std::vector list(std::string const&) + { + THROW_ERROR("filesystem::list() not implemented on KickOS"); + } + + std::vector readFile(std::string const&) + { + THROW_ERROR("filesystem::readFile() not implemented on KickOS"); + } + + void writeFile(std::string const&, void const*, std::size_t) + { + THROW_ERROR("filesystem::writeFile() not implemented on KickOS"); + } +} diff --git a/lib/src/OS/SoftPll.cc b/lib/src/OS/SoftPll.cc index ba9c89b3..44b72009 100644 --- a/lib/src/OS/SoftPll.cc +++ b/lib/src/OS/SoftPll.cc @@ -1,5 +1,5 @@ #include "kickcat/OS/SoftPll.h" -#include "kickcat/OS/math.h" +#include "kickcat/utils/math.h" namespace kickcat { diff --git a/lib/src/OS/Timer.cc b/lib/src/OS/Timer.cc index a1f357db..0e438c90 100644 --- a/lib/src/OS/Timer.cc +++ b/lib/src/OS/Timer.cc @@ -1,5 +1,6 @@ // \brief OS agnostic Timer API - shared logic #include "kickcat/OS/Timer.h" +#include "kickcat/Error.h" namespace kickcat { @@ -8,6 +9,10 @@ namespace kickcat , pll_config_{pll_config} , pll_{period, pll_config} { + if (period_ <= 0ns) + { + THROW_ERROR("Timer period shall be strictly positive"); + } } nanoseconds Timer::period() const @@ -30,6 +35,11 @@ namespace kickcat void Timer::update_period(nanoseconds period) { + if (period <= 0ns) + { + THROW_ERROR("Timer period shall be strictly positive"); + } + period_ = period; // The grid changed, so the PLL's learned target phase is stale: rebuild on the new cycle. pll_ = SoftPll{period, pll_config_}; diff --git a/lib/src/OS/Unix/ConditionVariable.cc b/lib/src/OS/Unix/ConditionVariable.cc index b0c3751f..f122def2 100644 --- a/lib/src/OS/Unix/ConditionVariable.cc +++ b/lib/src/OS/Unix/ConditionVariable.cc @@ -1,4 +1,7 @@ #include +#include +#include +#include #include "Error.h" #include "OS/ConditionVariable.h" @@ -24,7 +27,10 @@ namespace kickcat int rc = pthread_cond_destroy(pcond_); if (rc != 0) { - THROW_SYSTEM_ERROR_CODE("pthread_cond_destroy()", rc); + // Threads are still waiting on it: they would never be woken, and the storage is + // about to go away. A destructor cannot report this by throwing. + std::fprintf(stderr, "~ConditionVariable: pthread_cond_destroy() failed: %s\n", std::strerror(rc)); + std::abort(); } } } diff --git a/lib/src/OS/Unix/Filesystem.cc b/lib/src/OS/Unix/Filesystem.cc new file mode 100644 index 00000000..b094c963 --- /dev/null +++ b/lib/src/OS/Unix/Filesystem.cc @@ -0,0 +1,182 @@ +#include +#include +#include +#include +#include + +#include "Error.h" +#include "OS/Filesystem.h" + +namespace kickcat::filesystem +{ + namespace + { + constexpr std::size_t CHUNK_SIZE = 64 * 1024; + } + + bool exists(std::string const& path) + { + struct stat info; + return ::stat(path.c_str(), &info) == 0; + } + + bool isDirectory(std::string const& path) + { + struct stat info; + if (::stat(path.c_str(), &info) != 0) + { + return false; + } + return S_ISDIR(info.st_mode); + } + + bool createDirectory(std::string const& path) + { + if (::mkdir(path.c_str(), 0755) == 0) + { + return true; + } + if (errno == EEXIST) + { + return false; + } + THROW_SYSTEM_ERROR("mkdir()"); + } + + bool removeFile(std::string const& path) + { + if (::unlink(path.c_str()) == 0) + { + return true; + } + if (errno == ENOENT) + { + return false; + } + THROW_SYSTEM_ERROR("unlink()"); + } + + bool removeDirectory(std::string const& path) + { + if (::rmdir(path.c_str()) == 0) + { + return true; + } + if (errno == ENOENT) + { + return false; + } + THROW_SYSTEM_ERROR("rmdir()"); + } + + std::vector list(std::string const& path) + { + DIR* directory = ::opendir(path.c_str()); + if (directory == nullptr) + { + THROW_SYSTEM_ERROR("opendir()"); + } + + std::vector entries; + while (true) + { + errno = 0; // readdir returns nullptr for both end-of-stream and error + dirent const* item = ::readdir(directory); + if (item == nullptr) + { + break; + } + + std::string name{item->d_name}; + if ((name == ".") or (name == "..")) + { + continue; + } + + bool is_directory = (item->d_type == DT_DIR); + if (item->d_type == DT_UNKNOWN) + { + // Not every filesystem fills d_type. + is_directory = isDirectory(join(path, name)); + } + entries.push_back(Entry{name, is_directory}); + } + + int code = errno; + ::closedir(directory); + if (code != 0) + { + THROW_SYSTEM_ERROR_CODE("readdir()", code); + } + return entries; + } + + std::vector readFile(std::string const& path) + { + int fd = ::open(path.c_str(), O_RDONLY); + if (fd < 0) + { + THROW_SYSTEM_ERROR("open()"); + } + + std::vector content; + struct stat info; + if ((::fstat(fd, &info) == 0) and (info.st_size > 0)) + { + content.reserve(static_cast(info.st_size)); + } + + uint8_t chunk[CHUNK_SIZE]; + while (true) + { + ssize_t count = ::read(fd, chunk, sizeof(chunk)); + if (count == 0) + { + break; + } + if (count < 0) + { + if (errno == EINTR) + { + continue; + } + int code = errno; + ::close(fd); + THROW_SYSTEM_ERROR_CODE("read()", code); + } + content.insert(content.end(), chunk, chunk + count); + } + + ::close(fd); + return content; + } + + void writeFile(std::string const& path, void const* data, std::size_t size) + { + int fd = ::open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, 0644); + if (fd < 0) + { + THROW_SYSTEM_ERROR("open()"); + } + + auto const* bytes = static_cast(data); + std::size_t written = 0; + while (written < size) + { + ssize_t count = ::write(fd, bytes + written, size - written); + if (count < 0) + { + if (errno == EINTR) + { + continue; + } + int code = errno; + ::close(fd); + THROW_SYSTEM_ERROR_CODE("write()", code); + } + written += static_cast(count); + } + + ::close(fd); + } +} diff --git a/lib/src/OS/Unix/Mutex.cc b/lib/src/OS/Unix/Mutex.cc index a54a838f..5f73a103 100644 --- a/lib/src/OS/Unix/Mutex.cc +++ b/lib/src/OS/Unix/Mutex.cc @@ -1,3 +1,7 @@ +#include +#include +#include + #include "Error.h" #include "OS/Mutex.h" @@ -22,7 +26,10 @@ namespace kickcat int rc = pthread_mutex_destroy(pmutex_); if (rc != 0) { - THROW_SYSTEM_ERROR_CODE("pthread_mutex_destroy()", rc); + // Still locked or referenced, and the storage is about to go away: whoever holds it + // would be left with a dangling mutex. A destructor cannot report this by throwing. + std::fprintf(stderr, "~Mutex: pthread_mutex_destroy() failed: %s\n", std::strerror(rc)); + std::abort(); } } } diff --git a/lib/src/OS/Windows/Filesystem.cc b/lib/src/OS/Windows/Filesystem.cc new file mode 100644 index 00000000..b85a4b82 --- /dev/null +++ b/lib/src/OS/Windows/Filesystem.cc @@ -0,0 +1,174 @@ +#include + +#include + +#include "Error.h" +#include "OS/Filesystem.h" + +namespace kickcat::filesystem +{ + #define THROW_LAST_ERROR(msg) (throw std::system_error(static_cast(GetLastError()), std::system_category(), LOCATION(": " msg))) + + namespace + { + constexpr DWORD CHUNK_SIZE = 64 * 1024; + + bool isAbsent(DWORD error) + { + return (error == ERROR_FILE_NOT_FOUND) or (error == ERROR_PATH_NOT_FOUND); + } + } + + bool exists(std::string const& path) + { + return GetFileAttributesA(path.c_str()) != INVALID_FILE_ATTRIBUTES; + } + + bool isDirectory(std::string const& path) + { + DWORD attributes = GetFileAttributesA(path.c_str()); + if (attributes == INVALID_FILE_ATTRIBUTES) + { + return false; + } + return (attributes & FILE_ATTRIBUTE_DIRECTORY) != 0; + } + + bool createDirectory(std::string const& path) + { + if (CreateDirectoryA(path.c_str(), nullptr) != 0) + { + return true; + } + if (GetLastError() == ERROR_ALREADY_EXISTS) + { + return false; + } + THROW_LAST_ERROR("CreateDirectory() failed"); + } + + bool removeFile(std::string const& path) + { + if (DeleteFileA(path.c_str()) != 0) + { + return true; + } + if (isAbsent(GetLastError())) + { + return false; + } + THROW_LAST_ERROR("DeleteFile() failed"); + } + + bool removeDirectory(std::string const& path) + { + if (RemoveDirectoryA(path.c_str()) != 0) + { + return true; + } + if (isAbsent(GetLastError())) + { + return false; + } + THROW_LAST_ERROR("RemoveDirectory() failed"); + } + + std::vector list(std::string const& path) + { + WIN32_FIND_DATAA item; + HANDLE search = FindFirstFileA(join(path, "*").c_str(), &item); + if (search == INVALID_HANDLE_VALUE) + { + THROW_LAST_ERROR("FindFirstFile() failed"); + } + + std::vector entries; + do + { + std::string name{item.cFileName}; + if ((name == ".") or (name == "..")) + { + continue; + } + entries.push_back(Entry{name, (item.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) != 0}); + } + while (FindNextFileA(search, &item) != 0); + + DWORD error = GetLastError(); + FindClose(search); + if (error != ERROR_NO_MORE_FILES) + { + throw std::system_error(static_cast(error), std::system_category(), LOCATION(": FindNextFile() failed")); + } + return entries; + } + + std::vector readFile(std::string const& path) + { + HANDLE file = CreateFileA(path.c_str(), GENERIC_READ, FILE_SHARE_READ, nullptr, + OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, nullptr); + if (file == INVALID_HANDLE_VALUE) + { + THROW_LAST_ERROR("CreateFile() for reading failed"); + } + + std::vector content; + LARGE_INTEGER size; + if ((GetFileSizeEx(file, &size) != 0) and (size.QuadPart > 0)) + { + content.reserve(static_cast(size.QuadPart)); + } + + std::vector chunk(CHUNK_SIZE); + while (true) + { + DWORD count = 0; + if (ReadFile(file, chunk.data(), CHUNK_SIZE, &count, nullptr) == 0) + { + DWORD error = GetLastError(); + CloseHandle(file); + throw std::system_error(static_cast(error), std::system_category(), LOCATION(": ReadFile() failed")); + } + if (count == 0) + { + break; + } + content.insert(content.end(), chunk.begin(), chunk.begin() + count); + } + + CloseHandle(file); + return content; + } + + void writeFile(std::string const& path, void const* data, std::size_t size) + { + HANDLE file = CreateFileA(path.c_str(), GENERIC_WRITE, 0, nullptr, + CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, nullptr); + if (file == INVALID_HANDLE_VALUE) + { + THROW_LAST_ERROR("CreateFile() for writing failed"); + } + + auto const* bytes = static_cast(data); + std::size_t written = 0; + while (written < size) + { + DWORD chunk = CHUNK_SIZE; + if ((size - written) < CHUNK_SIZE) + { + chunk = static_cast(size - written); + } + + DWORD count = 0; + if (WriteFile(file, bytes + written, chunk, &count, nullptr) == 0) + { + DWORD error = GetLastError(); + CloseHandle(file); + throw std::system_error(static_cast(error), std::system_category(), LOCATION(": WriteFile() failed")); + } + written += count; + } + + CloseHandle(file); + } +} diff --git a/test/integration/bench/esi_boot.cc b/test/integration/bench/esi_boot.cc index 947495fb..e03d1521 100644 --- a/test/integration/bench/esi_boot.cc +++ b/test/integration/bench/esi_boot.cc @@ -13,7 +13,6 @@ #include #include #include -#include #include #include #include @@ -23,6 +22,7 @@ #include +#include "kickcat/OS/Filesystem.h" #include "kickcat/OS/Mutex.h" #include "kickcat/CoE/OD.h" @@ -39,7 +39,6 @@ #include "kickcat/LoopbackSocket.h" using namespace kickcat; -namespace fs = std::filesystem; enum class Reached { INIT_FAIL, INIT, PRE_OP, SAFE_OP, OP }; @@ -216,17 +215,17 @@ int main(int argc, char** argv) return 2; } - fs::path root = path_arg; + std::string const& root = path_arg; if (num_threads < 1) { num_threads = 1; } setvbuf(stdout, nullptr, _IONBF, 0); - std::vector xmls; - if (fs::is_directory(root)) + std::vector xmls; + if (filesystem::isDirectory(root)) { - for (auto& e : fs::recursive_directory_iterator(root)) + for (auto& file : filesystem::listFilesRecursive(root)) { - if (e.path().extension() == ".xml") { xmls.push_back(e.path()); } + if (filesystem::extension(file) == ".xml") { xmls.push_back(file); } } std::sort(xmls.begin(), xmls.end()); } @@ -261,7 +260,7 @@ int main(int argc, char** argv) std::vector devs; try { - devs = parser.loadAllDevices(xmls[idx].string(), &errs); + devs = parser.loadAllDevices(xmls[idx], &errs); } catch (std::exception&) { @@ -270,7 +269,7 @@ int main(int argc, char** argv) Counts local; std::string failures; - std::string fname = xmls[idx].filename().string(); + std::string fname = filesystem::filename(xmls[idx]); for (auto& dev : devs) { local.total++; diff --git a/test/integration/bench/esi_validate.cc b/test/integration/bench/esi_validate.cc index e7d217db..d64f18bb 100644 --- a/test/integration/bench/esi_validate.cc +++ b/test/integration/bench/esi_validate.cc @@ -1,6 +1,5 @@ #include #include -#include #include #include #include @@ -9,12 +8,12 @@ #include "kickcat/ESI/Parser.h" #include "kickcat/ESI/SIIBuilder.h" +#include "kickcat/OS/Filesystem.h" #include "kickcat/SIIParser.h" #include "kickcat/CoE/OD.h" #include "kickcat/CoE/protocol.h" using namespace kickcat; -namespace fs = std::filesystem; // Replicates the slave-side SAFE_OP mapping resolution (PDO::parseAssignment + // parsePdoMap, ETG.1000.6 Tables 74/75): every PDO in a SyncManager assignment @@ -79,7 +78,7 @@ int main(int argc, char** argv) return 2; } - fs::path dir = path_arg; + std::string const& dir = path_arg; int files = 0, total = 0, built = 0, build_fail = 0, sii_ok = 0, sii_fail = 0; int map_ok = 0, map_fail = 0; @@ -90,31 +89,31 @@ int main(int argc, char** argv) if (hb) { std::fputs(s.c_str(), hb); std::fclose(hb); } }; - std::vector xmls; - for (auto& e : fs::recursive_directory_iterator(dir)) + std::vector xmls; + for (auto& file : filesystem::listFilesRecursive(dir)) { - if (e.path().extension() == ".xml") { xmls.push_back(e.path()); } + if (filesystem::extension(file) == ".xml") { xmls.push_back(file); } } std::sort(xmls.begin(), xmls.end()); for (auto& path : xmls) { files++; - heartbeat("PARSING " + path.filename().string()); + heartbeat("PARSING " + filesystem::filename(path)); ESI::Parser p; std::vector errs; std::vector devs; - try { devs = p.loadAllDevices(path.string(), &errs); } - catch (std::exception& ex) { printf("FILE-FAIL %s : %s\n", path.filename().string().c_str(), ex.what()); continue; } + try { devs = p.loadAllDevices(path, &errs); } + catch (std::exception& ex) { printf("FILE-FAIL %s : %s\n", filesystem::filename(path).c_str(), ex.what()); continue; } total += (int)devs.size() + (int)errs.size(); built += (int)devs.size(); build_fail += (int)errs.size(); - for (auto& er : errs) { printf("BUILD-FAIL %s | %s\n", path.filename().string().c_str(), er.c_str()); } + for (auto& er : errs) { printf("BUILD-FAIL %s | %s\n", filesystem::filename(path).c_str(), er.c_str()); } for (auto& d : devs) { - heartbeat(path.filename().string() + " | " + d.type); + heartbeat(filesystem::filename(path) + " | " + d.type); try { auto img = ESI::buildEepromImage(d); @@ -124,19 +123,19 @@ int main(int argc, char** argv) && (s.info.product_code == d.product_code) && (eeprom::computeInfoCRC(s.info) == s.info.crc); if (ok) { sii_ok++; } - else { sii_fail++; printf("SII-BAD %s | %s pc=0x%08x\n", path.filename().string().c_str(), d.type.c_str(), d.product_code); } + else { sii_fail++; printf("SII-BAD %s | %s pc=0x%08x\n", filesystem::filename(path).c_str(), d.type.c_str(), d.product_code); } CoE::materializeStorage(d.dictionary); std::string mfail; bool mok = resolveAssignment(d.dictionary, 0x1C12, mfail) && resolveAssignment(d.dictionary, 0x1C13, mfail); if (mok) { map_ok++; } - else { map_fail++; printf("MAP-FAIL %s | %s rev=0x%x : %s\n", path.filename().string().c_str(), d.type.c_str(), d.revision_no, mfail.c_str()); } + else { map_fail++; printf("MAP-FAIL %s | %s rev=0x%x : %s\n", filesystem::filename(path).c_str(), d.type.c_str(), d.revision_no, mfail.c_str()); } } catch (std::exception& ex) { sii_fail++; - printf("SII-THROW %s | %s : %s\n", path.filename().string().c_str(), d.type.c_str(), ex.what()); + printf("SII-THROW %s | %s : %s\n", filesystem::filename(path).c_str(), d.type.c_str(), ex.what()); } } } diff --git a/unit/CMakeLists.txt b/unit/CMakeLists.txt index 1c3b4392..4b9def9a 100644 --- a/unit/CMakeLists.txt +++ b/unit/CMakeLists.txt @@ -6,6 +6,7 @@ add_executable(kickcat_unit src/adler32_sum-t.cc src/debughelpers-t.cc src/diagnostics-t.cc src/error-t.cc + src/Filesystem-t.cc src/frame-t.cc src/gateway-t.cc src/kickcat-t.cc @@ -26,6 +27,7 @@ add_executable(kickcat_unit src/adler32_sum-t.cc src/ESMStateInit-t.cc src/ESMStatePreOP-t.cc src/ESMStateSafeOP-t.cc + src/utils/math-t.cc src/Units-t.cc src/Timer-t.cc src/CoE/protocol-t.cc @@ -44,6 +46,8 @@ add_executable(kickcat_unit src/adler32_sum-t.cc src/slave/PDO-t.cc src/Mutex-t.cc + src/unbuffered_output.cc + # Whole-TU override of the real Unix/Time.cc linked into libkickcat.a: # same symbol set (now/since_unix_epoch/sleep), resolved from this # exe's own object first. Must stay in symbol-lockstep with it. @@ -58,6 +62,8 @@ set(KICKCAT_ESI_FIXTURES ${CMAKE_CURRENT_SOURCE_DIR}/kickcat_esi_test_complex.xml ${CMAKE_CURRENT_SOURCE_DIR}/kickcat_esi_test_multi_device.xml ${CMAKE_CURRENT_SOURCE_DIR}/kickcat_esi_test_sm_fmmu.xml + ${CMAKE_SOURCE_DIR}/simulation/slave_configs/ecat402-drive.json + ${CMAKE_SOURCE_DIR}/simulation/slave_configs/ecat402-drive.xml ) set(KICKCAT_ESI_FIXTURES_COPIED "") @@ -79,16 +85,26 @@ target_link_libraries(kickcat_unit kickcat GTest::gmock_main) # Simulation-support tests. lib/simulation is processed after unit/, so gate on # the build condition (not TARGET); the link resolves at generation time. if (ENABLE_ESI_PARSER AND BUILD_SIMULATION) - target_sources(kickcat_unit PRIVATE src/simulation-t.cc src/SimulatorControl-t.cc) + target_sources(kickcat_unit PRIVATE src/simulation-t.cc src/SimulatorControl-t.cc src/Ds402Motor-t.cc) target_link_libraries(kickcat_unit kickcat_simulation) endif() +if (WIN32) + # The exe died at load with STATUS_ENTRYPOINT_NOT_FOUND: it links against the import libraries + # of the toolchain that built it, while the loader searches the exe's directory and PATH first + # and can find an older libstdc++-6.dll there. Carrying those two in the binary removes the skew. + target_link_options(kickcat_unit PRIVATE -static-libgcc -static-libstdc++) +endif() + target_include_directories(kickcat_unit PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) set_kickcat_properties(kickcat_unit) set_target_properties(kickcat_unit PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) add_test(NAME kickcat COMMAND kickcat_unit WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) +# The suite runs in about a second: anything past this is a hang, and ctest's default 1500s would +# burn a whole CI job on it. +set_tests_properties(kickcat PROPERTIES TIMEOUT 120) if (ENABLE_CODE_COVERAGE) set(EXCLUDE_FILES "unit/*" ".*gtest.*" "examples/*" ".*gmock.*" ".*/OS/.*" "tools/*" diff --git a/unit/src/CoE/DS402Drive-t.cc b/unit/src/CoE/DS402Drive-t.cc index 6f01e86b..7b8fbe87 100644 --- a/unit/src/CoE/DS402Drive-t.cc +++ b/unit/src/CoE/DS402Drive-t.cc @@ -1,4 +1,6 @@ #include + +#include #include #include "mocks/Link.h" @@ -349,3 +351,102 @@ TEST(DS402DrivePdoLayoutTest, packed_struct_sizes) EXPECT_EQ(sizeof(Drive::Input), 16U); EXPECT_EQ(sizeof(Drive::Output), 14U); } + + +// --- integration limits --- + +TEST_F(DS402DriveTest, set_limits_rejects_inverted_position_range) +{ + Drive::Limits limits; + limits.min_position_rad = 1.0; + limits.max_position_rad = -1.0; + EXPECT_THROW(drive.setLimits(limits), kickcat::Error); +} + +TEST_F(DS402DriveTest, set_limits_rejects_negative_magnitudes) +{ + Drive::Limits limits; + limits.max_velocity_rad_per_s = -1.0; + EXPECT_THROW(drive.setLimits(limits), kickcat::Error); + + Drive::Limits torque; + torque.max_torque_Nm = -1.0; + EXPECT_THROW(drive.setLimits(torque), kickcat::Error); +} + +TEST_F(DS402DriveTest, setpoints_are_held_inside_the_limits) +{ + drive.setUnits({1000.0, 1.0, 10.0}); // 1000 ticks/rad-ish, 10 Nm rated + + Drive::Limits limits; + limits.min_position_rad = -0.5; + limits.max_position_rad = 0.5; + limits.max_velocity_rad_per_s = 2.0; + limits.max_torque_Nm = 1.0; + drive.setLimits(limits); + + double const ticks_per_rad = 1000.0 / kickcat::tau; + + drive.setTargetPosition(100.0); + EXPECT_EQ(static_cast(0.5 * ticks_per_rad), rx.target_position); + drive.setTargetPosition(-100.0); + EXPECT_EQ(static_cast(-0.5 * ticks_per_rad), rx.target_position); + + drive.setTargetVelocity(50.0); + EXPECT_EQ(static_cast(2.0 * ticks_per_rad), rx.target_velocity); + + drive.setTargetTorque(50.0); + EXPECT_EQ(static_cast(1.0 * 1000.0 / 10.0), rx.target_torque); // per-mille of rated +} + +TEST_F(DS402DriveTest, wide_open_limits_still_clamp_at_the_wire_type) +{ + // Nothing physical bounds this one, so it must not wrap into a reversed setpoint. + drive.setUnits({1e9, 1.0, 1.0}); + drive.setTargetPosition(1e6); + + EXPECT_EQ(INT32_MAX, rx.target_position); +} + +TEST_F(DS402DriveTest, raw_setpoints_bypass_the_limits) +{ + Drive::Limits limits; + limits.min_position_rad = 0.0; + limits.max_position_rad = 0.0; + drive.setLimits(limits); + + drive.setTargetPositionRaw(12345); + EXPECT_EQ(12345, rx.target_position); +} + + +TEST_F(DS402DriveTest, set_units_rejects_nan) +{ + double const nan = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(drive.setUnits({nan, 1.0, 1.0}), kickcat::Error); + EXPECT_THROW(drive.setUnits({1.0, nan, 1.0}), kickcat::Error); + EXPECT_THROW(drive.setUnits({1.0, 1.0, nan}), kickcat::Error); +} + +TEST_F(DS402DriveTest, set_limits_rejects_nan) +{ + // NaN compares false against every bound, so it passes the ordered checks and would then + // collapse the clamp in saturate(). + double const nan = std::numeric_limits::quiet_NaN(); + + Drive::Limits position; + position.min_position_rad = nan; + EXPECT_THROW(drive.setLimits(position), kickcat::Error); + + Drive::Limits maximum; + maximum.max_position_rad = nan; + EXPECT_THROW(drive.setLimits(maximum), kickcat::Error); + + Drive::Limits velocity; + velocity.max_velocity_rad_per_s = nan; + EXPECT_THROW(drive.setLimits(velocity), kickcat::Error); + + Drive::Limits torque; + torque.max_torque_Nm = nan; + EXPECT_THROW(drive.setLimits(torque), kickcat::Error); +} diff --git a/unit/src/CoE/protocol-t.cc b/unit/src/CoE/protocol-t.cc index 55d32266..c33c71db 100644 --- a/unit/src/CoE/protocol-t.cc +++ b/unit/src/CoE/protocol-t.cc @@ -144,3 +144,17 @@ TEST(CoE, pdo_mapping_word_pack_unpack) // A padding gap (index 0) round-trips its bit length. EXPECT_EQ(fromMappingWord(toMappingWord({0x0000, 0x00, 0x08})).bitlen, 0x08); } + + +TEST(CoE_Protocol, sdo_information_response_size_is_known_at_compile_time) +{ + using namespace CoE::SDO::information; + constexpr std::size_t headers = sizeof(CoE::Header) + sizeof(CoE::ServiceDataInfo); + + static_assert(responseSize(GET_OD_LIST_REQ) == headers + sizeof(ListType) + 5 * sizeof(uint16_t)); + static_assert(responseSize(GET_OD_REQ) == headers + sizeof(ObjectDescription)); + static_assert(responseSize(GET_ED_REQ) == headers + sizeof(EntryDescription)); + + // Anything else is answered with an abort code. + static_assert(responseSize(0x7F) == headers + sizeof(uint32_t)); +} diff --git a/unit/src/Ds402Motor-t.cc b/unit/src/Ds402Motor-t.cc new file mode 100644 index 00000000..4df7937a --- /dev/null +++ b/unit/src/Ds402Motor-t.cc @@ -0,0 +1,269 @@ +// Integration tests for the simulated CiA-402 motor (kickcat::sim::Ds402Motor): +// a real master Bus and DS402 Drive drive an emulated slave built from a DS402 ESI +// over the in-process loopback, so the motor is observed only through its PDO image. +#include + +#include +#include +#include +#include +#include + +#include "mocks/Time.h" + +#include "kickcat/Bus.h" +#include "kickcat/CoE/CiA/DS402/Drive.h" +#include "kickcat/Link.h" +#include "kickcat/OS/Filesystem.h" +#include "kickcat/LoopbackSocket.h" +#include "kickcat/Slave.h" +#include "kickcat/SocketNull.h" +#include "kickcat/simulation/SimulatedSlave.h" + +using namespace kickcat; +using namespace kickcat::CoE::CiA::DS402; + + +namespace +{ + // Guard: no loop below may run away. Kept tight on purpose - every now() call advances the + // mock clock 1 ms, and once it passes real monotonic uptime the OS waits behind Timer and + // ConditionVariable stop returning immediately (see lib/src/OS/Test/Time.cc). The loops here + // converge in under 60 cycles. + constexpr int MAX_CYCLES = 300; + + // Statuswords the emulated motor exposes for the CiA-402 states the master walks through. + constexpr uint16_t SW_SWITCH_ON_DISABLED = status::masks::SWITCH_ON_DISABLED; + constexpr uint16_t SW_READY = status::masks::READY_TO_SWITCH_ON + | status::masks::VOLTAGE_ENABLED + | status::masks::QUICK_STOP; + constexpr uint16_t SW_OPERATION_ENABLED = SW_READY + | status::masks::SWITCHED_ON + | status::masks::OPERATION_ENABLE; + + + + class Ds402MotorTest : public testing::Test + { + protected: + void TearDown() override + { + resetMockClock(); // do not leak burnt mock time into the suites that follow + if (not temp_config_.empty()) + { + filesystem::removeFile(temp_config_); + } + } + + // A config in a temporary directory, so plant parameters can be tuned per test. + // The ESI is referenced absolutely: buildSlave resolves it against the config's directory. + std::string writeConfig(std::string const& name, std::string const& extra_params) + { + temp_config_ = name; // beside the binary, next to the fixture its esi refers to + filesystem::writeFile(temp_config_, + "{\"esi\": \"ecat402-drive.xml\", \"ds402_motor\": true" + extra_params + "}"); + return temp_config_; + } + + void bringToOperational(std::string const& config, control::ControlMode mode) + { + resetMockClock(); + iomap_.assign(4096, 0); + + sim_ = std::make_unique(sim::buildSlave(config)); + ASSERT_NE(sim_->device, nullptr); + sim_->slave->start(); + + auto tick = [this]() + { + sim_->slave->routine(); + if (op_phase_ and sim_->slave->state() == State::SAFE_OP) + { + sim_->slave->validateOutputData(); + } + sim_->device->step(); + }; + + loopback_ = std::make_shared(std::vector{sim_->esc.get()}, tick); + link_ = std::make_shared(loopback_, std::make_shared(), [](){}); + bus_ = std::make_unique(link_); + + bus_->init(100ms); + ASSERT_EQ(bus_->slaves().size(), 1u); + + drive_ = std::make_unique(*bus_, bus_->slaves().at(0)); + drive_->configure(mode, 0x1600, 0x1A00, Drive::PaddingStyle::Auto); + bus_->createMapping(iomap_.data(), iomap_.size()); + drive_->attach(); + + bus_->requestState(State::SAFE_OP); + bus_->waitForState(State::SAFE_OP, 500ms); + cycle(); + + op_phase_ = true; + bus_->requestState(State::OPERATIONAL); + bus_->waitForState(State::OPERATIONAL, 500ms, [this]() { cycle(); }); + ASSERT_EQ(sim_->slave->state(), State::OPERATIONAL); + } + + void cycle() + { + auto noop = [](DatagramState const&) {}; + bus_->processDataRead(noop); + drive_->update(); + bus_->processDataWrite(noop); + } + + void enableDrive() + { + drive_->enable(); + int cycles = 0; + while (not drive_->isEnabled() and cycles < MAX_CYCLES) + { + cycle(); + ++cycles; + } + ASSERT_TRUE(drive_->isEnabled()); + } + + void run(int cycles) + { + for (int i = 0; i < cycles; ++i) + { + cycle(); + } + } + + std::vector iomap_; + std::unique_ptr sim_; + std::shared_ptr loopback_; + std::shared_ptr link_; + std::unique_ptr bus_; + std::unique_ptr drive_; + bool op_phase_{false}; + std::string temp_config_; + }; +} + +TEST_F(Ds402MotorTest, master_enable_sequence_walks_the_motor_to_operation_enabled) +{ + ASSERT_NO_FATAL_FAILURE(bringToOperational(std::string{"ecat402-drive.json"}, control::VELOCITY_CYCLIC)); + + EXPECT_EQ(drive_->statusWord(), SW_SWITCH_ON_DISABLED); + EXPECT_EQ(drive_->modeOfOperationDisplay(), control::VELOCITY_CYCLIC); + + std::vector sequence; + drive_->enable(); + int cycles = 0; + while (not drive_->isEnabled() and cycles < MAX_CYCLES) + { + cycle(); + if (sequence.empty() or sequence.back() != drive_->statusWord()) + { + sequence.push_back(drive_->statusWord()); + } + ++cycles; + } + + ASSERT_TRUE(drive_->isEnabled()); + EXPECT_FALSE(drive_->isFaulted()); + // The master's state machine jumps from SHUTDOWN straight to ENABLE_OPERATION, so + // SWITCHED-ON is never commanded and never observed. + EXPECT_EQ(sequence, std::vector({SW_SWITCH_ON_DISABLED, SW_READY, SW_OPERATION_ENABLED})); + EXPECT_EQ(drive_->statusWord(), SW_OPERATION_ENABLED); +} + +TEST_F(Ds402MotorTest, cyclic_velocity_follows_the_commanded_direction) +{ + ASSERT_NO_FATAL_FAILURE(bringToOperational(std::string{"ecat402-drive.json"}, control::VELOCITY_CYCLIC)); + ASSERT_NO_FATAL_FAILURE(enableDrive()); + + constexpr int32_t TARGET = 50000; // ticks/s + int32_t const start = drive_->actualPositionRaw(); + drive_->setTargetVelocityRaw(TARGET); + run(100); // vel_tau is 10 ms at a 1 ms plant cycle: the velocity loop has settled + + EXPECT_GT(drive_->actualVelocityRaw(), TARGET / 2); + EXPECT_LE(drive_->actualVelocityRaw(), TARGET); + EXPECT_GT(drive_->actualPositionRaw(), start); + + int32_t previous = drive_->actualPositionRaw(); + for (int i = 0; i < 50; ++i) + { + cycle(); + EXPECT_GT(drive_->actualPositionRaw(), previous); + previous = drive_->actualPositionRaw(); + } + + drive_->setTargetVelocityRaw(-TARGET); + run(100); + + EXPECT_LT(drive_->actualVelocityRaw(), -TARGET / 2); + EXPECT_GE(drive_->actualVelocityRaw(), -TARGET); + + previous = drive_->actualPositionRaw(); + for (int i = 0; i < 50; ++i) + { + cycle(); + EXPECT_LT(drive_->actualPositionRaw(), previous); + previous = drive_->actualPositionRaw(); + } +} + +TEST_F(Ds402MotorTest, cyclic_position_converges_to_the_commanded_target) +{ + ASSERT_NO_FATAL_FAILURE(bringToOperational(std::string{"ecat402-drive.json"}, control::POSITION_CYCLIC)); + ASSERT_NO_FATAL_FAILURE(enableDrive()); + + constexpr int32_t STEP = 100000; // ticks + int32_t const target = drive_->actualPositionRaw() + STEP; + drive_->setTargetPositionRaw(target); + + run(20); + int64_t const early_error = std::abs(static_cast(target) - drive_->actualPositionRaw()); + EXPECT_LT(early_error, STEP); // the plant lags, but it is already closing the gap + + int cycles = 0; + int64_t error = early_error; + while (error > STEP / 100 and cycles < MAX_CYCLES) + { + cycle(); + error = std::abs(static_cast(target) - drive_->actualPositionRaw()); + ++cycles; + } + + EXPECT_LE(error, STEP / 100); + EXPECT_LT(cycles, MAX_CYCLES); +} + +// Regression: the plant state is a double and outgrows the INT32 feedback objects. It +// must clamp on the way into the PDO, not wrap (nor trap on the float-to-int cast). +TEST_F(Ds402MotorTest, position_feedback_saturates_instead_of_wrapping) +{ + // A 10 ms plant cycle equal to vel_tau makes the velocity loop reach its target in one + // step, so the position crosses the INT32 range in a few hundred cycles. + std::string config = writeConfig("kickcat_ds402_saturation-t.json", ", \"motor_cycle_ms\": 10.0"); + ASSERT_NO_FATAL_FAILURE(bringToOperational(config, control::VELOCITY_CYCLIC)); + ASSERT_NO_FATAL_FAILURE(enableDrive()); + + drive_->setTargetVelocityRaw(INT32_MAX); + + int32_t previous = drive_->actualPositionRaw(); + int cycles = 0; + while (drive_->actualPositionRaw() != INT32_MAX and cycles < MAX_CYCLES) + { + cycle(); + ASSERT_GE(drive_->actualPositionRaw(), previous) << "position wrapped at cycle " << cycles; + previous = drive_->actualPositionRaw(); + ++cycles; + } + + EXPECT_GT(cycles, 0); // the clamp must have been reached by moving, not from the start + EXPECT_EQ(drive_->actualPositionRaw(), INT32_MAX); + EXPECT_EQ(drive_->actualVelocityRaw(), INT32_MAX); + EXPECT_EQ(drive_->actualTorqueRaw(), INT16_MAX); + + // Position holds at the clamp instead of rolling over on the next cycles. + run(20); + EXPECT_EQ(drive_->actualPositionRaw(), INT32_MAX); +} diff --git a/unit/src/Filesystem-t.cc b/unit/src/Filesystem-t.cc new file mode 100644 index 00000000..61331478 --- /dev/null +++ b/unit/src/Filesystem-t.cc @@ -0,0 +1,230 @@ +#include + +#include + +#include "kickcat/Error.h" +#include "kickcat/OS/Filesystem.h" + +using namespace kickcat; +using namespace kickcat::filesystem; + +TEST(FilesystemPath, parent_keeps_the_root_and_yields_nothing_for_a_bare_name) +{ + EXPECT_EQ("", parent("config.json")); + EXPECT_EQ("dir", parent("dir/config.json")); + EXPECT_EQ("a/b", parent("a/b/c")); + EXPECT_EQ("/", parent("/config.json")); + EXPECT_EQ("/a", parent("/a/b")); + EXPECT_EQ("", parent("")); +} + +TEST(FilesystemPath, filename_is_everything_after_the_last_separator) +{ + EXPECT_EQ("config.json", filename("config.json")); + EXPECT_EQ("config.json", filename("dir/config.json")); + EXPECT_EQ("config.json", filename("/a/b/config.json")); + EXPECT_EQ("", filename("dir/")); + EXPECT_EQ("", filename("")); +} + +TEST(FilesystemPath, extension_ignores_a_leading_dot_and_directories) +{ + EXPECT_EQ(".xml", extension("device.xml")); + EXPECT_EQ(".xml", extension("a/b/device.xml")); + EXPECT_EQ(".gz", extension("archive.tar.gz")); + EXPECT_EQ("", extension("Makefile")); + // A dotfile has no extension, and a dot in a parent must not be mistaken for one. + EXPECT_EQ("", extension(".gitignore")); + EXPECT_EQ("", extension("a.b/Makefile")); +} + +TEST(FilesystemPath, join_inserts_one_separator_and_only_where_needed) +{ + EXPECT_EQ("dir/name", join("dir", "name")); + EXPECT_EQ("dir/name", join("dir/", "name")); + EXPECT_EQ("/name", join("/", "name")); + // parent() of a bare filename is empty: joining onto it must not produce a rooted path. + EXPECT_EQ("name", join("", "name")); +} + +class FilesystemTest : public testing::Test +{ +protected: + void SetUp() override + { + // Beside the test binary, like the ESI fixtures. + root_ = "kickcat_filesystem-t.d"; + cleanup(); + ASSERT_TRUE(createDirectory(root_)); + } + + void TearDown() override + { + cleanup(); + } + + void cleanup() + { + if (not isDirectory(root_)) + { + removeFile(root_); + return; + } + for (auto const& file : listFilesRecursive(root_)) + { + removeFile(file); + } + // Deepest first: removeDirectory only takes empty ones. + std::vector directories; + collectDirectories(root_, directories); + std::sort(directories.begin(), directories.end(), + [](std::string const& a, std::string const& b) { return a.size() > b.size(); }); + for (auto const& directory : directories) + { + removeDirectory(directory); + } + removeDirectory(root_); + } + + void collectDirectories(std::string const& directory, std::vector& out) + { + for (auto const& entry : list(directory)) + { + if (entry.is_directory) + { + std::string path = join(directory, entry.name); + out.push_back(path); + collectDirectories(path, out); + } + } + } + + std::string root_; +}; + +TEST_F(FilesystemTest, write_then_read_returns_the_same_bytes) +{ + std::string path = join(root_, "payload.bin"); + std::vector written{0x00, 0x01, 0xFF, 0x7F, 0x00, 0x42}; + writeFile(path, written.data(), written.size()); + + EXPECT_TRUE(exists(path)); + EXPECT_FALSE(isDirectory(path)); + EXPECT_EQ(written, readFile(path)); +} + +TEST_F(FilesystemTest, write_truncates_an_existing_file) +{ + std::string path = join(root_, "payload.bin"); + writeFile(path, std::string{"a long first content"}); + writeFile(path, std::string{"short"}); + + std::vector content = readFile(path); + EXPECT_EQ(std::string(content.begin(), content.end()), "short"); +} + +TEST_F(FilesystemTest, an_empty_file_reads_back_empty) +{ + std::string path = join(root_, "empty.bin"); + writeFile(path, nullptr, 0); + + EXPECT_TRUE(exists(path)); + EXPECT_TRUE(readFile(path).empty()); +} + +TEST_F(FilesystemTest, a_payload_bigger_than_one_read_chunk_survives) +{ + // The backends read and write in 64k chunks: cross that boundary with a non-repeating pattern. + std::vector written(200000); + for (std::size_t i = 0; i < written.size(); ++i) + { + written[i] = static_cast(i * 7); + } + + std::string path = join(root_, "big.bin"); + writeFile(path, written.data(), written.size()); + EXPECT_EQ(written, readFile(path)); +} + +TEST_F(FilesystemTest, reading_an_absent_file_throws) +{ + EXPECT_THROW(readFile(join(root_, "absent.bin")), std::system_error); +} + +TEST_F(FilesystemTest, absent_paths_are_reported_absent) +{ + std::string path = join(root_, "absent.bin"); + EXPECT_FALSE(exists(path)); + EXPECT_FALSE(isDirectory(path)); + EXPECT_FALSE(isDirectory(join(root_, "absent.d"))); +} + +TEST_F(FilesystemTest, removing_a_file_reports_whether_it_was_there) +{ + std::string path = join(root_, "payload.bin"); + writeFile(path, std::string{"content"}); + + EXPECT_TRUE(removeFile(path)); + EXPECT_FALSE(exists(path)); + EXPECT_FALSE(removeFile(path)); +} + +TEST_F(FilesystemTest, creating_and_removing_a_directory_report_whether_it_was_there) +{ + std::string path = join(root_, "nested"); + + EXPECT_TRUE(createDirectory(path)); + EXPECT_TRUE(isDirectory(path)); + EXPECT_FALSE(createDirectory(path)); + + EXPECT_TRUE(removeDirectory(path)); + EXPECT_FALSE(exists(path)); + EXPECT_FALSE(removeDirectory(path)); +} + +TEST_F(FilesystemTest, list_reports_one_level_with_its_kinds_and_no_dot_entries) +{ + writeFile(join(root_, "a.xml"), std::string{"a"}); + writeFile(join(root_, "b.bin"), std::string{"b"}); + ASSERT_TRUE(createDirectory(join(root_, "sub"))); + writeFile(join(root_, "sub/deep.xml"), std::string{"deep"}); + + std::vector entries = list(root_); + ASSERT_EQ(3u, entries.size()) << "'.' and '..' must not be listed, and 'sub' is not descended"; + + std::sort(entries.begin(), entries.end(), + [](Entry const& a, Entry const& b) { return a.name < b.name; }); + EXPECT_EQ("a.xml", entries[0].name); + EXPECT_FALSE(entries[0].is_directory); + EXPECT_EQ("b.bin", entries[1].name); + EXPECT_FALSE(entries[1].is_directory); + EXPECT_EQ("sub", entries[2].name); + EXPECT_TRUE(entries[2].is_directory); +} + +TEST_F(FilesystemTest, list_of_an_absent_directory_throws) +{ + EXPECT_THROW(list(join(root_, "absent.d")), std::system_error); +} + +TEST_F(FilesystemTest, list_of_an_empty_directory_is_empty) +{ + EXPECT_TRUE(list(root_).empty()); +} + +TEST_F(FilesystemTest, recursive_list_returns_files_only_as_full_paths) +{ + writeFile(join(root_, "top.xml"), std::string{"top"}); + ASSERT_TRUE(createDirectory(join(root_, "sub"))); + ASSERT_TRUE(createDirectory(join(root_, "sub/deeper"))); + writeFile(join(root_, "sub/middle.xml"), std::string{"middle"}); + writeFile(join(root_, "sub/deeper/bottom.xml"), std::string{"bottom"}); + + std::vector files = listFilesRecursive(root_); + std::sort(files.begin(), files.end()); + + ASSERT_EQ(3u, files.size()); + EXPECT_EQ(join(root_, "sub/deeper/bottom.xml"), files[0]); + EXPECT_EQ(join(root_, "sub/middle.xml"), files[1]); + EXPECT_EQ(join(root_, "top.xml"), files[2]); +} diff --git a/unit/src/Timer-t.cc b/unit/src/Timer-t.cc index 5c4ea031..86bcfdb2 100644 --- a/unit/src/Timer-t.cc +++ b/unit/src/Timer-t.cc @@ -5,6 +5,7 @@ #include +#include "kickcat/Error.h" #include "kickcat/OS/Timer.h" using namespace kickcat; @@ -44,3 +45,16 @@ TEST(TimerPllTest, update_period_rebuilds_the_pll_on_the_new_grid) EXPECT_EQ(0u, timer.pll().samples()); EXPECT_FALSE(timer.locked()); } + + +TEST(TimerPllTest, non_positive_period_is_rejected) +{ + // start() divides the elapsed time by the period to align the next deadline: a zero period + // must be refused where the mistake is made rather than divide by zero later. + EXPECT_THROW((Timer{0ns}), Error); + EXPECT_THROW((Timer{-1ms}), Error); + + Timer timer{1ms}; + EXPECT_THROW(timer.update_period(0ns), Error); + EXPECT_EQ(1ms, timer.period()); +} diff --git a/unit/src/gateway-t.cc b/unit/src/gateway-t.cc index 36173f30..bfeea726 100644 --- a/unit/src/gateway-t.cc +++ b/unit/src/gateway-t.cc @@ -62,6 +62,7 @@ TEST_F(GatewayTest, incoherent_request) // Message too big for the targeted mailbox Mailbox mbx; mbx.recv_size = 4; + mbx.send_size = 4; EXPECT_CALL(*socket_, recv(_, _)) .WillOnce([&](void* frame, int32_t) @@ -90,6 +91,7 @@ TEST_F(GatewayTest, nominal_loop) uint16_t GEN_GATEWAY_INDEX = 2; Mailbox mbx; mbx.recv_size = 128; + mbx.send_size = 128; EXPECT_CALL(*socket_, sendTo(_, 18, GEN_GATEWAY_INDEX)) .WillOnce([](void const* frame, int32_t size, uint16_t) @@ -150,6 +152,7 @@ TEST_F(GatewayTest, evict_failed_requests) uint16_t GEN_GATEWAY_INDEX = 2; Mailbox mbx; mbx.recv_size = 128; + mbx.send_size = 128; EXPECT_CALL(*socket_, recv(_, _)) .WillOnce([&](void* frame, int32_t) diff --git a/unit/src/mailbox/CoE/request-t.cc b/unit/src/mailbox/CoE/request-t.cc index 832a0c1f..b052a3d6 100644 --- a/unit/src/mailbox/CoE/request-t.cc +++ b/unit/src/mailbox/CoE/request-t.cc @@ -708,3 +708,112 @@ TEST_F(CoE_Request, sdo_information_wrong_opcode) ASSERT_TRUE(mailbox.receive(raw_message)); ASSERT_EQ(MessageStatus::COE_WRONG_SERVICE, message->status()); } + + +// The slave receive and send mailboxes are declared separately in the SII and may differ. A reply +// is read from the send mailbox, so that is what bounds its payload reads. + +TEST_F(CoE_Request, asymmetric_mailbox_reply_longer_than_send_mailbox_is_rejected) +{ + // The announced length fits the request buffer but not the reply. The reply buffer is exactly + // send_size, so any read past it is a heap overflow (ASan gates this in CI). + mailbox.recv_size = 256; + mailbox.send_size = 32; + + uint8_t client[200] = {0}; + uint32_t client_size = sizeof(client); + mailbox.createSDO(0x1018, 1, false, CoE::SDO::request::UPLOAD, client, &client_size); + auto message = mailbox.send(); + + std::vector reply(mailbox.send_size, 0); + auto* reply_header = pointData(reply.data()); + auto* reply_coe = pointData(reply_header); + auto* reply_sdo = pointData(reply_coe); + + reply_header->type = mailbox::Type::CoE; + reply_header->len = 210; // fits recv_size (256) but not send_size (32) + reply_coe->service = CoE::Service::SDO_RESPONSE; + reply_sdo->command = CoE::SDO::response::UPLOAD; + reply_sdo->index = 0x1018; + reply_sdo->subindex = 1; + reply_sdo->transfer_type = 0; // standard transfer: reads a size then payload + uint32_t complete_size = sizeof(client); + std::memcpy(pointData(reply_sdo), &complete_size, sizeof(complete_size)); + + ASSERT_TRUE(mailbox.receive(reply.data())); + ASSERT_EQ(MessageStatus::COE_WRONG_SERVICE, message->status()); +} + + +TEST_F(CoE_Request, asymmetric_mailbox_reply_larger_than_recv_mailbox_is_accepted) +{ + // A reply larger than the request buffer is legal and must be processed, not refused. + mailbox.recv_size = 32; + mailbox.send_size = 256; + + uint8_t expected[200]; + std::mt19937 rng{0xA5A5F00Du}; + for (auto& byte : expected) { byte = static_cast(rng()); } + + uint8_t client[sizeof(expected)] = {0}; + uint32_t client_size = sizeof(client); + mailbox.createSDO(0x1018, 1, false, CoE::SDO::request::UPLOAD, client, &client_size); + auto message = mailbox.send(); + + std::vector reply(mailbox.send_size, 0); + auto* reply_header = pointData(reply.data()); + auto* reply_coe = pointData(reply_header); + auto* reply_sdo = pointData(reply_coe); + + reply_header->type = mailbox::Type::CoE; + reply_header->len = static_cast(10 + sizeof(expected)); + reply_coe->service = CoE::Service::SDO_RESPONSE; + reply_sdo->command = CoE::SDO::response::UPLOAD; + reply_sdo->index = 0x1018; + reply_sdo->subindex = 1; + reply_sdo->transfer_type = 0; + uint32_t complete_size = sizeof(expected); + auto* reply_payload = pointData(reply_sdo); + std::memcpy(reply_payload, &complete_size, sizeof(complete_size)); + std::memcpy(reply_payload + sizeof(complete_size), expected, sizeof(expected)); + + ASSERT_TRUE(mailbox.receive(reply.data())); + ASSERT_EQ(MessageStatus::SUCCESS, message->status()); + ASSERT_EQ(sizeof(expected), client_size); + ASSERT_EQ(0, std::memcmp(client, expected, sizeof(expected))); +} + + +// Each message checks the footprint it writes against the buffer it was given, so the limit is the +// request's own size rather than one figure for all of CoE. Inflating the buffer instead would make +// the master write past the slave's mailbox SyncManager. +TEST_F(CoE_Request, mailbox_too_small_for_the_request_it_must_hold) +{ + uint32_t data{0}; + uint32_t data_size = sizeof(data); + + // An SDO request writes the headers, the service data and four expedited/size bytes. + for (uint16_t size : {1, 6, 8, 12, 15}) + { + mailbox.recv_size = size; + mailbox.send_size = size; + ASSERT_THROW(mailbox.createSDO(0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size), Error) + << "recv_size " << size; + } + + mailbox.recv_size = 16; + mailbox.send_size = 16; + ASSERT_NO_THROW(mailbox.createSDO(0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size)); + + // An SDO information request carries only its own payload, so it fits in less. + for (uint16_t size : {1, 6, 12, 13}) + { + mailbox.recv_size = size; + mailbox.send_size = size; + ASSERT_THROW(mailbox.createSDOInfoGetOD(0x1018, &data, &data_size), Error) << "recv_size " << size; + } + + mailbox.recv_size = 14; + mailbox.send_size = 14; + ASSERT_NO_THROW(mailbox.createSDOInfoGetOD(0x1018, &data, &data_size)); +} diff --git a/unit/src/mailbox/CoE/response-t.cc b/unit/src/mailbox/CoE/response-t.cc index 456f0721..4e81a15c 100644 --- a/unit/src/mailbox/CoE/response-t.cc +++ b/unit/src/mailbox/CoE/response-t.cc @@ -23,7 +23,7 @@ std::vector createTestReadSDO(uint16_t index, uint8_t subindex, bool CA { uint32_t data; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, index, subindex, CA, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, TEST_MAILBOX_SIZE, index, subindex, CA, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; std::vector raw_message; raw_message.insert(raw_message.begin(), msg.data(), msg.data() + TEST_MAILBOX_SIZE); @@ -33,7 +33,7 @@ std::vector createTestReadSDO(uint16_t index, uint8_t subindex, bool CA std::vector createTestWriteSDO(uint16_t index, uint8_t subindex, uint32_t data, bool CA=false) { uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, index, subindex, CA, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, TEST_MAILBOX_SIZE, index, subindex, CA, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; std::vector raw_message; raw_message.insert(raw_message.begin(), msg.data(), msg.data() + TEST_MAILBOX_SIZE); @@ -315,7 +315,7 @@ TEST_F(CoE_Response, SDO_write_complete_OK) uint32_t data_size = sizeof(data); std::vector raw_message; { - mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, 0x7000, 1, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, TEST_MAILBOX_SIZE, 0x7000, 1, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; raw_message.insert(raw_message.begin(), msg.data(), msg.data() + TEST_MAILBOX_SIZE); } @@ -604,7 +604,7 @@ TEST_F(CoE_Response, SDO_write_CA_unauthorized_entry) uint32_t data_size = sizeof(data); std::vector raw_message; { - mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, 0xB000, 0, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, TEST_MAILBOX_SIZE, 0xB000, 0, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; raw_message.insert(raw_message.begin(), msg.data(), msg.data() + TEST_MAILBOX_SIZE); } auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); @@ -631,7 +631,7 @@ TEST_F(CoE_Response, SDO_write_CA_subindex_0) uint32_t data_size = sizeof(data); std::vector raw_message; { - mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, 0x7000, 0, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{TEST_MAILBOX_SIZE, TEST_MAILBOX_SIZE, 0x7000, 0, true, CoE::SDO::request::DOWNLOAD, &data, &data_size, 1ms}; raw_message.insert(raw_message.begin(), msg.data(), msg.data() + TEST_MAILBOX_SIZE); } auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); @@ -870,18 +870,52 @@ TEST_F(CoE_Response, createSDOMessage_invalid_service) } } -TEST_F(CoE_Response, SDO_invalid_command) +// ETG.1000.6 Table 109 transition 6: a command specifier above 3 is not a client service (4 is the +// server's own abort, 5..7 are reserved), so the slave answers with a mailbox error. The check comes +// before the object lookup, so the answer does not depend on the dictionary. +TEST_F(CoE_Response, SDO_command_specifier_above_three_is_an_invalid_header) { - std::vector raw_message = createTestReadSDO(0x1018, 2); + for (uint8_t command : {0x04, 0x05, 0x06, 0x07}) { - auto header = pointData(raw_message.data()); - auto coe = pointData(header); - auto sdo = pointData(coe); - sdo->command = 0x07; // Invalid command (max value for 3-bit field, not in enum) + std::vector raw_message = createTestReadSDO(0x1018, 2); + { + auto sdo = pointData( + pointData(pointData(raw_message.data()))); + sdo->command = command; + } + auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()) << "command " << int(command); + + auto const& msg = mbx.readyToSend(); + auto resp_header = pointData(msg.data()); + ASSERT_EQ(mailbox::ERR, resp_header->type) << "command " << int(command); + ASSERT_EQ(mailbox::Error::INVALID_HEADER, + pointData(resp_header)->detail) << "command " << int(command); } - auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); +} + +// ETG.1000.6 Table 109 transitions 12 and 18: a segment request with no transfer in progress is out +// of sequence and is answered with an SDO abort. +TEST_F(CoE_Response, SDO_segment_with_no_transfer_in_progress_aborts) +{ + for (uint8_t command : {CoE::SDO::request::DOWNLOAD_SEGMENTED, CoE::SDO::request::UPLOAD_SEGMENTED}) + { + std::vector raw_message = createTestReadSDO(0x1018, 2); + { + auto sdo = pointData( + pointData(pointData(raw_message.data()))); + sdo->command = command; + } + auto response_msg = createSDOMessage(&mbx, std::move(raw_message)); + ASSERT_EQ(mailbox::ProcessingResult::FINALIZE, response_msg->process()) << "command " << int(command); - ASSERT_EQ(mailbox::ProcessingResult::NOOP, response_msg->process()); + auto const& msg = mbx.readyToSend(); + auto coe = pointData(pointData(msg.data())); + auto sdo = pointData(coe); + auto payload = pointData(sdo); + ASSERT_EQ(CoE::SDO::request::ABORT, sdo->command) << "command " << int(command); + ASSERT_EQ(CoE::SDO::abort::COMMAND_SPECIFIER_INVALID, *payload) << "command " << int(command); + } } TEST_F(CoE_Response, SDO_process_with_raw_message) @@ -1094,6 +1128,52 @@ TEST(CoE_Roundtrip, sdo_segmented_upload_master_slave) ASSERT_EQ(0, std::memcmp(received, blob, sizeof(blob))); } +// A truncated segment request arriving mid-transfer must be rejected without touching memory past +// the received buffer (ASan gates this in CI). +TEST(CoE_Roundtrip, sdo_segmented_upload_survives_truncated_segment_request) +{ + constexpr uint16_t MBX = 32; + + CoE::Dictionary dict; + { + CoE::Object object{0x2000, CoE::ObjectCode::VAR, "Big blob", {}}; + object.entries.emplace_back(0, 50 * 8, 0, CoE::Access::READ, CoE::DataType::OCTET_STRING, "blob"); + object.entries.back().data = std::calloc(50, 1); + dict.push_back(std::move(object)); + } + + Mailbox slave{MBX, 1}; + slave.enableCoE(dict); + + mailbox::request::Mailbox master; + master.recv_size = MBX; + master.send_size = MBX; + + uint8_t received[50] = {0}; + uint32_t received_size = sizeof(received); + master.createSDO(0x2000, 0, false, CoE::SDO::request::UPLOAD, received, &received_size); + + // Initiate the upload so the slave keeps a segmented transfer alive... + auto msg = master.send(); + std::vector initiate(msg->data(), msg->data() + msg->size()); + std::vector reply = slave.processRequest(std::move(initiate)); + ASSERT_FALSE(reply.empty()); + + // ...then hand it every truncation of the follow-up segment request. Replies are queued, so a + // given call may pop an earlier one; every answer produced must still be a well-formed message. + master.receive(reply.data()); + auto segment = master.send(); + for (std::size_t size = 0; size <= segment->size(); ++size) + { + std::vector truncated(segment->data(), segment->data() + size); + std::vector answer = slave.processRequest(std::move(truncated)); + if (not answer.empty()) + { + EXPECT_GE(answer.size(), sizeof(mailbox::Header)) << "size " << size; + } + } +} + // Mirror of the upload coherency check: the master's segmented-download sender and the slave's // segmented-download receiver must agree end to end. TEST(CoE_Roundtrip, sdo_segmented_download_master_slave) diff --git a/unit/src/mailbox/request-t.cc b/unit/src/mailbox/request-t.cc index cc9c6a9e..0414b7ad 100644 --- a/unit/src/mailbox/request-t.cc +++ b/unit/src/mailbox/request-t.cc @@ -1,5 +1,8 @@ #include +#include +#include + #include "kickcat/Mailbox.h" using namespace kickcat; @@ -52,3 +55,63 @@ TEST_F(Mailbox_Request, received_unknown_message) { ASSERT_FALSE(mailbox.receive(raw_message)); } + + +// The reply is read from the send mailbox, which an asymmetric SII can declare larger than the +// receive mailbox the request buffer was sized from. +TEST_F(Mailbox_Request, gateway_reply_larger_than_the_request_buffer) +{ + mailbox.recv_size = 32; + mailbox.send_size = 256; + + std::vector request(mailbox.recv_size, 0); + auto* request_header = pointData(request.data()); + request_header->len = 10; + request_header->address = 0x1001; + request_header->type = mailbox::Type::CoE; + + auto msg = mailbox.createGatewayMessage(request.data(), static_cast(request.size()), 1); + ASSERT_NE(nullptr, msg); + ASSERT_EQ(msg, mailbox.send()); + + std::vector reply(mailbox.send_size, 0); + auto* reply_header = pointData(reply.data()); + reply_header->len = 210; // 216 bytes: fits the send mailbox, not the request buffer + reply_header->address = mailbox::GATEWAY_MESSAGE_MASK | 1; + reply_header->type = mailbox::Type::CoE; + + ASSERT_TRUE(mailbox.receive(reply.data())); + EXPECT_EQ(MessageStatus::SUCCESS, msg->status()); + EXPECT_EQ(216u, msg->size()); + + // Storing the reply grows the buffer, so the cached header pointer must follow it: the address + // restored below lands in the reply the client receives. + EXPECT_EQ(0x1001, pointData(msg->data())->address); +} + + +TEST_F(Mailbox_Request, gateway_reply_longer_than_the_send_mailbox_is_dropped) +{ + mailbox.recv_size = 256; + mailbox.send_size = 32; + + std::vector request(64, 0); + auto* request_header = pointData(request.data()); + request_header->len = 10; + request_header->address = 0x1001; + request_header->type = mailbox::Type::CoE; + + auto msg = mailbox.createGatewayMessage(request.data(), static_cast(request.size()), 2); + ASSERT_NE(nullptr, msg); + ASSERT_EQ(msg, mailbox.send()); + + // Only send_size bytes were fetched from the slave, so a longer claim cannot be read. + std::vector reply(mailbox.send_size, 0); + auto* reply_header = pointData(reply.data()); + reply_header->len = 100; + reply_header->address = mailbox::GATEWAY_MESSAGE_MASK | 2; + reply_header->type = mailbox::Type::CoE; + + EXPECT_FALSE(mailbox.receive(reply.data())); + EXPECT_EQ(MessageStatus::RUNNING, msg->status()); +} diff --git a/unit/src/mailbox/response-t.cc b/unit/src/mailbox/response-t.cc index 88db6bdb..0483cc55 100644 --- a/unit/src/mailbox/response-t.cc +++ b/unit/src/mailbox/response-t.cc @@ -55,7 +55,7 @@ class Mailbox_Response : public ::testing::Test { uint32_t data; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage msg{RESP_MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{RESP_MBX_SIZE, RESP_MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; std::vector raw(RESP_MBX_SIZE, 0); std::memcpy(raw.data(), msg.data(), RESP_MBX_SIZE); @@ -399,7 +399,7 @@ class Mailbox_Response_Standalone : public ::testing::Test { uint32_t data; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage msg{RESP_MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{RESP_MBX_SIZE, RESP_MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; std::vector raw(RESP_MBX_SIZE, 0); std::memcpy(raw.data(), msg.data(), RESP_MBX_SIZE); diff --git a/unit/src/masterOD-gateway-t.cc b/unit/src/masterOD-gateway-t.cc index e69aa1c8..b035dcb7 100644 --- a/unit/src/masterOD-gateway-t.cc +++ b/unit/src/masterOD-gateway-t.cc @@ -50,7 +50,7 @@ TEST_F(MasterGatewayTest, address_zero_reads_master_identity) // SDO upload 0x1018:01 (Vendor ID) with the default mailbox header address == 0 targets the master OD. uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; ASSERT_EQ(0u, sdo_msg.address()); auto gw_msg = bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 42); @@ -82,13 +82,106 @@ TEST_F(MasterGatewayTest, malformed_tiny_request_returns_nullptr) } +// A client picks `size` and `len` independently, so the pair is never trusted to be coherent. +static std::vector createRawRequest(std::size_t size, uint16_t len, uint16_t service) +{ + std::vector raw(size, 0); + auto* header = reinterpret_cast(raw.data()); + header->len = len; + header->address = 0; // master OD + header->type = mailbox::Type::CoE; + + if (size >= (sizeof(mailbox::Header) + sizeof(CoE::Header))) + { + pointData(header)->service = service; + } + return raw; +} + + +static uint16_t mailboxErrorDetail(uint8_t const* reply) +{ + auto const* header = reinterpret_cast(reply); + EXPECT_EQ(mailbox::ERR, header->type); + return pointData(header)->detail; +} + + +TEST_F(MasterGatewayTest, header_only_request_reports_size_too_short) +{ + // A bare mailbox header is the smallest request the gateway accepts. + auto raw = createRawRequest(sizeof(mailbox::Header), 0, CoE::Service::SDO_REQUEST); + + auto gw_msg = bus.addGatewayMessage(raw.data(), static_cast(raw.size()), 3); + ASSERT_NE(nullptr, gw_msg); + EXPECT_EQ(mailbox::request::MessageStatus::SUCCESS, gw_msg->status()); + EXPECT_EQ(mailbox::Error::SIZE_TOO_SHORT, mailboxErrorDetail(gw_msg->data())); +} + + +TEST_F(MasterGatewayTest, announced_len_beyond_received_bytes_reports_invalid_size) +{ + // header->len locates every payload field, so it must not exceed the bytes received. + auto raw = createRawRequest(sizeof(mailbox::Header) + sizeof(CoE::Header), 100, CoE::Service::SDO_REQUEST); + + auto gw_msg = bus.addGatewayMessage(raw.data(), static_cast(raw.size()), 4); + ASSERT_NE(nullptr, gw_msg); + EXPECT_EQ(mailbox::request::MessageStatus::SUCCESS, gw_msg->status()); + EXPECT_EQ(mailbox::Error::INVALID_SIZE, mailboxErrorDetail(gw_msg->data())); +} + + +TEST_F(MasterGatewayTest, truncated_sdo_request_reports_size_too_short) +{ + // Coherent header/len pair, still too short for the announced service. + auto raw = createRawRequest(sizeof(mailbox::Header) + sizeof(CoE::Header), + sizeof(CoE::Header), CoE::Service::SDO_REQUEST); + + auto gw_msg = bus.addGatewayMessage(raw.data(), static_cast(raw.size()), 5); + ASSERT_NE(nullptr, gw_msg); + EXPECT_EQ(mailbox::Error::SIZE_TOO_SHORT, mailboxErrorDetail(gw_msg->data())); +} + + +TEST_F(MasterGatewayTest, truncated_sdo_information_request_reports_size_too_short) +{ + // Same for SDO information, whose opcode sits further into the payload. + auto raw = createRawRequest(sizeof(mailbox::Header) + sizeof(CoE::Header), + sizeof(CoE::Header), CoE::Service::SDO_INFORMATION); + + auto gw_msg = bus.addGatewayMessage(raw.data(), static_cast(raw.size()), 6); + ASSERT_NE(nullptr, gw_msg); + EXPECT_EQ(mailbox::Error::SIZE_TOO_SHORT, mailboxErrorDetail(gw_msg->data())); +} + + +TEST_F(MasterGatewayTest, every_truncation_of_a_valid_request_is_handled) +{ + // Truncating a well-formed request at any offset yields a clean rejection or an error reply, + // never an out-of-bounds access (ASan gates this in CI). + uint32_t data{0}; + uint32_t data_size = sizeof(data); + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + + for (std::size_t size = 0; size <= sdo_msg.size(); ++size) + { + std::vector raw(sdo_msg.data(), sdo_msg.data() + size); + auto gw_msg = bus.addGatewayMessage(raw.data(), static_cast(size), 8); + if (gw_msg != nullptr) + { + EXPECT_EQ(mailbox::request::MessageStatus::SUCCESS, gw_msg->status()) << "size " << size; + } + } +} + + TEST_F(MasterGatewayTest, unknown_slave_address_still_returns_nullptr_when_master_mailbox_set) { // Regression: installing a master mailbox must not reroute slave-addressed requests to it. // Unknown slave addresses continue to fall through to the "no slave on the bus" error path. uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; sdo_msg.setAddress(0x1234); EXPECT_EQ(nullptr, bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 1)); @@ -101,7 +194,7 @@ TEST_F(MasterGatewayTest, address_zero_unknown_object_returns_sdo_abort) // the error reply and the gateway path must deliver it to the client. uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x9999, 0, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x9999, 0, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; auto gw_msg = bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 7); ASSERT_NE(nullptr, gw_msg); @@ -125,7 +218,7 @@ TEST(MasterGatewayNoMailbox, address_zero_without_master_mailbox_returns_nullptr uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; EXPECT_EQ(nullptr, bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 1)); } @@ -139,7 +232,7 @@ TEST(MasterGatewayNoMailbox, unknown_slave_address_still_returns_nullptr) uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; sdo_msg.setAddress(0x1234); EXPECT_EQ(nullptr, bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 1)); @@ -159,7 +252,7 @@ TEST_F(MasterGatewayTest, full_udp_loop_through_gateway) uint32_t data{0}; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage sdo_msg{MBX_SIZE, 0x1018, 2, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 2, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; std::vector udp_frame(sizeof(EthercatHeader) + sdo_msg.size()); auto* eth_header = reinterpret_cast(udp_frame.data()); @@ -197,3 +290,92 @@ TEST_F(MasterGatewayTest, full_udp_loop_through_gateway) gateway.fetchRequest(); gateway.processPendingRequests(); } + + +// A gateway client sizes its request to its own length, but the SDO information handlers write +// their response into that same buffer: the answer has to fit too. +TEST_F(MasterGatewayTest, sdo_information_request_too_small_for_its_response) +{ + struct Case + { + char const* name; + uint8_t opcode; + std::size_t size; + }; + + // Fixed response sizes are 24 (list), 18 (object description) and 22 (entry description). + Case const cases[] = { + {"GET_OD_LIST", CoE::SDO::information::GET_OD_LIST_REQ, 14}, + {"GET_OD", CoE::SDO::information::GET_OD_REQ, 14}, + {"GET_ED", CoE::SDO::information::GET_ED_REQ, 16}, + {"GET_OD_LIST", CoE::SDO::information::GET_OD_LIST_REQ, 23}, + }; + + for (auto const& test : cases) + { + uint8_t raw[32] = {0}; + auto* header = reinterpret_cast(raw); + header->len = static_cast(test.size - sizeof(mailbox::Header)); + header->address = 0; + header->type = mailbox::Type::CoE; + auto* coe = pointData(header); + coe->service = CoE::Service::SDO_INFORMATION; + pointData(coe)->opcode = test.opcode; + + auto gw_msg = bus.addGatewayMessage(raw, static_cast(test.size), 9); + ASSERT_NE(nullptr, gw_msg) << test.name << " " << test.size; + EXPECT_EQ(mailbox::Error::SIZE_TOO_SHORT, mailboxErrorDetail(gw_msg->data())) + << test.name << " " << test.size; + } +} + + +TEST_F(MasterGatewayTest, sdo_information_list_request_that_fits_is_served) +{ + // 24 bytes is the smallest buffer the list response fits in: it must not be refused. + uint8_t raw[24] = {0}; + auto* header = reinterpret_cast(raw); + header->len = static_cast(sizeof(raw) - sizeof(mailbox::Header)); + header->address = 0; + header->type = mailbox::Type::CoE; + auto* coe = pointData(header); + coe->service = CoE::Service::SDO_INFORMATION; + auto* sdo = pointData(coe); + sdo->opcode = CoE::SDO::information::GET_OD_LIST_REQ; + auto const list_type = CoE::SDO::information::ListType::NUMBER; + std::memcpy(pointData(sdo), &list_type, sizeof(list_type)); + + auto gw_msg = bus.addGatewayMessage(raw, static_cast(sizeof(raw)), 10); + ASSERT_NE(nullptr, gw_msg); + + auto const* reply = reinterpret_cast(gw_msg->data()); + EXPECT_EQ(mailbox::Type::CoE, reply->type); + EXPECT_EQ(CoE::SDO::information::GET_OD_LIST_RESP, + pointData(pointData(reply))->opcode); +} + + +TEST_F(MasterGatewayTest, an_abort_does_not_wedge_the_master_object_dictionary) +{ + // A message the dispatcher does not serve used to stay queued forever, and max_msgs is 1: every + // later request was then answered NO_MORE_MEMORY for the process lifetime. + uint32_t data{0}; + uint32_t data_size = sizeof(data); + mailbox::request::SDOMessage sdo_msg{MBX_SIZE, MBX_SIZE, 0x1018, 1, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + + std::vector abort_request(sdo_msg.data(), sdo_msg.data() + sdo_msg.size()); + pointData(pointData( + pointData(abort_request.data())))->command = CoE::SDO::request::ABORT; + + auto aborted = bus.addGatewayMessage(abort_request.data(), static_cast(abort_request.size()), 11); + ASSERT_NE(nullptr, aborted); + EXPECT_EQ(mailbox::Error::INVALID_HEADER, mailboxErrorDetail(aborted->data())); + + // The dictionary must still answer afterwards. + auto served = bus.addGatewayMessage(sdo_msg.data(), static_cast(sdo_msg.size()), 12); + ASSERT_NE(nullptr, served); + auto const* reply = reinterpret_cast(served->data()); + auto const* sdo = pointData(pointData(reply)); + EXPECT_EQ(CoE::SDO::response::UPLOAD, sdo->command); + EXPECT_EQ(testIdentity().vendor_id, *pointData(sdo)); +} diff --git a/unit/src/masterOD-t.cc b/unit/src/masterOD-t.cc index b8030f3a..9c1132e2 100644 --- a/unit/src/masterOD-t.cc +++ b/unit/src/masterOD-t.cc @@ -29,7 +29,7 @@ static std::vector buildSDOUpload(uint16_t index, uint8_t subindex) { uint32_t data; uint32_t data_size = sizeof(data); - mailbox::request::SDOMessage msg{MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; + mailbox::request::SDOMessage msg{MBX_SIZE, MBX_SIZE, index, subindex, false, CoE::SDO::request::UPLOAD, &data, &data_size, 1ms}; std::vector raw(MBX_SIZE, 0); std::memcpy(raw.data(), msg.data(), MBX_SIZE); diff --git a/unit/src/unbuffered_output.cc b/unit/src/unbuffered_output.cc new file mode 100644 index 00000000..b9a1e19e --- /dev/null +++ b/unit/src/unbuffered_output.cc @@ -0,0 +1,10 @@ +#include + +namespace +{ + // ctest reads the test binary's stdout through a pipe, so the C runtime block-buffers it. A run + // that hangs or dies abnormally never flushes, and the whole suite's output is lost: the log + // then shows nothing at all, not even the test that stopped. Unbuffered from before main so the + // last line written is always the last line reached. + [[maybe_unused]] int const unbuffered_stdout = std::setvbuf(stdout, nullptr, _IONBF, 0); +} diff --git a/unit/src/utils/math-t.cc b/unit/src/utils/math-t.cc new file mode 100644 index 00000000..6d20a220 --- /dev/null +++ b/unit/src/utils/math-t.cc @@ -0,0 +1,125 @@ +#include + +#include +#include + +#include "kickcat/utils/math.h" + +using namespace kickcat; + +// Bounds wider than every destination type used here, so these cases reach the type reduction the +// way a caller with no meaningful limit of its own would. +constexpr double WIDE_LO = -1e10; +constexpr double WIDE_HI = 1e10; + +// The out-of-range cases below are undefined behaviour with a plain static_cast. + +TEST(NumericConversion, in_range_truncates_toward_zero) +{ + EXPECT_EQ(1, saturate(1.9, WIDE_LO, WIDE_HI)); + EXPECT_EQ(-1, saturate(-1.9, WIDE_LO, WIDE_HI)); + EXPECT_EQ(0, saturate(0.0, WIDE_LO, WIDE_HI)); +} + + +TEST(NumericConversion, caller_bounds_clamp_inside_them) +{ + EXPECT_EQ(1000, saturate(5000.0, -1000.0, 1000.0)); + EXPECT_EQ(-1000, saturate(-5000.0, -1000.0, 1000.0)); + EXPECT_EQ(250, saturate(250.0, -1000.0, 1000.0)); +} + + +TEST(NumericConversion, caller_bounds_wider_than_the_type_are_reduced_to_it) +{ + // Losing the type bound would put the conversion back in undefined territory. + EXPECT_EQ(INT32_MAX, saturate(3e9, WIDE_LO, WIDE_HI)); + EXPECT_EQ(INT32_MIN, saturate(-3e9, WIDE_LO, WIDE_HI)); + EXPECT_EQ(INT16_MAX, saturate(1e9, 1e8, 2e9)); // whole range above the type + EXPECT_EQ(INT16_MIN, saturate(-1e9, -2e9, -1e8)); // whole range below the type + EXPECT_EQ(255, saturate(300.0, WIDE_LO, WIDE_HI)); + EXPECT_EQ(0, saturate(-5.0, WIDE_LO, WIDE_HI)); +} + + +TEST(NumericConversion, exact_bounds_are_preserved) +{ + // The 32 bit bounds are exactly representable in a double, so clamping must not round them + // past the destination range. + EXPECT_EQ(INT32_MAX, saturate(2147483647.0, WIDE_LO, WIDE_HI)); + EXPECT_EQ(INT32_MIN, saturate(-2147483648.0, WIDE_LO, WIDE_HI)); + EXPECT_EQ(UINT32_MAX, saturate(4294967295.0, WIDE_LO, WIDE_HI)); +} + + +TEST(NumericConversion, inverted_caller_bounds_describe_the_same_interval) +{ + EXPECT_EQ(100, saturate(5000.0, 100.0, -100.0)); + EXPECT_EQ(-100, saturate(-5000.0, 100.0, -100.0)); + EXPECT_EQ(0, saturate(0.0, 100.0, -100.0)); +} + + +TEST(NumericConversion, narrowing_to_float_clamps_at_the_float_range) +{ + // double -> float is undefined outside the float range too, and the negative bound must come + // from lowest(), not min() (which is the smallest positive normal). + constexpr double lo = std::numeric_limits::lowest(); + constexpr double hi = std::numeric_limits::max(); + + EXPECT_EQ(std::numeric_limits::max(), saturate(1e300, lo, hi)); + EXPECT_EQ(std::numeric_limits::lowest(), saturate(-1e300, lo, hi)); + EXPECT_FLOAT_EQ(-1.5f, saturate(-1.5, lo, hi)); + EXPECT_FLOAT_EQ(2.5f, saturate(2.5, -10.0, 10.0)); + EXPECT_FLOAT_EQ(-10.0f, saturate(-1e300, -10.0, 10.0)); +} + + +TEST(NumericConversion, usable_in_a_constant_expression) +{ + static_assert(saturate(3e9, WIDE_LO, WIDE_HI) == INT32_MAX); + static_assert(saturate(-40000.0, WIDE_LO, WIDE_HI) == INT16_MIN); + static_assert(saturate(500.0, -100.0, 100.0) == 100); +} + + +TEST(NumericConversion, sixty_four_bit_destinations_saturate_at_their_exact_bound) +{ + // numeric_limits::max() has no exact double image (it rounds up to 2^63), so the bound + // is compared against and the exact integer returned rather than converted back. + constexpr double lo = std::numeric_limits::lowest(); + constexpr double hi = std::numeric_limits::max(); + + EXPECT_EQ(INT64_MAX, saturate(1e300, lo, hi)); + EXPECT_EQ(INT64_MIN, saturate(-1e300, lo, hi)); + EXPECT_EQ(UINT64_MAX, saturate(1e300, lo, hi)); + EXPECT_EQ(0u, saturate(-1e300, lo, hi)); + EXPECT_EQ(5, saturate(5.5, lo, hi)); + EXPECT_EQ(-5, saturate(-5.5, lo, hi)); + EXPECT_EQ(1000, saturate(1e300, -1000.0, 1000.0)); +} + + +TEST(Math, round_to_int_rounds_to_nearest_and_saturates) +{ + EXPECT_EQ(2, round_to_int(1.5)); + EXPECT_EQ(-2, round_to_int(-1.5)); + EXPECT_EQ(1, round_to_int(1.4)); + EXPECT_EQ(0, round_to_int(0.0)); + + // A bare cast of these would be undefined; the destination range is the only bound available. + EXPECT_EQ(INT64_MAX, round_to_int(1e300)); + EXPECT_EQ(INT64_MIN, round_to_int(-1e300)); + + static_assert(round_to_int(1.5) == 2); + static_assert(round_to_int(1e300) == INT64_MAX); +} + + +TEST(Math, clamp_and_abs_are_usable_in_a_constant_expression) +{ + static_assert(kickcat::clamp(5, 0, 3) == 3); + static_assert(kickcat::clamp(-5, 0, 3) == 0); + static_assert(abs_value(-7) == 7); + static_assert(abs_value(7) == 7); +}