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..75b5651 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -8,8 +8,12 @@ */ #include "udp_connections.hpp" +#include #include +#include #include +#include +#include "aog_packet_parser.hpp" #include "logging_utils.hpp" #if !defined(_WIN32) @@ -153,15 +157,52 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) return static_cast(result & 0xFF); } +namespace +{ + /// 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) + { + 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++) + { + 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]) : '.'; + } + if (count < datagram.size()) + { + hex << " ..."; + } + 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) + { + 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; + } +} + 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 +210,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; + packetCallback(src, pgn, data); } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; - } - - // 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", sender_endpoint, datagram, result); } else { @@ -224,13 +227,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 +239,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::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 + 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])) { - // 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", sender_endpoint, 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; +}