Skip to content

Parse each AOG UDP datagram on its own so short packets do not drop the next one #107

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
gunicsba wants to merge 2 commits into
base: develop
Choose a base branch
from
Open

Parse each AOG UDP datagram on its own so short packets do not drop the next one #107

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
18a6bb5
Parse each AOG UDP datagram on its own so short packets do not drop t…
gunicsba Oct 14, 2026
cbf1e59
Log the sender and bytes of datagrams that are not AOG packets
gunicsba Oct 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
78 changes: 78 additions & 0 deletions include/aog_packet_parser.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
/**
* @brief Splits one AgIO/AOG UDP datagram into its packets
*
* A packet is: 0x80 0x81, source, PGN, length, <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 <cstddef>
#include <cstdint>
#include <span>

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<typename Callback>
ParseResult parse_datagram(std::span<std::uint8_t> 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
187 changes: 73 additions & 114 deletions src/udp_connections.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,12 @@
*/

#include "udp_connections.hpp"
#include <algorithm>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <sstream>
#include "aog_packet_parser.hpp"
#include "logging_utils.hpp"

#if !defined(_WIN32)
Expand Down Expand Up @@ -153,68 +157,67 @@ std::uint8_t UdpConnections::calculate_crc(std::span<std::uint8_t> data)
return static_cast<std::uint8_t>(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<std::uint8_t> 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<int>(datagram[i]);
text += ((datagram[i] >= 0x20) && (datagram[i] < 0x7F)) ? static_cast<char>(datagram[i]) : '.';
}
if (count < datagram.size())
{
hex << " ...";
}
return hex.str() + " \"" + text + "\"";
}

void log_parse_stop(const char *socketName, const udp::endpoint &sender, std::span<std::uint8_t> 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<std::uint8_t, 512> rxBuffer;
static std::size_t rxIndex = 0;
static std::array<std::uint8_t, MAX_PACKET_SIZE> 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)
{
// No data available
}
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<std::uint8_t> datagram(rxBuffer.data(), bytesReceived);
auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span<std::uint8_t> 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<size_t>(len) + 3 });
// if (crc != crcCalc)
// {
// std::cout << "CRC mismatch (for PGN " << static_cast<int>(pgn) << "?), expected " << static_cast<int>(crc) << " but got " << static_cast<int>(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
{
Expand All @@ -224,85 +227,41 @@ void UdpConnections::handle_incoming_packets()

void UdpConnections::handle_address_detection()
{
static std::array<std::uint8_t, 512> rxBuffer;
static std::size_t rxIndex = 0;
static std::array<std::uint8_t, MAX_PACKET_SIZE> 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)
{
// No data available
}
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<size_t>(len) + 3 });
// if (crc != crcCalc)
// {
// std::cout << "CRC mismatch (for PGN " << static_cast<int>(pgn) << "?), expected " << static_cast<int>(crc) << " but got " << static_cast<int>(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<std::uint8_t> datagram(rxBuffer.data(), bytesReceived);
auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span<std::uint8_t> 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
{
Expand Down
Loading
Loading