From 41e8feb41599d5ea5f126568e7806b0dcdbdd841 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sat, 18 Apr 2026 02:43:09 +0000 Subject: [PATCH 1/3] test: add regression coverage for partial transfers and invalid request directions Co-authored-by: Eric Cox --- test/interface_request_tests.hpp | 112 +++++++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) diff --git a/test/interface_request_tests.hpp b/test/interface_request_tests.hpp index c6b6a8f..28fbd3e 100644 --- a/test/interface_request_tests.hpp +++ b/test/interface_request_tests.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -88,6 +89,65 @@ class TestExecutor : public ::testing::Test roboclaw_serial::Interface interface_; }; +class ChunkedDummyDevice : public roboclaw_serial::SerialDevice +{ +public: + ChunkedDummyDevice( + std::vector expected_write_buffer, std::vector read_buffer, + std::size_t max_write_chunk, std::size_t max_read_chunk) + : expected_write_buffer_(std::move(expected_write_buffer)), + read_buffer_(std::move(read_buffer)), + max_write_chunk_(max_write_chunk), + max_read_chunk_(max_read_chunk) + { + connect("dummy chunked device"); + } + + bool connect(const std::string &) override + { + connected_ = true; + return true; + } + + void disconnect() override {connected_ = false;} + + std::size_t write(const std::byte * buffer, std::size_t count) override + { + const std::size_t chunk = std::min(max_write_chunk_, count); + for (std::size_t i = 0; i < chunk; ++i) { + observed_write_buffer_.push_back(buffer[i]); + } + + return chunk; + } + + std::size_t read(std::byte * buffer, std::size_t count) override + { + const std::size_t remaining = read_buffer_.size() - read_cursor_; + if (remaining == 0) { + return 0; + } + + const std::size_t chunk = std::min({remaining, count, max_read_chunk_}); + for (std::size_t i = 0; i < chunk; ++i) { + buffer[i] = read_buffer_[read_cursor_ + i]; + } + + read_cursor_ += chunk; + return chunk; + } + + bool writeBufferMatches() const {return observed_write_buffer_ == expected_write_buffer_;} + +private: + std::vector expected_write_buffer_; + std::vector observed_write_buffer_; + std::vector read_buffer_; + std::size_t read_cursor_ = 0; + std::size_t max_write_chunk_; + std::size_t max_read_chunk_; +}; + TEST_F(TestExecutor, WriteVelocityPIDConstantsM1SerializationTest) { this->executeTest( @@ -124,3 +184,55 @@ TEST_F(TestExecutor, ReadEncoderCountersSerializationTest) {0x00, 0x00, 0x30, 0xf0, 0x00, 0x00, 0xcf, 0xdc, 0xd4, 0xdb} // readBytes }}); } + +TEST_F(TestExecutor, WriteRetriesUntilCompleteWhenDeviceWritesPartialChunks) +{ + auto device = std::make_shared( + create_byte_vector( + {0x80, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0xb7, 0x00, 0x00, 0x03, 0x6e, 0x00, + 0x03, 0x13, 0x5a, 0x7d, 0x8e}), + create_byte_vector({0xff}), 3, 1); + + roboclaw_serial::Interface interface(device); + roboclaw_serial::VelocityPIDConstantsM1 request; + request.fields = std::make_tuple(0, 8631, 878, 201562); + + ASSERT_NO_THROW(interface.write(request)); + ASSERT_TRUE(device->writeBufferMatches()); +} + +TEST_F(TestExecutor, ReadRetriesUntilCompleteWhenDeviceReadsPartialChunks) +{ + auto device = std::make_shared( + create_byte_vector({0x80, 0x4e}), + create_byte_vector({0x00, 0x00, 0x30, 0xf0, 0x00, 0x00, 0xcf, 0xdc, 0xd4, 0xdb}), 2, 2); + + roboclaw_serial::Interface interface(device); + roboclaw_serial::EncoderCounters request; + + interface.read(request); + ASSERT_EQ(std::make_tuple(12528, 53212), request.fields); + ASSERT_TRUE(device->writeBufferMatches()); +} + +TEST_F(TestExecutor, ReadThrowsForWriteOnlyRequest) +{ + auto device = std::make_shared( + create_byte_vector({0xff}), create_byte_vector({0x80, 0xff})); + + roboclaw_serial::Interface interface(device); + roboclaw_serial::DriveM1M2WithSignedSpeed request; + + ASSERT_THROW(interface.read(request), std::invalid_argument); +} + +TEST_F(TestExecutor, WriteThrowsForReadOnlyRequest) +{ + auto device = std::make_shared( + create_byte_vector({0xff}), create_byte_vector({0x80, 0xff, 0x43, 0x43})); + + roboclaw_serial::Interface interface(device); + roboclaw_serial::FirmwareVersion request; + + ASSERT_THROW(interface.write(request), std::invalid_argument); +} From 74f319ae05d395a78678050d18fac87c1f1057eb Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sat, 18 Apr 2026 02:45:00 +0000 Subject: [PATCH 2/3] test: capture red-state for partial transfer and direction checks Co-authored-by: Eric Cox --- .gitignore | 1 + test/interface_request_tests.hpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..84c048a --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/build/ diff --git a/test/interface_request_tests.hpp b/test/interface_request_tests.hpp index 28fbd3e..c9b1e53 100644 --- a/test/interface_request_tests.hpp +++ b/test/interface_request_tests.hpp @@ -232,7 +232,7 @@ TEST_F(TestExecutor, WriteThrowsForReadOnlyRequest) create_byte_vector({0xff}), create_byte_vector({0x80, 0xff, 0x43, 0x43})); roboclaw_serial::Interface interface(device); - roboclaw_serial::FirmwareVersion request; + roboclaw_serial::MainBatteryVoltage request; ASSERT_THROW(interface.write(request), std::invalid_argument); } From 1a1c17c00540dd979958765310e32c4964cd09cc Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sat, 18 Apr 2026 02:47:19 +0000 Subject: [PATCH 3/3] fix: harden serial transfers and enforce request direction safety Co-authored-by: Eric Cox --- include/roboclaw_serial/device.hpp | 176 +++++++++++++++++++++----- include/roboclaw_serial/interface.hpp | 130 +++++++++++++++++-- 2 files changed, 263 insertions(+), 43 deletions(-) diff --git a/include/roboclaw_serial/device.hpp b/include/roboclaw_serial/device.hpp index 3e98904..6414d7f 100644 --- a/include/roboclaw_serial/device.hpp +++ b/include/roboclaw_serial/device.hpp @@ -14,13 +14,17 @@ #pragma once +#include #include #include #include +#include #include +#include #include #include +#include #include namespace roboclaw_serial @@ -33,16 +37,32 @@ class SerialDevice SerialDevice() = default; - explicit SerialDevice(const std::string device) {connect(device);} + explicit SerialDevice(const std::string & device) {connect(device);} virtual ~SerialDevice() {disconnect();} + void setReadTimeoutUs(const std::size_t timeout_us) {read_timeout_us_ = timeout_us;} + + std::size_t readTimeoutUs() const {return read_timeout_us_;} + + void setBaudRate(const speed_t baud_rate) {baud_rate_ = baud_rate;} + virtual bool connect(const std::string & device) { - fd_ = open(device.c_str(), O_RDWR | O_NOCTTY); + if (connected_) { + disconnect(); + } + + fd_ = open(device.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); connected_ = fd_ != -1; if (connected_) { - setSerialDeviceOptions(); + try { + setSerialDeviceOptions(); + } catch (const std::exception & e) { + std::cerr << "Failed to configure serial device: " << device << " (" << e.what() << ")" + << std::endl; + disconnect(); + } } else { std::cerr << "Failed to open serial device: " << device << std::endl; perror("Error"); @@ -56,6 +76,7 @@ class SerialDevice if (connected_) { close(fd_); connected_ = false; + fd_ = -1; } } @@ -63,62 +84,153 @@ class SerialDevice virtual std::size_t write(const std::byte * buffer, std::size_t count) { - ssize_t result = ::write(fd_, buffer, count); - if (result < 0) { - // Error writing to device + if (!connected_ || fd_ < 0) { + throw std::runtime_error("Serial device is not connected!"); + } + + while (true) { + const ssize_t result = ::write(fd_, buffer, count); + if (result >= 0) { + return static_cast(result); + } + + if (errno == EINTR) { + continue; + } + + if (errno == EAGAIN || errno == EWOULDBLOCK) { + waitForWritable(); + continue; + } + throw std::range_error("Error writing to the device!"); } - return static_cast(result) == count; } virtual std::size_t read(std::byte * buffer, std::size_t count) { - fd_set set; - struct timeval timeout; + if (!connected_ || fd_ < 0) { + throw std::runtime_error("Serial device is not connected!"); + } - /* Initialize the file descriptor set. */ - FD_ZERO(&set); - FD_SET(fd_, &set); + waitForReadable(); - /* Initialize the timeout data structure. */ - timeout.tv_sec = 0; - timeout.tv_usec = 10000; // 10ms + while (true) { + const ssize_t result = ::read(fd_, buffer, count); + if (result >= 0) { + return static_cast(result); + } + + if (errno == EINTR) { + continue; + } + + if (errno == EAGAIN || errno == EWOULDBLOCK) { + waitForReadable(); + continue; + } - /* select returns 0 if timeout, 1 if input available, -1 if error. */ - int res = select(FD_SETSIZE, &set, NULL, NULL, &timeout); - if (res < 0) { - throw std::range_error("Error reading from the serial device!"); - } else if (res == 0) { - throw std::runtime_error("Read timeout!"); - } - ssize_t result = ::read(fd_, buffer, count); - if (result < 0) { - // Error reading from the device throw std::range_error("Error reading from the serial device!"); } - - return static_cast(result); } protected: bool connected_ = false; private: + void waitForReadable() const + { + while (true) { + fd_set set; + FD_ZERO(&set); + FD_SET(fd_, &set); + + struct timeval timeout = timeoutStruct(); + const int ready = select(fd_ + 1, &set, nullptr, nullptr, &timeout); + if (ready > 0) { + return; + } + + if (ready == 0) { + throw std::runtime_error("Read timeout!"); + } + + if (errno == EINTR) { + continue; + } + + throw std::range_error("Error reading from the serial device!"); + } + } + + void waitForWritable() const + { + while (true) { + fd_set set; + FD_ZERO(&set); + FD_SET(fd_, &set); + + struct timeval timeout = timeoutStruct(); + const int ready = select(fd_ + 1, nullptr, &set, nullptr, &timeout); + if (ready > 0) { + return; + } + + if (ready == 0) { + throw std::runtime_error("Write timeout!"); + } + + if (errno == EINTR) { + continue; + } + + throw std::range_error("Error writing to the device!"); + } + } + + struct timeval timeoutStruct() const + { + struct timeval timeout; + timeout.tv_sec = static_cast(read_timeout_us_ / 1000000U); + timeout.tv_usec = static_cast(read_timeout_us_ % 1000000U); + return timeout; + } + void setSerialDeviceOptions() { - struct termios options; - tcgetattr(fd_, &options); - options.c_cflag = CS8 | CLOCAL | CREAD; + struct termios options {}; + if (tcgetattr(fd_, &options) < 0) { + throw std::runtime_error("Unable to read serial options"); + } + + cfmakeraw(&options); + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~(PARENB | CSTOPB | CRTSCTS); + options.c_cflag |= CS8; options.c_iflag = IGNPAR; options.c_oflag = 0; options.c_lflag = 0; + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 0; + + if (cfsetispeed(&options, baud_rate_) < 0 || cfsetospeed(&options, baud_rate_) < 0) { + throw std::runtime_error("Unable to set baud rate"); + } + tcflush(fd_, TCIFLUSH); - tcsetattr(fd_, TCSANOW, &options); + if (tcsetattr(fd_, TCSANOW, &options) < 0) { + throw std::runtime_error("Unable to apply serial options"); + } // Set the file descriptor to non-blocking mode - fcntl(fd_, F_SETFL, O_NONBLOCK); + const int flags = fcntl(fd_, F_GETFL, 0); + if (flags < 0 || fcntl(fd_, F_SETFL, flags | O_NONBLOCK) < 0) { + throw std::runtime_error("Unable to set non-blocking mode"); + } } + speed_t baud_rate_ = B38400; + std::size_t read_timeout_us_ = 10000; int fd_ = -1; }; diff --git a/include/roboclaw_serial/interface.hpp b/include/roboclaw_serial/interface.hpp index b39784c..79ed51a 100644 --- a/include/roboclaw_serial/interface.hpp +++ b/include/roboclaw_serial/interface.hpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include #include "roboclaw_serial/command.hpp" @@ -45,12 +47,20 @@ class Interface template void read(Request & request, const unsigned char address = 128) { + if constexpr (Request::read_command == uint8_t(Command::NONE)) { + throw std::invalid_argument("Request does not support reads"); + } request.fields = read(address); } template typename Request::ArgsTuple read(const unsigned char address = 128) { + if constexpr (Request::read_command == uint8_t(Command::NONE)) { + throw std::invalid_argument("Request does not support reads"); + } + ensureConnected(); + // Prevent parallel reads/writes std::lock_guard lock(mutex_); @@ -61,16 +71,26 @@ class Interface crc16::update(crc_, byte); } - // Write the buffer to the serial device - device_->write(buffer_.data(), buffer_.size()); - - // Set the buffer to the size of the fields, size of CRC - buffer_.resize(buffer_.max_size()); - - // Read the response from the device - std::size_t bytes_read = device_->read(buffer_.data(), buffer_.size()); + // Write the read command, retrying partial writes. + writeAll(buffer_.data(), buffer_.size()); + + // Read exactly-sized fixed payloads (numeric requests) and read-until-idle for + // dynamic payloads (e.g. firmware version strings). + std::size_t bytes_read = 0; + if constexpr (requestHasDynamicSize()) { + buffer_.resize(buffer_.max_size()); + bytes_read = readUntilIdle(buffer_.data(), buffer_.size()); + } else { + constexpr std::size_t response_size = requestResponseSize(); + buffer_.resize(response_size); + readExactly(buffer_.data(), buffer_.size()); + bytes_read = response_size; + } buffer_.resize(bytes_read); + if (buffer_.size() < sizeof(uint16_t)) { + throw std::logic_error("response was too short"); + } // Extract the CRC from the the back of the buffer auto recv_crc = buffer_.pop_back(); @@ -101,6 +121,10 @@ class Interface template void write(const Request & request, const unsigned char address = 128) { + if constexpr (Request::write_command == uint8_t(Command::NONE)) { + throw std::invalid_argument("Request does not support writes"); + } + // Write the fields to the roboclaw write(request.fields, address); } @@ -108,14 +132,19 @@ class Interface template void write(const typename Request::ArgsTuple & fields, const unsigned char address = 128) { + if constexpr (Request::write_command == uint8_t(Command::NONE)) { + throw std::invalid_argument("Request does not support writes"); + } + ensureConnected(); + // Prevent parallel read/writes std::lock_guard lock(mutex_); // Initialize buffer with Write request, fields, and CRC this->bufferSetupWrite(address, fields); - // Write the request - device_->write(buffer_.data(), buffer_.size()); + // Write the request, retrying partial writes. + writeAll(buffer_.data(), buffer_.size()); if (!this->readAck()) { throw std::logic_error("did not get an ack!"); @@ -188,11 +217,90 @@ class Interface { // We only expect an ACK from the roboclaw buffer_.resize(1); - device_->read(buffer_.data(), buffer_.size()); + readExactly(buffer_.data(), buffer_.size()); return buffer_.pop_back() == ACK; } + void ensureConnected() const + { + if (!device_ || !device_->connected()) { + throw std::runtime_error("serial device is not connected"); + } + } + + void writeAll(const std::byte * data, const std::size_t size) + { + std::size_t bytes_written = 0; + while (bytes_written < size) { + const auto written = device_->write(data + bytes_written, size - bytes_written); + if (written == 0) { + throw std::runtime_error("serial write made no progress"); + } + bytes_written += written; + } + } + + void readExactly(std::byte * data, const std::size_t size) + { + std::size_t bytes_read = 0; + while (bytes_read < size) { + const auto received = device_->read(data + bytes_read, size - bytes_read); + if (received == 0) { + throw std::runtime_error("serial read made no progress"); + } + bytes_read += received; + } + } + + std::size_t readUntilIdle(std::byte * data, const std::size_t max_size) + { + std::size_t bytes_read = 0; + while (bytes_read < max_size) { + try { + const auto received = device_->read(data + bytes_read, max_size - bytes_read); + if (received == 0) { + break; + } + bytes_read += received; + } catch (const std::runtime_error &) { + if (bytes_read == 0) { + throw; + } + break; + } + } + + return bytes_read; + } + + template + static constexpr std::size_t tupleStaticByteSize(std::index_sequence) + { + return (sizeof(std::tuple_element_t) + ... + 0U); + } + + template + static constexpr bool tupleHasDynamicType(std::index_sequence) + { + return (std::is_same_v, std::string> || ... || false); + } + + template + static constexpr bool requestHasDynamicSize() + { + using ArgsTuple = typename Request::ArgsTuple; + return tupleHasDynamicType(std::make_index_sequence>{}); + } + + template + static constexpr std::size_t requestResponseSize() + { + using ArgsTuple = typename Request::ArgsTuple; + return tupleStaticByteSize(std::make_index_sequence>{}) + + sizeof(uint16_t); + } + const std::byte ACK = std::byte(255U); uint16_t crc_; SerialDevice::SharedPtr device_;