From 110d37bfd0a92b8afc3887ad996ed788d488068a Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 7 Apr 2026 11:25:27 -0400 Subject: [PATCH 01/20] Create packetization-capable transports for serial and can --- test/zcm/PacketizedSerialTransportTest.hpp | 266 +++++++ test/zcm/wscript | 9 + wscript | 2 +- zcm/transport/packetized_serial_transport.c | 650 ++++++++++++++++++ zcm/transport/packetized_serial_transport.h | 27 + zcm/transport/packetized_transport_can.cpp | 357 ++++++++++ zcm/transport/packetized_transport_serial.cpp | 441 ++++++++++++ zcm/wscript | 10 +- 8 files changed, 1758 insertions(+), 4 deletions(-) create mode 100644 test/zcm/PacketizedSerialTransportTest.hpp create mode 100644 zcm/transport/packetized_serial_transport.c create mode 100644 zcm/transport/packetized_serial_transport.h create mode 100644 zcm/transport/packetized_transport_can.cpp create mode 100644 zcm/transport/packetized_transport_serial.cpp diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp new file mode 100644 index 000000000..4b70943d6 --- /dev/null +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -0,0 +1,266 @@ +#ifndef PACKETIZED_SERIAL_TRANSPORT_TEST_HPP +#define PACKETIZED_SERIAL_TRANSPORT_TEST_HPP + +#include "cxxtest/TestSuite.h" + +extern "C" { +#include "zcm/transport.h" +zcm_trans_t* zcm_trans_packetized_serial_create( + size_t (*get)(uint8_t* data, size_t nData, void* usr), + size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize); +int packetized_serial_update_rx(zcm_trans_t* zt); +int packetized_serial_update_tx(zcm_trans_t* zt); +} + +#include +#include +#include +#include + +using std::string; +using std::vector; + +struct PacketizedLinkEndpoint +{ + vector rx; + vector txParse; + PacketizedLinkEndpoint* peer{ nullptr }; + + bool dropEnabled{ false }; + bool dropDone{ false }; + uint16_t dropPacketId{ 0 }; + string dropChannel; +}; + +static bool decode_frame(const vector& buf, size_t start, size_t& frame_end, + string& channel, vector& data) +{ + if (buf.size() < start + 7) return false; + if (buf[start] != 0xcc || buf[start + 1] != 0x00) return false; + + size_t cur = start + 2; + uint8_t chan_len = buf[cur++]; + uint32_t data_len = ((uint32_t)buf[cur] << 24) | ((uint32_t)buf[cur + 1] << 16) | + ((uint32_t)buf[cur + 2] << 8) | (uint32_t)buf[cur + 3]; + cur += 4; + + channel.clear(); + channel.reserve(chan_len); + for (uint8_t i = 0; i < chan_len; ++i) { + if (cur >= buf.size()) return false; + uint8_t c = buf[cur++]; + if (c == 0xcc) { + if (cur >= buf.size()) return false; + uint8_t c2 = buf[cur++]; + if (c2 != 0xcc) return false; + c = c2; + } + channel.push_back((char)c); + } + + data.clear(); + data.reserve(data_len); + for (uint32_t i = 0; i < data_len; ++i) { + if (cur >= buf.size()) return false; + uint8_t c = buf[cur++]; + if (c == 0xcc) { + if (cur >= buf.size()) return false; + uint8_t c2 = buf[cur++]; + if (c2 != 0xcc) return false; + c = c2; + } + data.push_back(c); + } + + if (cur + 2 > buf.size()) return false; + frame_end = cur + 2; + return true; +} + +static size_t endpoint_get(uint8_t* data, size_t nData, void* usr) +{ + auto* ep = (PacketizedLinkEndpoint*)usr; + size_t n = ep->rx.size() < nData ? ep->rx.size() : nData; + if (n == 0) return 0; + memcpy(data, ep->rx.data(), n); + ep->rx.erase(ep->rx.begin(), ep->rx.begin() + (ptrdiff_t)n); + return n; +} + +static size_t endpoint_put(const uint8_t* data, size_t nData, void* usr) +{ + auto* ep = (PacketizedLinkEndpoint*)usr; + if (nData == 0) return 0; + + ep->txParse.insert(ep->txParse.end(), data, data + nData); + + size_t consumed = 0; + while (consumed < ep->txParse.size()) { + if (ep->txParse[consumed] != 0xcc) { + ++consumed; + continue; + } + + string channel; + vector payload; + size_t frame_end = 0; + if (!decode_frame(ep->txParse, consumed, frame_end, channel, payload)) break; + + bool drop = false; + if (ep->dropEnabled && !ep->dropDone && channel == ep->dropChannel && + payload.size() >= 6) { + uint8_t type = payload[0]; + uint8_t body_len = payload[3]; + if (type == 2 && payload.size() == 4u + body_len && body_len >= 2) { + uint16_t packet_id = ((uint16_t)payload[4] << 8) | (uint16_t)payload[5]; + if (packet_id == ep->dropPacketId) { + drop = true; + ep->dropDone = true; + } + } + } + + if (!drop) { + ep->peer->rx.insert(ep->peer->rx.end(), + ep->txParse.begin() + (ptrdiff_t)consumed, + ep->txParse.begin() + (ptrdiff_t)frame_end); + } + + consumed = frame_end; + } + + if (consumed > 0) { + ep->txParse.erase(ep->txParse.begin(), ep->txParse.begin() + (ptrdiff_t)consumed); + } + + return nData; +} + +static uint64_t fake_now(void* usr) { return *(uint64_t*)usr; } + +static void pump(zcm_trans_t* tx, zcm_trans_t* rx, int iters) +{ + for (int i = 0; i < iters; ++i) { + TS_ASSERT_EQUALS(packetized_serial_update_tx(tx), ZCM_EOK); + TS_ASSERT_EQUALS(packetized_serial_update_rx(rx), ZCM_EOK); + } +} + +class PacketizedSerialTransportTest : public CxxTest::TestSuite +{ + public: + void testPacketizedRoundTrip() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 1000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(512); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0x5a); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$BIG"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 20); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("$BIG")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + + void testRetransmissionRequestPath() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + a.dropEnabled = true; + a.dropPacketId = 1; + a.dropChannel = "$RETX"; + + uint64_t now = 2000; + zcm_trans_t* ta = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768); + zcm_trans_t* tb = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768); + TSM_ASSERT("failed creating transports", ta && tb); + + vector payload(700); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i + 11); + + zcm_msg_t m; + m.utime = now; + m.channel = (char*)"$RETX"; + m.len = payload.size(); + m.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(ta, m), ZCM_EOK); + pump(ta, tb, 25); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(tb, &out, 0), ZCM_EAGAIN); + + now += 300000; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(tb, &out, 0), ZCM_EAGAIN); + + uint8_t pingbuf[1] = { 0x42 }; + zcm_msg_t ping; + ping.utime = now; + ping.channel = (char*)"PING"; + ping.len = 1; + ping.buf = pingbuf; + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tb, ping), ZCM_EOK); + pump(tb, ta, 10); + + TS_ASSERT_EQUALS(zcm_trans_recvmsg(ta, &out, 0), ZCM_EOK); + + uint8_t pongbuf[1] = { 0x24 }; + zcm_msg_t pong; + pong.utime = now; + pong.channel = (char*)"PONG"; + pong.len = 1; + pong.buf = pongbuf; + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(ta, pong), ZCM_EOK); + pump(ta, tb, 20); + + bool gotRetx = false; + for (int i = 0; i < 6; ++i) { + int ret = zcm_trans_recvmsg(tb, &out, 0); + if (ret != ZCM_EOK) continue; + if (string(out.channel) == "$RETX") { + gotRetx = true; + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + break; + } + } + TS_ASSERT(gotRetx); + TS_ASSERT(a.dropDone); + + zcm_trans_destroy(ta); + zcm_trans_destroy(tb); + } +}; + +#endif diff --git a/test/zcm/wscript b/test/zcm/wscript index c0c6f672d..fb9582336 100644 --- a/test/zcm/wscript +++ b/test/zcm/wscript @@ -7,3 +7,12 @@ def build(ctx): source = 'multi_file.cpp', rpath = ctx.env.RPATH_zcm, install_path = None) + + ctx.stlib(target = 'packetized_test_support', + use = 'default', + source = [ + '../../zcm/transport/packetized_serial_transport.c', + '../../zcm/transport/generic_serial_transport.c', + '../../zcm/transport/generic_serial_circ_buff.c', + ], + install_path = None) diff --git a/wscript b/wscript index 1c97d910b..6af0c9c40 100644 --- a/wscript +++ b/wscript @@ -482,7 +482,7 @@ def build(ctx): if ctx.env.USING_CXXTEST: ctx.cxxtest(use = ['zcm', 'zcm_tools_util', 'testzcmtypes', 'testzcmtypes_cpp', 'testzcmtypes_c_stlib', - 'multifile_lib']) + 'multifile_lib', 'packetized_test_support']) else: ctx.recurse('scripts') ctx.recurse('zcm') diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c new file mode 100644 index 000000000..853864aa6 --- /dev/null +++ b/zcm/transport/packetized_serial_transport.c @@ -0,0 +1,650 @@ +#include "zcm/transport.h" +#include "zcm/zcm.h" + +#include "generic_serial_transport.h" +#include "packetized_serial_transport.h" + +#include +#include +#include +#include + +#define ASSERT(x) + +#define PACKETIZED_HEADER_BYTES (4) +#define PACKETIZED_DATA_OVERHEAD_BYTES (2) +#define PACKETIZED_METADATA_BODY_BYTES (11) +#define PACKETIZED_RETRANS_TIMEOUT_US (200000) +#define PACKETIZED_MAX_MTU ((size_t)UINT32_MAX) + +typedef enum packetized_msg_type_t +{ + PACKETIZED_MSG_METADATA = 1, + PACKETIZED_MSG_DATA = 2, + PACKETIZED_MSG_RETRANS_REQ = 3, +} packetized_msg_type_t; + +typedef struct packetized_rx_state_t packetized_rx_state_t; +struct packetized_rx_state_t +{ + int active; + char channel[ZCM_CHANNEL_MAXLEN + 1]; + uint16_t session_id; + uint16_t total_packets; + uint8_t packet_data_size; + uint32_t total_message_size; + uint32_t expected_crc; + uint64_t last_update_utime; + + uint8_t* data; + uint8_t* packet_received; + uint16_t received_count; + + int retrans_pending; + uint16_t* missing_ids; + uint16_t missing_count; +}; + +typedef struct packetized_tx_state_t packetized_tx_state_t; +struct packetized_tx_state_t +{ + int active; + char channel[ZCM_CHANNEL_MAXLEN + 1]; + uint16_t session_id; + uint16_t total_packets; + uint8_t packet_data_size; + uint32_t total_message_size; + uint8_t* data; + + uint16_t* retrans_ids; + uint16_t retrans_count; +}; + +typedef struct zcm_trans_packetized_serial_t zcm_trans_packetized_serial_t; +struct zcm_trans_packetized_serial_t +{ + zcm_trans_t trans; + zcm_trans_t* inner; + + size_t inner_mtu; + size_t mtu; + + uint8_t* pkt_buf; + size_t pkt_buf_size; + + uint8_t* out_buf; + size_t out_buf_size; + size_t out_len; + char out_channel[ZCM_CHANNEL_MAXLEN + 1]; + int out_pending; + + uint16_t next_session_id; + packetized_rx_state_t rx; + packetized_tx_state_t tx; + + uint64_t (*time)(void* usr); + void* time_usr; +}; + +static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt); + +static uint16_t read_u16_be(const uint8_t* p) +{ + return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; +} + +static uint32_t read_u32_be(const uint8_t* p) +{ + return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | + (uint32_t)p[3]; +} + +static void write_u16_be(uint8_t* p, uint16_t v) +{ + p[0] = (uint8_t)((v >> 8) & 0xff); + p[1] = (uint8_t)(v & 0xff); +} + +static void write_u32_be(uint8_t* p, uint32_t v) +{ + p[0] = (uint8_t)((v >> 24) & 0xff); + p[1] = (uint8_t)((v >> 16) & 0xff); + p[2] = (uint8_t)((v >> 8) & 0xff); + p[3] = (uint8_t)(v & 0xff); +} + +static uint32_t crc32_update_byte(uint32_t crc, uint8_t b) +{ + crc ^= b; + for (int i = 0; i < 8; ++i) { + uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u)); + crc = (crc >> 1) ^ (0xedb88320u & mask); + } + return crc; +} + +static uint32_t crc32_compute(const uint8_t* data, size_t len) +{ + uint32_t crc = 0xffffffffu; + for (size_t i = 0; i < len; ++i) crc = crc32_update_byte(crc, data[i]); + return ~crc; +} + +static int send_inner_with_retry(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) +{ + int ret = zcm_trans_sendmsg(zt->inner, msg); + if (ret == ZCM_EOK) return ZCM_EOK; + if (ret != ZCM_EAGAIN) return ret; + + for (int i = 0; i < 4; ++i) { + zcm_trans_update(zt->inner); + ret = zcm_trans_sendmsg(zt->inner, msg); + if (ret == ZCM_EOK) return ZCM_EOK; + if (ret != ZCM_EAGAIN) return ret; + } + + return ZCM_EAGAIN; +} + +static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, + uint16_t session_id, uint8_t type, const uint8_t* body, + uint8_t body_len) +{ + if (PACKETIZED_HEADER_BYTES + (size_t)body_len > zt->pkt_buf_size) + return ZCM_EINVALID; + + zt->pkt_buf[0] = type; + write_u16_be(&zt->pkt_buf[1], session_id); + zt->pkt_buf[3] = body_len; + if (body_len > 0 && body != &zt->pkt_buf[PACKETIZED_HEADER_BYTES]) { + memcpy(&zt->pkt_buf[PACKETIZED_HEADER_BYTES], body, body_len); + } + + zcm_msg_t out; + out.utime = 0; + out.channel = channel; + out.len = PACKETIZED_HEADER_BYTES + body_len; + out.buf = zt->pkt_buf; + return send_inner_with_retry(zt, out); +} + +static void rx_clear(packetized_rx_state_t* rx) +{ + free(rx->data); + free(rx->packet_received); + free(rx->missing_ids); + memset(rx, 0, sizeof(*rx)); +} + +static void tx_clear(packetized_tx_state_t* tx) +{ + free(tx->data); + free(tx->retrans_ids); + memset(tx, 0, sizeof(*tx)); +} + +static int is_packetized_channel(const char* channel) +{ + return channel && channel[0] == '$'; +} + +static int send_retrans_request(zcm_trans_packetized_serial_t* zt) +{ + packetized_rx_state_t* rx = &zt->rx; + if (!rx->retrans_pending || !rx->active || rx->missing_count == 0) return ZCM_EOK; + + size_t max_ids_per_req = (zt->inner_mtu > PACKETIZED_HEADER_BYTES + 1) + ? (zt->inner_mtu - PACKETIZED_HEADER_BYTES - 1) / 2 + : 0; + if (max_ids_per_req == 0) return ZCM_EINVALID; + + size_t sent = 0; + while (sent < rx->missing_count) { + size_t remaining = rx->missing_count - sent; + size_t count = remaining < max_ids_per_req ? remaining : max_ids_per_req; + size_t body_len = 1 + count * 2; + uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; + body[0] = (uint8_t)count; + for (size_t i = 0; i < count; ++i) { + write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); + } + + int ret = send_packet(zt, rx->channel, rx->session_id, PACKETIZED_MSG_RETRANS_REQ, + body, (uint8_t)body_len); + if (ret != ZCM_EOK) return ret; + sent += count; + } + + rx->retrans_pending = 0; + return ZCM_EOK; +} + +static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) +{ + packetized_tx_state_t* tx = &zt->tx; + if (!tx->active || tx->retrans_count == 0) return ZCM_EOK; + + size_t chunk = tx->packet_data_size; + uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; + + for (size_t i = 0; i < tx->retrans_count; ++i) { + uint16_t packet_id = tx->retrans_ids[i]; + if (packet_id >= tx->total_packets) continue; + + uint32_t offset = (uint32_t)packet_id * (uint32_t)chunk; + uint32_t remaining = tx->total_message_size - offset; + uint8_t payload_len = (uint8_t)(remaining < chunk ? remaining : chunk); + + write_u16_be(&body[0], packet_id); + if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); + + int ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, + (uint8_t)(PACKETIZED_DATA_OVERHEAD_BYTES + payload_len)); + if (ret != ZCM_EOK) return ret; + } + + free(tx->retrans_ids); + tx->retrans_ids = NULL; + tx->retrans_count = 0; + return ZCM_EOK; +} + +static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* channel, + uint16_t session_id, const uint8_t* body, size_t body_len, + uint64_t utime) +{ + if (body_len != PACKETIZED_METADATA_BODY_BYTES) return ZCM_EINVALID; + + uint16_t total_packets = read_u16_be(&body[0]); + uint8_t packet_data_size = body[2]; + uint32_t total_message_size = read_u32_be(&body[3]); + uint32_t expected_crc = read_u32_be(&body[7]); + + if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; + if (packet_data_size > 253) return ZCM_EINVALID; + if (total_message_size > zt->mtu) return ZCM_EINVALID; + + uint32_t expected_packets = + total_message_size == 0 ? 1 + : (total_message_size + (uint32_t)packet_data_size - 1u) / + (uint32_t)packet_data_size; + if (expected_packets != total_packets) return ZCM_EINVALID; + + rx_clear(&zt->rx); + packetized_rx_state_t* rx = &zt->rx; + + rx->data = malloc(total_message_size == 0 ? 1 : total_message_size); + rx->packet_received = calloc(total_packets, sizeof(uint8_t)); + if (rx->data == NULL || rx->packet_received == NULL) { + rx_clear(rx); + return ZCM_EMEMORY; + } + + strncpy(rx->channel, channel, ZCM_CHANNEL_MAXLEN); + rx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; + rx->active = 1; + rx->session_id = session_id; + rx->total_packets = total_packets; + rx->packet_data_size = packet_data_size; + rx->total_message_size = total_message_size; + rx->expected_crc = expected_crc; + rx->last_update_utime = utime; + return ZCM_EOK; +} + +static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_id, + const uint8_t* body, size_t body_len, uint64_t utime) +{ + packetized_rx_state_t* rx = &zt->rx; + if (!rx->active || rx->session_id != session_id) return ZCM_EOK; + if (body_len < PACKETIZED_DATA_OVERHEAD_BYTES) return ZCM_EINVALID; + + uint16_t packet_id = read_u16_be(&body[0]); + uint8_t payload_len = (uint8_t)(body_len - PACKETIZED_DATA_OVERHEAD_BYTES); + + if (packet_id >= rx->total_packets) return ZCM_EINVALID; + + uint32_t offset = (uint32_t)packet_id * (uint32_t)rx->packet_data_size; + if (offset > rx->total_message_size) return ZCM_EINVALID; + + uint32_t remaining = rx->total_message_size - offset; + uint8_t expected_len = + (uint8_t)(remaining < rx->packet_data_size ? remaining : rx->packet_data_size); + if (payload_len != expected_len) return ZCM_EINVALID; + + if (!rx->packet_received[packet_id]) { + if (payload_len > 0) memcpy(rx->data + offset, &body[2], payload_len); + rx->packet_received[packet_id] = 1; + ++rx->received_count; + } + + rx->last_update_utime = utime; + + if (rx->received_count == rx->total_packets) { + uint32_t crc = crc32_compute(rx->data, rx->total_message_size); + if (crc != rx->expected_crc) { + rx_clear(rx); + return ZCM_EINVALID; + } + + if (zt->out_buf_size < rx->total_message_size) { + uint8_t* newbuf = realloc(zt->out_buf, rx->total_message_size); + if (newbuf == NULL) { + rx_clear(rx); + return ZCM_EMEMORY; + } + zt->out_buf = newbuf; + zt->out_buf_size = rx->total_message_size; + } + + if (rx->total_message_size > 0) + memcpy(zt->out_buf, rx->data, rx->total_message_size); + strncpy(zt->out_channel, rx->channel, ZCM_CHANNEL_MAXLEN); + zt->out_channel[ZCM_CHANNEL_MAXLEN] = '\0'; + zt->out_len = rx->total_message_size; + zt->out_pending = 1; + + rx_clear(rx); + } + + return ZCM_EOK; +} + +static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t session_id, + const uint8_t* body, size_t body_len) +{ + packetized_tx_state_t* tx = &zt->tx; + if (!tx->active || tx->session_id != session_id) return ZCM_EOK; + if (body_len < 1) return ZCM_EINVALID; + + uint8_t count = body[0]; + if ((size_t)(1 + (size_t)count * 2) != body_len) return ZCM_EINVALID; + + free(tx->retrans_ids); + tx->retrans_ids = NULL; + tx->retrans_count = 0; + if (count == 0) return ZCM_EOK; + + tx->retrans_ids = malloc((size_t)count * sizeof(uint16_t)); + if (tx->retrans_ids == NULL) return ZCM_EMEMORY; + tx->retrans_count = count; + for (uint8_t i = 0; i < count; ++i) { + tx->retrans_ids[i] = read_u16_be(&body[1 + (size_t)i * 2]); + } + + return ZCM_EOK; +} + +static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, + uint64_t now) +{ + packetized_rx_state_t* rx = &zt->rx; + if (!rx->active || rx->retrans_pending) return; + if (rx->received_count == rx->total_packets) return; + if (now <= rx->last_update_utime) return; + if (now - rx->last_update_utime < PACKETIZED_RETRANS_TIMEOUT_US) return; + + uint16_t missing_count = (uint16_t)(rx->total_packets - rx->received_count); + if (missing_count == 0) return; + + free(rx->missing_ids); + rx->missing_ids = malloc((size_t)missing_count * sizeof(uint16_t)); + if (rx->missing_ids == NULL) return; + + uint16_t idx = 0; + for (uint16_t i = 0; i < rx->total_packets; ++i) { + if (!rx->packet_received[i]) rx->missing_ids[idx++] = i; + } + rx->missing_count = idx; + rx->retrans_pending = idx > 0; +} + +size_t packetized_serial_get_mtu(zcm_trans_packetized_serial_t* zt) { return zt->mtu; } + +int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) +{ + size_t chan_len = strlen(msg.channel); + if (chan_len > ZCM_CHANNEL_MAXLEN) return ZCM_EINVALID; + + int ret = send_pending_retransmissions(zt); + if (ret != ZCM_EOK) return ret; + ret = send_retrans_request(zt); + if (ret != ZCM_EOK) return ret; + + if (!is_packetized_channel(msg.channel)) { + if (msg.len > zt->inner_mtu) return ZCM_EINVALID; + return send_inner_with_retry(zt, msg); + } + + uint8_t packet_data_size = (uint8_t)(zt->inner_mtu - PACKETIZED_HEADER_BYTES - + PACKETIZED_DATA_OVERHEAD_BYTES); + if (packet_data_size == 0 || packet_data_size > 253) return ZCM_EINVALID; + if (msg.len > zt->mtu) return ZCM_EINVALID; + + uint32_t total_message_size = (uint32_t)msg.len; + uint16_t total_packets = + (uint16_t)((total_message_size + packet_data_size - 1u) / packet_data_size); + if (total_packets == 0) total_packets = 1; + + tx_clear(&zt->tx); + packetized_tx_state_t* tx = &zt->tx; + tx->data = malloc(msg.len == 0 ? 1 : msg.len); + if (tx->data == NULL) { + tx_clear(tx); + return ZCM_EMEMORY; + } + if (msg.len > 0) memcpy(tx->data, msg.buf, msg.len); + + tx->active = 1; + tx->session_id = ++zt->next_session_id; + tx->packet_data_size = packet_data_size; + tx->total_packets = total_packets; + tx->total_message_size = total_message_size; + strncpy(tx->channel, msg.channel, ZCM_CHANNEL_MAXLEN); + tx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; + + uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; + write_u16_be(&meta[0], total_packets); + meta[2] = packet_data_size; + write_u32_be(&meta[3], total_message_size); + write_u32_be(&meta[7], crc32_compute(msg.buf, msg.len)); + ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, + PACKETIZED_METADATA_BODY_BYTES); + if (ret != ZCM_EOK) return ret; + + uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; + for (uint16_t packet_id = 0; packet_id < total_packets; ++packet_id) { + uint32_t offset = (uint32_t)packet_id * (uint32_t)packet_data_size; + uint32_t remaining = total_message_size - offset; + uint8_t payload_len = + (uint8_t)(remaining < packet_data_size ? remaining : packet_data_size); + + write_u16_be(&body[0], packet_id); + if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); + + ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, + (uint8_t)(PACKETIZED_DATA_OVERHEAD_BYTES + payload_len)); + if (ret != ZCM_EOK) return ret; + } + + return ZCM_EOK; +} + +int packetized_serial_recvmsg_enable(zcm_trans_packetized_serial_t* zt, + const char* channel, bool enable) +{ + return zcm_trans_recvmsg_enable(zt->inner, channel, enable); +} + +int packetized_serial_recvmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t* msg, + unsigned timeoutMs) +{ + (void)timeoutMs; + + if (zt->out_pending) { + msg->utime = zt->time(zt->time_usr); + msg->channel = zt->out_channel; + msg->len = zt->out_len; + msg->buf = zt->out_buf; + zt->out_pending = 0; + return ZCM_EOK; + } + + while (1) { + zcm_msg_t in; + int ret = zcm_trans_recvmsg(zt->inner, &in, 0); + if (ret == ZCM_EAGAIN) { + maybe_schedule_retrans_request(zt, zt->time(zt->time_usr)); + return ZCM_EAGAIN; + } + if (ret != ZCM_EOK) return ret; + + if (!is_packetized_channel(in.channel)) { + *msg = in; + return ZCM_EOK; + } + + if (in.len < PACKETIZED_HEADER_BYTES) continue; + uint8_t type = in.buf[0]; + uint16_t session_id = read_u16_be(&in.buf[1]); + uint8_t body_len = in.buf[3]; + if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; + + const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; + uint64_t now = in.utime == 0 ? zt->time(zt->time_usr) : in.utime; + + if (type == PACKETIZED_MSG_METADATA) { + ret = begin_rx_session(zt, in.channel, session_id, body, body_len, now); + if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; + } else if (type == PACKETIZED_MSG_DATA) { + ret = process_rx_data(zt, session_id, body, body_len, now); + if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; + if (zt->out_pending) { + msg->utime = now; + msg->channel = zt->out_channel; + msg->len = zt->out_len; + msg->buf = zt->out_buf; + zt->out_pending = 0; + return ZCM_EOK; + } + } else if (type == PACKETIZED_MSG_RETRANS_REQ) { + ret = process_retrans_request(zt, session_id, body, body_len); + if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; + } + } +} + +int packetized_serial_update_rx(zcm_trans_t* _zt) +{ + zcm_trans_packetized_serial_t* zt = cast(_zt); + return serial_update_rx(zt->inner); +} + +int packetized_serial_update_tx(zcm_trans_t* _zt) +{ + zcm_trans_packetized_serial_t* zt = cast(_zt); + return serial_update_tx(zt->inner); +} + +static size_t _packetized_serial_get_mtu(zcm_trans_t* zt) +{ + return packetized_serial_get_mtu(cast(zt)); +} + +static int _packetized_serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) +{ + return packetized_serial_sendmsg(cast(zt), msg); +} + +static int _packetized_serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, + bool enable) +{ + return packetized_serial_recvmsg_enable(cast(zt), channel, enable); +} + +static int _packetized_serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeoutMs) +{ + return packetized_serial_recvmsg(cast(zt), msg, timeoutMs); +} + +static int _packetized_serial_update(zcm_trans_t* zt) +{ + int rxRet = packetized_serial_update_rx(zt); + int txRet = packetized_serial_update_tx(zt); + return rxRet == ZCM_EOK ? txRet : rxRet; +} + +static zcm_trans_methods_t methods = { + &_packetized_serial_get_mtu, + &_packetized_serial_sendmsg, + &_packetized_serial_recvmsg_enable, + &_packetized_serial_recvmsg, + NULL, + &_packetized_serial_update, + &zcm_trans_packetized_serial_destroy, +}; + +static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt) +{ + assert(zt->vtbl == &methods); + return (zcm_trans_packetized_serial_t*)zt; +} + +zcm_trans_t* zcm_trans_packetized_serial_create( + size_t (*get)(uint8_t* data, size_t nData, void* usr), + size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize) +{ + zcm_trans_packetized_serial_t* zt = calloc(1, sizeof(*zt)); + if (zt == NULL) return NULL; + + zt->inner = zcm_trans_generic_serial_create(get, put, put_get_usr, timestamp_now, + time_usr, MTU, bufSize); + if (zt->inner == NULL) { + free(zt); + return NULL; + } + + zt->inner_mtu = zcm_trans_get_mtu(zt->inner); + if (zt->inner_mtu < PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES + 1 || + zt->inner_mtu < PACKETIZED_HEADER_BYTES + PACKETIZED_METADATA_BODY_BYTES) { + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } + + zt->mtu = PACKETIZED_MAX_MTU; + if (zt->mtu < zt->inner_mtu) zt->mtu = zt->inner_mtu; + + zt->pkt_buf_size = zt->inner_mtu; + zt->pkt_buf = malloc(zt->pkt_buf_size); + if (zt->pkt_buf == NULL) { + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } + + zt->out_buf = NULL; + zt->out_buf_size = 0; + zt->out_len = 0; + zt->out_channel[0] = '\0'; + zt->out_pending = 0; + + zt->time = timestamp_now; + zt->time_usr = time_usr; + + zt->trans.trans_type = ZCM_NONBLOCKING; + zt->trans.vtbl = &methods; + return (zcm_trans_t*)zt; +} + +void zcm_trans_packetized_serial_destroy(zcm_trans_t* _zt) +{ + zcm_trans_packetized_serial_t* zt = cast(_zt); + if (zt->inner) zcm_trans_generic_serial_destroy(zt->inner); + free(zt->pkt_buf); + free(zt->out_buf); + rx_clear(&zt->rx); + tx_clear(&zt->tx); + free(zt); +} diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h new file mode 100644 index 000000000..4bb609bfd --- /dev/null +++ b/zcm/transport/packetized_serial_transport.h @@ -0,0 +1,27 @@ +#ifndef _ZCM_TRANS_PACKETIZED_SERIAL_H +#define _ZCM_TRANS_PACKETIZED_SERIAL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "zcm/transport.h" +#include "zcm/zcm.h" + +zcm_trans_t* zcm_trans_packetized_serial_create( + size_t (*get)(uint8_t* data, size_t nData, void* usr), + size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize); + +void zcm_trans_packetized_serial_destroy(zcm_trans_t* zt); + +int packetized_serial_update_rx(zcm_trans_t* zt); +int packetized_serial_update_tx(zcm_trans_t* zt); + +#ifdef __cplusplus +} +#endif + +#endif /* _ZCM_TRANS_PACKETIZED_SERIAL_H */ diff --git a/zcm/transport/packetized_transport_can.cpp b/zcm/transport/packetized_transport_can.cpp new file mode 100644 index 000000000..f90f11de3 --- /dev/null +++ b/zcm/transport/packetized_transport_can.cpp @@ -0,0 +1,357 @@ +#include "zcm/transport.h" +#include "zcm/transport_register.hpp" +#include "zcm/transport_registrar.h" +#include "zcm/util/debug.h" + +#include "packetized_serial_transport.h" + +#include "util/TimeUtil.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#define MASK_29B ((1 << 29) - 1) +#define MASK_11B ((1 << 11) - 1) + +// Define this the class name you want +#define ZCM_TRANS_CLASSNAME PacketizedTransportCan +#define MTU (1 << 14) + +using namespace std; + +struct ZCM_TRANS_CLASSNAME : public zcm_trans_t +{ + unordered_map options; + uint32_t msgId; + uint32_t txId; + string address; + + int soc = -1; + bool socSettingsGood = false; + struct sockaddr_can addr; + struct ifreq ifr; + + zcm_trans_t* gst = nullptr; + + uint64_t recvTimeoutUs = 0; + uint64_t recvMsgStartUtime = 0; + + uint8_t leftoverBuffer[CAN_MAX_DLEN]; + size_t leftoverBytes = 0; + size_t leftoverOffset = 0; + + string* findOption(const string& s) + { + auto it = options.find(s); + if (it == options.end()) return nullptr; + return &it->second; + } + + ZCM_TRANS_CLASSNAME(zcm_url_t* url) + { + trans_type = ZCM_BLOCKING; + vtbl = &methods; + + auto* opts = zcm_url_opts(url); + for (size_t i = 0; i < opts->numopts; ++i) + options[opts->name[i]] = opts->value[i]; + + msgId = 0; + auto* msgIdStr = findOption("msgid"); + if (!msgIdStr) { + ZCM_DEBUG("Msg Id unspecified"); + return; + } else { + char* endptr; + msgId = (strtoul(msgIdStr->c_str(), &endptr, 10) & MASK_29B); + if (*endptr != '\0') { + ZCM_DEBUG("Msg Id unspecified"); + return; + } + } + + auto* txAddrMode = findOption("tx_addr_mode"); + bool extendedTx; + if (!txAddrMode || string("extended") == txAddrMode->c_str()) { + extendedTx = true; + } else if (string("standard") == txAddrMode->c_str()) { + extendedTx = false; + } else { + ZCM_DEBUG("Invalid rx_addr_mode. Use 'extended' or 'standard'"); + return; + } + if (!extendedTx && ((msgId & MASK_11B) != msgId)) { + ZCM_DEBUG("Msg Id too long for standard can addresses. " + "Use 'tx_extended_addr=true'"); + return; + } + if (extendedTx && ((msgId & MASK_29B) != msgId)) { + ZCM_DEBUG("Msg Id too long for extended can addresses."); + return; + } + txId = extendedTx ? msgId | CAN_EFF_FLAG : msgId; + + struct can_filter rfilter[2]; + size_t numRxFilters = 0; + + auto* rxAddrMode = findOption("rx_addr_mode"); + if (!rxAddrMode || string("both") == rxAddrMode->c_str()) { + rfilter[0].can_id = msgId; + rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK); + rfilter[1].can_id = msgId | CAN_EFF_FLAG; + rfilter[1].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK); + numRxFilters = 2; + } else if (string("standard") == rxAddrMode->c_str()) { + rfilter[0].can_id = msgId; + rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK); + numRxFilters = 1; + } else if (string("extended") == rxAddrMode->c_str()) { + rfilter[0].can_id = msgId | CAN_EFF_FLAG; + rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK); + numRxFilters = 1; + } else { + ZCM_DEBUG("Invalid rx_addr_mode. Use 'extended', 'standard', or 'both'"); + return; + } + + address = zcm_url_address(url); + + if ((soc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + ZCM_DEBUG("Unable to make socket"); + return; + } + + strcpy(ifr.ifr_name, address.c_str()); + ioctl(soc, SIOCGIFINDEX, &ifr); + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(soc, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + ZCM_DEBUG("Failed to bind"); + close(soc); + return; + } + + if (setsockopt(soc, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, + numRxFilters * sizeof(struct can_filter)) < 0) { + ZCM_DEBUG("Failed to set filter"); + return; + } + + gst = zcm_trans_packetized_serial_create( + &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, + &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10); + socSettingsGood = true; + } + + ~ZCM_TRANS_CLASSNAME() + { + if (gst) zcm_trans_packetized_serial_destroy(gst); + if (soc != -1 && close(soc) < 0) { ZCM_DEBUG("Failed to close"); } + soc = -1; + } + + bool good() { return soc != -1 && socSettingsGood; } + + static size_t get(uint8_t* data, size_t nData, void* usr) + { + ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); + size_t totalRead = 0; + + if (me->leftoverBytes > 0) { + size_t availableLeftover = me->leftoverBytes - me->leftoverOffset; + size_t fromLeftover = min(nData, availableLeftover); + memcpy(data, &me->leftoverBuffer[me->leftoverOffset], fromLeftover); + me->leftoverOffset += fromLeftover; + totalRead += fromLeftover; + + if (me->leftoverOffset >= me->leftoverBytes) { + me->leftoverBytes = 0; + me->leftoverOffset = 0; + } + + if (totalRead >= nData) return totalRead; + } + + struct can_frame frame; + int nbytes = read(me->soc, &frame, sizeof(struct can_frame)); + if (nbytes != sizeof(struct can_frame)) { + if (nbytes < 0) { + uint64_t timeoutConsumedUs = TimeUtil::utime() - me->recvMsgStartUtime; + if (timeoutConsumedUs >= me->recvTimeoutUs) return totalRead; + usleep(me->recvTimeoutUs - timeoutConsumedUs); + } + return totalRead; + } + + size_t frameDataSize = (size_t)frame.can_dlc; + nData -= totalRead; + size_t fromNewFrame = min(nData, frameDataSize); + memcpy(&data[totalRead], frame.data, fromNewFrame); + totalRead += fromNewFrame; + + if (fromNewFrame < frameDataSize) { + me->leftoverBytes = frameDataSize - fromNewFrame; + me->leftoverOffset = 0; + memcpy(me->leftoverBuffer, (uint8_t*)frame.data + fromNewFrame, + me->leftoverBytes); + } + + return totalRead; + } + + static size_t sendFrame(const uint8_t* data, size_t nData, void* usr) + { + ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); + + struct can_frame frame; + frame.can_id = me->txId; + + size_t ret = min(nData, (size_t)CAN_MAX_DLEN); + frame.can_dlc = ret; + memcpy(frame.data, data, ret); + + if (write(me->soc, &frame, sizeof(struct can_frame)) != + sizeof(struct can_frame)) { + ZCM_DEBUG("Failed to write data"); + return 0; + } + + return ret; + } + + static size_t put(const uint8_t* data, size_t nData, void* usr) + { + size_t ret = 0; + while (ret < nData) { + size_t left = nData - ret; + size_t written = sendFrame(&data[ret], left, usr); + if (written == 0) return ret; + ret += written; + } + return ret; + } + + static uint64_t timestamp_now(void* usr) + { + ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); + + struct timeval time; + if (ioctl(me->soc, SIOCGSTAMP, &time) == -1) return 0; + return time.tv_sec * 1e6 + time.tv_usec; + } + + size_t get_mtu() { return zcm_trans_get_mtu(this->gst); } + + int sendmsg(zcm_msg_t msg) + { + int ret = zcm_trans_sendmsg(this->gst, msg); + if (ret != ZCM_EOK) return ret; + return packetized_serial_update_tx(this->gst); + } + + int recvmsgEnable(const char* channel, bool enable) + { + return zcm_trans_recvmsg_enable(this->gst, channel, enable); + } + + int recvmsg(zcm_msg_t* msg, unsigned timeoutMs) + { + recvMsgStartUtime = TimeUtil::utime(); + recvTimeoutUs = timeoutMs * 1000ULL; + + do { + int ret = zcm_trans_recvmsg(this->gst, msg, 0); + if (ret == ZCM_EOK) return ret; + + uint64_t timeoutConsumedUs = TimeUtil::utime() - recvMsgStartUtime; + if (timeoutConsumedUs > recvTimeoutUs) break; + + uint64_t socketTimeout = recvTimeoutUs - timeoutConsumedUs; + unsigned timeoutS = socketTimeout / 1000000; + unsigned timeoutUs = socketTimeout - timeoutS * 1000000; + struct timeval tm = { + timeoutS, + timeoutUs, + }; + if (setsockopt(soc, SOL_SOCKET, SO_RCVTIMEO, (char*)&tm, sizeof(tm)) < 0) { + ZCM_DEBUG("Failed to settimeout"); + return ZCM_EUNKNOWN; + } + + packetized_serial_update_rx(this->gst); + } while (true); + return ZCM_EAGAIN; + } + + static zcm_trans_methods_t methods; + static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) + { + assert(zt->vtbl == &methods); + return (ZCM_TRANS_CLASSNAME*)zt; + } + + static size_t _get_mtu(zcm_trans_t* zt) { return cast(zt)->get_mtu(); } + + static int _sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return cast(zt)->sendmsg(msg); } + + static int _recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) + { + return cast(zt)->recvmsgEnable(channel, enable); + } + + static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) + { + return cast(zt)->recvmsg(msg, timeout); + } + + static void _destroy(zcm_trans_t* zt) { delete cast(zt); } + + static const TransportRegister reg; +}; + +zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { + &ZCM_TRANS_CLASSNAME::_get_mtu, + &ZCM_TRANS_CLASSNAME::_sendmsg, + &ZCM_TRANS_CLASSNAME::_recvmsg_enable, + &ZCM_TRANS_CLASSNAME::_recvmsg, + NULL, + NULL, + &ZCM_TRANS_CLASSNAME::_destroy, +}; + +static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) +{ + if (opt_errmsg) *opt_errmsg = NULL; + + auto* trans = new ZCM_TRANS_CLASSNAME(url); + if (trans->good()) return trans; + + delete trans; + return nullptr; +} + +#ifdef USING_TRANS_CAN +const TransportRegister ZCM_TRANS_CLASSNAME::reg( + "can+pkt", + "Transfer data via packetized socket CAN connection on a single id " + "(e.g. 'can+pkt://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true')", + create); +#endif diff --git a/zcm/transport/packetized_transport_serial.cpp b/zcm/transport/packetized_transport_serial.cpp new file mode 100644 index 000000000..f20efa425 --- /dev/null +++ b/zcm/transport/packetized_transport_serial.cpp @@ -0,0 +1,441 @@ +#include "zcm/transport.h" +#include "zcm/transport_register.hpp" +#include "zcm/transport_registrar.h" +#include "zcm/util/debug.h" +#include "zcm/util/lockfile.h" + +#include "packetized_serial_transport.h" + +#include "util/TimeUtil.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +using namespace std; + +#define ZCM_TRANS_CLASSNAME PacketizedTransportSerial +#define MTU (1 << 14) + +using u8 = uint8_t; +using u64 = uint64_t; + +struct PacketizedSerialDevice +{ + PacketizedSerialDevice() {} + ~PacketizedSerialDevice() { close(); } + + bool open(const string& port, int baud, bool hwFlowControl); + bool isOpen() { return fd > 0; } + void close(); + + int write(const u8* buf, size_t sz); + int read(u8* buf, size_t sz, u64 timeoutUs); + static int convertBaud(int baud); + + PacketizedSerialDevice(const PacketizedSerialDevice&) = delete; + PacketizedSerialDevice(PacketizedSerialDevice&&) = delete; + PacketizedSerialDevice& operator=(const PacketizedSerialDevice&) = delete; + PacketizedSerialDevice& operator=(PacketizedSerialDevice&&) = delete; + + private: + string port; + int fd = -1; + lockfile_t* lf; +}; + +bool PacketizedSerialDevice::open(const string& port_, int baud, bool hwFlowControl) +{ + if (baud == 0) { + fprintf(stderr, "Serial baud rate not specified in url. " + "Proceeding without setting baud\n"); + } else if (!(baud = convertBaud(baud))) { + fprintf(stderr, "Unrecognized baudrate. Failed to open serial device.\n "); + return false; + } + + lf = lockfile_trylock(port_.c_str()); + if (!lf) { + ZCM_DEBUG("failed to create lock file, refusing to open serial device (%s)", + port_.c_str()); + return false; + } + this->port = port_; + + int flags = O_RDWR | O_NOCTTY | O_SYNC; + fd = ::open(port.c_str(), flags, 0); + if (fd < 0) { + ZCM_DEBUG("failed to open serial device (%s): %s", port.c_str(), strerror(errno)); + goto fail; + } + + ioctl(fd, USBDEVFS_RESET, 0); + + struct termios opts; + if (tcgetattr(fd, &opts)) { + ZCM_DEBUG("failed to get termios options on fd: %s", strerror(errno)); + goto fail; + } + + if (baud != 0) { + cfsetispeed(&opts, baud); + cfsetospeed(&opts, baud); + } + cfmakeraw(&opts); + + opts.c_cflag &= ~CSTOPB; + opts.c_cflag |= CS8; + opts.c_cflag &= ~PARENB; + if (hwFlowControl) opts.c_cflag |= CRTSCTS; + opts.c_cc[VTIME] = 1; + opts.c_cc[VMIN] = 30; + + if (tcsetattr(fd, TCSANOW, &opts)) { + ZCM_DEBUG("failed to set termios options on fd: %s", strerror(errno)); + goto fail; + } + + tcflush(fd, TCIOFLUSH); + + return true; + +fail: + if (fd > 0) { + const int saved_errno = errno; + int result; + do { + result = ::close(fd); + } while (result == -1 && errno == EINTR); + errno = saved_errno; + } + this->fd = -1; + + if (lf) { + lockfile_unlock(lf); + lf = nullptr; + } + this->port = ""; + + return false; +} + +void PacketizedSerialDevice::close() +{ + if (isOpen()) { + ::close(fd); + fd = 0; + } + if (port != "") port = ""; + if (lf) { + lockfile_unlock(lf); + lf = nullptr; + } +} + +int PacketizedSerialDevice::write(const u8* buf, size_t sz) +{ + assert(this->isOpen()); + int ret = ::write(fd, buf, sz); + if (ret == -1) { + ZCM_DEBUG("ERR: write failed: %s", strerror(errno)); + return -1; + } + return ret; +} + +int PacketizedSerialDevice::read(u8* buf, size_t sz, u64 timeoutUs) +{ + assert(this->isOpen()); + fd_set fds; + FD_ZERO(&fds); + FD_SET(fd, &fds); + + struct timeval timeout; + timeout.tv_sec = timeoutUs / 1000000; + timeout.tv_usec = timeoutUs - timeout.tv_sec * 1000000; + int status = ::select(fd + 1, &fds, NULL, NULL, &timeout); + + if (status > 0) { + if (FD_ISSET(fd, &fds)) { + int ret = ::read(fd, buf, sz); + if (ret == -1) { + ZCM_DEBUG("ERR: serial read failed: %s", strerror(errno)); + } else if (ret == 0) { + ZCM_DEBUG("ERR: serial device unplugged"); + close(); + assert(false && "ERR: serial device unplugged\n" && + "ZCM does not support reconnecting to serial devices"); + return -3; + } + return ret; + } else { + ZCM_DEBUG("ERR: serial bytes not ready"); + return -1; + } + } else { + ZCM_DEBUG("ERR: serial read timed out"); + return -2; + } +} + +int PacketizedSerialDevice::convertBaud(int baud) +{ + switch (baud) { + case 4800: return B4800; + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + case 460800: return B460800; + default: return 0; + } +} + +struct ZCM_TRANS_CLASSNAME : public zcm_trans_t +{ + PacketizedSerialDevice ser; + + int baud; + bool hwFlowControl; + + bool raw; + string rawChan; + int rawSize; + std::unique_ptr rawBuf; + + string address; + unordered_map options; + zcm_trans_t* gst; + uint64_t timeoutLeftUs; + + string* findOption(const string& s) + { + auto it = options.find(s); + if (it == options.end()) return nullptr; + return &it->second; + } + + ZCM_TRANS_CLASSNAME(zcm_url_t* url) + { + trans_type = ZCM_BLOCKING; + vtbl = &methods; + + auto* opts = zcm_url_opts(url); + for (size_t i = 0; i < opts->numopts; ++i) + options[opts->name[i]] = opts->value[i]; + + baud = 0; + auto* baudStr = findOption("baud"); + if (!baudStr) { + fprintf(stderr, "Baud unspecified. Bypassing serial baud setup.\n"); + } else { + baud = atoi(baudStr->c_str()); + if (baud == 0) { + ZCM_DEBUG("expected integer argument for 'baud'"); + return; + } + } + + hwFlowControl = false; + auto* hwFlowControlStr = findOption("hw_flow_control"); + if (hwFlowControlStr) { + if (*hwFlowControlStr == "true") { + hwFlowControl = true; + } else if (*hwFlowControlStr == "false") { + hwFlowControl = false; + } else { + ZCM_DEBUG("expected boolean argument for 'hw_flow_control'"); + return; + } + } + + raw = false; + auto* rawStr = findOption("raw"); + if (rawStr) { + if (*rawStr == "true") { + raw = true; + } else if (*rawStr == "false") { + raw = false; + } else { + ZCM_DEBUG("expected boolean argument for 'raw'"); + return; + } + } + + rawChan = ""; + auto* rawChanStr = findOption("raw_channel"); + if (rawChanStr) { rawChan = *rawChanStr; } + + rawSize = 1024; + auto* rawSizeStr = findOption("raw_size"); + if (rawSizeStr) { + rawSize = atoi(rawSizeStr->c_str()); + if (rawSize <= 0) { + ZCM_DEBUG("expected positive integer argument for 'raw_size'"); + return; + } + } + + address = zcm_url_address(url); + ser.open(address, baud, hwFlowControl); + + if (raw) { + rawBuf.reset(new uint8_t[rawSize]); + gst = nullptr; + } else { + gst = zcm_trans_packetized_serial_create( + &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, + &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10); + } + } + + ~ZCM_TRANS_CLASSNAME() + { + ser.close(); + if (gst) zcm_trans_packetized_serial_destroy(gst); + } + + bool good() { return ser.isOpen(); } + + static size_t get(uint8_t* data, size_t nData, void* usr) + { + ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); + uint64_t startUtime = TimeUtil::utime(); + int ret = me->ser.read(data, nData, me->timeoutLeftUs); + uint64_t diff = TimeUtil::utime() - startUtime; + me->timeoutLeftUs = me->timeoutLeftUs > diff ? me->timeoutLeftUs - diff : 0; + return ret < 0 ? 0 : ret; + } + + static size_t put(const uint8_t* data, size_t nData, void* usr) + { + ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); + int ret = me->ser.write(data, nData); + return ret < 0 ? 0 : ret; + } + + static uint64_t timestamp_now(void* usr) + { + (void)usr; + return TimeUtil::utime(); + } + + size_t getMtu() { return raw ? MTU : zcm_trans_get_mtu(this->gst); } + + int sendmsg(zcm_msg_t msg) + { + if (raw) { + if (put(msg.buf, msg.len, this) != 0) return ZCM_EOK; + return ZCM_EAGAIN; + } else { + int ret = zcm_trans_sendmsg(this->gst, msg); + if (ret != ZCM_EOK) return ret; + return packetized_serial_update_tx(this->gst); + } + } + + int recvmsgEnable(const char* channel, bool enable) + { + return raw ? ZCM_EOK : zcm_trans_recvmsg_enable(this->gst, channel, enable); + } + + int recvmsg(zcm_msg_t* msg, unsigned timeoutMs) + { + timeoutLeftUs = timeoutMs * 1e3; + + if (raw) { + size_t sz = get(rawBuf.get(), rawSize, this); + if (sz == 0 || rawChan.empty()) return ZCM_EAGAIN; + + msg->utime = timestamp_now(this); + msg->channel = rawChan.c_str(); + msg->len = sz; + msg->buf = rawBuf.get(); + return ZCM_EOK; + } else { + do { + uint64_t startUtime = TimeUtil::utime(); + + int ret = zcm_trans_recvmsg(this->gst, msg, 0); + if (ret == ZCM_EOK) return ret; + + uint64_t diff = TimeUtil::utime() - startUtime; + startUtime = TimeUtil::utime(); + timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; + + packetized_serial_update_rx(this->gst); + + diff = TimeUtil::utime() - startUtime; + timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; + + } while (timeoutLeftUs > 0); + + return ZCM_EAGAIN; + } + } + + static zcm_trans_methods_t methods; + static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) + { + assert(zt->vtbl == &methods); + return (ZCM_TRANS_CLASSNAME*)zt; + } + + static size_t _getMtu(zcm_trans_t* zt) { return cast(zt)->getMtu(); } + + static int _sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return cast(zt)->sendmsg(msg); } + + static int _recvmsgEnable(zcm_trans_t* zt, const char* channel, bool enable) + { + return cast(zt)->recvmsgEnable(channel, enable); + } + + static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) + { + return cast(zt)->recvmsg(msg, timeout); + } + + static void _destroy(zcm_trans_t* zt) { delete cast(zt); } + + static const TransportRegister reg; +}; + +zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { + &ZCM_TRANS_CLASSNAME::_getMtu, + &ZCM_TRANS_CLASSNAME::_sendmsg, + &ZCM_TRANS_CLASSNAME::_recvmsgEnable, + &ZCM_TRANS_CLASSNAME::_recvmsg, + NULL, + NULL, + &ZCM_TRANS_CLASSNAME::_destroy, +}; + +static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) +{ + if (opt_errmsg) *opt_errmsg = NULL; + auto* trans = new ZCM_TRANS_CLASSNAME(url); + if (trans->good()) return trans; + + delete trans; + return nullptr; +} + +#ifdef USING_TRANS_SERIAL +const TransportRegister ZCM_TRANS_CLASSNAME::reg( + "serial+pkt", + "Transfer data via a packetized serial connection " + "(e.g. 'serial+pkt:///dev/ttyUSB0?baud=115200&hw_flow_control=true' or " + "'serial+pkt:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", + create); +#endif diff --git a/zcm/wscript b/zcm/wscript index 1faddddee..eb9a8dac2 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -19,9 +19,11 @@ def build(ctx): if not ctx.env.USING_TRANS_UDP: srcExcludes += ['transport/udp/**/*'] if not ctx.env.USING_TRANS_SERIAL: - srcExcludes += ['transport/transport_serial.cpp'] + srcExcludes += ['transport/transport_serial.cpp', + 'transport/packetized_transport_serial.cpp'] if not ctx.env.USING_TRANS_CAN: - srcExcludes += ['transport/transport_can.cpp'] + srcExcludes += ['transport/transport_can.cpp', + 'transport/packetized_transport_can.cpp'] if not ctx.env.USING_TRANS_IPCSHM: srcExcludes += ['transport/transport_ipcshm.cpp', 'transport/lockfree/lf_*.c'] if not ctx.env.USING_THIRD_PARTY: @@ -43,11 +45,12 @@ def build(ctx): 'transport/lockfree/lf_*.c'], excl=srcExcludes)) - embedSource = ['zcm.h', 'zcm_private.h', 'zcm.c', 'zcm-cpp.hpp', 'zcm-cpp-impl.hpp', 'zcm_coretypes.h', 'transport.h', 'nonblocking.h', 'nonblocking.c', 'transport/generic_serial_transport.h', 'transport/generic_serial_transport.c', + 'transport/packetized_serial_transport.h', + 'transport/packetized_serial_transport.c', 'transport/generic_serial_circ_buff.h', 'transport/generic_serial_circ_buff.c', 'transport/generic_serial_fletcher.h'] @@ -104,6 +107,7 @@ def build(ctx): ctx.install_files('${PREFIX}/include/zcm/transport', ['transport/generic_serial_transport.h', + 'transport/packetized_serial_transport.h', 'transport/generic_serial_circ_buff.h', 'transport/generic_serial_fletcher.h']) From ded46a17073839dd536a2e4253609353462a85d0 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 7 Apr 2026 12:08:45 -0400 Subject: [PATCH 02/20] C89 compliant --- test/run-tests.sh | 2 +- zcm/transport/packetized_serial_transport.c | 29 ++++++++++++++------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/test/run-tests.sh b/test/run-tests.sh index b33ca03ea..3a644a5da 100755 --- a/test/run-tests.sh +++ b/test/run-tests.sh @@ -38,7 +38,7 @@ echo "**********************************" $ROOTDIR/build/$BLD/test/runner echo "Success" -if [ -n "$1" ]; then +if [ -n "${1:-}" ]; then echo "Skipping non c/c++ lanugage tests in sanitizer mode" exit 0 fi diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 853864aa6..079043c5f 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -115,8 +115,9 @@ static void write_u32_be(uint8_t* p, uint32_t v) static uint32_t crc32_update_byte(uint32_t crc, uint8_t b) { + int i; crc ^= b; - for (int i = 0; i < 8; ++i) { + for (i = 0; i < 8; ++i) { uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u)); crc = (crc >> 1) ^ (0xedb88320u & mask); } @@ -125,18 +126,20 @@ static uint32_t crc32_update_byte(uint32_t crc, uint8_t b) static uint32_t crc32_compute(const uint8_t* data, size_t len) { + size_t i; uint32_t crc = 0xffffffffu; - for (size_t i = 0; i < len; ++i) crc = crc32_update_byte(crc, data[i]); + for (i = 0; i < len; ++i) crc = crc32_update_byte(crc, data[i]); return ~crc; } static int send_inner_with_retry(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) { + int i; int ret = zcm_trans_sendmsg(zt->inner, msg); if (ret == ZCM_EOK) return ZCM_EOK; if (ret != ZCM_EAGAIN) return ret; - for (int i = 0; i < 4; ++i) { + for (i = 0; i < 4; ++i) { zcm_trans_update(zt->inner); ret = zcm_trans_sendmsg(zt->inner, msg); if (ret == ZCM_EOK) return ZCM_EOK; @@ -190,6 +193,7 @@ static int is_packetized_channel(const char* channel) static int send_retrans_request(zcm_trans_packetized_serial_t* zt) { + size_t sent; packetized_rx_state_t* rx = &zt->rx; if (!rx->retrans_pending || !rx->active || rx->missing_count == 0) return ZCM_EOK; @@ -198,14 +202,15 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) : 0; if (max_ids_per_req == 0) return ZCM_EINVALID; - size_t sent = 0; + sent = 0; while (sent < rx->missing_count) { + size_t i; size_t remaining = rx->missing_count - sent; size_t count = remaining < max_ids_per_req ? remaining : max_ids_per_req; size_t body_len = 1 + count * 2; uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; body[0] = (uint8_t)count; - for (size_t i = 0; i < count; ++i) { + for (i = 0; i < count; ++i) { write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); } @@ -221,13 +226,14 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) { + size_t i; packetized_tx_state_t* tx = &zt->tx; if (!tx->active || tx->retrans_count == 0) return ZCM_EOK; size_t chunk = tx->packet_data_size; uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - for (size_t i = 0; i < tx->retrans_count; ++i) { + for (i = 0; i < tx->retrans_count; ++i) { uint16_t packet_id = tx->retrans_ids[i]; if (packet_id >= tx->total_packets) continue; @@ -353,6 +359,7 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t session_id, const uint8_t* body, size_t body_len) { + uint8_t i; packetized_tx_state_t* tx = &zt->tx; if (!tx->active || tx->session_id != session_id) return ZCM_EOK; if (body_len < 1) return ZCM_EINVALID; @@ -368,7 +375,7 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s tx->retrans_ids = malloc((size_t)count * sizeof(uint16_t)); if (tx->retrans_ids == NULL) return ZCM_EMEMORY; tx->retrans_count = count; - for (uint8_t i = 0; i < count; ++i) { + for (i = 0; i < count; ++i) { tx->retrans_ids[i] = read_u16_be(&body[1 + (size_t)i * 2]); } @@ -378,6 +385,7 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, uint64_t now) { + uint16_t i; packetized_rx_state_t* rx = &zt->rx; if (!rx->active || rx->retrans_pending) return; if (rx->received_count == rx->total_packets) return; @@ -392,7 +400,7 @@ static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, if (rx->missing_ids == NULL) return; uint16_t idx = 0; - for (uint16_t i = 0; i < rx->total_packets; ++i) { + for (i = 0; i < rx->total_packets; ++i) { if (!rx->packet_received[i]) rx->missing_ids[idx++] = i; } rx->missing_count = idx; @@ -403,7 +411,8 @@ size_t packetized_serial_get_mtu(zcm_trans_packetized_serial_t* zt) { return zt- int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) { - size_t chan_len = strlen(msg.channel); + uint16_t packet_id; + size_t chan_len = strlen(msg.channel); if (chan_len > ZCM_CHANNEL_MAXLEN) return ZCM_EINVALID; int ret = send_pending_retransmissions(zt); @@ -453,7 +462,7 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) if (ret != ZCM_EOK) return ret; uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - for (uint16_t packet_id = 0; packet_id < total_packets; ++packet_id) { + for (packet_id = 0; packet_id < total_packets; ++packet_id) { uint32_t offset = (uint32_t)packet_id * (uint32_t)packet_data_size; uint32_t remaining = total_message_size - offset; uint8_t payload_len = From d1380af269e0c10ab65a615ce3ab8a50b4402ce5 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 7 Apr 2026 15:26:17 -0400 Subject: [PATCH 03/20] Make packet size configurable using URL option --- test/zcm/PacketizedSerialTransportTest.hpp | 11 +++--- zcm/transport/packetized_serial_transport.c | 34 ++++++++++++++++--- zcm/transport/packetized_serial_transport.h | 3 +- zcm/transport/packetized_transport_can.cpp | 19 +++++++++-- zcm/transport/packetized_transport_serial.cpp | 22 +++++++++--- 5 files changed, 72 insertions(+), 17 deletions(-) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 4b70943d6..8c38b12fa 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -8,7 +8,8 @@ extern "C" { zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, - uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize); + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, + uint8_t packet_data_size); int packetized_serial_update_rx(zcm_trans_t* zt); int packetized_serial_update_tx(zcm_trans_t* zt); } @@ -159,9 +160,9 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite uint64_t now = 1000; zcm_trans_t* tx = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768); + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0); zcm_trans_t* rx = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768); + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0); TSM_ASSERT("failed creating transports", tx && rx); vector payload(512); @@ -199,9 +200,9 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite uint64_t now = 2000; zcm_trans_t* ta = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768); + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0); zcm_trans_t* tb = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768); + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0); TSM_ASSERT("failed creating transports", ta && tb); vector payload(700); diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 079043c5f..c7b5b31fd 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -66,8 +66,9 @@ struct zcm_trans_packetized_serial_t zcm_trans_t trans; zcm_trans_t* inner; - size_t inner_mtu; - size_t mtu; + size_t inner_mtu; + size_t mtu; + uint8_t configured_packet_data_size; uint8_t* pkt_buf; size_t pkt_buf_size; @@ -425,8 +426,7 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) return send_inner_with_retry(zt, msg); } - uint8_t packet_data_size = (uint8_t)(zt->inner_mtu - PACKETIZED_HEADER_BYTES - - PACKETIZED_DATA_OVERHEAD_BYTES); + uint8_t packet_data_size = zt->configured_packet_data_size; if (packet_data_size == 0 || packet_data_size > 253) return ZCM_EINVALID; if (msg.len > zt->mtu) return ZCM_EINVALID; @@ -602,7 +602,8 @@ static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt) zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, - uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize) + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, + uint8_t packet_data_size) { zcm_trans_packetized_serial_t* zt = calloc(1, sizeof(*zt)); if (zt == NULL) return NULL; @@ -625,6 +626,29 @@ zcm_trans_t* zcm_trans_packetized_serial_create( zt->mtu = PACKETIZED_MAX_MTU; if (zt->mtu < zt->inner_mtu) zt->mtu = zt->inner_mtu; + if (packet_data_size == 0) { + size_t max_payload = + zt->inner_mtu - PACKETIZED_HEADER_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES; + if (max_payload > 253) max_payload = 253; + zt->configured_packet_data_size = (uint8_t)max_payload; + } else { + zt->configured_packet_data_size = packet_data_size; + } + + if (zt->configured_packet_data_size == 0 || zt->configured_packet_data_size > 253) { + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } + + if (PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES + + zt->configured_packet_data_size > + zt->inner_mtu) { + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } + zt->pkt_buf_size = zt->inner_mtu; zt->pkt_buf = malloc(zt->pkt_buf_size); if (zt->pkt_buf == NULL) { diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index 4bb609bfd..c48af982e 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -13,7 +13,8 @@ extern "C" { zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, - uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize); + uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, + uint8_t packet_data_size); void zcm_trans_packetized_serial_destroy(zcm_trans_t* zt); diff --git a/zcm/transport/packetized_transport_can.cpp b/zcm/transport/packetized_transport_can.cpp index f90f11de3..d504868e1 100644 --- a/zcm/transport/packetized_transport_can.cpp +++ b/zcm/transport/packetized_transport_can.cpp @@ -39,6 +39,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t unordered_map options; uint32_t msgId; uint32_t txId; + uint8_t packetDataSize; string address; int soc = -1; @@ -71,6 +72,18 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t for (size_t i = 0; i < opts->numopts; ++i) options[opts->name[i]] = opts->value[i]; + packetDataSize = 0; + auto* pktSizeStr = findOption("pkt_size"); + if (pktSizeStr) { + char* endptr; + unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) { + ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); + return; + } + packetDataSize = (uint8_t)parsed; + } + msgId = 0; auto* msgIdStr = findOption("msgid"); if (!msgIdStr) { @@ -157,7 +170,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t gst = zcm_trans_packetized_serial_create( &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10); + &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10, packetDataSize); socSettingsGood = true; } @@ -352,6 +365,8 @@ static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can+pkt", "Transfer data via packetized socket CAN connection on a single id " - "(e.g. 'can+pkt://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true')", + "(e.g. " + "'can+pkt://" + "can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true&pkt_size=128')", create); #endif diff --git a/zcm/transport/packetized_transport_serial.cpp b/zcm/transport/packetized_transport_serial.cpp index f20efa425..945171914 100644 --- a/zcm/transport/packetized_transport_serial.cpp +++ b/zcm/transport/packetized_transport_serial.cpp @@ -206,8 +206,9 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { PacketizedSerialDevice ser; - int baud; - bool hwFlowControl; + int baud; + bool hwFlowControl; + uint8_t packetDataSize; bool raw; string rawChan; @@ -260,6 +261,18 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } } + packetDataSize = 0; + auto* pktSizeStr = findOption("pkt_size"); + if (pktSizeStr) { + char* endptr; + unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) { + ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); + return; + } + packetDataSize = (uint8_t)parsed; + } + raw = false; auto* rawStr = findOption("raw"); if (rawStr) { @@ -296,7 +309,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } else { gst = zcm_trans_packetized_serial_create( &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10); + &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10, + packetDataSize); } } @@ -435,7 +449,7 @@ static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "serial+pkt", "Transfer data via a packetized serial connection " - "(e.g. 'serial+pkt:///dev/ttyUSB0?baud=115200&hw_flow_control=true' or " + "(e.g. 'serial+pkt:///dev/ttyUSB0?baud=115200&hw_flow_control=true&pkt_size=128' or " "'serial+pkt:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", create); #endif From 6f39efd8c07a1f5b0eb97f24a3da886c6a93fb8d Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 8 Apr 2026 14:04:04 -0400 Subject: [PATCH 04/20] Strip $ on message dispatch --- test/zcm/PacketizedSerialTransportTest.hpp | 4 ++-- zcm/transport/packetized_serial_transport.c | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 8c38b12fa..5289ab3ab 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -179,7 +179,7 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_msg_t out; TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); - TS_ASSERT_EQUALS(string(out.channel), string("$BIG")); + TS_ASSERT_EQUALS(string(out.channel), string("BIG")); TS_ASSERT_EQUALS(out.len, payload.size()); TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); @@ -249,7 +249,7 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite for (int i = 0; i < 6; ++i) { int ret = zcm_trans_recvmsg(tb, &out, 0); if (ret != ZCM_EOK) continue; - if (string(out.channel) == "$RETX") { + if (string(out.channel) == "RETX") { gotRetx = true; TS_ASSERT_EQUALS(out.len, payload.size()); TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index c7b5b31fd..19e99595c 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -346,7 +346,11 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (rx->total_message_size > 0) memcpy(zt->out_buf, rx->data, rx->total_message_size); - strncpy(zt->out_channel, rx->channel, ZCM_CHANNEL_MAXLEN); + { + const char* out_channel = rx->channel; + if (out_channel[0] == '$') out_channel += 1; + strncpy(zt->out_channel, out_channel, ZCM_CHANNEL_MAXLEN); + } zt->out_channel[ZCM_CHANNEL_MAXLEN] = '\0'; zt->out_len = rx->total_message_size; zt->out_pending = 1; From b57e374317e98c4c4efe984dbddbb33eca9fe65f Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 8 Apr 2026 15:18:17 -0400 Subject: [PATCH 05/20] Simplify implementation --- docs/transports.md | 8 +- test/zcm/PacketizedSerialTransportTest.hpp | 19 - zcm/transport/packetized_serial_transport.c | 180 ++++--- zcm/transport/packetized_transport_can.cpp | 372 -------------- zcm/transport/packetized_transport_serial.cpp | 455 ------------------ zcm/transport/transport_can.cpp | 43 +- zcm/transport/transport_serial.cpp | 36 +- zcm/wscript | 6 +- 8 files changed, 185 insertions(+), 934 deletions(-) delete mode 100644 zcm/transport/packetized_transport_can.cpp delete mode 100644 zcm/transport/packetized_transport_serial.cpp diff --git a/docs/transports.md b/docs/transports.md index 4333c3cb7..dc59293c4 100644 --- a/docs/transports.md +++ b/docs/transports.md @@ -48,13 +48,13 @@ be used to *summon* the transport: Serial - serial://<path-to-device>?baud=<baud> - zcm_create("serial:///dev/ttyUSB0?baud=115200") + serial://<path-to-device>?baud=<baud>[&pkt_size=<n>] + zcm_create("serial:///dev/ttyUSB0?baud=115200"), zcm_create("serial:///dev/ttyUSB0?baud=115200&pkt_size=128") CAN - can://<interface>?msgid=<id> - zcm_create("can://can0?msgid=65536") + can://<interface>?msgid=<id>[&pkt_size=<n>] + zcm_create("can://can0?msgid=65536"), zcm_create("can://can0?msgid=65536&pkt_size=32") Inter-process via Shared Memory (IPCSHM) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 5289ab3ab..8e60cd59a 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -223,26 +223,7 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite now += 300000; TS_ASSERT_EQUALS(zcm_trans_recvmsg(tb, &out, 0), ZCM_EAGAIN); - uint8_t pingbuf[1] = { 0x42 }; - zcm_msg_t ping; - ping.utime = now; - ping.channel = (char*)"PING"; - ping.len = 1; - ping.buf = pingbuf; - - TS_ASSERT_EQUALS(zcm_trans_sendmsg(tb, ping), ZCM_EOK); pump(tb, ta, 10); - - TS_ASSERT_EQUALS(zcm_trans_recvmsg(ta, &out, 0), ZCM_EOK); - - uint8_t pongbuf[1] = { 0x24 }; - zcm_msg_t pong; - pong.utime = now; - pong.channel = (char*)"PONG"; - pong.len = 1; - pong.buf = pongbuf; - - TS_ASSERT_EQUALS(zcm_trans_sendmsg(ta, pong), ZCM_EOK); pump(ta, tb, 20); bool gotRetx = false; diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 19e99595c..af993932f 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -15,7 +15,8 @@ #define PACKETIZED_DATA_OVERHEAD_BYTES (2) #define PACKETIZED_METADATA_BODY_BYTES (11) #define PACKETIZED_RETRANS_TIMEOUT_US (200000) -#define PACKETIZED_MAX_MTU ((size_t)UINT32_MAX) +#define PACKETIZED_MAX_PACKET_DATA_SIZE (253) +#define PACKETIZED_MAX_PACKETS ((size_t)UINT16_MAX) typedef enum packetized_msg_type_t { @@ -76,6 +77,7 @@ struct zcm_trans_packetized_serial_t uint8_t* out_buf; size_t out_buf_size; size_t out_len; + uint64_t out_utime; char out_channel[ZCM_CHANNEL_MAXLEN + 1]; int out_pending; @@ -225,6 +227,42 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) return ZCM_EOK; } +static int queue_output(zcm_trans_packetized_serial_t* zt, const char* channel, + const uint8_t* data, size_t len, uint64_t utime, + int strip_packetized_prefix) +{ + if (zt->out_buf_size < len) { + uint8_t* newbuf = realloc(zt->out_buf, len == 0 ? 1 : len); + if (newbuf == NULL) return ZCM_EMEMORY; + zt->out_buf = newbuf; + zt->out_buf_size = len; + } + + if (len > 0) memcpy(zt->out_buf, data, len); + + if (strip_packetized_prefix && channel[0] == '$') channel += 1; + strncpy(zt->out_channel, channel, ZCM_CHANNEL_MAXLEN); + zt->out_channel[ZCM_CHANNEL_MAXLEN] = '\0'; + zt->out_len = len; + zt->out_utime = utime; + zt->out_pending = 1; + return ZCM_EOK; +} + +static void deliver_pending(zcm_trans_packetized_serial_t* zt, zcm_msg_t* msg) +{ + msg->utime = zt->out_utime; + msg->channel = zt->out_channel; + msg->len = zt->out_len; + msg->buf = zt->out_buf; + zt->out_pending = 0; +} + +static size_t packetized_max_mtu(uint8_t packet_data_size) +{ + return (size_t)packet_data_size * PACKETIZED_MAX_PACKETS; +} + static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) { size_t i; @@ -334,26 +372,11 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i return ZCM_EINVALID; } - if (zt->out_buf_size < rx->total_message_size) { - uint8_t* newbuf = realloc(zt->out_buf, rx->total_message_size); - if (newbuf == NULL) { - rx_clear(rx); - return ZCM_EMEMORY; - } - zt->out_buf = newbuf; - zt->out_buf_size = rx->total_message_size; - } - - if (rx->total_message_size > 0) - memcpy(zt->out_buf, rx->data, rx->total_message_size); - { - const char* out_channel = rx->channel; - if (out_channel[0] == '$') out_channel += 1; - strncpy(zt->out_channel, out_channel, ZCM_CHANNEL_MAXLEN); + int ret = queue_output(zt, rx->channel, rx->data, rx->total_message_size, utime, 1); + if (ret != ZCM_EOK) { + rx_clear(rx); + return ret; } - zt->out_channel[ZCM_CHANNEL_MAXLEN] = '\0'; - zt->out_len = rx->total_message_size; - zt->out_pending = 1; rx_clear(rx); } @@ -431,13 +454,16 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) } uint8_t packet_data_size = zt->configured_packet_data_size; - if (packet_data_size == 0 || packet_data_size > 253) return ZCM_EINVALID; + if (packet_data_size == 0 || packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) + return ZCM_EINVALID; if (msg.len > zt->mtu) return ZCM_EINVALID; uint32_t total_message_size = (uint32_t)msg.len; - uint16_t total_packets = - (uint16_t)((total_message_size + packet_data_size - 1u) / packet_data_size); - if (total_packets == 0) total_packets = 1; + size_t total_packets_sz = + (size_t)((total_message_size + packet_data_size - 1u) / packet_data_size); + if (total_packets_sz == 0) total_packets_sz = 1; + if (total_packets_sz > PACKETIZED_MAX_PACKETS) return ZCM_EINVALID; + uint16_t total_packets = (uint16_t)total_packets_sz; tx_clear(&zt->tx); packetized_tx_state_t* tx = &zt->tx; @@ -489,73 +515,87 @@ int packetized_serial_recvmsg_enable(zcm_trans_packetized_serial_t* zt, return zcm_trans_recvmsg_enable(zt->inner, channel, enable); } +static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) +{ + while (!zt->out_pending) { + zcm_msg_t in; + int ret = zcm_trans_recvmsg(zt->inner, &in, 0); + if (ret != ZCM_EOK) return ret; + + if (!is_packetized_channel(in.channel)) { + return queue_output(zt, in.channel, in.buf, in.len, in.utime, 0); + } + + if (in.len < PACKETIZED_HEADER_BYTES) continue; + { + uint8_t type = in.buf[0]; + uint16_t session_id = read_u16_be(&in.buf[1]); + uint8_t body_len = in.buf[3]; + if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; + + const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; + uint64_t now = in.utime == 0 ? zt->time(zt->time_usr) : in.utime; + + if (type == PACKETIZED_MSG_METADATA) { + ret = begin_rx_session(zt, in.channel, session_id, body, body_len, now); + } else if (type == PACKETIZED_MSG_DATA) { + ret = process_rx_data(zt, session_id, body, body_len, now); + } else if (type == PACKETIZED_MSG_RETRANS_REQ) { + ret = process_retrans_request(zt, session_id, body, body_len); + } else { + continue; + } + + if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; + } + } + + return ZCM_EOK; +} + int packetized_serial_recvmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t* msg, unsigned timeoutMs) { (void)timeoutMs; if (zt->out_pending) { - msg->utime = zt->time(zt->time_usr); - msg->channel = zt->out_channel; - msg->len = zt->out_len; - msg->buf = zt->out_buf; - zt->out_pending = 0; + deliver_pending(zt, msg); return ZCM_EOK; } while (1) { - zcm_msg_t in; - int ret = zcm_trans_recvmsg(zt->inner, &in, 0); + int ret = process_incoming_messages(zt); if (ret == ZCM_EAGAIN) { maybe_schedule_retrans_request(zt, zt->time(zt->time_usr)); + ret = send_retrans_request(zt); + if (ret != ZCM_EOK) return ret; return ZCM_EAGAIN; } if (ret != ZCM_EOK) return ret; - if (!is_packetized_channel(in.channel)) { - *msg = in; - return ZCM_EOK; - } - - if (in.len < PACKETIZED_HEADER_BYTES) continue; - uint8_t type = in.buf[0]; - uint16_t session_id = read_u16_be(&in.buf[1]); - uint8_t body_len = in.buf[3]; - if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; - - const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; - uint64_t now = in.utime == 0 ? zt->time(zt->time_usr) : in.utime; - - if (type == PACKETIZED_MSG_METADATA) { - ret = begin_rx_session(zt, in.channel, session_id, body, body_len, now); - if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; - } else if (type == PACKETIZED_MSG_DATA) { - ret = process_rx_data(zt, session_id, body, body_len, now); - if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; - if (zt->out_pending) { - msg->utime = now; - msg->channel = zt->out_channel; - msg->len = zt->out_len; - msg->buf = zt->out_buf; - zt->out_pending = 0; - return ZCM_EOK; - } - } else if (type == PACKETIZED_MSG_RETRANS_REQ) { - ret = process_retrans_request(zt, session_id, body, body_len); - if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; - } + deliver_pending(zt, msg); + return ZCM_EOK; } } int packetized_serial_update_rx(zcm_trans_t* _zt) { zcm_trans_packetized_serial_t* zt = cast(_zt); - return serial_update_rx(zt->inner); + int ret = serial_update_rx(zt->inner); + if (ret != ZCM_EOK) return ret; + + ret = process_incoming_messages(zt); + if (ret != ZCM_EOK && ret != ZCM_EAGAIN) return ret; + + maybe_schedule_retrans_request(zt, zt->time(zt->time_usr)); + return send_retrans_request(zt); } int packetized_serial_update_tx(zcm_trans_t* _zt) { zcm_trans_packetized_serial_t* zt = cast(_zt); + int ret = send_pending_retransmissions(zt); + if (ret != ZCM_EOK) return ret; return serial_update_tx(zt->inner); } @@ -627,19 +667,18 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } - zt->mtu = PACKETIZED_MAX_MTU; - if (zt->mtu < zt->inner_mtu) zt->mtu = zt->inner_mtu; - if (packet_data_size == 0) { size_t max_payload = zt->inner_mtu - PACKETIZED_HEADER_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES; - if (max_payload > 253) max_payload = 253; + if (max_payload > PACKETIZED_MAX_PACKET_DATA_SIZE) + max_payload = PACKETIZED_MAX_PACKET_DATA_SIZE; zt->configured_packet_data_size = (uint8_t)max_payload; } else { zt->configured_packet_data_size = packet_data_size; } - if (zt->configured_packet_data_size == 0 || zt->configured_packet_data_size > 253) { + if (zt->configured_packet_data_size == 0 || + zt->configured_packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) { zcm_trans_generic_serial_destroy(zt->inner); free(zt); return NULL; @@ -653,6 +692,8 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } + zt->mtu = packetized_max_mtu(zt->configured_packet_data_size); + zt->pkt_buf_size = zt->inner_mtu; zt->pkt_buf = malloc(zt->pkt_buf_size); if (zt->pkt_buf == NULL) { @@ -664,6 +705,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( zt->out_buf = NULL; zt->out_buf_size = 0; zt->out_len = 0; + zt->out_utime = 0; zt->out_channel[0] = '\0'; zt->out_pending = 0; diff --git a/zcm/transport/packetized_transport_can.cpp b/zcm/transport/packetized_transport_can.cpp deleted file mode 100644 index d504868e1..000000000 --- a/zcm/transport/packetized_transport_can.cpp +++ /dev/null @@ -1,372 +0,0 @@ -#include "zcm/transport.h" -#include "zcm/transport_register.hpp" -#include "zcm/transport_registrar.h" -#include "zcm/util/debug.h" - -#include "packetized_serial_transport.h" - -#include "util/TimeUtil.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#define MASK_29B ((1 << 29) - 1) -#define MASK_11B ((1 << 11) - 1) - -// Define this the class name you want -#define ZCM_TRANS_CLASSNAME PacketizedTransportCan -#define MTU (1 << 14) - -using namespace std; - -struct ZCM_TRANS_CLASSNAME : public zcm_trans_t -{ - unordered_map options; - uint32_t msgId; - uint32_t txId; - uint8_t packetDataSize; - string address; - - int soc = -1; - bool socSettingsGood = false; - struct sockaddr_can addr; - struct ifreq ifr; - - zcm_trans_t* gst = nullptr; - - uint64_t recvTimeoutUs = 0; - uint64_t recvMsgStartUtime = 0; - - uint8_t leftoverBuffer[CAN_MAX_DLEN]; - size_t leftoverBytes = 0; - size_t leftoverOffset = 0; - - string* findOption(const string& s) - { - auto it = options.find(s); - if (it == options.end()) return nullptr; - return &it->second; - } - - ZCM_TRANS_CLASSNAME(zcm_url_t* url) - { - trans_type = ZCM_BLOCKING; - vtbl = &methods; - - auto* opts = zcm_url_opts(url); - for (size_t i = 0; i < opts->numopts; ++i) - options[opts->name[i]] = opts->value[i]; - - packetDataSize = 0; - auto* pktSizeStr = findOption("pkt_size"); - if (pktSizeStr) { - char* endptr; - unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0 || parsed > 253) { - ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); - return; - } - packetDataSize = (uint8_t)parsed; - } - - msgId = 0; - auto* msgIdStr = findOption("msgid"); - if (!msgIdStr) { - ZCM_DEBUG("Msg Id unspecified"); - return; - } else { - char* endptr; - msgId = (strtoul(msgIdStr->c_str(), &endptr, 10) & MASK_29B); - if (*endptr != '\0') { - ZCM_DEBUG("Msg Id unspecified"); - return; - } - } - - auto* txAddrMode = findOption("tx_addr_mode"); - bool extendedTx; - if (!txAddrMode || string("extended") == txAddrMode->c_str()) { - extendedTx = true; - } else if (string("standard") == txAddrMode->c_str()) { - extendedTx = false; - } else { - ZCM_DEBUG("Invalid rx_addr_mode. Use 'extended' or 'standard'"); - return; - } - if (!extendedTx && ((msgId & MASK_11B) != msgId)) { - ZCM_DEBUG("Msg Id too long for standard can addresses. " - "Use 'tx_extended_addr=true'"); - return; - } - if (extendedTx && ((msgId & MASK_29B) != msgId)) { - ZCM_DEBUG("Msg Id too long for extended can addresses."); - return; - } - txId = extendedTx ? msgId | CAN_EFF_FLAG : msgId; - - struct can_filter rfilter[2]; - size_t numRxFilters = 0; - - auto* rxAddrMode = findOption("rx_addr_mode"); - if (!rxAddrMode || string("both") == rxAddrMode->c_str()) { - rfilter[0].can_id = msgId; - rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK); - rfilter[1].can_id = msgId | CAN_EFF_FLAG; - rfilter[1].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK); - numRxFilters = 2; - } else if (string("standard") == rxAddrMode->c_str()) { - rfilter[0].can_id = msgId; - rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK); - numRxFilters = 1; - } else if (string("extended") == rxAddrMode->c_str()) { - rfilter[0].can_id = msgId | CAN_EFF_FLAG; - rfilter[0].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK); - numRxFilters = 1; - } else { - ZCM_DEBUG("Invalid rx_addr_mode. Use 'extended', 'standard', or 'both'"); - return; - } - - address = zcm_url_address(url); - - if ((soc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - ZCM_DEBUG("Unable to make socket"); - return; - } - - strcpy(ifr.ifr_name, address.c_str()); - ioctl(soc, SIOCGIFINDEX, &ifr); - - memset(&addr, 0, sizeof(addr)); - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - if (bind(soc, (struct sockaddr*)&addr, sizeof(addr)) < 0) { - ZCM_DEBUG("Failed to bind"); - close(soc); - return; - } - - if (setsockopt(soc, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, - numRxFilters * sizeof(struct can_filter)) < 0) { - ZCM_DEBUG("Failed to set filter"); - return; - } - - gst = zcm_trans_packetized_serial_create( - &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10, packetDataSize); - socSettingsGood = true; - } - - ~ZCM_TRANS_CLASSNAME() - { - if (gst) zcm_trans_packetized_serial_destroy(gst); - if (soc != -1 && close(soc) < 0) { ZCM_DEBUG("Failed to close"); } - soc = -1; - } - - bool good() { return soc != -1 && socSettingsGood; } - - static size_t get(uint8_t* data, size_t nData, void* usr) - { - ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); - size_t totalRead = 0; - - if (me->leftoverBytes > 0) { - size_t availableLeftover = me->leftoverBytes - me->leftoverOffset; - size_t fromLeftover = min(nData, availableLeftover); - memcpy(data, &me->leftoverBuffer[me->leftoverOffset], fromLeftover); - me->leftoverOffset += fromLeftover; - totalRead += fromLeftover; - - if (me->leftoverOffset >= me->leftoverBytes) { - me->leftoverBytes = 0; - me->leftoverOffset = 0; - } - - if (totalRead >= nData) return totalRead; - } - - struct can_frame frame; - int nbytes = read(me->soc, &frame, sizeof(struct can_frame)); - if (nbytes != sizeof(struct can_frame)) { - if (nbytes < 0) { - uint64_t timeoutConsumedUs = TimeUtil::utime() - me->recvMsgStartUtime; - if (timeoutConsumedUs >= me->recvTimeoutUs) return totalRead; - usleep(me->recvTimeoutUs - timeoutConsumedUs); - } - return totalRead; - } - - size_t frameDataSize = (size_t)frame.can_dlc; - nData -= totalRead; - size_t fromNewFrame = min(nData, frameDataSize); - memcpy(&data[totalRead], frame.data, fromNewFrame); - totalRead += fromNewFrame; - - if (fromNewFrame < frameDataSize) { - me->leftoverBytes = frameDataSize - fromNewFrame; - me->leftoverOffset = 0; - memcpy(me->leftoverBuffer, (uint8_t*)frame.data + fromNewFrame, - me->leftoverBytes); - } - - return totalRead; - } - - static size_t sendFrame(const uint8_t* data, size_t nData, void* usr) - { - ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); - - struct can_frame frame; - frame.can_id = me->txId; - - size_t ret = min(nData, (size_t)CAN_MAX_DLEN); - frame.can_dlc = ret; - memcpy(frame.data, data, ret); - - if (write(me->soc, &frame, sizeof(struct can_frame)) != - sizeof(struct can_frame)) { - ZCM_DEBUG("Failed to write data"); - return 0; - } - - return ret; - } - - static size_t put(const uint8_t* data, size_t nData, void* usr) - { - size_t ret = 0; - while (ret < nData) { - size_t left = nData - ret; - size_t written = sendFrame(&data[ret], left, usr); - if (written == 0) return ret; - ret += written; - } - return ret; - } - - static uint64_t timestamp_now(void* usr) - { - ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); - - struct timeval time; - if (ioctl(me->soc, SIOCGSTAMP, &time) == -1) return 0; - return time.tv_sec * 1e6 + time.tv_usec; - } - - size_t get_mtu() { return zcm_trans_get_mtu(this->gst); } - - int sendmsg(zcm_msg_t msg) - { - int ret = zcm_trans_sendmsg(this->gst, msg); - if (ret != ZCM_EOK) return ret; - return packetized_serial_update_tx(this->gst); - } - - int recvmsgEnable(const char* channel, bool enable) - { - return zcm_trans_recvmsg_enable(this->gst, channel, enable); - } - - int recvmsg(zcm_msg_t* msg, unsigned timeoutMs) - { - recvMsgStartUtime = TimeUtil::utime(); - recvTimeoutUs = timeoutMs * 1000ULL; - - do { - int ret = zcm_trans_recvmsg(this->gst, msg, 0); - if (ret == ZCM_EOK) return ret; - - uint64_t timeoutConsumedUs = TimeUtil::utime() - recvMsgStartUtime; - if (timeoutConsumedUs > recvTimeoutUs) break; - - uint64_t socketTimeout = recvTimeoutUs - timeoutConsumedUs; - unsigned timeoutS = socketTimeout / 1000000; - unsigned timeoutUs = socketTimeout - timeoutS * 1000000; - struct timeval tm = { - timeoutS, - timeoutUs, - }; - if (setsockopt(soc, SOL_SOCKET, SO_RCVTIMEO, (char*)&tm, sizeof(tm)) < 0) { - ZCM_DEBUG("Failed to settimeout"); - return ZCM_EUNKNOWN; - } - - packetized_serial_update_rx(this->gst); - } while (true); - return ZCM_EAGAIN; - } - - static zcm_trans_methods_t methods; - static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) - { - assert(zt->vtbl == &methods); - return (ZCM_TRANS_CLASSNAME*)zt; - } - - static size_t _get_mtu(zcm_trans_t* zt) { return cast(zt)->get_mtu(); } - - static int _sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return cast(zt)->sendmsg(msg); } - - static int _recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) - { - return cast(zt)->recvmsgEnable(channel, enable); - } - - static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) - { - return cast(zt)->recvmsg(msg, timeout); - } - - static void _destroy(zcm_trans_t* zt) { delete cast(zt); } - - static const TransportRegister reg; -}; - -zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { - &ZCM_TRANS_CLASSNAME::_get_mtu, - &ZCM_TRANS_CLASSNAME::_sendmsg, - &ZCM_TRANS_CLASSNAME::_recvmsg_enable, - &ZCM_TRANS_CLASSNAME::_recvmsg, - NULL, - NULL, - &ZCM_TRANS_CLASSNAME::_destroy, -}; - -static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) -{ - if (opt_errmsg) *opt_errmsg = NULL; - - auto* trans = new ZCM_TRANS_CLASSNAME(url); - if (trans->good()) return trans; - - delete trans; - return nullptr; -} - -#ifdef USING_TRANS_CAN -const TransportRegister ZCM_TRANS_CLASSNAME::reg( - "can+pkt", - "Transfer data via packetized socket CAN connection on a single id " - "(e.g. " - "'can+pkt://" - "can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true&pkt_size=128')", - create); -#endif diff --git a/zcm/transport/packetized_transport_serial.cpp b/zcm/transport/packetized_transport_serial.cpp deleted file mode 100644 index 945171914..000000000 --- a/zcm/transport/packetized_transport_serial.cpp +++ /dev/null @@ -1,455 +0,0 @@ -#include "zcm/transport.h" -#include "zcm/transport_register.hpp" -#include "zcm/transport_registrar.h" -#include "zcm/util/debug.h" -#include "zcm/util/lockfile.h" - -#include "packetized_serial_transport.h" - -#include "util/TimeUtil.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -using namespace std; - -#define ZCM_TRANS_CLASSNAME PacketizedTransportSerial -#define MTU (1 << 14) - -using u8 = uint8_t; -using u64 = uint64_t; - -struct PacketizedSerialDevice -{ - PacketizedSerialDevice() {} - ~PacketizedSerialDevice() { close(); } - - bool open(const string& port, int baud, bool hwFlowControl); - bool isOpen() { return fd > 0; } - void close(); - - int write(const u8* buf, size_t sz); - int read(u8* buf, size_t sz, u64 timeoutUs); - static int convertBaud(int baud); - - PacketizedSerialDevice(const PacketizedSerialDevice&) = delete; - PacketizedSerialDevice(PacketizedSerialDevice&&) = delete; - PacketizedSerialDevice& operator=(const PacketizedSerialDevice&) = delete; - PacketizedSerialDevice& operator=(PacketizedSerialDevice&&) = delete; - - private: - string port; - int fd = -1; - lockfile_t* lf; -}; - -bool PacketizedSerialDevice::open(const string& port_, int baud, bool hwFlowControl) -{ - if (baud == 0) { - fprintf(stderr, "Serial baud rate not specified in url. " - "Proceeding without setting baud\n"); - } else if (!(baud = convertBaud(baud))) { - fprintf(stderr, "Unrecognized baudrate. Failed to open serial device.\n "); - return false; - } - - lf = lockfile_trylock(port_.c_str()); - if (!lf) { - ZCM_DEBUG("failed to create lock file, refusing to open serial device (%s)", - port_.c_str()); - return false; - } - this->port = port_; - - int flags = O_RDWR | O_NOCTTY | O_SYNC; - fd = ::open(port.c_str(), flags, 0); - if (fd < 0) { - ZCM_DEBUG("failed to open serial device (%s): %s", port.c_str(), strerror(errno)); - goto fail; - } - - ioctl(fd, USBDEVFS_RESET, 0); - - struct termios opts; - if (tcgetattr(fd, &opts)) { - ZCM_DEBUG("failed to get termios options on fd: %s", strerror(errno)); - goto fail; - } - - if (baud != 0) { - cfsetispeed(&opts, baud); - cfsetospeed(&opts, baud); - } - cfmakeraw(&opts); - - opts.c_cflag &= ~CSTOPB; - opts.c_cflag |= CS8; - opts.c_cflag &= ~PARENB; - if (hwFlowControl) opts.c_cflag |= CRTSCTS; - opts.c_cc[VTIME] = 1; - opts.c_cc[VMIN] = 30; - - if (tcsetattr(fd, TCSANOW, &opts)) { - ZCM_DEBUG("failed to set termios options on fd: %s", strerror(errno)); - goto fail; - } - - tcflush(fd, TCIOFLUSH); - - return true; - -fail: - if (fd > 0) { - const int saved_errno = errno; - int result; - do { - result = ::close(fd); - } while (result == -1 && errno == EINTR); - errno = saved_errno; - } - this->fd = -1; - - if (lf) { - lockfile_unlock(lf); - lf = nullptr; - } - this->port = ""; - - return false; -} - -void PacketizedSerialDevice::close() -{ - if (isOpen()) { - ::close(fd); - fd = 0; - } - if (port != "") port = ""; - if (lf) { - lockfile_unlock(lf); - lf = nullptr; - } -} - -int PacketizedSerialDevice::write(const u8* buf, size_t sz) -{ - assert(this->isOpen()); - int ret = ::write(fd, buf, sz); - if (ret == -1) { - ZCM_DEBUG("ERR: write failed: %s", strerror(errno)); - return -1; - } - return ret; -} - -int PacketizedSerialDevice::read(u8* buf, size_t sz, u64 timeoutUs) -{ - assert(this->isOpen()); - fd_set fds; - FD_ZERO(&fds); - FD_SET(fd, &fds); - - struct timeval timeout; - timeout.tv_sec = timeoutUs / 1000000; - timeout.tv_usec = timeoutUs - timeout.tv_sec * 1000000; - int status = ::select(fd + 1, &fds, NULL, NULL, &timeout); - - if (status > 0) { - if (FD_ISSET(fd, &fds)) { - int ret = ::read(fd, buf, sz); - if (ret == -1) { - ZCM_DEBUG("ERR: serial read failed: %s", strerror(errno)); - } else if (ret == 0) { - ZCM_DEBUG("ERR: serial device unplugged"); - close(); - assert(false && "ERR: serial device unplugged\n" && - "ZCM does not support reconnecting to serial devices"); - return -3; - } - return ret; - } else { - ZCM_DEBUG("ERR: serial bytes not ready"); - return -1; - } - } else { - ZCM_DEBUG("ERR: serial read timed out"); - return -2; - } -} - -int PacketizedSerialDevice::convertBaud(int baud) -{ - switch (baud) { - case 4800: return B4800; - case 9600: return B9600; - case 19200: return B19200; - case 38400: return B38400; - case 57600: return B57600; - case 115200: return B115200; - case 230400: return B230400; - case 460800: return B460800; - default: return 0; - } -} - -struct ZCM_TRANS_CLASSNAME : public zcm_trans_t -{ - PacketizedSerialDevice ser; - - int baud; - bool hwFlowControl; - uint8_t packetDataSize; - - bool raw; - string rawChan; - int rawSize; - std::unique_ptr rawBuf; - - string address; - unordered_map options; - zcm_trans_t* gst; - uint64_t timeoutLeftUs; - - string* findOption(const string& s) - { - auto it = options.find(s); - if (it == options.end()) return nullptr; - return &it->second; - } - - ZCM_TRANS_CLASSNAME(zcm_url_t* url) - { - trans_type = ZCM_BLOCKING; - vtbl = &methods; - - auto* opts = zcm_url_opts(url); - for (size_t i = 0; i < opts->numopts; ++i) - options[opts->name[i]] = opts->value[i]; - - baud = 0; - auto* baudStr = findOption("baud"); - if (!baudStr) { - fprintf(stderr, "Baud unspecified. Bypassing serial baud setup.\n"); - } else { - baud = atoi(baudStr->c_str()); - if (baud == 0) { - ZCM_DEBUG("expected integer argument for 'baud'"); - return; - } - } - - hwFlowControl = false; - auto* hwFlowControlStr = findOption("hw_flow_control"); - if (hwFlowControlStr) { - if (*hwFlowControlStr == "true") { - hwFlowControl = true; - } else if (*hwFlowControlStr == "false") { - hwFlowControl = false; - } else { - ZCM_DEBUG("expected boolean argument for 'hw_flow_control'"); - return; - } - } - - packetDataSize = 0; - auto* pktSizeStr = findOption("pkt_size"); - if (pktSizeStr) { - char* endptr; - unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0 || parsed > 253) { - ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); - return; - } - packetDataSize = (uint8_t)parsed; - } - - raw = false; - auto* rawStr = findOption("raw"); - if (rawStr) { - if (*rawStr == "true") { - raw = true; - } else if (*rawStr == "false") { - raw = false; - } else { - ZCM_DEBUG("expected boolean argument for 'raw'"); - return; - } - } - - rawChan = ""; - auto* rawChanStr = findOption("raw_channel"); - if (rawChanStr) { rawChan = *rawChanStr; } - - rawSize = 1024; - auto* rawSizeStr = findOption("raw_size"); - if (rawSizeStr) { - rawSize = atoi(rawSizeStr->c_str()); - if (rawSize <= 0) { - ZCM_DEBUG("expected positive integer argument for 'raw_size'"); - return; - } - } - - address = zcm_url_address(url); - ser.open(address, baud, hwFlowControl); - - if (raw) { - rawBuf.reset(new uint8_t[rawSize]); - gst = nullptr; - } else { - gst = zcm_trans_packetized_serial_create( - &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10, - packetDataSize); - } - } - - ~ZCM_TRANS_CLASSNAME() - { - ser.close(); - if (gst) zcm_trans_packetized_serial_destroy(gst); - } - - bool good() { return ser.isOpen(); } - - static size_t get(uint8_t* data, size_t nData, void* usr) - { - ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); - uint64_t startUtime = TimeUtil::utime(); - int ret = me->ser.read(data, nData, me->timeoutLeftUs); - uint64_t diff = TimeUtil::utime() - startUtime; - me->timeoutLeftUs = me->timeoutLeftUs > diff ? me->timeoutLeftUs - diff : 0; - return ret < 0 ? 0 : ret; - } - - static size_t put(const uint8_t* data, size_t nData, void* usr) - { - ZCM_TRANS_CLASSNAME* me = cast((zcm_trans_t*)usr); - int ret = me->ser.write(data, nData); - return ret < 0 ? 0 : ret; - } - - static uint64_t timestamp_now(void* usr) - { - (void)usr; - return TimeUtil::utime(); - } - - size_t getMtu() { return raw ? MTU : zcm_trans_get_mtu(this->gst); } - - int sendmsg(zcm_msg_t msg) - { - if (raw) { - if (put(msg.buf, msg.len, this) != 0) return ZCM_EOK; - return ZCM_EAGAIN; - } else { - int ret = zcm_trans_sendmsg(this->gst, msg); - if (ret != ZCM_EOK) return ret; - return packetized_serial_update_tx(this->gst); - } - } - - int recvmsgEnable(const char* channel, bool enable) - { - return raw ? ZCM_EOK : zcm_trans_recvmsg_enable(this->gst, channel, enable); - } - - int recvmsg(zcm_msg_t* msg, unsigned timeoutMs) - { - timeoutLeftUs = timeoutMs * 1e3; - - if (raw) { - size_t sz = get(rawBuf.get(), rawSize, this); - if (sz == 0 || rawChan.empty()) return ZCM_EAGAIN; - - msg->utime = timestamp_now(this); - msg->channel = rawChan.c_str(); - msg->len = sz; - msg->buf = rawBuf.get(); - return ZCM_EOK; - } else { - do { - uint64_t startUtime = TimeUtil::utime(); - - int ret = zcm_trans_recvmsg(this->gst, msg, 0); - if (ret == ZCM_EOK) return ret; - - uint64_t diff = TimeUtil::utime() - startUtime; - startUtime = TimeUtil::utime(); - timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; - - packetized_serial_update_rx(this->gst); - - diff = TimeUtil::utime() - startUtime; - timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; - - } while (timeoutLeftUs > 0); - - return ZCM_EAGAIN; - } - } - - static zcm_trans_methods_t methods; - static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) - { - assert(zt->vtbl == &methods); - return (ZCM_TRANS_CLASSNAME*)zt; - } - - static size_t _getMtu(zcm_trans_t* zt) { return cast(zt)->getMtu(); } - - static int _sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return cast(zt)->sendmsg(msg); } - - static int _recvmsgEnable(zcm_trans_t* zt, const char* channel, bool enable) - { - return cast(zt)->recvmsgEnable(channel, enable); - } - - static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) - { - return cast(zt)->recvmsg(msg, timeout); - } - - static void _destroy(zcm_trans_t* zt) { delete cast(zt); } - - static const TransportRegister reg; -}; - -zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { - &ZCM_TRANS_CLASSNAME::_getMtu, - &ZCM_TRANS_CLASSNAME::_sendmsg, - &ZCM_TRANS_CLASSNAME::_recvmsgEnable, - &ZCM_TRANS_CLASSNAME::_recvmsg, - NULL, - NULL, - &ZCM_TRANS_CLASSNAME::_destroy, -}; - -static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) -{ - if (opt_errmsg) *opt_errmsg = NULL; - auto* trans = new ZCM_TRANS_CLASSNAME(url); - if (trans->good()) return trans; - - delete trans; - return nullptr; -} - -#ifdef USING_TRANS_SERIAL -const TransportRegister ZCM_TRANS_CLASSNAME::reg( - "serial+pkt", - "Transfer data via a packetized serial connection " - "(e.g. 'serial+pkt:///dev/ttyUSB0?baud=115200&hw_flow_control=true&pkt_size=128' or " - "'serial+pkt:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", - create); -#endif diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 84090313c..772607b2f 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -4,6 +4,7 @@ #include "zcm/util/debug.h" #include "generic_serial_transport.h" +#include "packetized_serial_transport.h" #include "util/TimeUtil.hpp" @@ -38,6 +39,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t unordered_map options; uint32_t msgId; uint32_t txId; + uint8_t packetDataSize; string address; int soc = -1; @@ -72,6 +74,19 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t options[opts->name[i]] = opts->value[i]; msgId = 0; + packetDataSize = 0; + + auto* pktSizeStr = findOption("pkt_size"); + if (pktSizeStr) { + char* endptr; + unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) { + ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); + return; + } + packetDataSize = (uint8_t)parsed; + } + auto* msgIdStr = findOption("msgid"); if (!msgIdStr) { ZCM_DEBUG("Msg Id unspecified"); @@ -155,18 +170,29 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return; } - gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, - &ZCM_TRANS_CLASSNAME::put, - this, - &ZCM_TRANS_CLASSNAME::timestamp_now, - this, - MTU, MTU * 10); + if (packetDataSize != 0) { + gst = zcm_trans_packetized_serial_create(&ZCM_TRANS_CLASSNAME::get, + &ZCM_TRANS_CLASSNAME::put, + this, + &ZCM_TRANS_CLASSNAME::timestamp_now, + this, + MTU, MTU * 10, + packetDataSize); + } else { + gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, + &ZCM_TRANS_CLASSNAME::put, + this, + &ZCM_TRANS_CLASSNAME::timestamp_now, + this, + MTU, MTU * 10); + } + if (!gst) return; socSettingsGood = true; } ~ZCM_TRANS_CLASSNAME() { - if (gst) zcm_trans_generic_serial_destroy(gst); + if (gst) zcm_trans_destroy(gst); if (soc != -1 && close(soc) < 0) { ZCM_DEBUG("Failed to close"); } @@ -366,5 +392,6 @@ static zcm_trans_t *create(zcm_url_t* url, char **opt_errmsg) #ifdef USING_TRANS_CAN const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can", "Transfer data via a socket CAN connection on a single id " - "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true')", create); + "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " + "'can://can0?msgid=65536&pkt_size=8')", create); #endif diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index b0cb8e060..2d940e04a 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -5,6 +5,7 @@ #include "zcm/util/debug.h" #include "generic_serial_transport.h" +#include "packetized_serial_transport.h" #include "util/TimeUtil.hpp" @@ -242,6 +243,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t int baud; bool hwFlowControl; + bool packetized; + uint8_t packetDataSize; bool raw; string rawChan; @@ -298,6 +301,20 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } } + packetized = false; + packetDataSize = 0; + auto* pktSizeStr = findOption("pkt_size"); + if (pktSizeStr) { + char* endptr; + unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) { + ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); + return; + } + packetized = true; + packetDataSize = (uint8_t)parsed; + } + raw = false; auto* rawStr = findOption("raw"); if (rawStr) { @@ -310,6 +327,10 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return; } } + if (raw && packetized) { + ZCM_DEBUG("'raw' and 'pkt_size' options are mutually exclusive"); + return; + } rawChan = ""; auto* rawChanStr = findOption("raw_channel"); @@ -333,6 +354,14 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t if (raw) { rawBuf.reset(new uint8_t[rawSize]); gst = nullptr; + } else if (packetized) { + gst = zcm_trans_packetized_serial_create(&ZCM_TRANS_CLASSNAME::get, + &ZCM_TRANS_CLASSNAME::put, + this, + &ZCM_TRANS_CLASSNAME::timestamp_now, + nullptr, + MTU, MTU * 10, + packetDataSize); } else { gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, @@ -346,12 +375,12 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t ~ZCM_TRANS_CLASSNAME() { ser.close(); - if (gst) zcm_trans_generic_serial_destroy(gst); + if (gst) zcm_trans_destroy(gst); } bool good() { - return ser.isOpen(); + return ser.isOpen() && (raw || gst != nullptr); } static size_t get(uint8_t* data, size_t nData, void* usr) @@ -489,7 +518,8 @@ static zcm_trans_t* create(zcm_url_t* url, char **opt_errmsg) // Register this transport with ZCM const TransportRegister ZCM_TRANS_CLASSNAME::reg( "serial", "Transfer data via a serial connection " - "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true' or " + "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true', " + "'serial:///dev/ttyUSB0?baud=115200&pkt_size=128', or " "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", create); #endif diff --git a/zcm/wscript b/zcm/wscript index eb9a8dac2..53638cc8f 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -19,11 +19,9 @@ def build(ctx): if not ctx.env.USING_TRANS_UDP: srcExcludes += ['transport/udp/**/*'] if not ctx.env.USING_TRANS_SERIAL: - srcExcludes += ['transport/transport_serial.cpp', - 'transport/packetized_transport_serial.cpp'] + srcExcludes += ['transport/transport_serial.cpp'] if not ctx.env.USING_TRANS_CAN: - srcExcludes += ['transport/transport_can.cpp', - 'transport/packetized_transport_can.cpp'] + srcExcludes += ['transport/transport_can.cpp'] if not ctx.env.USING_TRANS_IPCSHM: srcExcludes += ['transport/transport_ipcshm.cpp', 'transport/lockfree/lf_*.c'] if not ctx.env.USING_THIRD_PARTY: From 694227715e17e6084c8727ef784207f73eae4fb2 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 8 Apr 2026 16:48:44 -0400 Subject: [PATCH 06/20] Some more optimization --- docs/transports.md | 8 +- test/zcm/PacketizedSerialTransportTest.hpp | 73 +++++-- zcm/transport/packetized_serial_protocol.h | 74 +++++++ zcm/transport/packetized_serial_transport.c | 230 +++++++++----------- zcm/transport/packetized_serial_transport.h | 33 ++- zcm/transport/transport_can.cpp | 26 ++- zcm/transport/transport_serial.cpp | 35 ++- zcm/wscript | 10 +- 8 files changed, 305 insertions(+), 184 deletions(-) create mode 100644 zcm/transport/packetized_serial_protocol.h diff --git a/docs/transports.md b/docs/transports.md index dc59293c4..80edd102f 100644 --- a/docs/transports.md +++ b/docs/transports.md @@ -48,13 +48,13 @@ be used to *summon* the transport: Serial - serial://<path-to-device>?baud=<baud>[&pkt_size=<n>] - zcm_create("serial:///dev/ttyUSB0?baud=115200"), zcm_create("serial:///dev/ttyUSB0?baud=115200&pkt_size=128") + serial://<path-to-device>?baud=<baud>[&pkt_size=<n>&pkt_buf_size=<n>] + zcm_create("serial:///dev/ttyUSB0?baud=115200"), zcm_create("serial:///dev/ttyUSB0?baud=115200&pkt_size=128&pkt_buf_size=1024") CAN - can://<interface>?msgid=<id>[&pkt_size=<n>] - zcm_create("can://can0?msgid=65536"), zcm_create("can://can0?msgid=65536&pkt_size=32") + can://<interface>?msgid=<id>[&pkt_size=<n>&pkt_buf_size=<n>] + zcm_create("can://can0?msgid=65536"), zcm_create("can://can0?msgid=65536&pkt_size=32&pkt_buf_size=1024") Inter-process via Shared Memory (IPCSHM) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 8e60cd59a..b25db88c3 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -2,17 +2,8 @@ #define PACKETIZED_SERIAL_TRANSPORT_TEST_HPP #include "cxxtest/TestSuite.h" - -extern "C" { -#include "zcm/transport.h" -zcm_trans_t* zcm_trans_packetized_serial_create( - size_t (*get)(uint8_t* data, size_t nData, void* usr), - size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, - uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, - uint8_t packet_data_size); -int packetized_serial_update_rx(zcm_trans_t* zt); -int packetized_serial_update_tx(zcm_trans_t* zt); -} +#include "zcm/transport/packetized_serial_transport.h" +#include "zcm/transport/packetized_serial_protocol.h" #include #include @@ -34,6 +25,11 @@ struct PacketizedLinkEndpoint string dropChannel; }; +// decode_frame parses one generic_serial framing layer frame from buf[start..]. +// The outer frame format is: 0xCC 0x00 chan_len data_len(4B) *chan *data checksum(2B), +// with 0xCC bytes in chan/data escaped as 0xCC 0xCC. +// NOTE: This mirrors the private framing in generic_serial_transport.c. If that +// framing changes, this function must be updated accordingly. static bool decode_frame(const vector& buf, size_t start, size_t& frame_end, string& channel, vector& data) { @@ -110,11 +106,14 @@ static size_t endpoint_put(const uint8_t* data, size_t nData, void* usr) bool drop = false; if (ep->dropEnabled && !ep->dropDone && channel == ep->dropChannel && - payload.size() >= 6) { - uint8_t type = payload[0]; - uint8_t body_len = payload[3]; - if (type == 2 && payload.size() == 4u + body_len && body_len >= 2) { - uint16_t packet_id = ((uint16_t)payload[4] << 8) | (uint16_t)payload[5]; + payload.size() >= PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES) { + uint8_t type = payload[0]; + uint8_t body_len = payload[3]; + if (type == PACKETIZED_MSG_DATA && + payload.size() == (size_t)PACKETIZED_HEADER_BYTES + body_len && + body_len >= PACKETIZED_DATA_OVERHEAD_BYTES) { + uint16_t packet_id = packetized_read_u16_be( + &payload[PACKETIZED_HEADER_BYTES]); if (packet_id == ep->dropPacketId) { drop = true; ep->dropDone = true; @@ -160,9 +159,9 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite uint64_t now = 1000; zcm_trans_t* tx = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0); + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0, 1024); zcm_trans_t* rx = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0); + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0, 1024); TSM_ASSERT("failed creating transports", tx && rx); vector payload(512); @@ -200,9 +199,9 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite uint64_t now = 2000; zcm_trans_t* ta = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0); + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0, 1024); zcm_trans_t* tb = zcm_trans_packetized_serial_create( - endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0); + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0, 1024); TSM_ASSERT("failed creating transports", ta && tb); vector payload(700); @@ -243,6 +242,40 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_trans_destroy(ta); zcm_trans_destroy(tb); } + + void testConfiguredMaxMessageSize() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 3000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0, 128); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0, 128); + TSM_ASSERT("failed creating transports", tx && rx); + + vector smallPayload(128, 0x5a); + zcm_msg_t small; + small.utime = now; + small.channel = (char*)"$SMALL"; + small.len = smallPayload.size(); + small.buf = smallPayload.data(); + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, small), ZCM_EOK); + + vector largePayload(129, 0x6b); + zcm_msg_t large; + large.utime = now; + large.channel = (char*)"$LARGE"; + large.len = largePayload.size(); + large.buf = largePayload.data(); + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, large), ZCM_EINVALID); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } }; #endif diff --git a/zcm/transport/packetized_serial_protocol.h b/zcm/transport/packetized_serial_protocol.h new file mode 100644 index 000000000..225247e6a --- /dev/null +++ b/zcm/transport/packetized_serial_protocol.h @@ -0,0 +1,74 @@ +#ifndef _ZCM_TRANS_PACKETIZED_SERIAL_PROTOCOL_H +#define _ZCM_TRANS_PACKETIZED_SERIAL_PROTOCOL_H + +#include +#include + +#define PACKETIZED_HEADER_BYTES (4) +#define PACKETIZED_DATA_OVERHEAD_BYTES (2) +#define PACKETIZED_METADATA_BODY_BYTES (11) +#define PACKETIZED_RETRANS_TIMEOUT_US (200000) +#define PACKETIZED_MAX_PACKET_DATA_SIZE (253) +#define PACKETIZED_MAX_PACKETS ((size_t)UINT16_MAX) +#define PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE (1024) + +typedef enum packetized_msg_type_t +{ + PACKETIZED_MSG_METADATA = 1, + PACKETIZED_MSG_DATA = 2, + PACKETIZED_MSG_RETRANS_REQ = 3, +} packetized_msg_type_t; + +static inline uint16_t packetized_read_u16_be(const uint8_t* p) +{ + return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; +} + +static inline uint32_t packetized_read_u32_be(const uint8_t* p) +{ + return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | + (uint32_t)p[3]; +} + +static inline void packetized_write_u16_be(uint8_t* p, uint16_t v) +{ + p[0] = (uint8_t)((v >> 8) & 0xff); + p[1] = (uint8_t)(v & 0xff); +} + +static inline void packetized_write_u32_be(uint8_t* p, uint32_t v) +{ + p[0] = (uint8_t)((v >> 24) & 0xff); + p[1] = (uint8_t)((v >> 16) & 0xff); + p[2] = (uint8_t)((v >> 8) & 0xff); + p[3] = (uint8_t)(v & 0xff); +} + +static inline uint32_t packetized_crc32_update_byte(uint32_t crc, uint8_t b) +{ + int i; + crc ^= b; + for (i = 0; i < 8; ++i) { + uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u)); + crc = (crc >> 1) ^ (0xedb88320u & mask); + } + return crc; +} + +static inline uint32_t packetized_crc32_compute(const uint8_t* data, size_t len) +{ + size_t i; + uint32_t crc = 0xffffffffu; + for (i = 0; i < len; ++i) crc = packetized_crc32_update_byte(crc, data[i]); + return ~crc; +} + +static inline size_t packetized_message_packet_count(size_t message_size, + uint8_t packet_data_size) +{ + if (packet_data_size == 0) return 0; + if (message_size == 0) return 1; + return (message_size + packet_data_size - 1u) / packet_data_size; +} + +#endif /* _ZCM_TRANS_PACKETIZED_SERIAL_PROTOCOL_H */ diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index af993932f..22105823c 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -2,6 +2,7 @@ #include "zcm/zcm.h" #include "generic_serial_transport.h" +#include "packetized_serial_protocol.h" #include "packetized_serial_transport.h" #include @@ -9,22 +10,6 @@ #include #include -#define ASSERT(x) - -#define PACKETIZED_HEADER_BYTES (4) -#define PACKETIZED_DATA_OVERHEAD_BYTES (2) -#define PACKETIZED_METADATA_BODY_BYTES (11) -#define PACKETIZED_RETRANS_TIMEOUT_US (200000) -#define PACKETIZED_MAX_PACKET_DATA_SIZE (253) -#define PACKETIZED_MAX_PACKETS ((size_t)UINT16_MAX) - -typedef enum packetized_msg_type_t -{ - PACKETIZED_MSG_METADATA = 1, - PACKETIZED_MSG_DATA = 2, - PACKETIZED_MSG_RETRANS_REQ = 3, -} packetized_msg_type_t; - typedef struct packetized_rx_state_t packetized_rx_state_t; struct packetized_rx_state_t { @@ -44,6 +29,7 @@ struct packetized_rx_state_t int retrans_pending; uint16_t* missing_ids; uint16_t missing_count; + size_t packet_capacity; }; typedef struct packetized_tx_state_t packetized_tx_state_t; @@ -59,6 +45,7 @@ struct packetized_tx_state_t uint16_t* retrans_ids; uint16_t retrans_count; + size_t packet_capacity; }; typedef struct zcm_trans_packetized_serial_t zcm_trans_packetized_serial_t; @@ -69,13 +56,13 @@ struct zcm_trans_packetized_serial_t size_t inner_mtu; size_t mtu; + size_t max_message_size; uint8_t configured_packet_data_size; uint8_t* pkt_buf; size_t pkt_buf_size; uint8_t* out_buf; - size_t out_buf_size; size_t out_len; uint64_t out_utime; char out_channel[ZCM_CHANNEL_MAXLEN + 1]; @@ -91,50 +78,6 @@ struct zcm_trans_packetized_serial_t static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt); -static uint16_t read_u16_be(const uint8_t* p) -{ - return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; -} - -static uint32_t read_u32_be(const uint8_t* p) -{ - return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | - (uint32_t)p[3]; -} - -static void write_u16_be(uint8_t* p, uint16_t v) -{ - p[0] = (uint8_t)((v >> 8) & 0xff); - p[1] = (uint8_t)(v & 0xff); -} - -static void write_u32_be(uint8_t* p, uint32_t v) -{ - p[0] = (uint8_t)((v >> 24) & 0xff); - p[1] = (uint8_t)((v >> 16) & 0xff); - p[2] = (uint8_t)((v >> 8) & 0xff); - p[3] = (uint8_t)(v & 0xff); -} - -static uint32_t crc32_update_byte(uint32_t crc, uint8_t b) -{ - int i; - crc ^= b; - for (i = 0; i < 8; ++i) { - uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u)); - crc = (crc >> 1) ^ (0xedb88320u & mask); - } - return crc; -} - -static uint32_t crc32_compute(const uint8_t* data, size_t len) -{ - size_t i; - uint32_t crc = 0xffffffffu; - for (i = 0; i < len; ++i) crc = crc32_update_byte(crc, data[i]); - return ~crc; -} - static int send_inner_with_retry(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) { int i; @@ -160,7 +103,7 @@ static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, return ZCM_EINVALID; zt->pkt_buf[0] = type; - write_u16_be(&zt->pkt_buf[1], session_id); + packetized_write_u16_be(&zt->pkt_buf[1], session_id); zt->pkt_buf[3] = body_len; if (body_len > 0 && body != &zt->pkt_buf[PACKETIZED_HEADER_BYTES]) { memcpy(&zt->pkt_buf[PACKETIZED_HEADER_BYTES], body, body_len); @@ -176,17 +119,34 @@ static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, static void rx_clear(packetized_rx_state_t* rx) { - free(rx->data); - free(rx->packet_received); - free(rx->missing_ids); - memset(rx, 0, sizeof(*rx)); + /* Resets all session state to idle. Allocated buffers (data, packet_received, + * missing_ids) are kept intact and reused for the next session. */ + rx->active = 0; + rx->channel[0] = '\0'; + rx->session_id = 0; + rx->total_packets = 0; + rx->packet_data_size = 0; + rx->total_message_size = 0; + rx->expected_crc = 0; + rx->last_update_utime = 0; + rx->received_count = 0; + rx->retrans_pending = 0; + rx->missing_count = 0; + if (rx->packet_received && rx->packet_capacity > 0) + memset(rx->packet_received, 0, rx->packet_capacity); } static void tx_clear(packetized_tx_state_t* tx) { - free(tx->data); - free(tx->retrans_ids); - memset(tx, 0, sizeof(*tx)); + /* Resets all session state to idle. Allocated buffers (data, retrans_ids) + * are kept intact and reused for the next session. */ + tx->active = 0; + tx->channel[0] = '\0'; + tx->session_id = 0; + tx->total_packets = 0; + tx->packet_data_size = 0; + tx->total_message_size = 0; + tx->retrans_count = 0; } static int is_packetized_channel(const char* channel) @@ -214,7 +174,7 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; body[0] = (uint8_t)count; for (i = 0; i < count; ++i) { - write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); + packetized_write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); } int ret = send_packet(zt, rx->channel, rx->session_id, PACKETIZED_MSG_RETRANS_REQ, @@ -231,11 +191,8 @@ static int queue_output(zcm_trans_packetized_serial_t* zt, const char* channel, const uint8_t* data, size_t len, uint64_t utime, int strip_packetized_prefix) { - if (zt->out_buf_size < len) { - uint8_t* newbuf = realloc(zt->out_buf, len == 0 ? 1 : len); - if (newbuf == NULL) return ZCM_EMEMORY; - zt->out_buf = newbuf; - zt->out_buf_size = len; + if (zt->max_message_size < len) { + return ZCM_EINVALID; } if (len > 0) memcpy(zt->out_buf, data, len); @@ -263,6 +220,12 @@ static size_t packetized_max_mtu(uint8_t packet_data_size) return (size_t)packet_data_size * PACKETIZED_MAX_PACKETS; } +static size_t packet_capacity_for(size_t max_message_size, uint8_t packet_data_size) +{ + size_t packet_count = packetized_message_packet_count(max_message_size, packet_data_size); + return packet_count == 0 ? 1 : packet_count; +} + static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) { size_t i; @@ -280,7 +243,7 @@ static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) uint32_t remaining = tx->total_message_size - offset; uint8_t payload_len = (uint8_t)(remaining < chunk ? remaining : chunk); - write_u16_be(&body[0], packet_id); + packetized_write_u16_be(&body[0], packet_id); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); int ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -288,8 +251,6 @@ static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) if (ret != ZCM_EOK) return ret; } - free(tx->retrans_ids); - tx->retrans_ids = NULL; tx->retrans_count = 0; return ZCM_EOK; } @@ -300,30 +261,23 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann { if (body_len != PACKETIZED_METADATA_BODY_BYTES) return ZCM_EINVALID; - uint16_t total_packets = read_u16_be(&body[0]); + uint16_t total_packets = packetized_read_u16_be(&body[0]); uint8_t packet_data_size = body[2]; - uint32_t total_message_size = read_u32_be(&body[3]); - uint32_t expected_crc = read_u32_be(&body[7]); + uint32_t total_message_size = packetized_read_u32_be(&body[3]); + uint32_t expected_crc = packetized_read_u32_be(&body[7]); if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; if (packet_data_size > 253) return ZCM_EINVALID; - if (total_message_size > zt->mtu) return ZCM_EINVALID; + if (total_message_size > zt->max_message_size || total_message_size > zt->mtu) + return ZCM_EINVALID; - uint32_t expected_packets = - total_message_size == 0 ? 1 - : (total_message_size + (uint32_t)packet_data_size - 1u) / - (uint32_t)packet_data_size; + size_t expected_packets = packetized_message_packet_count(total_message_size, + packet_data_size); if (expected_packets != total_packets) return ZCM_EINVALID; rx_clear(&zt->rx); packetized_rx_state_t* rx = &zt->rx; - - rx->data = malloc(total_message_size == 0 ? 1 : total_message_size); - rx->packet_received = calloc(total_packets, sizeof(uint8_t)); - if (rx->data == NULL || rx->packet_received == NULL) { - rx_clear(rx); - return ZCM_EMEMORY; - } + if ((size_t)total_packets > rx->packet_capacity) return ZCM_EINVALID; strncpy(rx->channel, channel, ZCM_CHANNEL_MAXLEN); rx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; @@ -344,7 +298,7 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (!rx->active || rx->session_id != session_id) return ZCM_EOK; if (body_len < PACKETIZED_DATA_OVERHEAD_BYTES) return ZCM_EINVALID; - uint16_t packet_id = read_u16_be(&body[0]); + uint16_t packet_id = packetized_read_u16_be(&body[0]); uint8_t payload_len = (uint8_t)(body_len - PACKETIZED_DATA_OVERHEAD_BYTES); if (packet_id >= rx->total_packets) return ZCM_EINVALID; @@ -366,7 +320,7 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i rx->last_update_utime = utime; if (rx->received_count == rx->total_packets) { - uint32_t crc = crc32_compute(rx->data, rx->total_message_size); + uint32_t crc = packetized_crc32_compute(rx->data, rx->total_message_size); if (crc != rx->expected_crc) { rx_clear(rx); return ZCM_EINVALID; @@ -395,16 +349,13 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s uint8_t count = body[0]; if ((size_t)(1 + (size_t)count * 2) != body_len) return ZCM_EINVALID; - free(tx->retrans_ids); - tx->retrans_ids = NULL; tx->retrans_count = 0; if (count == 0) return ZCM_EOK; - tx->retrans_ids = malloc((size_t)count * sizeof(uint16_t)); - if (tx->retrans_ids == NULL) return ZCM_EMEMORY; + if ((size_t)count > tx->packet_capacity) return ZCM_EINVALID; tx->retrans_count = count; for (i = 0; i < count; ++i) { - tx->retrans_ids[i] = read_u16_be(&body[1 + (size_t)i * 2]); + tx->retrans_ids[i] = packetized_read_u16_be(&body[1 + (size_t)i * 2]); } return ZCM_EOK; @@ -423,9 +374,7 @@ static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t missing_count = (uint16_t)(rx->total_packets - rx->received_count); if (missing_count == 0) return; - free(rx->missing_ids); - rx->missing_ids = malloc((size_t)missing_count * sizeof(uint16_t)); - if (rx->missing_ids == NULL) return; + if ((size_t)missing_count > rx->packet_capacity) return; uint16_t idx = 0; for (i = 0; i < rx->total_packets; ++i) { @@ -435,9 +384,9 @@ static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, rx->retrans_pending = idx > 0; } -size_t packetized_serial_get_mtu(zcm_trans_packetized_serial_t* zt) { return zt->mtu; } +static size_t packetized_serial_get_mtu(zcm_trans_packetized_serial_t* zt) { return zt->mtu; } -int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) +static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) { uint16_t packet_id; size_t chan_len = strlen(msg.channel); @@ -456,22 +405,17 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) uint8_t packet_data_size = zt->configured_packet_data_size; if (packet_data_size == 0 || packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; - if (msg.len > zt->mtu) return ZCM_EINVALID; + if (msg.len > zt->mtu || msg.len > zt->max_message_size) return ZCM_EINVALID; uint32_t total_message_size = (uint32_t)msg.len; - size_t total_packets_sz = - (size_t)((total_message_size + packet_data_size - 1u) / packet_data_size); + size_t total_packets_sz = packetized_message_packet_count(total_message_size, + packet_data_size); if (total_packets_sz == 0) total_packets_sz = 1; if (total_packets_sz > PACKETIZED_MAX_PACKETS) return ZCM_EINVALID; uint16_t total_packets = (uint16_t)total_packets_sz; tx_clear(&zt->tx); packetized_tx_state_t* tx = &zt->tx; - tx->data = malloc(msg.len == 0 ? 1 : msg.len); - if (tx->data == NULL) { - tx_clear(tx); - return ZCM_EMEMORY; - } if (msg.len > 0) memcpy(tx->data, msg.buf, msg.len); tx->active = 1; @@ -483,10 +427,10 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) tx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - write_u16_be(&meta[0], total_packets); + packetized_write_u16_be(&meta[0], total_packets); meta[2] = packet_data_size; - write_u32_be(&meta[3], total_message_size); - write_u32_be(&meta[7], crc32_compute(msg.buf, msg.len)); + packetized_write_u32_be(&meta[3], total_message_size); + packetized_write_u32_be(&meta[7], packetized_crc32_compute(msg.buf, msg.len)); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, PACKETIZED_METADATA_BODY_BYTES); if (ret != ZCM_EOK) return ret; @@ -498,7 +442,7 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) uint8_t payload_len = (uint8_t)(remaining < packet_data_size ? remaining : packet_data_size); - write_u16_be(&body[0], packet_id); + packetized_write_u16_be(&body[0], packet_id); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -509,7 +453,7 @@ int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) return ZCM_EOK; } -int packetized_serial_recvmsg_enable(zcm_trans_packetized_serial_t* zt, +static int packetized_serial_recvmsg_enable(zcm_trans_packetized_serial_t* zt, const char* channel, bool enable) { return zcm_trans_recvmsg_enable(zt->inner, channel, enable); @@ -529,7 +473,7 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) if (in.len < PACKETIZED_HEADER_BYTES) continue; { uint8_t type = in.buf[0]; - uint16_t session_id = read_u16_be(&in.buf[1]); + uint16_t session_id = packetized_read_u16_be(&in.buf[1]); uint8_t body_len = in.buf[3]; if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; @@ -553,7 +497,7 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) return ZCM_EOK; } -int packetized_serial_recvmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t* msg, +static int packetized_serial_recvmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_t* msg, unsigned timeoutMs) { (void)timeoutMs; @@ -647,7 +591,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, - uint8_t packet_data_size) + uint8_t packet_data_size, size_t max_message_size) { zcm_trans_packetized_serial_t* zt = calloc(1, sizeof(*zt)); if (zt == NULL) return NULL; @@ -667,6 +611,9 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } + if (max_message_size == 0) max_message_size = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; + zt->max_message_size = max_message_size; + if (packet_data_size == 0) { size_t max_payload = zt->inner_mtu - PACKETIZED_HEADER_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES; @@ -693,6 +640,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( } zt->mtu = packetized_max_mtu(zt->configured_packet_data_size); + if (zt->mtu > zt->max_message_size) zt->mtu = zt->max_message_size; zt->pkt_buf_size = zt->inner_mtu; zt->pkt_buf = malloc(zt->pkt_buf_size); @@ -702,13 +650,42 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } - zt->out_buf = NULL; - zt->out_buf_size = 0; + zt->out_buf = malloc(zt->max_message_size == 0 ? 1 : zt->max_message_size); + if (zt->out_buf == NULL) { + free(zt->pkt_buf); + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } zt->out_len = 0; zt->out_utime = 0; zt->out_channel[0] = '\0'; zt->out_pending = 0; + zt->rx.packet_capacity = packet_capacity_for(zt->max_message_size, + zt->configured_packet_data_size); + zt->rx.data = malloc(zt->max_message_size == 0 ? 1 : zt->max_message_size); + zt->rx.packet_received = calloc(zt->rx.packet_capacity, sizeof(uint8_t)); + zt->rx.missing_ids = malloc(zt->rx.packet_capacity * sizeof(uint16_t)); + zt->tx.data = malloc(zt->max_message_size == 0 ? 1 : zt->max_message_size); + zt->tx.packet_capacity = zt->rx.packet_capacity; + zt->tx.retrans_ids = malloc(zt->tx.packet_capacity * sizeof(uint16_t)); + if (zt->rx.data == NULL || zt->rx.packet_received == NULL || zt->rx.missing_ids == NULL || + zt->tx.data == NULL || zt->tx.retrans_ids == NULL) { + free(zt->tx.retrans_ids); + free(zt->tx.data); + free(zt->rx.missing_ids); + free(zt->rx.packet_received); + free(zt->rx.data); + free(zt->out_buf); + free(zt->pkt_buf); + zcm_trans_generic_serial_destroy(zt->inner); + free(zt); + return NULL; + } + rx_clear(&zt->rx); + tx_clear(&zt->tx); + zt->time = timestamp_now; zt->time_usr = time_usr; @@ -723,7 +700,10 @@ void zcm_trans_packetized_serial_destroy(zcm_trans_t* _zt) if (zt->inner) zcm_trans_generic_serial_destroy(zt->inner); free(zt->pkt_buf); free(zt->out_buf); - rx_clear(&zt->rx); - tx_clear(&zt->tx); + free(zt->rx.missing_ids); + free(zt->rx.packet_received); + free(zt->rx.data); + free(zt->tx.retrans_ids); + free(zt->tx.data); free(zt); } diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index c48af982e..51eab2b9f 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -14,7 +14,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, - uint8_t packet_data_size); + uint8_t packet_data_size, size_t max_message_size); void zcm_trans_packetized_serial_destroy(zcm_trans_t* zt); @@ -25,4 +25,35 @@ int packetized_serial_update_tx(zcm_trans_t* zt); } #endif +#ifdef __cplusplus +#include +#include + +// Parse a "pkt_size" URL option value into a packet_data_size byte. +// Returns true and sets out on success; returns false on invalid input. +// Valid range is [1, 253]. +static inline bool parsePacketDataSize(const std::string* opt, uint8_t& out) +{ + if (!opt) { out = 0; return true; } + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) return false; + out = (uint8_t)parsed; + return true; +} + +// Parse a "pkt_buf_size" URL option value into a max_message_size. +// Returns true and sets out on success; returns false on invalid input. +// A null opt leaves out unchanged (caller should pre-set the default). +static inline bool parsePacketBufSize(const std::string* opt, size_t& out) +{ + if (!opt) return true; + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0) return false; + out = (size_t)parsed; + return true; +} +#endif /* __cplusplus */ + #endif /* _ZCM_TRANS_PACKETIZED_SERIAL_H */ diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 772607b2f..4e60bfc07 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -31,6 +31,7 @@ // Define this the class name you want #define ZCM_TRANS_CLASSNAME TransportCan #define MTU (1<<14) +#define PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT (1024) using namespace std; @@ -40,6 +41,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t uint32_t msgId; uint32_t txId; uint8_t packetDataSize; + size_t packetizedMaxMessageSize; string address; int soc = -1; @@ -75,16 +77,15 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t msgId = 0; packetDataSize = 0; + packetizedMaxMessageSize = PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT; - auto* pktSizeStr = findOption("pkt_size"); - if (pktSizeStr) { - char* endptr; - unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0 || parsed > 253) { - ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); - return; - } - packetDataSize = (uint8_t)parsed; + if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { + ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); + return; + } + if (!parsePacketBufSize(findOption("pkt_buf_size"), packetizedMaxMessageSize)) { + ZCM_DEBUG("Invalid pkt_buf_size. Expected positive integer"); + return; } auto* msgIdStr = findOption("msgid"); @@ -177,7 +178,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10, - packetDataSize); + packetDataSize, + packetizedMaxMessageSize); } else { gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, @@ -392,6 +394,6 @@ static zcm_trans_t *create(zcm_url_t* url, char **opt_errmsg) #ifdef USING_TRANS_CAN const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can", "Transfer data via a socket CAN connection on a single id " - "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " - "'can://can0?msgid=65536&pkt_size=8')", create); + "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " + "'can://can0?msgid=65536&pkt_size=8&pkt_buf_size=1024')", create); #endif diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index 2d940e04a..ca695d420 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -33,6 +33,7 @@ using namespace std; // Define this the class name you want #define ZCM_TRANS_CLASSNAME TransportSerial #define MTU (1<<14) +#define PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT (1024) #define ESCAPE_CHAR (0xcc) #define SERIAL_TIMEOUT_US 1e5 // u-seconds @@ -243,8 +244,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t int baud; bool hwFlowControl; - bool packetized; uint8_t packetDataSize; + size_t packetizedMaxMessageSize; bool raw; string rawChan; @@ -301,18 +302,15 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } } - packetized = false; packetDataSize = 0; - auto* pktSizeStr = findOption("pkt_size"); - if (pktSizeStr) { - char* endptr; - unsigned long parsed = strtoul(pktSizeStr->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0 || parsed > 253) { - ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); - return; - } - packetized = true; - packetDataSize = (uint8_t)parsed; + packetizedMaxMessageSize = PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT; + if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { + ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); + return; + } + if (!parsePacketBufSize(findOption("pkt_buf_size"), packetizedMaxMessageSize)) { + ZCM_DEBUG("expected positive integer argument for 'pkt_buf_size'"); + return; } raw = false; @@ -327,7 +325,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return; } } - if (raw && packetized) { + if (raw && packetDataSize != 0) { ZCM_DEBUG("'raw' and 'pkt_size' options are mutually exclusive"); return; } @@ -354,14 +352,15 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t if (raw) { rawBuf.reset(new uint8_t[rawSize]); gst = nullptr; - } else if (packetized) { + } else if (packetDataSize != 0) { gst = zcm_trans_packetized_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10, - packetDataSize); + packetDataSize, + packetizedMaxMessageSize); } else { gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, @@ -518,8 +517,8 @@ static zcm_trans_t* create(zcm_url_t* url, char **opt_errmsg) // Register this transport with ZCM const TransportRegister ZCM_TRANS_CLASSNAME::reg( "serial", "Transfer data via a serial connection " - "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true', " - "'serial:///dev/ttyUSB0?baud=115200&pkt_size=128', or " - "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", + "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true', " + "'serial:///dev/ttyUSB0?baud=115200&pkt_size=128&pkt_buf_size=1024', or " + "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", create); #endif diff --git a/zcm/wscript b/zcm/wscript index 53638cc8f..43cba2d90 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -47,6 +47,7 @@ def build(ctx): 'zcm_coretypes.h', 'transport.h', 'nonblocking.h', 'nonblocking.c', 'transport/generic_serial_transport.h', 'transport/generic_serial_transport.c', + 'transport/packetized_serial_protocol.h', 'transport/packetized_serial_transport.h', 'transport/packetized_serial_transport.c', 'transport/generic_serial_circ_buff.h', @@ -104,10 +105,11 @@ def build(ctx): ['json/json.h', 'json/json-forwards.h']) ctx.install_files('${PREFIX}/include/zcm/transport', - ['transport/generic_serial_transport.h', - 'transport/packetized_serial_transport.h', - 'transport/generic_serial_circ_buff.h', - 'transport/generic_serial_fletcher.h']) + ['transport/generic_serial_transport.h', + 'transport/packetized_serial_protocol.h', + 'transport/packetized_serial_transport.h', + 'transport/generic_serial_circ_buff.h', + 'transport/generic_serial_fletcher.h']) ctx.install_files('${PREFIX}/share/embedded', ['zcm-embed.tar.gz']) From 477d7c88744a3ea198ba1664a5673191528a4819 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 9 Apr 2026 10:04:21 -0400 Subject: [PATCH 07/20] final_final_v5 --- test/zcm/PacketizedSerialTransportTest.hpp | 72 +++++++++++++++++++++ zcm/transport/packetized_serial_transport.c | 27 +++----- zcm/transport/packetized_serial_transport.h | 32 +-------- zcm/transport/transport_can.cpp | 26 +++++++- zcm/transport/transport_serial.cpp | 26 +++++++- 5 files changed, 130 insertions(+), 53 deletions(-) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index b25db88c3..c381bb006 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -276,6 +276,78 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_trans_destroy(tx); zcm_trans_destroy(rx); } + + void testAsymmetricPacketSizesRoundTrip() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 4000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 8, 1024); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 32, 1024); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(512); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0x33); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$ASYM"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 40); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("ASYM")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + + void testAsymmetricPacketSizesReverseRoundTrip() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 5000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 32, 1024); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 8, 1024); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(512); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0x77); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$ASYM_REV"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 40); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("ASYM_REV")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } }; #endif diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 22105823c..ed7ebc25f 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -121,19 +121,19 @@ static void rx_clear(packetized_rx_state_t* rx) { /* Resets all session state to idle. Allocated buffers (data, packet_received, * missing_ids) are kept intact and reused for the next session. */ - rx->active = 0; - rx->channel[0] = '\0'; - rx->session_id = 0; - rx->total_packets = 0; - rx->packet_data_size = 0; + if (rx->packet_received && rx->total_packets > 0) + memset(rx->packet_received, 0, rx->total_packets); + rx->active = 0; + rx->channel[0] = '\0'; + rx->session_id = 0; + rx->total_packets = 0; + rx->packet_data_size = 0; rx->total_message_size = 0; rx->expected_crc = 0; rx->last_update_utime = 0; rx->received_count = 0; rx->retrans_pending = 0; rx->missing_count = 0; - if (rx->packet_received && rx->packet_capacity > 0) - memset(rx->packet_received, 0, rx->packet_capacity); } static void tx_clear(packetized_tx_state_t* tx) @@ -220,12 +220,6 @@ static size_t packetized_max_mtu(uint8_t packet_data_size) return (size_t)packet_data_size * PACKETIZED_MAX_PACKETS; } -static size_t packet_capacity_for(size_t max_message_size, uint8_t packet_data_size) -{ - size_t packet_count = packetized_message_packet_count(max_message_size, packet_data_size); - return packet_count == 0 ? 1 : packet_count; -} - static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) { size_t i; @@ -267,7 +261,7 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann uint32_t expected_crc = packetized_read_u32_be(&body[7]); if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; - if (packet_data_size > 253) return ZCM_EINVALID; + if (packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; if (total_message_size > zt->max_message_size || total_message_size > zt->mtu) return ZCM_EINVALID; @@ -374,8 +368,6 @@ static void maybe_schedule_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t missing_count = (uint16_t)(rx->total_packets - rx->received_count); if (missing_count == 0) return; - if ((size_t)missing_count > rx->packet_capacity) return; - uint16_t idx = 0; for (i = 0; i < rx->total_packets; ++i) { if (!rx->packet_received[i]) rx->missing_ids[idx++] = i; @@ -662,8 +654,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( zt->out_channel[0] = '\0'; zt->out_pending = 0; - zt->rx.packet_capacity = packet_capacity_for(zt->max_message_size, - zt->configured_packet_data_size); + zt->rx.packet_capacity = zt->max_message_size; zt->rx.data = malloc(zt->max_message_size == 0 ? 1 : zt->max_message_size); zt->rx.packet_received = calloc(zt->rx.packet_capacity, sizeof(uint8_t)); zt->rx.missing_ids = malloc(zt->rx.packet_capacity * sizeof(uint16_t)); diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index 51eab2b9f..a0603e142 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -9,6 +9,7 @@ extern "C" { #include "zcm/transport.h" #include "zcm/zcm.h" +#include "packetized_serial_protocol.h" zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), @@ -25,35 +26,4 @@ int packetized_serial_update_tx(zcm_trans_t* zt); } #endif -#ifdef __cplusplus -#include -#include - -// Parse a "pkt_size" URL option value into a packet_data_size byte. -// Returns true and sets out on success; returns false on invalid input. -// Valid range is [1, 253]. -static inline bool parsePacketDataSize(const std::string* opt, uint8_t& out) -{ - if (!opt) { out = 0; return true; } - char* endptr; - unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0 || parsed > 253) return false; - out = (uint8_t)parsed; - return true; -} - -// Parse a "pkt_buf_size" URL option value into a max_message_size. -// Returns true and sets out on success; returns false on invalid input. -// A null opt leaves out unchanged (caller should pre-set the default). -static inline bool parsePacketBufSize(const std::string* opt, size_t& out) -{ - if (!opt) return true; - char* endptr; - unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0) return false; - out = (size_t)parsed; - return true; -} -#endif /* __cplusplus */ - #endif /* _ZCM_TRANS_PACKETIZED_SERIAL_H */ diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 4e60bfc07..1aa3a790e 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -31,10 +31,32 @@ // Define this the class name you want #define ZCM_TRANS_CLASSNAME TransportCan #define MTU (1<<14) -#define PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT (1024) using namespace std; +static bool parsePacketDataSize(const string* opt, uint8_t& out) +{ + if (!opt) { + out = 0; + return true; + } + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) return false; + out = (uint8_t)parsed; + return true; +} + +static bool parsePacketBufSize(const string* opt, size_t& out) +{ + if (!opt) return true; + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0) return false; + out = (size_t)parsed; + return true; +} + struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { unordered_map options; @@ -77,7 +99,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t msgId = 0; packetDataSize = 0; - packetizedMaxMessageSize = PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT; + packetizedMaxMessageSize = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index ca695d420..a37834281 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -33,7 +33,6 @@ using namespace std; // Define this the class name you want #define ZCM_TRANS_CLASSNAME TransportSerial #define MTU (1<<14) -#define PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT (1024) #define ESCAPE_CHAR (0xcc) #define SERIAL_TIMEOUT_US 1e5 // u-seconds @@ -45,6 +44,29 @@ using u16 = uint16_t; using u32 = uint32_t; using u64 = uint64_t; +static bool parsePacketDataSize(const string* opt, uint8_t& out) +{ + if (!opt) { + out = 0; + return true; + } + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0 || parsed > 253) return false; + out = (uint8_t)parsed; + return true; +} + +static bool parsePacketBufSize(const string* opt, size_t& out) +{ + if (!opt) return true; + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0) return false; + out = (size_t)parsed; + return true; +} + struct Serial { Serial(){} @@ -303,7 +325,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } packetDataSize = 0; - packetizedMaxMessageSize = PACKETIZED_MAX_MESSAGE_SIZE_DEFAULT; + packetizedMaxMessageSize = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); return; From 4c747cc24a4fd7242038c93660de9673e5c8000d Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 9 Apr 2026 11:30:34 -0400 Subject: [PATCH 08/20] Fix vtable assert --- test/zcm/PacketizedSerialTransportTest.hpp | 49 ++++++++++++++++++++++ zcm/transport/transport_can.cpp | 10 ++++- zcm/transport/transport_serial.cpp | 10 ++++- 3 files changed, 65 insertions(+), 4 deletions(-) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index c381bb006..dbcb35514 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -147,6 +147,15 @@ static void pump(zcm_trans_t* tx, zcm_trans_t* rx, int iters) } } +// pump via zcm_trans_update (the vtable path used by TransportSerial/TransportCan wrappers) +static void pumpViaUpdate(zcm_trans_t* tx, zcm_trans_t* rx, int iters) +{ + for (int i = 0; i < iters; ++i) { + TS_ASSERT_EQUALS(zcm_trans_update(tx), ZCM_EOK); + TS_ASSERT_EQUALS(zcm_trans_update(rx), ZCM_EOK); + } +} + class PacketizedSerialTransportTest : public CxxTest::TestSuite { public: @@ -348,6 +357,46 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_trans_destroy(tx); zcm_trans_destroy(rx); } + + // Drive the transport via zcm_trans_update (the vtable update path) rather than the + // explicit packetized_serial_update_rx/tx functions. This mirrors how the + // TransportSerial and TransportCan wrappers operate and would have caught the + // assertion failure caused by calling serial_update_rx/tx on a packetized transport. + void testRoundTripViaVtableUpdate() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 6000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 8, 1024); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 8, 1024); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(256); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0xab); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$VTABLE"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pumpViaUpdate(tx, rx, 40); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("VTABLE")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } }; #endif diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 1aa3a790e..44dcd2542 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -72,6 +72,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t struct ifreq ifr; zcm_trans_t* gst = nullptr; + int (*gst_update_rx)(zcm_trans_t*) = nullptr; + int (*gst_update_tx)(zcm_trans_t*) = nullptr; uint64_t recvTimeoutUs = 0; uint64_t recvMsgStartUtime = 0; @@ -202,6 +204,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t MTU, MTU * 10, packetDataSize, packetizedMaxMessageSize); + gst_update_rx = packetized_serial_update_rx; + gst_update_tx = packetized_serial_update_tx; } else { gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, @@ -209,6 +213,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t &ZCM_TRANS_CLASSNAME::timestamp_now, this, MTU, MTU * 10); + gst_update_rx = serial_update_rx; + gst_update_tx = serial_update_tx; } if (!gst) return; socSettingsGood = true; @@ -327,7 +333,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { int ret = zcm_trans_sendmsg(this->gst, msg); if (ret != ZCM_EOK) return ret; - return serial_update_tx(this->gst); + return this->gst_update_tx(this->gst); } int recvmsgEnable(const char* channel, bool enable) @@ -359,7 +365,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return ZCM_EUNKNOWN; } - serial_update_rx(this->gst); + this->gst_update_rx(this->gst); } while (true); return ZCM_EAGAIN; } diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index a37834281..e5703b6d2 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -279,6 +279,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t unordered_map options; zcm_trans_t* gst; + int (*gst_update_rx)(zcm_trans_t*) = nullptr; + int (*gst_update_tx)(zcm_trans_t*) = nullptr; uint64_t timeoutLeftUs; @@ -383,6 +385,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t MTU, MTU * 10, packetDataSize, packetizedMaxMessageSize); + gst_update_rx = packetized_serial_update_rx; + gst_update_tx = packetized_serial_update_tx; } else { gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, @@ -390,6 +394,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10); + gst_update_rx = serial_update_rx; + gst_update_tx = serial_update_tx; } } @@ -439,7 +445,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t // and touch no variables related to receiving int ret = zcm_trans_sendmsg(this->gst, msg); if (ret != ZCM_EOK) return ret; - return serial_update_tx(this->gst); + return this->gst_update_tx(this->gst); } } @@ -477,7 +483,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t // `get` knows how long it has to exit timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; - serial_update_rx(this->gst); + this->gst_update_rx(this->gst); diff = TimeUtil::utime() - startUtime; timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; From c419526027fdf22932afab1af98432d97fd6fe3d Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Fri, 10 Apr 2026 10:52:26 -0400 Subject: [PATCH 09/20] A little more cleanup --- test/zcm/PacketizedSerialTransportTest.hpp | 2 +- zcm/transport/generic_serial_fletcher.h | 8 ++++ zcm/transport/packetized_serial_protocol.h | 49 ++------------------- zcm/transport/packetized_serial_transport.c | 36 +++++++-------- zcm/transport/transport_can.cpp | 8 +--- zcm/transport/transport_serial.cpp | 8 +--- zcm/util/byteorder.h | 39 ++++++++++++++++ zcm/wscript | 4 +- 8 files changed, 75 insertions(+), 79 deletions(-) create mode 100644 zcm/util/byteorder.h diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index dbcb35514..53210c688 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -112,7 +112,7 @@ static size_t endpoint_put(const uint8_t* data, size_t nData, void* usr) if (type == PACKETIZED_MSG_DATA && payload.size() == (size_t)PACKETIZED_HEADER_BYTES + body_len && body_len >= PACKETIZED_DATA_OVERHEAD_BYTES) { - uint16_t packet_id = packetized_read_u16_be( + uint16_t packet_id = zcm_read_u16_be( &payload[PACKETIZED_HEADER_BYTES]); if (packet_id == ep->dropPacketId) { drop = true; diff --git a/zcm/transport/generic_serial_fletcher.h b/zcm/transport/generic_serial_fletcher.h index f06e7de8a..a3637b111 100644 --- a/zcm/transport/generic_serial_fletcher.h +++ b/zcm/transport/generic_serial_fletcher.h @@ -19,4 +19,12 @@ static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) return (sumHigh << 8) | sumLow; } +static inline uint16_t fletcher16(const uint8_t* data, size_t len) +{ + size_t i; + uint16_t sum = 0xffff; + for (i = 0; i < len; ++i) sum = fletcherUpdate(data[i], sum); + return sum; +} + #endif /* _ZCM_TRANS_NONBLOCKING_FLETCHER_H */ diff --git a/zcm/transport/packetized_serial_protocol.h b/zcm/transport/packetized_serial_protocol.h index 225247e6a..60b50d39b 100644 --- a/zcm/transport/packetized_serial_protocol.h +++ b/zcm/transport/packetized_serial_protocol.h @@ -4,9 +4,12 @@ #include #include +#include "generic_serial_fletcher.h" +#include "zcm/util/byteorder.h" + #define PACKETIZED_HEADER_BYTES (4) #define PACKETIZED_DATA_OVERHEAD_BYTES (2) -#define PACKETIZED_METADATA_BODY_BYTES (11) +#define PACKETIZED_METADATA_BODY_BYTES (9) #define PACKETIZED_RETRANS_TIMEOUT_US (200000) #define PACKETIZED_MAX_PACKET_DATA_SIZE (253) #define PACKETIZED_MAX_PACKETS ((size_t)UINT16_MAX) @@ -19,50 +22,6 @@ typedef enum packetized_msg_type_t PACKETIZED_MSG_RETRANS_REQ = 3, } packetized_msg_type_t; -static inline uint16_t packetized_read_u16_be(const uint8_t* p) -{ - return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; -} - -static inline uint32_t packetized_read_u32_be(const uint8_t* p) -{ - return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | - (uint32_t)p[3]; -} - -static inline void packetized_write_u16_be(uint8_t* p, uint16_t v) -{ - p[0] = (uint8_t)((v >> 8) & 0xff); - p[1] = (uint8_t)(v & 0xff); -} - -static inline void packetized_write_u32_be(uint8_t* p, uint32_t v) -{ - p[0] = (uint8_t)((v >> 24) & 0xff); - p[1] = (uint8_t)((v >> 16) & 0xff); - p[2] = (uint8_t)((v >> 8) & 0xff); - p[3] = (uint8_t)(v & 0xff); -} - -static inline uint32_t packetized_crc32_update_byte(uint32_t crc, uint8_t b) -{ - int i; - crc ^= b; - for (i = 0; i < 8; ++i) { - uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u)); - crc = (crc >> 1) ^ (0xedb88320u & mask); - } - return crc; -} - -static inline uint32_t packetized_crc32_compute(const uint8_t* data, size_t len) -{ - size_t i; - uint32_t crc = 0xffffffffu; - for (i = 0; i < len; ++i) crc = packetized_crc32_update_byte(crc, data[i]); - return ~crc; -} - static inline size_t packetized_message_packet_count(size_t message_size, uint8_t packet_data_size) { diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index ed7ebc25f..627a431ba 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -19,7 +19,7 @@ struct packetized_rx_state_t uint16_t total_packets; uint8_t packet_data_size; uint32_t total_message_size; - uint32_t expected_crc; + uint16_t expected_checksum; uint64_t last_update_utime; uint8_t* data; @@ -103,7 +103,7 @@ static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, return ZCM_EINVALID; zt->pkt_buf[0] = type; - packetized_write_u16_be(&zt->pkt_buf[1], session_id); + zcm_write_u16_be(&zt->pkt_buf[1], session_id); zt->pkt_buf[3] = body_len; if (body_len > 0 && body != &zt->pkt_buf[PACKETIZED_HEADER_BYTES]) { memcpy(&zt->pkt_buf[PACKETIZED_HEADER_BYTES], body, body_len); @@ -129,7 +129,7 @@ static void rx_clear(packetized_rx_state_t* rx) rx->total_packets = 0; rx->packet_data_size = 0; rx->total_message_size = 0; - rx->expected_crc = 0; + rx->expected_checksum = 0; rx->last_update_utime = 0; rx->received_count = 0; rx->retrans_pending = 0; @@ -174,7 +174,7 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; body[0] = (uint8_t)count; for (i = 0; i < count; ++i) { - packetized_write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); + zcm_write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); } int ret = send_packet(zt, rx->channel, rx->session_id, PACKETIZED_MSG_RETRANS_REQ, @@ -237,7 +237,7 @@ static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) uint32_t remaining = tx->total_message_size - offset; uint8_t payload_len = (uint8_t)(remaining < chunk ? remaining : chunk); - packetized_write_u16_be(&body[0], packet_id); + zcm_write_u16_be(&body[0], packet_id); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); int ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -255,10 +255,10 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann { if (body_len != PACKETIZED_METADATA_BODY_BYTES) return ZCM_EINVALID; - uint16_t total_packets = packetized_read_u16_be(&body[0]); + uint16_t total_packets = zcm_read_u16_be(&body[0]); uint8_t packet_data_size = body[2]; - uint32_t total_message_size = packetized_read_u32_be(&body[3]); - uint32_t expected_crc = packetized_read_u32_be(&body[7]); + uint32_t total_message_size = zcm_read_u32_be(&body[3]); + uint16_t expected_checksum = zcm_read_u16_be(&body[7]); if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; if (packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; @@ -280,7 +280,7 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann rx->total_packets = total_packets; rx->packet_data_size = packet_data_size; rx->total_message_size = total_message_size; - rx->expected_crc = expected_crc; + rx->expected_checksum = expected_checksum; rx->last_update_utime = utime; return ZCM_EOK; } @@ -292,7 +292,7 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (!rx->active || rx->session_id != session_id) return ZCM_EOK; if (body_len < PACKETIZED_DATA_OVERHEAD_BYTES) return ZCM_EINVALID; - uint16_t packet_id = packetized_read_u16_be(&body[0]); + uint16_t packet_id = zcm_read_u16_be(&body[0]); uint8_t payload_len = (uint8_t)(body_len - PACKETIZED_DATA_OVERHEAD_BYTES); if (packet_id >= rx->total_packets) return ZCM_EINVALID; @@ -314,8 +314,8 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i rx->last_update_utime = utime; if (rx->received_count == rx->total_packets) { - uint32_t crc = packetized_crc32_compute(rx->data, rx->total_message_size); - if (crc != rx->expected_crc) { + uint16_t checksum = fletcher16(rx->data, rx->total_message_size); + if (checksum != rx->expected_checksum) { rx_clear(rx); return ZCM_EINVALID; } @@ -349,7 +349,7 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s if ((size_t)count > tx->packet_capacity) return ZCM_EINVALID; tx->retrans_count = count; for (i = 0; i < count; ++i) { - tx->retrans_ids[i] = packetized_read_u16_be(&body[1 + (size_t)i * 2]); + tx->retrans_ids[i] = zcm_read_u16_be(&body[1 + (size_t)i * 2]); } return ZCM_EOK; @@ -419,10 +419,10 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ tx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - packetized_write_u16_be(&meta[0], total_packets); + zcm_write_u16_be(&meta[0], total_packets); meta[2] = packet_data_size; - packetized_write_u32_be(&meta[3], total_message_size); - packetized_write_u32_be(&meta[7], packetized_crc32_compute(msg.buf, msg.len)); + zcm_write_u32_be(&meta[3], total_message_size); + zcm_write_u16_be(&meta[7], fletcher16(msg.buf, msg.len)); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, PACKETIZED_METADATA_BODY_BYTES); if (ret != ZCM_EOK) return ret; @@ -434,7 +434,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t payload_len = (uint8_t)(remaining < packet_data_size ? remaining : packet_data_size); - packetized_write_u16_be(&body[0], packet_id); + zcm_write_u16_be(&body[0], packet_id); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -465,7 +465,7 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) if (in.len < PACKETIZED_HEADER_BYTES) continue; { uint8_t type = in.buf[0]; - uint16_t session_id = packetized_read_u16_be(&in.buf[1]); + uint16_t session_id = zcm_read_u16_be(&in.buf[1]); uint8_t body_len = in.buf[3]; if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 44dcd2542..18bcf01d1 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -63,7 +63,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t uint32_t msgId; uint32_t txId; uint8_t packetDataSize; - size_t packetizedMaxMessageSize; string address; int soc = -1; @@ -101,16 +100,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t msgId = 0; packetDataSize = 0; - packetizedMaxMessageSize = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); return; } - if (!parsePacketBufSize(findOption("pkt_buf_size"), packetizedMaxMessageSize)) { - ZCM_DEBUG("Invalid pkt_buf_size. Expected positive integer"); - return; - } auto* msgIdStr = findOption("msgid"); if (!msgIdStr) { @@ -203,7 +197,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t this, MTU, MTU * 10, packetDataSize, - packetizedMaxMessageSize); + PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index e5703b6d2..4caa9e2bd 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -267,7 +267,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t int baud; bool hwFlowControl; uint8_t packetDataSize; - size_t packetizedMaxMessageSize; bool raw; string rawChan; @@ -327,15 +326,10 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } packetDataSize = 0; - packetizedMaxMessageSize = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); return; } - if (!parsePacketBufSize(findOption("pkt_buf_size"), packetizedMaxMessageSize)) { - ZCM_DEBUG("expected positive integer argument for 'pkt_buf_size'"); - return; - } raw = false; auto* rawStr = findOption("raw"); @@ -384,7 +378,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t nullptr, MTU, MTU * 10, packetDataSize, - packetizedMaxMessageSize); + PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { diff --git a/zcm/util/byteorder.h b/zcm/util/byteorder.h new file mode 100644 index 000000000..9d025ab74 --- /dev/null +++ b/zcm/util/byteorder.h @@ -0,0 +1,39 @@ +#ifndef __ZCM_BYTEORDER_H__ +#define __ZCM_BYTEORDER_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +static inline uint16_t zcm_read_u16_be(const uint8_t* p) +{ + return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; +} + +static inline uint32_t zcm_read_u32_be(const uint8_t* p) +{ + return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | + ((uint32_t)p[2] << 8) | (uint32_t)p[3]; +} + +static inline void zcm_write_u16_be(uint8_t* p, uint16_t v) +{ + p[0] = (uint8_t)((v >> 8) & 0xff); + p[1] = (uint8_t)(v & 0xff); +} + +static inline void zcm_write_u32_be(uint8_t* p, uint32_t v) +{ + p[0] = (uint8_t)((v >> 24) & 0xff); + p[1] = (uint8_t)((v >> 16) & 0xff); + p[2] = (uint8_t)((v >> 8) & 0xff); + p[3] = (uint8_t)(v & 0xff); +} + +#ifdef __cplusplus +} +#endif + +#endif /* __ZCM_BYTEORDER_H__ */ diff --git a/zcm/wscript b/zcm/wscript index 43cba2d90..f78961817 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -52,7 +52,8 @@ def build(ctx): 'transport/packetized_serial_transport.c', 'transport/generic_serial_circ_buff.h', 'transport/generic_serial_circ_buff.c', - 'transport/generic_serial_fletcher.h'] + 'transport/generic_serial_fletcher.h', + 'util/byteorder.h'] if ctx.env.USING_THIRD_PARTY: embedSource.append('transport/third-party/embedded/**') @@ -97,6 +98,7 @@ def build(ctx): ['tools/IndexerPlugin.hpp', 'tools/TranscoderPlugin.hpp']) + ctx.install_files('${PREFIX}/include/zcm/util', 'util/byteorder.h') ctx.install_files('${PREFIX}/include/zcm/util', 'util/debug.h') ctx.install_files('${PREFIX}/include/zcm/util', 'util/Filter.hpp') ctx.install_files('${PREFIX}/include/zcm/util', 'util/topology.hpp') From 74fc6e14bdc1e0e85900d4e1870b1c7d3f1486e4 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Fri, 10 Apr 2026 10:56:18 -0400 Subject: [PATCH 10/20] Final_final_final_v6 --- docs/transports.md | 4 ++-- zcm/transport/packetized_serial_transport.h | 11 +++++++++++ zcm/transport/transport_can.cpp | 12 +----------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/docs/transports.md b/docs/transports.md index 80edd102f..9b832acad 100644 --- a/docs/transports.md +++ b/docs/transports.md @@ -53,8 +53,8 @@ be used to *summon* the transport: CAN - can://<interface>?msgid=<id>[&pkt_size=<n>&pkt_buf_size=<n>] - zcm_create("can://can0?msgid=65536"), zcm_create("can://can0?msgid=65536&pkt_size=32&pkt_buf_size=1024") + can://<interface>?msgid=<id>[&pkt_size=<n>] + zcm_create("can://can0?msgid=65536"), zcm_create("can://can0?msgid=65536&pkt_size=32") Inter-process via Shared Memory (IPCSHM) diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index a0603e142..ac81ea31b 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -11,6 +11,17 @@ extern "C" { #include "zcm/zcm.h" #include "packetized_serial_protocol.h" +/* + * Channel name convention + * ----------------------- + * The packetized transport uses the leading '$' character as a signal that a + * message should be fragmented and reassembled using the packetization protocol. + * Any channel whose name begins with '$' is treated as a packetized channel; + * the '$' is stripped from the channel name on delivery to the receiver. + * Channels without a leading '$' are passed through to the underlying generic + * serial transport unchanged and are not packetized. + */ + zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 18bcf01d1..dd3875973 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -47,16 +47,6 @@ static bool parsePacketDataSize(const string* opt, uint8_t& out) return true; } -static bool parsePacketBufSize(const string* opt, size_t& out) -{ - if (!opt) return true; - char* endptr; - unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0) return false; - out = (size_t)parsed; - return true; -} - struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { unordered_map options; @@ -417,5 +407,5 @@ static zcm_trans_t *create(zcm_url_t* url, char **opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can", "Transfer data via a socket CAN connection on a single id " "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " - "'can://can0?msgid=65536&pkt_size=8&pkt_buf_size=1024')", create); + "'can://can0?msgid=65536&pkt_size=8')", create); #endif From 125770aca18f3ff8ef86fbd31ad4c4dac3bd2a68 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Mon, 13 Apr 2026 10:58:58 -0400 Subject: [PATCH 11/20] Fixed serial data RX regression in packetized mode --- test/zcm/PacketizedSerialTransportTest.hpp | 63 +++++++++++++++++++++ zcm/transport/packetized_serial_transport.c | 6 +- zcm/transport/transport_can.cpp | 34 +++++++++-- zcm/transport/transport_serial.cpp | 22 +++++-- 4 files changed, 112 insertions(+), 13 deletions(-) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 53210c688..e574a9684 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -358,6 +358,69 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_trans_destroy(rx); } + void testNonPacketizedPassthroughIgnoresPacketizedSizeLimit() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 5500; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 2048, 32768, 8, 128); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 2048, 32768, 8, 128); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(512); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0x19); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"PLAIN"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 20); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("PLAIN")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + + void testPacketizedMessageStillRespectsConfiguredSizeLimit() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 5600; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 2048, 32768, 8, 128); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 2048, 32768, 8, 128); + TSM_ASSERT("failed creating transports", tx && rx); + + vector payload(129, 0x2a); + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$TOO_BIG"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EINVALID); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + // Drive the transport via zcm_trans_update (the vtable update path) rather than the // explicit packetized_serial_update_rx/tx functions. This mirrors how the // TransportSerial and TransportCan wrappers operate and would have caught the diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 627a431ba..c031d8c88 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -63,6 +63,7 @@ struct zcm_trans_packetized_serial_t size_t pkt_buf_size; uint8_t* out_buf; + size_t out_buf_size; size_t out_len; uint64_t out_utime; char out_channel[ZCM_CHANNEL_MAXLEN + 1]; @@ -191,7 +192,7 @@ static int queue_output(zcm_trans_packetized_serial_t* zt, const char* channel, const uint8_t* data, size_t len, uint64_t utime, int strip_packetized_prefix) { - if (zt->max_message_size < len) { + if (zt->out_buf_size < len) { return ZCM_EINVALID; } @@ -642,7 +643,8 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } - zt->out_buf = malloc(zt->max_message_size == 0 ? 1 : zt->max_message_size); + zt->out_buf_size = zt->inner_mtu > zt->max_message_size ? zt->inner_mtu : zt->max_message_size; + zt->out_buf = malloc(zt->out_buf_size == 0 ? 1 : zt->out_buf_size); if (zt->out_buf == NULL) { free(zt->pkt_buf); zcm_trans_generic_serial_destroy(zt->inner); diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index dd3875973..c06206305 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -47,12 +47,29 @@ static bool parsePacketDataSize(const string* opt, uint8_t& out) return true; } +static bool parsePacketBufSize(const string* opt, size_t& out) +{ + if (!opt) return true; + char* endptr; + unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); + if (*endptr != '\0' || parsed == 0) return false; + out = (size_t)parsed; + return true; +} + +static size_t parsePacketizedMaxMessageSize(const string* opt) +{ + size_t out = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; + return parsePacketBufSize(opt, out) ? out : 0; +} + struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { unordered_map options; uint32_t msgId; uint32_t txId; uint8_t packetDataSize; + size_t packetizedMaxMessageSize; string address; int soc = -1; @@ -90,6 +107,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t msgId = 0; packetDataSize = 0; + packetizedMaxMessageSize = parsePacketizedMaxMessageSize(findOption("pkt_buf_size")); + if (packetizedMaxMessageSize == 0) { + ZCM_DEBUG("Invalid pkt_buf_size. Expected positive integer"); + return; + } if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); @@ -183,11 +205,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t gst = zcm_trans_packetized_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, - this, - MTU, MTU * 10, - packetDataSize, - PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE); + &ZCM_TRANS_CLASSNAME::timestamp_now, + this, + MTU, MTU * 10, + packetDataSize, + packetizedMaxMessageSize); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { @@ -407,5 +429,5 @@ static zcm_trans_t *create(zcm_url_t* url, char **opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can", "Transfer data via a socket CAN connection on a single id " "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " - "'can://can0?msgid=65536&pkt_size=8')", create); + "'can://can0?msgid=65536&pkt_size=8&pkt_buf_size=1024')", create); #endif diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index 4caa9e2bd..29cf8a7f4 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -67,6 +67,12 @@ static bool parsePacketBufSize(const string* opt, size_t& out) return true; } +static size_t parsePacketizedMaxMessageSize(const string* opt) +{ + size_t out = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; + return parsePacketBufSize(opt, out) ? out : 0; +} + struct Serial { Serial(){} @@ -267,6 +273,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t int baud; bool hwFlowControl; uint8_t packetDataSize; + size_t packetizedMaxMessageSize; bool raw; string rawChan; @@ -330,6 +337,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); return; } + packetizedMaxMessageSize = parsePacketizedMaxMessageSize(findOption("pkt_buf_size")); + if (packetizedMaxMessageSize == 0) { + ZCM_DEBUG("expected positive integer argument for 'pkt_buf_size'"); + return; + } raw = false; auto* rawStr = findOption("raw"); @@ -374,11 +386,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t gst = zcm_trans_packetized_serial_create(&ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, - &ZCM_TRANS_CLASSNAME::timestamp_now, - nullptr, - MTU, MTU * 10, - packetDataSize, - PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE); + &ZCM_TRANS_CLASSNAME::timestamp_now, + nullptr, + MTU, MTU * 10, + packetDataSize, + packetizedMaxMessageSize); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { From 696c0532c78bd43b44123287a0ebe465ec0d11fb Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Mon, 13 Apr 2026 11:21:40 -0400 Subject: [PATCH 12/20] Fix potential NULL dereference bug --- zcm/transport/transport_can.cpp | 12 +++++++++++- zcm/transport/transport_serial.cpp | 14 +++++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index c06206305..860d0b81d 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -376,6 +376,13 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return ZCM_EAGAIN; } + int update() + { + int rxRet = this->gst_update_rx(this->gst); + int txRet = this->gst_update_tx(this->gst); + return rxRet == ZCM_EOK ? txRet : rxRet; + } + /********************** STATICS **********************/ static zcm_trans_methods_t methods; static ZCM_TRANS_CLASSNAME *cast(zcm_trans_t *zt) @@ -396,6 +403,9 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t static int _recvmsg(zcm_trans_t *zt, zcm_msg_t *msg, unsigned timeout) { return cast(zt)->recvmsg(msg, timeout); } + static int _update(zcm_trans_t *zt) + { return cast(zt)->update(); } + static void _destroy(zcm_trans_t *zt) { delete cast(zt); } @@ -409,7 +419,7 @@ zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { &ZCM_TRANS_CLASSNAME::_recvmsg_enable, &ZCM_TRANS_CLASSNAME::_recvmsg, NULL, // drops - NULL, // update + &ZCM_TRANS_CLASSNAME::_update, &ZCM_TRANS_CLASSNAME::_destroy, }; diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index 29cf8a7f4..9201b7314 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -500,6 +500,15 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } } + int update() + { + if (raw) return ZCM_EOK; + + int rxRet = this->gst_update_rx(this->gst); + int txRet = this->gst_update_tx(this->gst); + return rxRet == ZCM_EOK ? txRet : rxRet; + } + /********************** STATICS **********************/ static zcm_trans_methods_t methods; static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) @@ -520,6 +529,9 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) { return cast(zt)->recvmsg(msg, timeout); } + static int _update(zcm_trans_t* zt) + { return cast(zt)->update(); } + static void _destroy(zcm_trans_t* zt) { delete cast(zt); } @@ -532,7 +544,7 @@ zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { &ZCM_TRANS_CLASSNAME::_recvmsgEnable, &ZCM_TRANS_CLASSNAME::_recvmsg, NULL, // drops - NULL, // update + &ZCM_TRANS_CLASSNAME::_update, &ZCM_TRANS_CLASSNAME::_destroy, }; From 5284ad0e7a993c0030ba99421476c1e853eddfad Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Mon, 20 Apr 2026 15:48:19 -0400 Subject: [PATCH 13/20] Add dynamic packet buffer resizing and use by default for cpp transports --- docs/transports.md | 4 +- zcm/transport/packetized_serial_transport.c | 53 +++++++++++++++++++-- zcm/transport/transport_can.cpp | 26 +--------- zcm/transport/transport_serial.cpp | 27 +---------- 4 files changed, 54 insertions(+), 56 deletions(-) diff --git a/docs/transports.md b/docs/transports.md index 9b832acad..dc59293c4 100644 --- a/docs/transports.md +++ b/docs/transports.md @@ -48,8 +48,8 @@ be used to *summon* the transport: Serial - serial://<path-to-device>?baud=<baud>[&pkt_size=<n>&pkt_buf_size=<n>] - zcm_create("serial:///dev/ttyUSB0?baud=115200"), zcm_create("serial:///dev/ttyUSB0?baud=115200&pkt_size=128&pkt_buf_size=1024") + serial://<path-to-device>?baud=<baud>[&pkt_size=<n>] + zcm_create("serial:///dev/ttyUSB0?baud=115200"), zcm_create("serial:///dev/ttyUSB0?baud=115200&pkt_size=128") CAN diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index c031d8c88..556cd1516 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -57,6 +57,7 @@ struct zcm_trans_packetized_serial_t size_t inner_mtu; size_t mtu; size_t max_message_size; + int dynamic; uint8_t configured_packet_data_size; uint8_t* pkt_buf; @@ -79,6 +80,36 @@ struct zcm_trans_packetized_serial_t static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt); +static int grow_buffers(zcm_trans_packetized_serial_t* zt, size_t new_size) +{ + uint8_t* rd = realloc(zt->rx.data, new_size); + uint8_t* rpr = realloc(zt->rx.packet_received, new_size * sizeof(uint8_t)); + uint16_t* rmi = realloc(zt->rx.missing_ids, new_size * sizeof(uint16_t)); + uint8_t* td = realloc(zt->tx.data, new_size); + uint16_t* tri = realloc(zt->tx.retrans_ids, new_size * sizeof(uint16_t)); + + if (rd) zt->rx.data = rd; + if (rpr) zt->rx.packet_received = rpr; + if (rmi) zt->rx.missing_ids = rmi; + if (td) zt->tx.data = td; + if (tri) zt->tx.retrans_ids = tri; + + if (!rd || !rpr || !rmi || !td || !tri) return ZCM_EAGAIN; + + size_t new_out = zt->inner_mtu > new_size ? zt->inner_mtu : new_size; + if (new_out > zt->out_buf_size) { + uint8_t* ob = realloc(zt->out_buf, new_out); + if (!ob) return ZCM_EAGAIN; + zt->out_buf = ob; + zt->out_buf_size = new_out; + } + + zt->rx.packet_capacity = new_size; + zt->tx.packet_capacity = new_size; + zt->max_message_size = new_size; + return ZCM_EOK; +} + static int send_inner_with_retry(zcm_trans_packetized_serial_t* zt, zcm_msg_t msg) { int i; @@ -263,8 +294,11 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; if (packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; - if (total_message_size > zt->max_message_size || total_message_size > zt->mtu) - return ZCM_EINVALID; + if (total_message_size > zt->mtu) return ZCM_EINVALID; + if (total_message_size > zt->max_message_size) { + if (!zt->dynamic) return ZCM_EINVALID; + if (grow_buffers(zt, total_message_size) != ZCM_EOK) return ZCM_EAGAIN; + } size_t expected_packets = packetized_message_packet_count(total_message_size, packet_data_size); @@ -272,7 +306,10 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann rx_clear(&zt->rx); packetized_rx_state_t* rx = &zt->rx; - if ((size_t)total_packets > rx->packet_capacity) return ZCM_EINVALID; + if ((size_t)total_packets > rx->packet_capacity) { + if (!zt->dynamic) return ZCM_EINVALID; + if (grow_buffers(zt, total_message_size) != ZCM_EOK) return ZCM_EAGAIN; + } strncpy(rx->channel, channel, ZCM_CHANNEL_MAXLEN); rx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; @@ -398,7 +435,11 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t packet_data_size = zt->configured_packet_data_size; if (packet_data_size == 0 || packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; - if (msg.len > zt->mtu || msg.len > zt->max_message_size) return ZCM_EINVALID; + if (msg.len > zt->mtu) return ZCM_EINVALID; + if (msg.len > zt->max_message_size) { + if (!zt->dynamic) return ZCM_EINVALID; + if (grow_buffers(zt, msg.len) != ZCM_EOK) return ZCM_EAGAIN; + } uint32_t total_message_size = (uint32_t)msg.len; size_t total_packets_sz = packetized_message_packet_count(total_message_size, @@ -604,6 +645,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( return NULL; } + zt->dynamic = (max_message_size == 0); if (max_message_size == 0) max_message_size = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; zt->max_message_size = max_message_size; @@ -633,7 +675,8 @@ zcm_trans_t* zcm_trans_packetized_serial_create( } zt->mtu = packetized_max_mtu(zt->configured_packet_data_size); - if (zt->mtu > zt->max_message_size) zt->mtu = zt->max_message_size; + if (!zt->dynamic && zt->mtu > zt->max_message_size) + zt->mtu = zt->max_message_size; zt->pkt_buf_size = zt->inner_mtu; zt->pkt_buf = malloc(zt->pkt_buf_size); diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 860d0b81d..79850afc5 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -47,21 +47,6 @@ static bool parsePacketDataSize(const string* opt, uint8_t& out) return true; } -static bool parsePacketBufSize(const string* opt, size_t& out) -{ - if (!opt) return true; - char* endptr; - unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0) return false; - out = (size_t)parsed; - return true; -} - -static size_t parsePacketizedMaxMessageSize(const string* opt) -{ - size_t out = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; - return parsePacketBufSize(opt, out) ? out : 0; -} struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { @@ -69,7 +54,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t uint32_t msgId; uint32_t txId; uint8_t packetDataSize; - size_t packetizedMaxMessageSize; string address; int soc = -1; @@ -107,12 +91,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t msgId = 0; packetDataSize = 0; - packetizedMaxMessageSize = parsePacketizedMaxMessageSize(findOption("pkt_buf_size")); - if (packetizedMaxMessageSize == 0) { - ZCM_DEBUG("Invalid pkt_buf_size. Expected positive integer"); - return; - } - if (!parsePacketDataSize(findOption("pkt_size"), packetDataSize)) { ZCM_DEBUG("Invalid pkt_size. Expected integer in [1,253]"); return; @@ -209,7 +187,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t this, MTU, MTU * 10, packetDataSize, - packetizedMaxMessageSize); + 0); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { @@ -439,5 +417,5 @@ static zcm_trans_t *create(zcm_url_t* url, char **opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "can", "Transfer data via a socket CAN connection on a single id " "(e.g. 'can://can0?msgid=65536&rx_extended_addr=standard&tx_extended_addr=true' or " - "'can://can0?msgid=65536&pkt_size=8&pkt_buf_size=1024')", create); + "'can://can0?msgid=65536&pkt_size=8')", create); #endif diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index 9201b7314..aa77df8f9 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -57,21 +57,6 @@ static bool parsePacketDataSize(const string* opt, uint8_t& out) return true; } -static bool parsePacketBufSize(const string* opt, size_t& out) -{ - if (!opt) return true; - char* endptr; - unsigned long parsed = strtoul(opt->c_str(), &endptr, 10); - if (*endptr != '\0' || parsed == 0) return false; - out = (size_t)parsed; - return true; -} - -static size_t parsePacketizedMaxMessageSize(const string* opt) -{ - size_t out = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; - return parsePacketBufSize(opt, out) ? out : 0; -} struct Serial { @@ -273,8 +258,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t int baud; bool hwFlowControl; uint8_t packetDataSize; - size_t packetizedMaxMessageSize; - bool raw; string rawChan; int rawSize; @@ -337,12 +320,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t ZCM_DEBUG("expected integer argument in [1,253] for 'pkt_size'"); return; } - packetizedMaxMessageSize = parsePacketizedMaxMessageSize(findOption("pkt_buf_size")); - if (packetizedMaxMessageSize == 0) { - ZCM_DEBUG("expected positive integer argument for 'pkt_buf_size'"); - return; - } - raw = false; auto* rawStr = findOption("raw"); if (rawStr) { @@ -390,7 +367,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t nullptr, MTU, MTU * 10, packetDataSize, - packetizedMaxMessageSize); + 0); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { @@ -564,7 +541,7 @@ static zcm_trans_t* create(zcm_url_t* url, char **opt_errmsg) const TransportRegister ZCM_TRANS_CLASSNAME::reg( "serial", "Transfer data via a serial connection " "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true', " - "'serial:///dev/ttyUSB0?baud=115200&pkt_size=128&pkt_buf_size=1024', or " + "'serial:///dev/ttyUSB0?baud=115200&pkt_size=128', or " "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", create); #endif From 60bd6cde17a6a0bb2b0be8bf3bda5cac3a0e8a12 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Mon, 20 Apr 2026 16:00:26 -0400 Subject: [PATCH 14/20] Test dynamic resizing buffers --- test/zcm/PacketizedSerialTransportTest.hpp | 78 ++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index e574a9684..3a6bbb9f1 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -421,6 +421,84 @@ class PacketizedSerialTransportTest : public CxxTest::TestSuite zcm_trans_destroy(rx); } + // Passing max_message_size=0 enables dynamic mode. Buffers start at + // PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE (1024) and grow to fit any message. + void testDynamicResizingRoundTrip() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 7000; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0, 0); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0, 0); + TSM_ASSERT("failed creating transports", tx && rx); + + // 2048 bytes exceeds PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE (1024) + vector payload(2048); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ 0xd5); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$DYN"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 40); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("DYN")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + + void testDynamicResizingMultipleGrowths() + { + PacketizedLinkEndpoint a; + PacketizedLinkEndpoint b; + a.peer = &b; + b.peer = &a; + + uint64_t now = 7100; + zcm_trans_t* tx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &a, fake_now, &now, 64, 32768, 0, 0); + zcm_trans_t* rx = zcm_trans_packetized_serial_create( + endpoint_get, endpoint_put, &b, fake_now, &now, 64, 32768, 0, 0); + TSM_ASSERT("failed creating transports", tx && rx); + + static const size_t sizes[] = { 512, 2048, 8192 }; + for (size_t si = 0; si < 3; ++si) { + vector payload(sizes[si]); + for (size_t i = 0; i < payload.size(); ++i) payload[i] = (uint8_t)(i ^ (uint8_t)si); + + zcm_msg_t msg; + msg.utime = now; + msg.channel = (char*)"$GROW"; + msg.len = payload.size(); + msg.buf = payload.data(); + + TS_ASSERT_EQUALS(zcm_trans_sendmsg(tx, msg), ZCM_EOK); + pump(tx, rx, 200); + + zcm_msg_t out; + TS_ASSERT_EQUALS(zcm_trans_recvmsg(rx, &out, 0), ZCM_EOK); + TS_ASSERT_EQUALS(string(out.channel), string("GROW")); + TS_ASSERT_EQUALS(out.len, payload.size()); + TS_ASSERT_SAME_DATA(out.buf, payload.data(), payload.size()); + } + + zcm_trans_destroy(tx); + zcm_trans_destroy(rx); + } + // Drive the transport via zcm_trans_update (the vtable update path) rather than the // explicit packetized_serial_update_rx/tx functions. This mirrors how the // TransportSerial and TransportCan wrappers operate and would have caught the From 795a4287e8e1b28c69f8360c08662a6817664b9f Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 23 Apr 2026 09:42:41 -0400 Subject: [PATCH 15/20] Update checksum calculation helper --- zcm/transport/generic_serial_fletcher.h | 7 +++---- zcm/transport/packetized_serial_transport.c | 4 ++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/zcm/transport/generic_serial_fletcher.h b/zcm/transport/generic_serial_fletcher.h index a3637b111..321dcd0d9 100644 --- a/zcm/transport/generic_serial_fletcher.h +++ b/zcm/transport/generic_serial_fletcher.h @@ -19,12 +19,11 @@ static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) return (sumHigh << 8) | sumLow; } -static inline uint16_t fletcher16(const uint8_t* data, size_t len) +static inline uint16_t fletcher16(const uint8_t* data, size_t len, uint16_t prevSum) { size_t i; - uint16_t sum = 0xffff; - for (i = 0; i < len; ++i) sum = fletcherUpdate(data[i], sum); - return sum; + for (i = 0; i < len; ++i) prevSum = fletcherUpdate(data[i], prevSum); + return prevSum; } #endif /* _ZCM_TRANS_NONBLOCKING_FLETCHER_H */ diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 556cd1516..5460cf1be 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -352,7 +352,7 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i rx->last_update_utime = utime; if (rx->received_count == rx->total_packets) { - uint16_t checksum = fletcher16(rx->data, rx->total_message_size); + uint16_t checksum = fletcher16(rx->data, rx->total_message_size, 0xFFFF); if (checksum != rx->expected_checksum) { rx_clear(rx); return ZCM_EINVALID; @@ -464,7 +464,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ zcm_write_u16_be(&meta[0], total_packets); meta[2] = packet_data_size; zcm_write_u32_be(&meta[3], total_message_size); - zcm_write_u16_be(&meta[7], fletcher16(msg.buf, msg.len)); + zcm_write_u16_be(&meta[7], fletcher16(msg.buf, msg.len, 0xFFFF)); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, PACKETIZED_METADATA_BODY_BYTES); if (ret != ZCM_EOK) return ret; From d3980d207abe6d77a7aa83cf3c813212ba49682a Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 23 Apr 2026 09:42:51 -0400 Subject: [PATCH 16/20] Clean up tranport description wrapping behavior --- zcm/transport_registrar.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/zcm/transport_registrar.c b/zcm/transport_registrar.c index 7852d2955..5bbfc1c83 100644 --- a/zcm/transport_registrar.c +++ b/zcm/transport_registrar.c @@ -35,9 +35,44 @@ zcm_trans_create_func *zcm_transport_find(const char *name) void zcm_transport_help(FILE *f) { + const int name_width = 20; + const int desc_width = 120 - name_width - 1; + fprintf(f, "Transport Name Description\n"); fprintf(f, "---------------------------------------------------------------------\n"); for (size_t i = 0; i < t_index; i++) { - fprintf(f, "%-20s %s\n", t_name[i], t_desc[i]); + const char *desc = t_desc[i]; + size_t len = strlen(desc); + size_t pos = 0; + bool first_line = true; + + do { + if (first_line) { + fprintf(f, "%-*s ", name_width, t_name[i]); + first_line = false; + } else { + fprintf(f, "%*s", name_width + 1, ""); + } + + // Check if remaining description will fit + size_t remaining = len - pos; + if (remaining <= (size_t)desc_width) { + fprintf(f, "%s\n", desc + pos); + break; + } + + // Look for natural boundary to break on + size_t brk = (size_t)desc_width; + while (brk > 0 && desc[pos + brk] != ' ') + brk--; + if (brk == 0) + brk = (size_t)desc_width; + + fprintf(f, "%.*s\n", (int)brk, desc + pos); + pos += brk; + while (pos < len && desc[pos] == ' ') + pos++; + } while (pos < len); + fprintf(f, "\n"); } } From 58fac859c7dfb9a61d78f2cf5e6d1a9064b7a3d6 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 23 Apr 2026 09:43:16 -0400 Subject: [PATCH 17/20] Revert "Fix potential NULL dereference bug" This reverts commit 696c0532c78bd43b44123287a0ebe465ec0d11fb. --- zcm/transport/transport_can.cpp | 12 +----------- zcm/transport/transport_serial.cpp | 14 +------------- 2 files changed, 2 insertions(+), 24 deletions(-) diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 79850afc5..3fe773874 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -354,13 +354,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return ZCM_EAGAIN; } - int update() - { - int rxRet = this->gst_update_rx(this->gst); - int txRet = this->gst_update_tx(this->gst); - return rxRet == ZCM_EOK ? txRet : rxRet; - } - /********************** STATICS **********************/ static zcm_trans_methods_t methods; static ZCM_TRANS_CLASSNAME *cast(zcm_trans_t *zt) @@ -381,9 +374,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t static int _recvmsg(zcm_trans_t *zt, zcm_msg_t *msg, unsigned timeout) { return cast(zt)->recvmsg(msg, timeout); } - static int _update(zcm_trans_t *zt) - { return cast(zt)->update(); } - static void _destroy(zcm_trans_t *zt) { delete cast(zt); } @@ -397,7 +387,7 @@ zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { &ZCM_TRANS_CLASSNAME::_recvmsg_enable, &ZCM_TRANS_CLASSNAME::_recvmsg, NULL, // drops - &ZCM_TRANS_CLASSNAME::_update, + NULL, // update &ZCM_TRANS_CLASSNAME::_destroy, }; diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index aa77df8f9..5bf6d9623 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -477,15 +477,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } } - int update() - { - if (raw) return ZCM_EOK; - - int rxRet = this->gst_update_rx(this->gst); - int txRet = this->gst_update_tx(this->gst); - return rxRet == ZCM_EOK ? txRet : rxRet; - } - /********************** STATICS **********************/ static zcm_trans_methods_t methods; static ZCM_TRANS_CLASSNAME* cast(zcm_trans_t* zt) @@ -506,9 +497,6 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t static int _recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) { return cast(zt)->recvmsg(msg, timeout); } - static int _update(zcm_trans_t* zt) - { return cast(zt)->update(); } - static void _destroy(zcm_trans_t* zt) { delete cast(zt); } @@ -521,7 +509,7 @@ zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { &ZCM_TRANS_CLASSNAME::_recvmsgEnable, &ZCM_TRANS_CLASSNAME::_recvmsg, NULL, // drops - &ZCM_TRANS_CLASSNAME::_update, + NULL, // update &ZCM_TRANS_CLASSNAME::_destroy, }; From 06cc4ddb102ba24a36ba72621dca3c3497baec00 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 23 Apr 2026 15:02:11 -0400 Subject: [PATCH 18/20] Use built-in coretypes to encode/decode int16s and int32s --- test/zcm/PacketizedSerialTransportTest.hpp | 7 ++-- zcm/transport/packetized_serial_protocol.h | 1 - zcm/transport/packetized_serial_transport.c | 36 ++++++++++++------- zcm/transport/packetized_serial_transport.h | 2 ++ zcm/transport/transport_can.cpp | 2 +- zcm/transport/transport_serial.cpp | 2 +- zcm/util/byteorder.h | 39 --------------------- zcm/wscript | 5 ++- 8 files changed, 33 insertions(+), 61 deletions(-) delete mode 100644 zcm/util/byteorder.h diff --git a/test/zcm/PacketizedSerialTransportTest.hpp b/test/zcm/PacketizedSerialTransportTest.hpp index 3a6bbb9f1..1fe127eaf 100644 --- a/test/zcm/PacketizedSerialTransportTest.hpp +++ b/test/zcm/PacketizedSerialTransportTest.hpp @@ -4,6 +4,7 @@ #include "cxxtest/TestSuite.h" #include "zcm/transport/packetized_serial_transport.h" #include "zcm/transport/packetized_serial_protocol.h" +#include "zcm/zcm_coretypes.h" #include #include @@ -112,9 +113,9 @@ static size_t endpoint_put(const uint8_t* data, size_t nData, void* usr) if (type == PACKETIZED_MSG_DATA && payload.size() == (size_t)PACKETIZED_HEADER_BYTES + body_len && body_len >= PACKETIZED_DATA_OVERHEAD_BYTES) { - uint16_t packet_id = zcm_read_u16_be( - &payload[PACKETIZED_HEADER_BYTES]); - if (packet_id == ep->dropPacketId) { + uint16_t packet_id = 0; + __int16_t_decode_array(payload.data(), PACKETIZED_HEADER_BYTES, 2, (int16_t*)(&packet_id), 1); + if (packet_id == ep->dropPacketId) { drop = true; ep->dropDone = true; } diff --git a/zcm/transport/packetized_serial_protocol.h b/zcm/transport/packetized_serial_protocol.h index 60b50d39b..2f00070f2 100644 --- a/zcm/transport/packetized_serial_protocol.h +++ b/zcm/transport/packetized_serial_protocol.h @@ -5,7 +5,6 @@ #include #include "generic_serial_fletcher.h" -#include "zcm/util/byteorder.h" #define PACKETIZED_HEADER_BYTES (4) #define PACKETIZED_DATA_OVERHEAD_BYTES (2) diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 5460cf1be..ac15c9c14 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -1,5 +1,6 @@ #include "zcm/transport.h" #include "zcm/zcm.h" +#include "zcm/zcm_coretypes.h" #include "generic_serial_transport.h" #include "packetized_serial_protocol.h" @@ -135,7 +136,7 @@ static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, return ZCM_EINVALID; zt->pkt_buf[0] = type; - zcm_write_u16_be(&zt->pkt_buf[1], session_id); + __int16_t_encode_array(zt->pkt_buf, 1, 2, (int16_t*)(&session_id), 1); zt->pkt_buf[3] = body_len; if (body_len > 0 && body != &zt->pkt_buf[PACKETIZED_HEADER_BYTES]) { memcpy(&zt->pkt_buf[PACKETIZED_HEADER_BYTES], body, body_len); @@ -206,7 +207,7 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; body[0] = (uint8_t)count; for (i = 0; i < count; ++i) { - zcm_write_u16_be(&body[1 + i * 2], rx->missing_ids[sent + i]); + __int16_t_encode_array(body, 1 + i * 2, 2, (int16_t*)(&rx->missing_ids[sent + i]), 1); } int ret = send_packet(zt, rx->channel, rx->session_id, PACKETIZED_MSG_RETRANS_REQ, @@ -269,7 +270,7 @@ static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) uint32_t remaining = tx->total_message_size - offset; uint8_t payload_len = (uint8_t)(remaining < chunk ? remaining : chunk); - zcm_write_u16_be(&body[0], packet_id); + __int16_t_encode_array(body, 0, 2, (int16_t*)(&packet_id), 1); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); int ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -287,10 +288,13 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann { if (body_len != PACKETIZED_METADATA_BODY_BYTES) return ZCM_EINVALID; - uint16_t total_packets = zcm_read_u16_be(&body[0]); + uint16_t total_packets = 0; uint8_t packet_data_size = body[2]; - uint32_t total_message_size = zcm_read_u32_be(&body[3]); - uint16_t expected_checksum = zcm_read_u16_be(&body[7]); + uint32_t total_message_size = 0; + uint16_t expected_checksum = 0; + if (__int16_t_decode_array(body, 0, 2, (int16_t*)(&total_packets), 1) < 0) return ZCM_EINVALID; + if (__int32_t_decode_array(body, 3, 4, (int32_t*)(&total_message_size), 1) < 0) return ZCM_EINVALID; + if (__int16_t_decode_array(body, 7, 2, (int16_t*)(&expected_checksum), 1) < 0) return ZCM_EINVALID; if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; if (packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; @@ -330,7 +334,8 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (!rx->active || rx->session_id != session_id) return ZCM_EOK; if (body_len < PACKETIZED_DATA_OVERHEAD_BYTES) return ZCM_EINVALID; - uint16_t packet_id = zcm_read_u16_be(&body[0]); + uint16_t packet_id = 0; + __int16_t_decode_array(body, 0, 2, (int16_t*)(&packet_id), 1); uint8_t payload_len = (uint8_t)(body_len - PACKETIZED_DATA_OVERHEAD_BYTES); if (packet_id >= rx->total_packets) return ZCM_EINVALID; @@ -387,7 +392,9 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s if ((size_t)count > tx->packet_capacity) return ZCM_EINVALID; tx->retrans_count = count; for (i = 0; i < count; ++i) { - tx->retrans_ids[i] = zcm_read_u16_be(&body[1 + (size_t)i * 2]); + uint16_t packet_id = 0; + __int16_t_decode_array(body, 1 + (size_t)i * 2, 2, (int16_t *)(&packet_id), 1); + tx->retrans_ids[i] = packet_id; } return ZCM_EOK; @@ -461,10 +468,11 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ tx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - zcm_write_u16_be(&meta[0], total_packets); + uint16_t fletcherCsum = fletcher16(msg.buf, msg.len, 0xFFFF); + __int16_t_encode_array(meta, 0, 2, (int16_t*)(&total_packets), 1); meta[2] = packet_data_size; - zcm_write_u32_be(&meta[3], total_message_size); - zcm_write_u16_be(&meta[7], fletcher16(msg.buf, msg.len, 0xFFFF)); + __int32_t_encode_array(meta, 3, 4, (int32_t*)(&total_message_size), 1); + __int16_t_encode_array(meta, 7, 2, (int16_t*)(&fletcherCsum), 1); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, PACKETIZED_METADATA_BODY_BYTES); if (ret != ZCM_EOK) return ret; @@ -476,7 +484,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t payload_len = (uint8_t)(remaining < packet_data_size ? remaining : packet_data_size); - zcm_write_u16_be(&body[0], packet_id); + __int16_t_encode_array(body, 0, 2, (int16_t*)(&packet_id), 1); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, @@ -507,7 +515,8 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) if (in.len < PACKETIZED_HEADER_BYTES) continue; { uint8_t type = in.buf[0]; - uint16_t session_id = zcm_read_u16_be(&in.buf[1]); + uint16_t session_id = 0; + __int16_t_decode_array(in.buf, 1, 2, (int16_t*)(&session_id), 1); uint8_t body_len = in.buf[3]; if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; @@ -621,6 +630,7 @@ static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt) return (zcm_trans_packetized_serial_t*)zt; } +// RRR: rename tx_packet_data_size zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index ac81ea31b..0c07ea8dc 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -22,6 +22,8 @@ extern "C" { * serial transport unchanged and are not packetized. */ +#define ZCM_TRANS_PACKETIZED_SERIAL_GROW_DYNAMICALLY 0 + zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, diff --git a/zcm/transport/transport_can.cpp b/zcm/transport/transport_can.cpp index 3fe773874..f0c178198 100644 --- a/zcm/transport/transport_can.cpp +++ b/zcm/transport/transport_can.cpp @@ -187,7 +187,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t this, MTU, MTU * 10, packetDataSize, - 0); + ZCM_TRANS_PACKETIZED_SERIAL_GROW_DYNAMICALLY); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index 5bf6d9623..86d375ed9 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -367,7 +367,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t nullptr, MTU, MTU * 10, packetDataSize, - 0); + ZCM_TRANS_PACKETIZED_SERIAL_GROW_DYNAMICALLY); gst_update_rx = packetized_serial_update_rx; gst_update_tx = packetized_serial_update_tx; } else { diff --git a/zcm/util/byteorder.h b/zcm/util/byteorder.h deleted file mode 100644 index 9d025ab74..000000000 --- a/zcm/util/byteorder.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef __ZCM_BYTEORDER_H__ -#define __ZCM_BYTEORDER_H__ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -static inline uint16_t zcm_read_u16_be(const uint8_t* p) -{ - return ((uint16_t)p[0] << 8) | (uint16_t)p[1]; -} - -static inline uint32_t zcm_read_u32_be(const uint8_t* p) -{ - return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | - ((uint32_t)p[2] << 8) | (uint32_t)p[3]; -} - -static inline void zcm_write_u16_be(uint8_t* p, uint16_t v) -{ - p[0] = (uint8_t)((v >> 8) & 0xff); - p[1] = (uint8_t)(v & 0xff); -} - -static inline void zcm_write_u32_be(uint8_t* p, uint32_t v) -{ - p[0] = (uint8_t)((v >> 24) & 0xff); - p[1] = (uint8_t)((v >> 16) & 0xff); - p[2] = (uint8_t)((v >> 8) & 0xff); - p[3] = (uint8_t)(v & 0xff); -} - -#ifdef __cplusplus -} -#endif - -#endif /* __ZCM_BYTEORDER_H__ */ diff --git a/zcm/wscript b/zcm/wscript index f78961817..87a8f2c5d 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -43,6 +43,7 @@ def build(ctx): 'transport/lockfree/lf_*.c'], excl=srcExcludes)) + embedSource = ['zcm.h', 'zcm_private.h', 'zcm.c', 'zcm-cpp.hpp', 'zcm-cpp-impl.hpp', 'zcm_coretypes.h', 'transport.h', 'nonblocking.h', 'nonblocking.c', 'transport/generic_serial_transport.h', @@ -52,8 +53,7 @@ def build(ctx): 'transport/packetized_serial_transport.c', 'transport/generic_serial_circ_buff.h', 'transport/generic_serial_circ_buff.c', - 'transport/generic_serial_fletcher.h', - 'util/byteorder.h'] + 'transport/generic_serial_fletcher.h'] if ctx.env.USING_THIRD_PARTY: embedSource.append('transport/third-party/embedded/**') @@ -98,7 +98,6 @@ def build(ctx): ['tools/IndexerPlugin.hpp', 'tools/TranscoderPlugin.hpp']) - ctx.install_files('${PREFIX}/include/zcm/util', 'util/byteorder.h') ctx.install_files('${PREFIX}/include/zcm/util', 'util/debug.h') ctx.install_files('${PREFIX}/include/zcm/util', 'util/Filter.hpp') ctx.install_files('${PREFIX}/include/zcm/util', 'util/topology.hpp') From 0d27eabb6fc24ccd71081743bd9ead88891ac58b Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Thu, 23 Apr 2026 16:34:11 -0400 Subject: [PATCH 19/20] Some cleanup --- zcm/transport/packetized_serial_transport.c | 66 ++++++++++----------- zcm/transport/packetized_serial_transport.h | 2 +- 2 files changed, 33 insertions(+), 35 deletions(-) diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index ac15c9c14..5834bc9eb 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -439,8 +439,8 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ return send_inner_with_retry(zt, msg); } - uint8_t packet_data_size = zt->configured_packet_data_size; - if (packet_data_size == 0 || packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) + uint8_t tx_packet_data_size = zt->configured_packet_data_size; + if (tx_packet_data_size == 0 || tx_packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; if (msg.len > zt->mtu) return ZCM_EINVALID; if (msg.len > zt->max_message_size) { @@ -450,7 +450,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint32_t total_message_size = (uint32_t)msg.len; size_t total_packets_sz = packetized_message_packet_count(total_message_size, - packet_data_size); + tx_packet_data_size); if (total_packets_sz == 0) total_packets_sz = 1; if (total_packets_sz > PACKETIZED_MAX_PACKETS) return ZCM_EINVALID; uint16_t total_packets = (uint16_t)total_packets_sz; @@ -461,7 +461,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ tx->active = 1; tx->session_id = ++zt->next_session_id; - tx->packet_data_size = packet_data_size; + tx->packet_data_size = tx_packet_data_size; tx->total_packets = total_packets; tx->total_message_size = total_message_size; strncpy(tx->channel, msg.channel, ZCM_CHANNEL_MAXLEN); @@ -470,7 +470,7 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; uint16_t fletcherCsum = fletcher16(msg.buf, msg.len, 0xFFFF); __int16_t_encode_array(meta, 0, 2, (int16_t*)(&total_packets), 1); - meta[2] = packet_data_size; + meta[2] = tx_packet_data_size; __int32_t_encode_array(meta, 3, 4, (int32_t*)(&total_message_size), 1); __int16_t_encode_array(meta, 7, 2, (int16_t*)(&fletcherCsum), 1); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, @@ -479,10 +479,10 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; for (packet_id = 0; packet_id < total_packets; ++packet_id) { - uint32_t offset = (uint32_t)packet_id * (uint32_t)packet_data_size; + uint32_t offset = (uint32_t)packet_id * (uint32_t)tx_packet_data_size; uint32_t remaining = total_message_size - offset; uint8_t payload_len = - (uint8_t)(remaining < packet_data_size ? remaining : packet_data_size); + (uint8_t)(remaining < tx_packet_data_size ? remaining : tx_packet_data_size); __int16_t_encode_array(body, 0, 2, (int16_t*)(&packet_id), 1); if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); @@ -513,28 +513,28 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) } if (in.len < PACKETIZED_HEADER_BYTES) continue; - { - uint8_t type = in.buf[0]; - uint16_t session_id = 0; - __int16_t_decode_array(in.buf, 1, 2, (int16_t*)(&session_id), 1); - uint8_t body_len = in.buf[3]; - if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; - - const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; - uint64_t now = in.utime == 0 ? zt->time(zt->time_usr) : in.utime; - - if (type == PACKETIZED_MSG_METADATA) { - ret = begin_rx_session(zt, in.channel, session_id, body, body_len, now); - } else if (type == PACKETIZED_MSG_DATA) { - ret = process_rx_data(zt, session_id, body, body_len, now); - } else if (type == PACKETIZED_MSG_RETRANS_REQ) { - ret = process_retrans_request(zt, session_id, body, body_len); - } else { - continue; - } - - if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; + + uint8_t type = in.buf[0]; + uint16_t session_id = 0; + __int16_t_decode_array(in.buf, 1, 2, (int16_t*)(&session_id), 1); + uint8_t body_len = in.buf[3]; + if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; + + const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; + uint64_t now = in.utime == 0 ? zt->time(zt->time_usr) : in.utime; + + if (type == PACKETIZED_MSG_METADATA) { + ret = begin_rx_session(zt, in.channel, session_id, body, body_len, now); + } else if (type == PACKETIZED_MSG_DATA) { + ret = process_rx_data(zt, session_id, body, body_len, now); + } else if (type == PACKETIZED_MSG_RETRANS_REQ) { + ret = process_retrans_request(zt, session_id, body, body_len); + } else { + continue; } + + // RRR: should we break? could end up blocking for a while otherwise + if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; } return ZCM_EOK; @@ -630,12 +630,11 @@ static zcm_trans_packetized_serial_t* cast(zcm_trans_t* zt) return (zcm_trans_packetized_serial_t*)zt; } -// RRR: rename tx_packet_data_size zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, - uint8_t packet_data_size, size_t max_message_size) + uint8_t tx_packet_data_size, size_t max_message_size) { zcm_trans_packetized_serial_t* zt = calloc(1, sizeof(*zt)); if (zt == NULL) return NULL; @@ -656,17 +655,16 @@ zcm_trans_t* zcm_trans_packetized_serial_create( } zt->dynamic = (max_message_size == 0); - if (max_message_size == 0) max_message_size = PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE; - zt->max_message_size = max_message_size; + zt->max_message_size = zt->dynamic ? PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE : max_message_size; - if (packet_data_size == 0) { + if (tx_packet_data_size == 0) { size_t max_payload = zt->inner_mtu - PACKETIZED_HEADER_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES; if (max_payload > PACKETIZED_MAX_PACKET_DATA_SIZE) max_payload = PACKETIZED_MAX_PACKET_DATA_SIZE; zt->configured_packet_data_size = (uint8_t)max_payload; } else { - zt->configured_packet_data_size = packet_data_size; + zt->configured_packet_data_size = tx_packet_data_size; } if (zt->configured_packet_data_size == 0 || diff --git a/zcm/transport/packetized_serial_transport.h b/zcm/transport/packetized_serial_transport.h index 0c07ea8dc..baff39057 100644 --- a/zcm/transport/packetized_serial_transport.h +++ b/zcm/transport/packetized_serial_transport.h @@ -28,7 +28,7 @@ zcm_trans_t* zcm_trans_packetized_serial_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), void* put_get_usr, uint64_t (*timestamp_now)(void* usr), void* time_usr, size_t MTU, size_t bufSize, - uint8_t packet_data_size, size_t max_message_size); + uint8_t tx_packet_data_size, size_t max_message_size); void zcm_trans_packetized_serial_destroy(zcm_trans_t* zt); From b8d27fce6526f0f0984228252e469371c6a8343d Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 6 May 2026 12:38:35 -0400 Subject: [PATCH 20/20] defines and docs --- docs/packetized_serial_transport.md | 275 ++++++++++++++++++++ zcm/transport/packetized_serial_protocol.h | 54 +++- zcm/transport/packetized_serial_transport.c | 110 +++++--- 3 files changed, 398 insertions(+), 41 deletions(-) create mode 100644 docs/packetized_serial_transport.md diff --git a/docs/packetized_serial_transport.md b/docs/packetized_serial_transport.md new file mode 100644 index 000000000..59166d6e0 --- /dev/null +++ b/docs/packetized_serial_transport.md @@ -0,0 +1,275 @@ +# Packetized Serial Transport + +The packetized serial transport (`packetized_serial_transport.c`) is a framing layer that sits on top of the [generic serial transport](../zcm/transport/generic_serial_transport.h). It fragments large ZCM messages into bounded packet bodies that fit within the inner transport's MTU, and reassembles them on the receiver side. This makes it possible to send messages larger than the serial MTU over byte-stream links. + +Messages are only packetized when their ZCM channel name begins with `$`. Packetized inner messages are sent on that same `$`-prefixed channel, and the `$` is stripped when the reassembled message is delivered. All other messages are forwarded directly to the inner transport unchanged, subject to the inner transport MTU. + +--- + +## Packet Wire Format + +Every packetized message sent over the inner transport has a fixed 4-byte header followed by a type-specific body. The `body_len` field is one byte, so a packetized body can be at most 255 bytes. + +The main wire-format constants are: + +- **`PACKETIZED_HEADER_BYTES` (4)** — the outer header present on every packet: `type` (1) + `session_id` (2) + `body_len` (1). +- **`PACKETIZED_MAX_BODY_BYTES` (255)** — the maximum body length represented by the one-byte `body_len` field. +- **`PACKETIZED_DATA_OVERHEAD_BYTES` (2)** — the non-payload prefix inside every DATA body: `packet_id` (2). +- **`PACKETIZED_MAX_PACKET_DATA_SIZE` (253)** — the maximum DATA payload bytes per packet: `PACKETIZED_MAX_BODY_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES`. +- **`PACKETIZED_RETRANS_MAX_IDS` (127)** — the maximum packet IDs in one RETRANS_REQ body: `(255 - 1) / 2`. + +The maximum usable payload per DATA packet is therefore `min(inner_mtu - PACKETIZED_HEADER_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES, PACKETIZED_MAX_PACKET_DATA_SIZE)`. A configured payload size may choose any smaller valid value. + +### Header (4 bytes, always present) + +| Offset | Size | Field | Description | +|--------|------|--------------|------------------------------------------| +| 0 | 1 | `type` | Message type (see below) | +| 1–2 | 2 | `session_id` | Big-endian uint16; identifies a transfer | +| 3 | 1 | `body_len` | Number of body bytes that follow | + +### METADATA body (9 bytes) — type = 1 + +Sent once at the start of each transfer. Contains everything the receiver needs to allocate state and validate the reassembled message. The checksum is Fletcher-16 over the full original message, seeded with `PACKETIZED_FLETCHER16_INIT` (`0xFFFF`). + +| Offset | Size | Field | Description | +|--------|------|----------------------|-------------------------------------------| +| 0–1 | 2 | `total_packets` | Big-endian uint16; number of DATA packets | +| 2 | 1 | `tx_packet_data_size`| Payload bytes per DATA packet | +| 3–6 | 4 | `total_message_size` | Big-endian uint32; full message bytes | +| 7–8 | 2 | `checksum` | Big-endian Fletcher-16 of the full message| + +### DATA body (2 + N bytes) — type = 2 + +One packet per chunk of the message. `N` can be zero for an empty message; otherwise `N <= tx_packet_data_size`, and the last packet may be shorter than earlier packets. + +| Offset | Size | Field | Description | +|--------|------|-------------|------------------------------------------| +| 0–1 | 2 | `packet_id` | Big-endian uint16; zero-based index | +| 2–... | N | `payload` | Slice of the original message | + +### RETRANS_REQ body (1 + 2·count bytes) — type = 3 + +Sent by the receiver when it detects missing packets after a timeout (200 ms). The receiver splits requests so each RETRANS_REQ fits both the inner MTU and the 255-byte body limit. A zero `count` is accepted as a no-op, although the current receiver only emits requests with missing IDs. + +| Offset | Size | Field | Description | +|--------|------|-----------|----------------------------------------------| +| 0 | 1 | `count` | Number of packet IDs being requested | +| 1–2 | 2 | `id[0]` | Big-endian uint16 packet_id | +| 3–4 | 2 | `id[1]` | … | + +--- + +## Validation Rules + +Incoming packetized messages are parsed only on `$`-prefixed channels. Packets shorter than `PACKETIZED_HEADER_BYTES`, packets whose `body_len` does not match the received length, and unknown packet types are ignored. + +METADATA packets are accepted only when: + +- `body_len == PACKETIZED_METADATA_BODY_BYTES`. +- `total_packets > 0` and `packet_data_size > 0`. +- `packet_data_size <= PACKETIZED_MAX_PACKET_DATA_SIZE`. +- `total_message_size <= zt->mtu`. +- `total_packets == ceil(total_message_size / packet_data_size)`, with zero-length messages represented by one DATA packet. +- The receive buffers are already large enough, or dynamic growth succeeds. + +DATA packets for inactive sessions or mismatched session IDs are ignored. DATA packets for the active session are invalid if the packet ID is out of range, if the body is shorter than the packet ID field, or if the payload length does not exactly match the expected chunk size for that packet. Duplicate DATA packets refresh the session timestamp, but do not rewrite data or increment `received_count`. + +RETRANS_REQ packets for inactive sessions or mismatched session IDs are ignored. RETRANS_REQ bodies must be exactly `1 + 2 * count` bytes, and `count` must fit the transmitter's retransmission ID buffer. Requested packet IDs outside the active transmit session are skipped when retransmissions are sent. + +`ZCM_EINVALID` from packet processing is treated as a dropped bad packet; the transport continues processing later packets. + +## Data Structures + +### `packetized_rx_state_t` + +Tracks the state of one active receive session. + +| Field | Type | Purpose | +|---------------------|------------|---------------------------------------------------------------| +| `active` | `int` | Non-zero while a session is being reassembled | +| `channel[]` | `char` | ZCM channel name for the in-progress message | +| `session_id` | `uint16_t` | Session ID from METADATA; used to match DATA packets | +| `total_packets` | `uint16_t` | Expected number of DATA packets | +| `packet_data_size` | `uint8_t` | Payload bytes per packet (from METADATA) | +| `total_message_size`| `uint32_t` | Full message size in bytes | +| `expected_checksum` | `uint16_t` | Fletcher-16 checksum to validate after reassembly | +| `last_update_utime` | `uint64_t` | Timestamp of the last received DATA packet (retrans trigger) | +| `data` | `uint8_t*` | Reassembly buffer — payload slices are written here | +| `packet_received[]` | `uint8_t*` | Per-packet arrival flag (1 = received, 0 = missing) | +| `received_count` | `uint16_t` | Number of DATA packets received so far | +| `retrans_pending` | `int` | A RETRANS_REQ needs to be sent on the next update | +| `missing_ids[]` | `uint16_t*`| List of packet IDs not yet received, built at retrans time | +| `missing_count` | `uint16_t` | Number of entries in `missing_ids` | +| `packet_capacity` | `size_t` | Allocated capacity for packet tracking arrays; kept in step with message buffer bytes | + +### `packetized_tx_state_t` + +Tracks the state of one active transmit session; kept alive to service retransmission requests. + +| Field | Type | Purpose | +|----------------------|------------|--------------------------------------------------------------| +| `active` | `int` | Non-zero while the session is live (awaiting possible retrans)| +| `channel[]` | `char` | ZCM channel name of the outgoing message | +| `session_id` | `uint16_t` | Session ID for this send; generated by incrementing `next_session_id` | +| `total_packets` | `uint16_t` | Total DATA packets sent | +| `packet_data_size` | `uint8_t` | Payload bytes per packet used for this session | +| `total_message_size` | `uint32_t` | Full message size in bytes | +| `data` | `uint8_t*` | Copy of the sent message, held for retransmission | +| `retrans_ids[]` | `uint16_t*`| Packet IDs requested for retransmission by the receiver | +| `retrans_count` | `uint16_t` | Number of pending retransmission requests | +| `packet_capacity` | `size_t` | Allocated capacity for retransmit IDs; kept in step with message buffer bytes | + +### `zcm_trans_packetized_serial_t` + +The top-level transport object. Owns both the TX and RX state plus all shared resources. + +| Field | Type | Purpose | +|------------------------------|---------------------|-----------------------------------------------------------------| +| `trans` | `zcm_trans_t` | The vtable pointer — satisfies the `zcm_trans_t` interface | +| `inner` | `zcm_trans_t*` | The underlying generic serial transport | +| `inner_mtu` | `size_t` | MTU of the inner transport; sets the maximum packet size | +| `mtu` | `size_t` | Max message size this transport advertises to ZCM | +| `max_message_size` | `size_t` | Current reassembly/transmit buffer capacity | +| `dynamic` | `int` | If non-zero, buffers grow automatically to fit larger messages | +| `configured_packet_data_size`| `uint8_t` | Payload bytes per DATA packet (auto-selected or user-specified) | +| `pkt_buf` / `pkt_buf_size` | `uint8_t*` / `size_t` | Scratch buffer for building one outbound inner-transport packet| +| `out_buf` / `out_buf_size` | `uint8_t*` / `size_t` | One-slot output queue for pass-through or reassembled messages | +| `out_len` / `out_utime` | `size_t` / `uint64_t` | Length and timestamp of the queued message | +| `out_channel[]` | `char` | Channel of the queued message; `$` prefix stripped after reassembly | +| `out_pending` | `int` | Non-zero if `out_buf` holds an undelivered message | +| `next_session_id` | `uint16_t` | Incremented for each packetized send; wraps naturally | +| `rx` | `packetized_rx_state_t` | Current receive session state | +| `tx` | `packetized_tx_state_t` | Current transmit session state | +| `time` / `time_usr` | function pointer | Callback for current timestamp in microseconds | + +--- + +## Flowcharts + +### Sending a Message + +``` +packetized_serial_sendmsg(msg) + │ + ▼ +send_pending_retransmissions() +send_retrans_request() + │ + ▼ + channel starts with '$'? + │ │ + No Yes + │ │ + ▼ ▼ +send to inner validate size and grow buffers if needed +transport tx_packet_data_size = configured_packet_data_size +(pass-through) total_packets = ceil(msg.len / tx_packet_data_size) + zero-length messages still use one DATA packet + │ + ▼ + copy msg to tx->data + save active session in tx state + │ + ▼ + Build METADATA packet: + [total_packets | tx_packet_data_size | + total_message_size | Fletcher-16 checksum] + Send via inner transport + │ + ▼ + For packet_id = 0 .. total_packets-1: + slice = msg.buf[packet_id * chunk .. +chunk] + Build DATA packet: [packet_id | slice] + Send via inner transport + │ + ▼ + return ZCM_EOK +``` + +### Receiving a Message + +``` +packetized_serial_recvmsg() + │ + ▼ + out_pending? ──Yes──► deliver queued message, return ZCM_EOK + │ + No + │ + ▼ + process_incoming_messages() + │ + ▼ + zcm_trans_recvmsg(inner) ──EAGAIN──► maybe_schedule_retrans_request() + │ │ + packet timeout elapsed & + received packets missing? + │ │ + ▼ Yes + channel starts with '$'? │ + │ │ ▼ + No Yes build missing_ids list + │ │ set retrans_pending = 1 + ▼ ▼ │ + queue output Parse header: ▼ + (pass-through) type / session_id / body_len send_retrans_request() + │ send RETRANS_REQ packet(s) + ┌───────────────┼────────────────┐ + ▼ ▼ ▼ + METADATA DATA RETRANS_REQ + │ │ │ + ▼ ▼ ▼ + begin_rx_session process_rx_data process_retrans_request + │ │ store retrans_ids in tx state + validate params validate exact │ + grow buffers payload length ▼ + if needed write new slice (retransmissions sent on + store session into rx->data next sendmsg or update_tx) + state │ + mark if new + │ + ▼ + all packets received? + │ │ + No Yes + │ │ + │ ▼ + │ validate Fletcher-16 checksum + │ pass? fail? + │ │ │ + │ ▼ ▼ + │ queue_output rx_clear + │ rx_clear return EINVALID + │ │ + └───────┘ + │ + ▼ + deliver message + return ZCM_EOK +``` + +--- + +## Retransmission + +When the receiver stops getting DATA packets mid-transfer, it detects the stall by comparing the current time against `rx->last_update_utime`. If the gap exceeds `PACKETIZED_RETRANS_TIMEOUT_US` (200 ms), it: + +1. Builds the list of missing `packet_id` values into `rx->missing_ids`. +2. Sets `rx->retrans_pending = 1`. +3. On the next call to `send_retrans_request`, sends one or more RETRANS_REQ packets (splitting across multiple inner-MTU packets if there are many missing IDs). + +`send_retrans_request` splits large missing-ID lists by the smaller of the inner MTU limit and `PACKETIZED_RETRANS_MAX_IDS`. On the transmitter side, `process_retrans_request` stores the requested IDs in `tx->retrans_ids`. They are sent by `send_pending_retransmissions`, which is called at the start of every `sendmsg` and `update_tx`. + +--- + +## Buffer Sizing and Dynamic Growth + +At creation time, `zcm_trans_packetized_serial_create` receives a `tx_packet_data_size` argument (0 = auto-select the largest value that fits in the inner MTU) and a `max_message_size` argument (0 = dynamic). + +- The inner MTU must fit at least one DATA packet with one payload byte and one METADATA packet. +- If `tx_packet_data_size == 0`, the transport chooses the largest DATA payload that fits the inner MTU, capped at `PACKETIZED_MAX_PACKET_DATA_SIZE`. +- If `tx_packet_data_size > 0`, it must be no larger than `PACKETIZED_MAX_PACKET_DATA_SIZE` and `PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES + tx_packet_data_size` must fit within the inner MTU. +- The advertised packetized MTU is `configured_packet_data_size * PACKETIZED_MAX_PACKETS`, capped to `max_message_size` in fixed mode. +- In **fixed** mode (`max_message_size > 0`), buffers are sized once at creation and packetized messages larger than `max_message_size` are rejected with `ZCM_EINVALID`. +- In **dynamic** mode (`max_message_size == 0`), buffers start at `PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE` and `grow_buffers()` is called whenever an incoming METADATA packet or outgoing message exceeds the current allocation. The RX data, RX packet flags, RX missing IDs, TX data, TX retransmit IDs, and output buffer are grown as needed. If growth fails, the operation returns `ZCM_EAGAIN`. diff --git a/zcm/transport/packetized_serial_protocol.h b/zcm/transport/packetized_serial_protocol.h index 2f00070f2..1b67e72b2 100644 --- a/zcm/transport/packetized_serial_protocol.h +++ b/zcm/transport/packetized_serial_protocol.h @@ -6,13 +6,59 @@ #include "generic_serial_fletcher.h" -#define PACKETIZED_HEADER_BYTES (4) -#define PACKETIZED_DATA_OVERHEAD_BYTES (2) -#define PACKETIZED_METADATA_BODY_BYTES (9) +#define PACKETIZED_U8_BYTES (1) +#define PACKETIZED_U16_BYTES (2) +#define PACKETIZED_U32_BYTES (4) +#define PACKETIZED_MAX_BODY_BYTES ((size_t)UINT8_MAX) + +#define PACKETIZED_HEADER_TYPE_OFFSET (0) +#define PACKETIZED_HEADER_TYPE_BYTES (PACKETIZED_U8_BYTES) +#define PACKETIZED_HEADER_SESSION_ID_OFFSET \ + (PACKETIZED_HEADER_TYPE_OFFSET + PACKETIZED_HEADER_TYPE_BYTES) +#define PACKETIZED_HEADER_SESSION_ID_BYTES (PACKETIZED_U16_BYTES) +#define PACKETIZED_HEADER_BODY_LEN_OFFSET \ + (PACKETIZED_HEADER_SESSION_ID_OFFSET + PACKETIZED_HEADER_SESSION_ID_BYTES) +#define PACKETIZED_HEADER_BODY_LEN_BYTES (PACKETIZED_U8_BYTES) +#define PACKETIZED_HEADER_BYTES \ + (PACKETIZED_HEADER_BODY_LEN_OFFSET + PACKETIZED_HEADER_BODY_LEN_BYTES) + +#define PACKETIZED_DATA_PACKET_ID_OFFSET (0) +#define PACKETIZED_DATA_PACKET_ID_BYTES (PACKETIZED_U16_BYTES) +#define PACKETIZED_DATA_PAYLOAD_OFFSET \ + (PACKETIZED_DATA_PACKET_ID_OFFSET + PACKETIZED_DATA_PACKET_ID_BYTES) +#define PACKETIZED_DATA_OVERHEAD_BYTES (PACKETIZED_DATA_PAYLOAD_OFFSET) +#define PACKETIZED_MIN_DATA_PAYLOAD_BYTES (1) +#define PACKETIZED_MAX_PACKET_DATA_SIZE \ + (PACKETIZED_MAX_BODY_BYTES - PACKETIZED_DATA_OVERHEAD_BYTES) + +#define PACKETIZED_METADATA_TOTAL_PACKETS_OFFSET (0) +#define PACKETIZED_METADATA_TOTAL_PACKETS_BYTES (PACKETIZED_U16_BYTES) +#define PACKETIZED_METADATA_PACKET_DATA_SIZE_OFFSET \ + (PACKETIZED_METADATA_TOTAL_PACKETS_OFFSET + PACKETIZED_METADATA_TOTAL_PACKETS_BYTES) +#define PACKETIZED_METADATA_PACKET_DATA_SIZE_BYTES (PACKETIZED_U8_BYTES) +#define PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_OFFSET \ + (PACKETIZED_METADATA_PACKET_DATA_SIZE_OFFSET + PACKETIZED_METADATA_PACKET_DATA_SIZE_BYTES) +#define PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_BYTES (PACKETIZED_U32_BYTES) +#define PACKETIZED_METADATA_CHECKSUM_OFFSET \ + (PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_OFFSET + PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_BYTES) +#define PACKETIZED_METADATA_CHECKSUM_BYTES (PACKETIZED_U16_BYTES) +#define PACKETIZED_METADATA_BODY_BYTES \ + (PACKETIZED_METADATA_CHECKSUM_OFFSET + PACKETIZED_METADATA_CHECKSUM_BYTES) + +#define PACKETIZED_RETRANS_COUNT_OFFSET (0) +#define PACKETIZED_RETRANS_COUNT_BYTES (PACKETIZED_U8_BYTES) +#define PACKETIZED_RETRANS_IDS_OFFSET \ + (PACKETIZED_RETRANS_COUNT_OFFSET + PACKETIZED_RETRANS_COUNT_BYTES) +#define PACKETIZED_RETRANS_ID_BYTES (PACKETIZED_U16_BYTES) +#define PACKETIZED_RETRANS_BODY_BYTES(count) \ + (PACKETIZED_RETRANS_IDS_OFFSET + ((size_t)(count) * PACKETIZED_RETRANS_ID_BYTES)) +#define PACKETIZED_RETRANS_MAX_IDS \ + ((PACKETIZED_MAX_BODY_BYTES - PACKETIZED_RETRANS_IDS_OFFSET) / PACKETIZED_RETRANS_ID_BYTES) + #define PACKETIZED_RETRANS_TIMEOUT_US (200000) -#define PACKETIZED_MAX_PACKET_DATA_SIZE (253) #define PACKETIZED_MAX_PACKETS ((size_t)UINT16_MAX) #define PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE (1024) +#define PACKETIZED_FLETCHER16_INIT (0xFFFFu) typedef enum packetized_msg_type_t { diff --git a/zcm/transport/packetized_serial_transport.c b/zcm/transport/packetized_serial_transport.c index 5834bc9eb..27aca1563 100644 --- a/zcm/transport/packetized_serial_transport.c +++ b/zcm/transport/packetized_serial_transport.c @@ -11,6 +11,9 @@ #include #include +#define PACKETIZED_CHANNEL_PREFIX '$' +#define PACKETIZED_SEND_RETRY_COUNT (4) + typedef struct packetized_rx_state_t packetized_rx_state_t; struct packetized_rx_state_t { @@ -118,7 +121,7 @@ static int send_inner_with_retry(zcm_trans_packetized_serial_t* zt, zcm_msg_t ms if (ret == ZCM_EOK) return ZCM_EOK; if (ret != ZCM_EAGAIN) return ret; - for (i = 0; i < 4; ++i) { + for (i = 0; i < PACKETIZED_SEND_RETRY_COUNT; ++i) { zcm_trans_update(zt->inner); ret = zcm_trans_sendmsg(zt->inner, msg); if (ret == ZCM_EOK) return ZCM_EOK; @@ -135,9 +138,10 @@ static int send_packet(zcm_trans_packetized_serial_t* zt, const char* channel, if (PACKETIZED_HEADER_BYTES + (size_t)body_len > zt->pkt_buf_size) return ZCM_EINVALID; - zt->pkt_buf[0] = type; - __int16_t_encode_array(zt->pkt_buf, 1, 2, (int16_t*)(&session_id), 1); - zt->pkt_buf[3] = body_len; + zt->pkt_buf[PACKETIZED_HEADER_TYPE_OFFSET] = type; + __int16_t_encode_array(zt->pkt_buf, PACKETIZED_HEADER_SESSION_ID_OFFSET, + PACKETIZED_HEADER_SESSION_ID_BYTES, (int16_t*)(&session_id), 1); + zt->pkt_buf[PACKETIZED_HEADER_BODY_LEN_OFFSET] = body_len; if (body_len > 0 && body != &zt->pkt_buf[PACKETIZED_HEADER_BYTES]) { memcpy(&zt->pkt_buf[PACKETIZED_HEADER_BYTES], body, body_len); } @@ -184,7 +188,7 @@ static void tx_clear(packetized_tx_state_t* tx) static int is_packetized_channel(const char* channel) { - return channel && channel[0] == '$'; + return channel && channel[0] == PACKETIZED_CHANNEL_PREFIX; } static int send_retrans_request(zcm_trans_packetized_serial_t* zt) @@ -193,9 +197,14 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) packetized_rx_state_t* rx = &zt->rx; if (!rx->retrans_pending || !rx->active || rx->missing_count == 0) return ZCM_EOK; - size_t max_ids_per_req = (zt->inner_mtu > PACKETIZED_HEADER_BYTES + 1) - ? (zt->inner_mtu - PACKETIZED_HEADER_BYTES - 1) / 2 + size_t max_ids_per_req = (zt->inner_mtu > PACKETIZED_HEADER_BYTES + + PACKETIZED_RETRANS_IDS_OFFSET) + ? (zt->inner_mtu - PACKETIZED_HEADER_BYTES - + PACKETIZED_RETRANS_IDS_OFFSET) / + PACKETIZED_RETRANS_ID_BYTES : 0; + if (max_ids_per_req > PACKETIZED_RETRANS_MAX_IDS) + max_ids_per_req = PACKETIZED_RETRANS_MAX_IDS; if (max_ids_per_req == 0) return ZCM_EINVALID; sent = 0; @@ -203,11 +212,14 @@ static int send_retrans_request(zcm_trans_packetized_serial_t* zt) size_t i; size_t remaining = rx->missing_count - sent; size_t count = remaining < max_ids_per_req ? remaining : max_ids_per_req; - size_t body_len = 1 + count * 2; + size_t body_len = PACKETIZED_RETRANS_BODY_BYTES(count); uint8_t* body = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - body[0] = (uint8_t)count; + body[PACKETIZED_RETRANS_COUNT_OFFSET] = (uint8_t)count; for (i = 0; i < count; ++i) { - __int16_t_encode_array(body, 1 + i * 2, 2, (int16_t*)(&rx->missing_ids[sent + i]), 1); + __int16_t_encode_array(body, PACKETIZED_RETRANS_IDS_OFFSET + + i * PACKETIZED_RETRANS_ID_BYTES, + PACKETIZED_RETRANS_ID_BYTES, + (int16_t*)(&rx->missing_ids[sent + i]), 1); } int ret = send_packet(zt, rx->channel, rx->session_id, PACKETIZED_MSG_RETRANS_REQ, @@ -230,7 +242,7 @@ static int queue_output(zcm_trans_packetized_serial_t* zt, const char* channel, if (len > 0) memcpy(zt->out_buf, data, len); - if (strip_packetized_prefix && channel[0] == '$') channel += 1; + if (strip_packetized_prefix && channel[0] == PACKETIZED_CHANNEL_PREFIX) channel += 1; strncpy(zt->out_channel, channel, ZCM_CHANNEL_MAXLEN); zt->out_channel[ZCM_CHANNEL_MAXLEN] = '\0'; zt->out_len = len; @@ -270,8 +282,10 @@ static int send_pending_retransmissions(zcm_trans_packetized_serial_t* zt) uint32_t remaining = tx->total_message_size - offset; uint8_t payload_len = (uint8_t)(remaining < chunk ? remaining : chunk); - __int16_t_encode_array(body, 0, 2, (int16_t*)(&packet_id), 1); - if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); + __int16_t_encode_array(body, PACKETIZED_DATA_PACKET_ID_OFFSET, + PACKETIZED_DATA_PACKET_ID_BYTES, (int16_t*)(&packet_id), 1); + if (payload_len > 0) + memcpy(&body[PACKETIZED_DATA_PAYLOAD_OFFSET], tx->data + offset, payload_len); int ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, (uint8_t)(PACKETIZED_DATA_OVERHEAD_BYTES + payload_len)); @@ -289,12 +303,21 @@ static int begin_rx_session(zcm_trans_packetized_serial_t* zt, const char* chann if (body_len != PACKETIZED_METADATA_BODY_BYTES) return ZCM_EINVALID; uint16_t total_packets = 0; - uint8_t packet_data_size = body[2]; + uint8_t packet_data_size = body[PACKETIZED_METADATA_PACKET_DATA_SIZE_OFFSET]; uint32_t total_message_size = 0; uint16_t expected_checksum = 0; - if (__int16_t_decode_array(body, 0, 2, (int16_t*)(&total_packets), 1) < 0) return ZCM_EINVALID; - if (__int32_t_decode_array(body, 3, 4, (int32_t*)(&total_message_size), 1) < 0) return ZCM_EINVALID; - if (__int16_t_decode_array(body, 7, 2, (int16_t*)(&expected_checksum), 1) < 0) return ZCM_EINVALID; + if (__int16_t_decode_array(body, PACKETIZED_METADATA_TOTAL_PACKETS_OFFSET, + PACKETIZED_METADATA_TOTAL_PACKETS_BYTES, + (int16_t*)(&total_packets), 1) < 0) + return ZCM_EINVALID; + if (__int32_t_decode_array(body, PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_OFFSET, + PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_BYTES, + (int32_t*)(&total_message_size), 1) < 0) + return ZCM_EINVALID; + if (__int16_t_decode_array(body, PACKETIZED_METADATA_CHECKSUM_OFFSET, + PACKETIZED_METADATA_CHECKSUM_BYTES, + (int16_t*)(&expected_checksum), 1) < 0) + return ZCM_EINVALID; if (total_packets == 0 || packet_data_size == 0) return ZCM_EINVALID; if (packet_data_size > PACKETIZED_MAX_PACKET_DATA_SIZE) return ZCM_EINVALID; @@ -335,7 +358,8 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (body_len < PACKETIZED_DATA_OVERHEAD_BYTES) return ZCM_EINVALID; uint16_t packet_id = 0; - __int16_t_decode_array(body, 0, 2, (int16_t*)(&packet_id), 1); + __int16_t_decode_array(body, PACKETIZED_DATA_PACKET_ID_OFFSET, + PACKETIZED_DATA_PACKET_ID_BYTES, (int16_t*)(&packet_id), 1); uint8_t payload_len = (uint8_t)(body_len - PACKETIZED_DATA_OVERHEAD_BYTES); if (packet_id >= rx->total_packets) return ZCM_EINVALID; @@ -349,7 +373,8 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i if (payload_len != expected_len) return ZCM_EINVALID; if (!rx->packet_received[packet_id]) { - if (payload_len > 0) memcpy(rx->data + offset, &body[2], payload_len); + if (payload_len > 0) + memcpy(rx->data + offset, &body[PACKETIZED_DATA_PAYLOAD_OFFSET], payload_len); rx->packet_received[packet_id] = 1; ++rx->received_count; } @@ -357,7 +382,8 @@ static int process_rx_data(zcm_trans_packetized_serial_t* zt, uint16_t session_i rx->last_update_utime = utime; if (rx->received_count == rx->total_packets) { - uint16_t checksum = fletcher16(rx->data, rx->total_message_size, 0xFFFF); + uint16_t checksum = + fletcher16(rx->data, rx->total_message_size, PACKETIZED_FLETCHER16_INIT); if (checksum != rx->expected_checksum) { rx_clear(rx); return ZCM_EINVALID; @@ -381,10 +407,10 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s uint8_t i; packetized_tx_state_t* tx = &zt->tx; if (!tx->active || tx->session_id != session_id) return ZCM_EOK; - if (body_len < 1) return ZCM_EINVALID; + if (body_len < PACKETIZED_RETRANS_IDS_OFFSET) return ZCM_EINVALID; - uint8_t count = body[0]; - if ((size_t)(1 + (size_t)count * 2) != body_len) return ZCM_EINVALID; + uint8_t count = body[PACKETIZED_RETRANS_COUNT_OFFSET]; + if (PACKETIZED_RETRANS_BODY_BYTES(count) != body_len) return ZCM_EINVALID; tx->retrans_count = 0; if (count == 0) return ZCM_EOK; @@ -393,7 +419,9 @@ static int process_retrans_request(zcm_trans_packetized_serial_t* zt, uint16_t s tx->retrans_count = count; for (i = 0; i < count; ++i) { uint16_t packet_id = 0; - __int16_t_decode_array(body, 1 + (size_t)i * 2, 2, (int16_t *)(&packet_id), 1); + __int16_t_decode_array(body, PACKETIZED_RETRANS_IDS_OFFSET + + (size_t)i * PACKETIZED_RETRANS_ID_BYTES, + PACKETIZED_RETRANS_ID_BYTES, (int16_t*)(&packet_id), 1); tx->retrans_ids[i] = packet_id; } @@ -468,11 +496,16 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ tx->channel[ZCM_CHANNEL_MAXLEN] = '\0'; uint8_t* meta = zt->pkt_buf + PACKETIZED_HEADER_BYTES; - uint16_t fletcherCsum = fletcher16(msg.buf, msg.len, 0xFFFF); - __int16_t_encode_array(meta, 0, 2, (int16_t*)(&total_packets), 1); - meta[2] = tx_packet_data_size; - __int32_t_encode_array(meta, 3, 4, (int32_t*)(&total_message_size), 1); - __int16_t_encode_array(meta, 7, 2, (int16_t*)(&fletcherCsum), 1); + uint16_t fletcherCsum = fletcher16(msg.buf, msg.len, PACKETIZED_FLETCHER16_INIT); + __int16_t_encode_array(meta, PACKETIZED_METADATA_TOTAL_PACKETS_OFFSET, + PACKETIZED_METADATA_TOTAL_PACKETS_BYTES, + (int16_t*)(&total_packets), 1); + meta[PACKETIZED_METADATA_PACKET_DATA_SIZE_OFFSET] = tx_packet_data_size; + __int32_t_encode_array(meta, PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_OFFSET, + PACKETIZED_METADATA_TOTAL_MESSAGE_SIZE_BYTES, + (int32_t*)(&total_message_size), 1); + __int16_t_encode_array(meta, PACKETIZED_METADATA_CHECKSUM_OFFSET, + PACKETIZED_METADATA_CHECKSUM_BYTES, (int16_t*)(&fletcherCsum), 1); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_METADATA, meta, PACKETIZED_METADATA_BODY_BYTES); if (ret != ZCM_EOK) return ret; @@ -484,8 +517,10 @@ static int packetized_serial_sendmsg(zcm_trans_packetized_serial_t* zt, zcm_msg_ uint8_t payload_len = (uint8_t)(remaining < tx_packet_data_size ? remaining : tx_packet_data_size); - __int16_t_encode_array(body, 0, 2, (int16_t*)(&packet_id), 1); - if (payload_len > 0) memcpy(&body[2], tx->data + offset, payload_len); + __int16_t_encode_array(body, PACKETIZED_DATA_PACKET_ID_OFFSET, + PACKETIZED_DATA_PACKET_ID_BYTES, (int16_t*)(&packet_id), 1); + if (payload_len > 0) + memcpy(&body[PACKETIZED_DATA_PAYLOAD_OFFSET], tx->data + offset, payload_len); ret = send_packet(zt, tx->channel, tx->session_id, PACKETIZED_MSG_DATA, body, (uint8_t)(PACKETIZED_DATA_OVERHEAD_BYTES + payload_len)); @@ -514,10 +549,11 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) if (in.len < PACKETIZED_HEADER_BYTES) continue; - uint8_t type = in.buf[0]; + uint8_t type = in.buf[PACKETIZED_HEADER_TYPE_OFFSET]; uint16_t session_id = 0; - __int16_t_decode_array(in.buf, 1, 2, (int16_t*)(&session_id), 1); - uint8_t body_len = in.buf[3]; + __int16_t_decode_array(in.buf, PACKETIZED_HEADER_SESSION_ID_OFFSET, + PACKETIZED_HEADER_SESSION_ID_BYTES, (int16_t*)(&session_id), 1); + uint8_t body_len = in.buf[PACKETIZED_HEADER_BODY_LEN_OFFSET]; if (in.len != PACKETIZED_HEADER_BYTES + body_len) continue; const uint8_t* body = &in.buf[PACKETIZED_HEADER_BYTES]; @@ -533,7 +569,6 @@ static int process_incoming_messages(zcm_trans_packetized_serial_t* zt) continue; } - // RRR: should we break? could end up blocking for a while otherwise if (ret != ZCM_EOK && ret != ZCM_EINVALID) return ret; } @@ -647,14 +682,15 @@ zcm_trans_t* zcm_trans_packetized_serial_create( } zt->inner_mtu = zcm_trans_get_mtu(zt->inner); - if (zt->inner_mtu < PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES + 1 || + if (zt->inner_mtu < PACKETIZED_HEADER_BYTES + PACKETIZED_DATA_OVERHEAD_BYTES + + PACKETIZED_MIN_DATA_PAYLOAD_BYTES || zt->inner_mtu < PACKETIZED_HEADER_BYTES + PACKETIZED_METADATA_BODY_BYTES) { zcm_trans_generic_serial_destroy(zt->inner); free(zt); return NULL; } - zt->dynamic = (max_message_size == 0); + zt->dynamic = (max_message_size == ZCM_TRANS_PACKETIZED_SERIAL_GROW_DYNAMICALLY); zt->max_message_size = zt->dynamic ? PACKETIZED_DEFAULT_MAX_MESSAGE_SIZE : max_message_size; if (tx_packet_data_size == 0) {