From 18a6bb5f3a3fd0561d7e6bbffc4a1312d4dbddcb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 08:32:29 +0200 Subject: [PATCH 1/2] Parse each AOG UDP datagram on its own so short packets do not drop the next one The receive loop kept unparsed bytes in a static buffer between datagrams and never reset its read index after moving leftover bytes to the front. A packet shorter than 8 bytes (AOG's 7-byte section control request, PGN 241) waited in the buffer for the next datagram, and the packet after it was then read from the wrong offset, logged as "Unknown start of message" and dropped. It also read the 2 start bytes with two unsequenced index++ in one expression, and did not check that a whole packet was present before handing it on. AgIO never splits a packet across datagrams, so each datagram is now parsed on its own by aog_packet::parse_datagram (header-only, unit tested), shared by the AOG and address detection sockets. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 10 ++ include/aog_packet_parser.hpp | 78 ++++++++++++++ src/udp_connections.cpp | 169 ++++++++++--------------------- tools/test_aog_packet_parser.cpp | 155 ++++++++++++++++++++++++++++ 4 files changed, 298 insertions(+), 114 deletions(-) create mode 100644 include/aog_packet_parser.hpp create mode 100644 tools/test_aog_packet_parser.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b49f5af..a93dec6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -194,6 +194,16 @@ if(AOG_TC_VALIDATE_IOP) add_dependencies(iop_validator test_async_log) add_test(NAME async_log COMMAND test_async_log) + # AOG UDP datagram parsing; the parser is header-only. + add_executable(test_aog_packet_parser + ${CMAKE_CURRENT_LIST_DIR}/tools/test_aog_packet_parser.cpp) + target_compile_features(test_aog_packet_parser PRIVATE cxx_std_20) + set_target_properties(test_aog_packet_parser PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_aog_packet_parser + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + add_dependencies(iop_validator test_aog_packet_parser) + add_test(NAME aog_packet_parser COMMAND test_aog_packet_parser) + # PASS_REGULAR_EXPRESSION makes CTest ignore the exit code, which is the part # a CI gate actually reads. add_test(NAME malformed_pool_exit_status COMMAND iop_validator diff --git a/include/aog_packet_parser.hpp b/include/aog_packet_parser.hpp new file mode 100644 index 0000000..8e779f0 --- /dev/null +++ b/include/aog_packet_parser.hpp @@ -0,0 +1,78 @@ +/** + * @brief Splits one AgIO/AOG UDP datagram into its packets + * + * A packet is: 0x80 0x81, source, PGN, length, data bytes, CRC. + * AgIO never splits a packet across datagrams, so each datagram is parsed on its own + * and nothing is carried over to the next one. + */ + +#pragma once + +#include +#include +#include + +namespace aog_packet +{ + inline constexpr std::uint8_t START_HIGH = 0x80; + inline constexpr std::uint8_t START_LOW = 0x81; + inline constexpr std::size_t HEADER_SIZE = 5; ///< Start (2), source, PGN, length + inline constexpr std::size_t CRC_SIZE = 1; + + /// @brief Why parsing a datagram stopped before its end + enum class ParseStop + { + Complete, ///< Every byte belonged to a packet + UnknownStart, ///< Bytes at the stop offset do not start with 0x80 0x81 + Truncated ///< A packet header claims more bytes than the datagram holds + }; + + struct ParseResult + { + ParseStop stop = ParseStop::Complete; + std::size_t offset = 0; ///< Where parsing stopped; equals the datagram size when complete + std::size_t packets = 0; ///< Number of packets handed to the callback + }; + + /** + * @brief Hands every complete packet in a datagram to a callback, in order + * @param datagram The bytes of one received datagram + * @param onPacket Called as onPacket(src, pgn, data) for each packet; data excludes the CRC + * @return Where and why parsing stopped + */ + template + ParseResult parse_datagram(std::span datagram, Callback &&onPacket) + { + ParseResult result; + while (result.offset < datagram.size()) + { + const std::size_t remaining = datagram.size() - result.offset; + if (remaining < HEADER_SIZE + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + const std::uint8_t *packet = datagram.data() + result.offset; + if ((START_HIGH != packet[0]) || (START_LOW != packet[1])) + { + result.stop = ParseStop::UnknownStart; + return result; + } + + const std::uint8_t src = packet[2]; + const std::uint8_t pgn = packet[3]; + const std::size_t length = packet[4]; + if (remaining < HEADER_SIZE + length + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + onPacket(src, pgn, datagram.subspan(result.offset + HEADER_SIZE, length)); + result.offset += HEADER_SIZE + length + CRC_SIZE; + result.packets++; + } + return result; + } +} // namespace aog_packet diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index a4e7540..82e59a9 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -10,6 +10,7 @@ #include "udp_connections.hpp" #include #include +#include "aog_packet_parser.hpp" #include "logging_utils.hpp" #if !defined(_WIN32) @@ -153,15 +154,37 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) return static_cast(result & 0xFF); } +namespace +{ + void log_parse_stop(const char *socketName, std::span datagram, const aog_packet::ParseResult &result) + { + if (aog_packet::ParseStop::Complete == result.stop) + { + return; + } + + const std::size_t dropped = datagram.size() - result.offset; + if (aog_packet::ParseStop::UnknownStart == result.stop) + { + const std::uint16_t start = static_cast((datagram[result.offset] << 8) | + ((result.offset + 1 < datagram.size()) ? datagram[result.offset + 1] : 0)); + log() << "Unknown start of message: 0x" << std::hex << start << std::dec + << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + } + else + { + log() << "Truncated AOG packet (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + } + } +} + void UdpConnections::handle_incoming_packets() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -169,52 +192,14 @@ void UdpConnections::handle_incoming_packets() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + if (packetCallback) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (packetCallback) - { - packetCallback(src, pgn, { rxBuffer.data() + index, len }); - } - index += len + 1; - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; + packetCallback(src, pgn, data); } - - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; - } - } + }); + log_parse_stop("AOG", datagram, result); } else { @@ -224,13 +209,11 @@ void UdpConnections::handle_incoming_packets() void UdpConnections::handle_address_detection() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -238,71 +221,29 @@ void UdpConnections::handle_address_detection() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + // 127 is source AgIO, 201 is subnet; data is 0xC9 0xC9 IP0 IP1 IP2 + if ((0x7F != src) || (0xC9 != pgn) || (5 != data.size()) || (0xC9 != data[0]) || (0xC9 != data[1])) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (src == 0x7F && pgn == 0xC9 && len == 5 // 127 is source, AGIO, 201 is subnet - && rxBuffer[index++] == 0xC9 && rxBuffer[index++] == 0xC9) - { - // 7-8-9 is IP0,IP1,IP2 - settings->set_subnet({ rxBuffer[index++], rxBuffer[index++], rxBuffer[index++] }); - - std::cout << "Subnet from AOG: "; - std::cout << int(settings->get_subnet()[0]) << "."; - std::cout << int(settings->get_subnet()[1]) << "."; - std::cout << int(settings->get_subnet()[2]); - std::cout << " rebinding UPD connection " << std::endl; - udpConnection.close(); - udpConnection.open(udp::v4()); - udpConnection.set_option(boost::asio::socket_base::broadcast(true)); - udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); - udpConnection.non_blocking(true); - udpConnection.bind(get_local_endpoint()); - - index += len - 4; - } - else - { - index += len + 1; - } - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; + return; } - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; - } - } + settings->set_subnet({ data[2], data[3], data[4] }); + + std::cout << "Subnet from AOG: "; + std::cout << int(settings->get_subnet()[0]) << "."; + std::cout << int(settings->get_subnet()[1]) << "."; + std::cout << int(settings->get_subnet()[2]); + std::cout << " rebinding UPD connection " << std::endl; + udpConnection.close(); + udpConnection.open(udp::v4()); + udpConnection.set_option(boost::asio::socket_base::broadcast(true)); + udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); + udpConnection.non_blocking(true); + udpConnection.bind(get_local_endpoint()); + }); + log_parse_stop("address detection", datagram, result); } else { diff --git a/tools/test_aog_packet_parser.cpp b/tools/test_aog_packet_parser.cpp new file mode 100644 index 0000000..d25db51 --- /dev/null +++ b/tools/test_aog_packet_parser.cpp @@ -0,0 +1,155 @@ +/** + * @brief Unit tests for aog_packet::parse_datagram, which splits an AgIO/AOG UDP datagram into packets. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "aog_packet_parser.hpp" + +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +struct Packet +{ + std::uint8_t src; + std::uint8_t pgn; + std::vector data; +}; + +static std::vector make_packet(std::uint8_t pgn, std::vector data) +{ + std::vector bytes = { 0x80, 0x81, 0x7F, pgn, static_cast(data.size()) }; + bytes.insert(bytes.end(), data.begin(), data.end()); + bytes.push_back(0x00); // CRC, not checked + return bytes; +} + +static aog_packet::ParseResult parse(std::vector &datagram, std::vector &packets) +{ + return aog_packet::parse_datagram(datagram, [&packets](std::uint8_t src, std::uint8_t pgn, std::span data) { + packets.push_back({ src, pgn, std::vector(data.begin(), data.end()) }); + }); +} + +static void test_short_packet_is_parsed_on_its_own() +{ + std::printf("test_short_packet_is_parsed_on_its_own\n"); + + // PGN 241 (section control request) is 7 bytes; it must not wait for the next datagram. + auto datagram = make_packet(0xF1, { 0x01 }); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a 7-byte datagram parses completely"); + check(1 == packets.size(), "one packet is delivered"); + check(!packets.empty() && 0xF1 == packets[0].pgn && 1 == packets[0].data.size() && 0x01 == packets[0].data[0], "PGN and data are correct"); +} + +static void test_short_datagram_does_not_eat_the_next_one() +{ + std::printf("test_short_datagram_does_not_eat_the_next_one\n"); + + // The field failure: a 7-byte PGN 241 was kept in the buffer, the next datagram (PGN 229) was + // appended to it, and the parser then read the second packet from the wrong offset and dropped it. + auto first = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }); + std::vector packets; + parse(first, packets); + auto result = parse(second, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "the following datagram parses completely"); + check(2 == packets.size(), "both packets are delivered"); + check(2 == packets.size() && 0xE5 == packets[1].pgn && 8 == packets[1].data.size() && 0x03 == packets[1].data[0], "the PGN 229 section bits arrive intact"); +} + +static void test_several_packets_in_one_datagram() +{ + std::printf("test_several_packets_in_one_datagram\n"); + + auto datagram = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0xFF, 0x0F, 0, 0, 0, 0, 0, 0 }); + auto third = make_packet(0xF2, { 0x8D, 0x01, 1, 2, 3, 4 }); + datagram.insert(datagram.end(), second.begin(), second.end()); + datagram.insert(datagram.end(), third.begin(), third.end()); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "parses completely"); + check(3 == packets.size(), "all three packets are delivered"); + check(3 == packets.size() && 0xF1 == packets[0].pgn && 0xE5 == packets[1].pgn && 0xF2 == packets[2].pgn, "in order"); + check(3 == packets.size() && 0x0F == packets[1].data[1] && 4 == packets[2].data[5], "with their own data"); +} + +static void test_unknown_start_stops_without_delivering_garbage() +{ + std::printf("test_unknown_start_stops_without_delivering_garbage\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.insert(datagram.end(), { 0x24, 0x47, 0x50, 0x47, 0x47, 0x41, 0x2C }); // "$GPGGA," after a valid packet + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::UnknownStart == result.stop, "stops at the unknown start"); + check(14 == result.offset, "reports where it stopped"); + check(1 == packets.size(), "the valid packet before it is still delivered"); +} + +static void test_truncated_packet_is_not_delivered() +{ + std::printf("test_truncated_packet_is_not_delivered\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.resize(datagram.size() - 3); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Truncated == result.stop, "reports a truncated packet"); + check(packets.empty(), "delivers nothing past the end of the datagram"); + + std::vector headerOnly = { 0x80, 0x81, 0x7F }; + packets.clear(); + result = parse(headerOnly, packets); + check(aog_packet::ParseStop::Truncated == result.stop, "a partial header is truncated, not read past"); + check(packets.empty(), "and delivers nothing"); +} + +static void test_empty_packet() +{ + std::printf("test_empty_packet\n"); + + auto datagram = make_packet(0xC8, {}); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a zero-length packet parses"); + check(1 == packets.size() && packets[0].data.empty(), "with empty data"); +} + +int main() +{ + test_short_packet_is_parsed_on_its_own(); + test_short_datagram_does_not_eat_the_next_one(); + test_several_packets_in_one_datagram(); + test_unknown_start_stops_without_delivering_garbage(); + test_truncated_packet_is_not_delivered(); + test_empty_packet(); + + if (0 != failures) + { + std::fprintf(stderr, "%d assertion(s) failed\n", failures); + return 1; + } + std::printf("All tests passed\n"); + return 0; +} From cbf1e59f22e602f9256a30221f3cca43bd28a52d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 18:02:21 +0200 Subject: [PATCH 2/2] Log the sender and bytes of datagrams that are not AOG packets The field log showed datagrams with 2 or 4 bytes after a complete packet, and others that are not AOG packets at all (one starts with "$P"). The log only said how many bytes were dropped, so the sender could not be identified. Log the sender's address and port, and up to 64 bytes as hex with a "|" where parsing stopped, followed by the printable characters. Co-Authored-By: Claude Opus 5.5 --- src/udp_connections.cpp | 46 ++++++++++++++++++++++++++++------------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index 82e59a9..75b5651 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -8,8 +8,11 @@ */ #include "udp_connections.hpp" +#include #include +#include #include +#include #include "aog_packet_parser.hpp" #include "logging_utils.hpp" @@ -156,25 +159,40 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) namespace { - void log_parse_stop(const char *socketName, std::span datagram, const aog_packet::ParseResult &result) + /// Bytes of a datagram shown in the log when it is not (only) AOG packets + constexpr std::size_t MAX_LOGGED_DATAGRAM_BYTES = 64; + + /// @brief Hex dump with a "|" where parsing stopped, then the printable characters (others as ".") + std::string describe_datagram(std::span datagram, std::size_t stopOffset) { - if (aog_packet::ParseStop::Complete == result.stop) + const std::size_t count = std::min(datagram.size(), MAX_LOGGED_DATAGRAM_BYTES); + std::ostringstream hex; + std::string text; + hex << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < count; i++) { - return; + hex << ((i == stopOffset) ? ((0 == i) ? "| " : " | ") : ((0 == i) ? "" : " ")) << std::setw(2) << static_cast(datagram[i]); + text += ((datagram[i] >= 0x20) && (datagram[i] < 0x7F)) ? static_cast(datagram[i]) : '.'; } - - const std::size_t dropped = datagram.size() - result.offset; - if (aog_packet::ParseStop::UnknownStart == result.stop) + if (count < datagram.size()) { - const std::uint16_t start = static_cast((datagram[result.offset] << 8) | - ((result.offset + 1 < datagram.size()) ? datagram[result.offset + 1] : 0)); - log() << "Unknown start of message: 0x" << std::hex << start << std::dec - << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + hex << " ..."; } - else + return hex.str() + " \"" + text + "\""; + } + + void log_parse_stop(const char *socketName, const udp::endpoint &sender, std::span datagram, const aog_packet::ParseResult &result) + { + if (aog_packet::ParseStop::Complete == result.stop) { - log() << "Truncated AOG packet (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + return; } + + const std::size_t dropped = datagram.size() - result.offset; + log() << ((aog_packet::ParseStop::UnknownStart == result.stop) ? "Unknown start of message" : "Truncated AOG packet") + << " from " << sender.address().to_string() << ":" << sender.port() + << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes): " + << describe_datagram(datagram, result.offset) << std::endl; } } @@ -199,7 +217,7 @@ void UdpConnections::handle_incoming_packets() packetCallback(src, pgn, data); } }); - log_parse_stop("AOG", datagram, result); + log_parse_stop("AOG", sender_endpoint, datagram, result); } else { @@ -243,7 +261,7 @@ void UdpConnections::handle_address_detection() udpConnection.non_blocking(true); udpConnection.bind(get_local_endpoint()); }); - log_parse_stop("address detection", datagram, result); + log_parse_stop("address detection", sender_endpoint, datagram, result); } else {