From 239afa9a667b6e200f79ddec7f34b3f29080fff4 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 7 Mar 2023 09:33:20 -0500 Subject: [PATCH 1/9] Add COBS implementation of ZCM serial transport --- .../generic_serial_cobs_transport.c | 326 ++++++++++++++++++ .../generic_serial_cobs_transport.h | 28 ++ zcm/transport/generic_serial_fletcher.h | 29 ++ zcm/util/buffer_utils.h | 56 +++ zcm/util/cobs.h | 86 +++++ 5 files changed, 525 insertions(+) create mode 100644 zcm/transport/cobs_serial/generic_serial_cobs_transport.c create mode 100644 zcm/transport/cobs_serial/generic_serial_cobs_transport.h create mode 100644 zcm/util/buffer_utils.h create mode 100644 zcm/util/cobs.h diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c new file mode 100644 index 00000000..0a073b7a --- /dev/null +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -0,0 +1,326 @@ +#include "zcm/transport/cobs_serial/generic_serial_cobs_transport.h" +#include "zcm/transport.h" +#include "zcm/transport/generic_serial_circ_buff.h" +#include "zcm/transport/generic_serial_fletcher.h" +#include "zcm/util/buffer_utils.h" +#include "zcm/util/cobs.h" +#include "zcm/zcm.h" + +#include +#include +#include +#include + +#ifndef ZCM_COBS_SERIAL_TERM_CHAR +#define ZCM_COBS_SERIAL_TERM_CHAR (0x00) +#endif + +#define ASSERT(x) + +// Framing (size = 8 + chanLen + data_len) +// chanLen +// data_len (4 bytes) +// *chan +// *data +// sum1(*chan, *data) +// sum2(*chan, *data) +// 0x00 -- termination char +#define FRAME_BYTES 8 + +typedef struct zcm_trans_cobs_serial_t { + zcm_trans_t trans; // This must be first to preserve pointer casting + + circBuffer_t sendBuffer; + circBuffer_t recvBuffer; + uint8_t recvChanName[ZCM_CHANNEL_MAXLEN + 1]; + size_t mtu; + uint8_t* sendMsgData; + uint8_t* sendMsgDataCobs; + uint8_t* recvMsgData; + uint8_t* recvMsgDataCobs; + + 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 (*time)(void* usr); + void* time_usr; +} zcm_trans_cobs_serial_t; + +static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt); + +size_t serial_cobs_get_mtu(zcm_trans_cobs_serial_t *zt) +{ return zt->mtu; } + +int +serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { + size_t chanLen = strlen(msg.channel); + size_t payloadLen = FRAME_BYTES + chanLen + msg.len; + + if (msg.len > zt->mtu || chanLen > ZCM_CHANNEL_MAXLEN) { + return ZCM_EINVALID; + } + if (payloadLen + cobsMaxOverhead(payloadLen) > cb_room(&zt->sendBuffer)) { + return ZCM_EAGAIN; + } + + uint8_t* pMsgData = zt->sendMsgData; + + // Copy channel and message length + *pMsgData++ = chanLen; + pMsgData = bufferUint32(pMsgData, (uint32_t)msg.len); + + // Copy channel name into msgData + memcpy(pMsgData, msg.channel, chanLen); + pMsgData += chanLen; + + // Copy message contents into msgData + memcpy(pMsgData, msg.buf, msg.len); + pMsgData += msg.len; + + // Calculate Fletcher-16 checksum and add to msgData + uint16_t checksum = fletcher16(zt->sendMsgData, payloadLen - 3); + pMsgData = bufferUint16(pMsgData, checksum); + + // COBS encode the message, subtract 1 for termination char + size_t bytesEncoded = cobsEncode(zt->sendMsgData, payloadLen - 1, zt->sendMsgDataCobs); + if (bytesEncoded <= payloadLen - 1) { + return ZCM_EAGAIN; // encoding failed for some reason + } + + // Push entire message into sendBuffer + for (int i = 0; i < bytesEncoded; ++i) { + cb_push_back(&zt->sendBuffer, zt->sendMsgDataCobs[i]); + } + cb_push_back(&zt->sendBuffer, ZCM_COBS_SERIAL_TERM_CHAR); // terminator byte + + return ZCM_EOK; +} + +int serial_cobs_recvmsg_enable(zcm_trans_cobs_serial_t *zt, const char *channel, bool enable) +{ + // NOTE: not implemented because it is unlikely that a microprocessor is + // going to be hearing messages on a USB comms that it doesn't want + // to hear + return ZCM_EOK; +} + +int +serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { + uint64_t utime = zt->time(zt->time_usr); + size_t incomingSize = cb_size(&zt->recvBuffer); + size_t minMsgSize = FRAME_BYTES + cobsMaxOverhead(FRAME_BYTES); + if (incomingSize < minMsgSize) { + return ZCM_EAGAIN; + } + + // Search for terminator bytes, starting at the back + int termAddr = -1; + for (int i = incomingSize - 1; i >= minMsgSize; --i) { + if (cb_front(&zt->recvBuffer, i) == ZCM_COBS_SERIAL_TERM_CHAR) { + termAddr = i; // keep track of terminator closest to front + } + } + + // Check if terminator byte was found + if (termAddr == -1) { + return ZCM_EAGAIN; + } + + // Pop CB from front to termAddr + for (int i = 0; i <= termAddr; ++i) { + zt->recvMsgDataCobs[i] = cb_front(&zt->recvBuffer, i); + } + cb_pop_front(&zt->recvBuffer, termAddr + 1); + + // COBS decode + size_t decodedBytes = cobsDecode(zt->recvMsgDataCobs, termAddr, zt->recvMsgData); + if (decodedBytes >= termAddr) { + return ZCM_EAGAIN; // decoding failed, probably missing some of message + } + + // Extract channel and message sizes + uint8_t chanLen = 0; + uint8_t* pMsgData = zt->recvMsgData; + + chanLen = *pMsgData++; + msg->len = pMsgData[0]; + msg->len |= pMsgData[1] << 8; + msg->len |= pMsgData[2] << 16; + msg->len |= pMsgData[3] << 24; + pMsgData += 4; + + // Value rationality checks + if (chanLen > ZCM_CHANNEL_MAXLEN) + return ZCM_EAGAIN; + + if (msg->len > zt->mtu) + return ZCM_EAGAIN; + + if (termAddr != FRAME_BYTES + chanLen + msg->len) + return ZCM_EAGAIN; + + // Calculate Fletcher-16 checksum and check against received + uint16_t checksum = 0; + uint16_t receivedCS = 0; + checksum = fletcher16(zt->recvMsgData, termAddr - 3); + receivedCS = zt->recvMsgData[termAddr - 3] | (zt->recvMsgData[termAddr - 2] << 8); + if (receivedCS != checksum) { + return ZCM_EINVALID; + } + + // Copy channel name + memset(&zt->recvChanName, '\0', ZCM_CHANNEL_MAXLEN); + memcpy(zt->recvChanName, pMsgData, chanLen); + pMsgData += chanLen; + + // Copy values into msg + msg->channel = (char*)zt->recvChanName; + msg->buf = pMsgData; + msg->utime = utime; + + return ZCM_EOK; +} + +int serial_cobs_update_rx(zcm_trans_t *_zt) +{ + zcm_trans_cobs_serial_t* zt = cast(_zt); + cb_flush_in(&zt->recvBuffer, zt->get, zt->put_get_usr); + return ZCM_EOK; +} + +int serial_cobs_update_tx(zcm_trans_t *_zt) +{ + zcm_trans_cobs_serial_t* zt = cast(_zt); + cb_flush_out(&zt->sendBuffer, zt->put, zt->put_get_usr); + return ZCM_EOK; +} + +/********************** STATICS **********************/ +static size_t +_serial_get_mtu(zcm_trans_t* zt) { + return serial_cobs_get_mtu(cast(zt)); +} + +static int +_serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { + return serial_cobs_sendmsg(cast(zt), msg); +} + +static int +_serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) { + return serial_cobs_recvmsg_enable(cast(zt), channel, enable); +} + +static int +_serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) { + return serial_cobs_recvmsg(cast(zt), msg, timeout); +} + +static int +_serial_update(zcm_trans_t* zt) { + int rxRet = serial_cobs_update_rx(zt); + int txRet = serial_cobs_update_tx(zt); + return rxRet == ZCM_EOK ? txRet : rxRet; +} + +static zcm_trans_methods_t methods = { + &_serial_get_mtu, + &_serial_sendmsg, + &_serial_recvmsg_enable, + &_serial_recvmsg, + &_serial_update, + &zcm_trans_generic_serial_cobs_destroy, +}; + +static zcm_trans_cobs_serial_t* +cast(zcm_trans_t* zt) { + assert(zt->vtbl == &methods); + return (zcm_trans_cobs_serial_t*)zt; +} + +zcm_trans_t* +zcm_trans_generic_serial_cobs_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) { + if (MTU == 0 || bufSize < FRAME_BYTES + MTU) + return NULL; + zcm_trans_cobs_serial_t* zt = malloc(sizeof(zcm_trans_cobs_serial_t)); + if (zt == NULL) + return NULL; + zt->mtu = MTU; + + // Bytes needed to construct full message + size_t maxPayloadSize = FRAME_BYTES + ZCM_CHANNEL_MAXLEN + zt->mtu; + zt->recvMsgData = malloc(maxPayloadSize * sizeof(uint8_t)); + if (zt->recvMsgData == NULL) { + free(zt); + return NULL; + } + zt->sendMsgData = malloc(maxPayloadSize * sizeof(uint8_t)); + if (zt->sendMsgData == NULL) { + free(zt->recvMsgData); + free(zt); + return NULL; + } + + // Bytes needed to construct full COBS-encoded message + maxPayloadSize += cobsMaxOverhead(maxPayloadSize); + zt->recvMsgDataCobs = malloc(maxPayloadSize * sizeof(uint8_t)); + if (zt->recvMsgDataCobs == NULL) { + free(zt->recvMsgData); + free(zt->sendMsgData); + free(zt); + return NULL; + } + zt->sendMsgDataCobs = malloc(maxPayloadSize * sizeof(uint8_t)); + if (zt->sendMsgDataCobs == NULL) { + free(zt->recvMsgData); + free(zt->sendMsgData); + free(zt->recvMsgDataCobs); + free(zt); + return NULL; + } + + zt->trans.trans_type = ZCM_NONBLOCKING; + zt->trans.vtbl = &methods; + if (!cb_init(&zt->sendBuffer, bufSize)) { + free(zt->recvMsgData); + free(zt->sendMsgData); + free(zt->recvMsgDataCobs); + free(zt->sendMsgDataCobs); + free(zt); + return NULL; + } + if (!cb_init(&zt->recvBuffer, bufSize)) { + cb_deinit(&zt->sendBuffer); + free(zt->recvMsgData); + free(zt->sendMsgData); + free(zt->recvMsgDataCobs); + free(zt->sendMsgDataCobs); + free(zt); + return NULL; + } + + zt->get = get; + zt->put = put; + zt->put_get_usr = put_get_usr; + + zt->time = timestamp_now; + zt->time_usr = time_usr; + + return (zcm_trans_t*)zt; +} + +void +zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* _zt) { + zcm_trans_cobs_serial_t* zt = cast(_zt); + cb_deinit(&zt->recvBuffer); + cb_deinit(&zt->sendBuffer); + free(zt->recvMsgData); + free(zt->sendMsgData); + free(zt->recvMsgDataCobs); + free(zt->sendMsgDataCobs); + free(zt); +} diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.h b/zcm/transport/cobs_serial/generic_serial_cobs_transport.h new file mode 100644 index 00000000..bf5110f4 --- /dev/null +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.h @@ -0,0 +1,28 @@ +#ifndef _ZCM_TRANS_NONBLOCKING_SERIAL_COBS_H +#define _ZCM_TRANS_NONBLOCKING_SERIAL_COBS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "zcm/zcm.h" +#include "zcm/transport.h" + +zcm_trans_t *zcm_trans_generic_serial_cobs_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); + +// frees all resources inside of zt and frees zt itself +void zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* zt); + +#ifdef __cplusplus +} +#endif + +#endif /* _ZCM_TRANS_NONBLOCKING_SERIAL_COBS_H */ diff --git a/zcm/transport/generic_serial_fletcher.h b/zcm/transport/generic_serial_fletcher.h index f06e7de8..8607905e 100644 --- a/zcm/transport/generic_serial_fletcher.h +++ b/zcm/transport/generic_serial_fletcher.h @@ -2,6 +2,7 @@ #define _ZCM_TRANS_NONBLOCKING_SERIAL_FLETCHER_H #include +#include static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) { @@ -19,4 +20,32 @@ static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) return (sumHigh << 8) | sumLow; } +/*! + * @brief Calculate Fletcher-16 checksum + * + * @param data - pointed to array of bytes to calculate checksum for + * @param len - length of byte array + */ +static inline uint16_t fletcher16(const uint8_t* data, size_t len) +{ + uint32_t c0, c1; + + /* Found by solving for c1 overflow: */ + /* n > 0 and n * (n+1) / 2 * (2^8-1) < (2^32-1). */ + for (c0 = c1 = 0; len > 0;) { + size_t blocklen = len; + if (blocklen > 5802) { + blocklen = 5802; + } + len -= blocklen; + do { + c0 = c0 + *data++; + c1 = c1 + c0; + } while (--blocklen); + c0 = c0 % 255; + c1 = c1 % 255; + } + return (c1 << 8 | c0); +} + #endif /* _ZCM_TRANS_NONBLOCKING_FLETCHER_H */ diff --git a/zcm/util/buffer_utils.h b/zcm/util/buffer_utils.h new file mode 100644 index 00000000..7e8c9411 --- /dev/null +++ b/zcm/util/buffer_utils.h @@ -0,0 +1,56 @@ +#ifndef __ZCM_BUFFER_UTILS_H__ +#define __ZCM_BUFFER_UTILS_H__ + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** Pull 1 uint8_t out of a uint32_t + @param var Value to break + @param byteNum Byte number to extract + + @return Extracted byte +*/ +static inline uint8_t breakUint32(uint32_t var, int byteNum) +{ + return (uint8_t)((uint32_t)(((var) >> ((byteNum) * 8)) & 0x00FF)); +} + +/** Break and buffer a uint16_t value - LSB first + @param pBuf Buffer to insert @val into + @param val Value to insert into @pBuf + + @return A pointer to the position immediately following the inserted value +*/ +static inline uint8_t* bufferUint16(uint8_t *pBuf, uint16_t val) +{ + *pBuf++ = val & 0xFF; + *pBuf++ = (val >> 8) & 0xFF; + + return (pBuf); +} + +/** Break and buffer a uint32_t value - LSB first + @param pBuf Buffer to insert @val into + @param val Value to insert into @pBuf + + @return A pointer to the position immediately following the inserted value +*/ +static inline uint8_t* bufferUint32(uint8_t *pBuf, uint32_t val) +{ + *pBuf++ = breakUint32(val, 0); + *pBuf++ = breakUint32(val, 1); + *pBuf++ = breakUint32(val, 2); + *pBuf++ = breakUint32(val, 3); + + return(pBuf); +} + +#ifdef __cplusplus +} +#endif + +#endif /* __ZCM_BUFFER_UTILS_H__ */ diff --git a/zcm/util/cobs.h b/zcm/util/cobs.h new file mode 100644 index 00000000..1eb57645 --- /dev/null +++ b/zcm/util/cobs.h @@ -0,0 +1,86 @@ +#ifndef _ZCM_TRANS_NONBLOCKING_COBS_H +#define _ZCM_TRANS_NONBLOCKING_COBS_H + +#include +#include + +/* Only define inline for C99 builds or better */ +#if (__STDC_VERSION__ >= 199901L) || (__cplusplus) +#define INLINE inline +#else +#define INLINE +#endif + +/** COBS encode data to buffer + @param data Pointer to input data to encode + @param length Number of bytes to encode + @param buffer Pointer to encoded output buffer + + @return Encoded buffer length in bytes + @note Does not output delimiter byte +*/ +static INLINE size_t +cobsEncode(const void* data, size_t length, uint8_t* buffer) { + assert(data && buffer); + + uint8_t* encode = buffer; // Encoded byte pointer + uint8_t* codep = encode++; // Output code pointer + uint8_t code = 1; // Code value + + for (const uint8_t* byte = (const uint8_t*)data; length--; ++byte) { + if (*byte) // Byte not zero, write it + *encode++ = *byte, ++code; + + if (!*byte || code == 0xff) // Input is zero or block completed, restart + { + *codep = code, code = 1, codep = encode; + if (!*byte || length) + ++encode; + } + } + *codep = code; // Write final code value + + return (size_t)(encode - buffer); +} + +/** COBS decode data from buffer + @param buffer Pointer to encoded input bytes + @param length Number of bytes to decode + @param data Pointer to decoded output data + + @return Number of bytes successfully decoded + @note Stops decoding if delimiter byte is found +*/ +static INLINE size_t +cobsDecode(const uint8_t* buffer, size_t length, void* data) { + assert(buffer && data); + + const uint8_t* byte = buffer; // Encoded input byte pointer + uint8_t* decode = (uint8_t*)data; // Decoded output byte pointer + + for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block) { + if (block) // Decode block byte + *decode++ = *byte++; + else { + if (code != 0xff) // Encoded zero, write it + *decode++ = 0; + block = code = *byte++; // Next block length + if (!code) // Delimiter code found + break; + } + } + + return (size_t)(decode - (uint8_t*)data); +} + +/** Calculate maximum COBS encoding overhead + @param msgSize Size of the pre-encoded message + + @return Maximum number of overhead bytes +*/ +static INLINE size_t +cobsMaxOverhead(size_t msgSize) { + return (msgSize + 254 - 1) / 254; // COBS overhead +} + +#endif /* _ZCM_TRANS_NONBLOCKING_COBS_H */ From 0fa861d41c0702fa58190916c14886228f2fe2a6 Mon Sep 17 00:00:00 2001 From: Jonathan Bendes Date: Wed, 8 Mar 2023 16:15:39 -0500 Subject: [PATCH 2/9] First review --- .../generic_serial_cobs_transport.c | 63 ++++++++++++++++++- zcm/transport/generic_serial_fletcher.h | 1 + zcm/util/buffer_utils.h | 3 + zcm/util/cobs.h | 53 +++++++++++++++- 4 files changed, 115 insertions(+), 5 deletions(-) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 0a073b7a..6783e172 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -17,6 +17,8 @@ #define ASSERT(x) +// RRR (Bendes): See comments in other files about + // Framing (size = 8 + chanLen + data_len) // chanLen // data_len (4 bytes) @@ -64,9 +66,20 @@ serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { return ZCM_EAGAIN; } + // RRR (Bendes): This process seems very inefficient. This feels like it + // should be a 0 copy function. Encode directly + // into zt->sendBuffer. I'm not seeing a reason to need 2 + // additional dynamic memory buffers + uint8_t* pMsgData = zt->sendMsgData; + // RRR (Bendes): Look at zcm_coretypes.h if you're looking for a function to + // do this for you. Generic serial just inlined it. I have no + // issue with inlining but if you want a function, use coretypes + // Copy channel and message length + // RRR (Bendes): Breach of SRP. Do the increment on its own line + // Very confusing to read *var++ = x *pMsgData++ = chanLen; pMsgData = bufferUint32(pMsgData, (uint32_t)msg.len); @@ -99,6 +112,9 @@ serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { int serial_cobs_recvmsg_enable(zcm_trans_cobs_serial_t *zt, const char *channel, bool enable) { + // RRR (Bendes): Unneccessary comment. Also this isn't only to be used on + // a microprocessor. This is a transport that can be used on + // any system running zcm // NOTE: not implemented because it is unlikely that a microprocessor is // going to be hearing messages on a USB comms that it doesn't want // to hear @@ -109,12 +125,18 @@ int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { uint64_t utime = zt->time(zt->time_usr); size_t incomingSize = cb_size(&zt->recvBuffer); + // RRR (Bendes): This is calculable at static time. No need to recalculate + // minMsgSize every time size_t minMsgSize = FRAME_BYTES + cobsMaxOverhead(FRAME_BYTES); if (incomingSize < minMsgSize) { return ZCM_EAGAIN; } + // RRR (Bendes): Can't start at the back. You must return messages in the + // order in which you received them + // Search for terminator bytes, starting at the back + // RRR (Bendes): Style in this repo is to refer to these as ...Idx not ...Addr int termAddr = -1; for (int i = incomingSize - 1; i >= minMsgSize; --i) { if (cb_front(&zt->recvBuffer, i) == ZCM_COBS_SERIAL_TERM_CHAR) { @@ -123,17 +145,26 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { } // Check if terminator byte was found + // RRR (Bendes): Can be a one liner without curly braces if (termAddr == -1) { return ZCM_EAGAIN; } // Pop CB from front to termAddr + // RRR (Bendes): As written, this can be multiple messages for (int i = 0; i <= termAddr; ++i) { zt->recvMsgDataCobs[i] = cb_front(&zt->recvBuffer, i); } cb_pop_front(&zt->recvBuffer, termAddr + 1); + // RRR (Bendes): Why copy? Just decode straight out of the circular buffer. + // No benefit to spending time doing multiple copies + // COBS decode + // RRR (Bendes): No need to decode into a temporary buffer before processing + // it. Process as you decode. You can always fail at any point. + // That's why circular buffer is written the way it is where + // you can peek into the buffer instead of popping off of it size_t decodedBytes = cobsDecode(zt->recvMsgDataCobs, termAddr, zt->recvMsgData); if (decodedBytes >= termAddr) { return ZCM_EAGAIN; // decoding failed, probably missing some of message @@ -143,7 +174,13 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { uint8_t chanLen = 0; uint8_t* pMsgData = zt->recvMsgData; + // RRR (Bendes) SRP. Dont increment on the same line where you're doing + // something else. Don't force me to pull up this page to + // understand what your code is doing lol: + // https://en.cppreference.com/w/c/language/operator_precedence chanLen = *pMsgData++; + // RRR (Bendes) Inconsistently using functions for bit manipulation vs + // inlining it. Do one or the other msg->len = pMsgData[0]; msg->len |= pMsgData[1] << 8; msg->len |= pMsgData[2] << 16; @@ -151,12 +188,15 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { pMsgData += 4; // Value rationality checks + // RRR (Bendes): join lines if (chanLen > ZCM_CHANNEL_MAXLEN) return ZCM_EAGAIN; + // RRR (Bendes): join lines if (msg->len > zt->mtu) return ZCM_EAGAIN; + // RRR (Bendes): join lines if (termAddr != FRAME_BYTES + chanLen + msg->len) return ZCM_EAGAIN; @@ -165,6 +205,7 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { uint16_t receivedCS = 0; checksum = fletcher16(zt->recvMsgData, termAddr - 3); receivedCS = zt->recvMsgData[termAddr - 3] | (zt->recvMsgData[termAddr - 2] << 8); + // RRR (Bendes): join lines. get rid of curlies if (receivedCS != checksum) { return ZCM_EINVALID; } @@ -197,26 +238,31 @@ int serial_cobs_update_tx(zcm_trans_t *_zt) } /********************** STATICS **********************/ +// RRR (Bendes): Match style of repo you're in static size_t _serial_get_mtu(zcm_trans_t* zt) { return serial_cobs_get_mtu(cast(zt)); } +// RRR (Bendes): Match style of repo you're in static int _serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return serial_cobs_sendmsg(cast(zt), msg); } +// RRR (Bendes): Match style of repo you're in static int _serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) { return serial_cobs_recvmsg_enable(cast(zt), channel, enable); } +// RRR (Bendes): Match style of repo you're in static int _serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) { return serial_cobs_recvmsg(cast(zt), msg, timeout); } +// RRR (Bendes): Match style of repo you're in static int _serial_update(zcm_trans_t* zt) { int rxRet = serial_cobs_update_rx(zt); @@ -233,12 +279,14 @@ static zcm_trans_methods_t methods = { &zcm_trans_generic_serial_cobs_destroy, }; +// RRR (Bendes): Match style of repo you're in static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt) { assert(zt->vtbl == &methods); return (zcm_trans_cobs_serial_t*)zt; } +// RRR (Bendes): Match style of repo you're in zcm_trans_t* zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), @@ -251,6 +299,15 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, return NULL; zt->mtu = MTU; + // RRR (Bendes): This is excessive. This is a ton of repeated code. + // Figure out a cleaner way to dealloc on failure or just + // use a goto. You can sacrifice speed here (null setting + // everything before allocation and checking them each on + // a failure of any). The constructor doesn't need to be + // doing minimum work. + + // RRR (Bendes): Not sure why so many buffers are needed + // Bytes needed to construct full message size_t maxPayloadSize = FRAME_BYTES + ZCM_CHANNEL_MAXLEN + zt->mtu; zt->recvMsgData = malloc(maxPayloadSize * sizeof(uint8_t)); @@ -283,8 +340,6 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, return NULL; } - zt->trans.trans_type = ZCM_NONBLOCKING; - zt->trans.vtbl = &methods; if (!cb_init(&zt->sendBuffer, bufSize)) { free(zt->recvMsgData); free(zt->sendMsgData); @@ -303,6 +358,9 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, return NULL; } + zt->trans.trans_type = ZCM_NONBLOCKING; + zt->trans.vtbl = &methods; + zt->get = get; zt->put = put; zt->put_get_usr = put_get_usr; @@ -313,6 +371,7 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, return (zcm_trans_t*)zt; } +// RRR (Bendes): Match style of repo you're in void zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* _zt) { zcm_trans_cobs_serial_t* zt = cast(_zt); diff --git a/zcm/transport/generic_serial_fletcher.h b/zcm/transport/generic_serial_fletcher.h index 8607905e..4a2e0727 100644 --- a/zcm/transport/generic_serial_fletcher.h +++ b/zcm/transport/generic_serial_fletcher.h @@ -26,6 +26,7 @@ static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) * @param data - pointed to array of bytes to calculate checksum for * @param len - length of byte array */ +// RRR (Bendes): why is this not deferring to the above? DRY principle :) static inline uint16_t fletcher16(const uint8_t* data, size_t len) { uint32_t c0, c1; diff --git a/zcm/util/buffer_utils.h b/zcm/util/buffer_utils.h index 7e8c9411..c89f103e 100644 --- a/zcm/util/buffer_utils.h +++ b/zcm/util/buffer_utils.h @@ -8,6 +8,9 @@ extern "C" { #endif +// RRR (Bendes): This functionality is already provided via zcm_coretypes.h +// I don't believe this file should be needed + /** Pull 1 uint8_t out of a uint32_t @param var Value to break @param byteNum Byte number to extract diff --git a/zcm/util/cobs.h b/zcm/util/cobs.h index 1eb57645..2d8003ef 100644 --- a/zcm/util/cobs.h +++ b/zcm/util/cobs.h @@ -11,33 +11,77 @@ #define INLINE #endif +// RRR (Bendes): Try to match style of the repo you're working in. +// This repo has curly braces for functions alone on the next line. +// function definitions all on one line. Line length attempted to +// not exceed 80 characters, but definitely not to exceed 90 +// characters + +// RRR (Bendes): C style is that destintations go first in function args. +// Example: cobsEncode(dest, src, len) /** COBS encode data to buffer @param data Pointer to input data to encode @param length Number of bytes to encode @param buffer Pointer to encoded output buffer + // RRR (Bendes): Next two lines are helpful comments. Above 4 are unhelpful clutter + // Use comments sparingly to communicate things that code + // can't do clearly alone. So for example I believe an assumption + // of this file is that buffer is at least "length" long. + // That should be communicated somewhere since it is not done + // so by the function definition. Another example: if buffer + // and data are not allowed to point at the same piece of memory, + // that should probably be communicated (although we typically + // just assume this). You can communicate that by a comment, or + // you could adjust the function definition to be + // cobsEncode(uint8_t* restrict buffer, const void* restrict data, size_t len) + // Doing so can also speed up the code. That's only valid in C, + // (c++ syntax is like __restrict__ or something like that) + // so you can use a similar #define like you did for INLINE + // above (something like RESTRICT). Just an example you don't + // need to do the restrict thing unless you want to @return Encoded buffer length in bytes @note Does not output delimiter byte */ static INLINE size_t cobsEncode(const void* data, size_t length, uint8_t* buffer) { - assert(data && buffer); - + assert(data && buffer); // RRR (Bendes): Feels unnecessary + + // RRR (Bendes): Code should be self commenting. If your code nees this + // many comments, think about how to name your variables + // differently so the comments aren't necessary. Comments + // that repeat information clearly communicated by code + // are a direct violation of DRY. + // uint8_t* encode = buffer; // Encoded byte pointer uint8_t* codep = encode++; // Output code pointer uint8_t code = 1; // Code value for (const uint8_t* byte = (const uint8_t*)data; length--; ++byte) { + // RRR (Bendes): Audit all of your comments and remove those that aren't + // necessary. If you need a comment because your code + // doesn't clearly communicate what the comment does, lean + // toward making the code clearer - not toward leaving a + // comment if (*byte) // Byte not zero, write it *encode++ = *byte, ++code; + // RRR (Bendes): Curlys on same line as if with a space. See other + // files in repo for style examples if (!*byte || code == 0xff) // Input is zero or block completed, restart { + // RRR (Bendes): SRP dictates that we have any one line of code + // do 1 thing and 1 thing only. Split up your variable + // assignemnts on to separate lines *codep = code, code = 1, codep = encode; + // RRR (Bendes): Match style: join lines in an if they fit on one line if (!*byte || length) ++encode; } } + // RRR (Bendes): Final time mentioning comments. Just assume the same + // review comment applies to every code comment across + // the whole PR *codep = code; // Write final code value return (size_t)(encode - buffer); @@ -51,14 +95,16 @@ cobsEncode(const void* data, size_t length, uint8_t* buffer) { @return Number of bytes successfully decoded @note Stops decoding if delimiter byte is found */ +// RRR (Bendes): Why is data not a uint8_t* if this whole function assumes it is? static INLINE size_t cobsDecode(const uint8_t* buffer, size_t length, void* data) { - assert(buffer && data); + assert(buffer && data); // RRR (Bendes): Feels unnecessary const uint8_t* byte = buffer; // Encoded input byte pointer uint8_t* decode = (uint8_t*)data; // Decoded output byte pointer for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block) { + // RRR (Bendes): If your else has curly braces, make sure your if does too if (block) // Decode block byte *decode++ = *byte++; else { @@ -80,6 +126,7 @@ cobsDecode(const uint8_t* buffer, size_t length, void* data) { */ static INLINE size_t cobsMaxOverhead(size_t msgSize) { + // RRR (Bendes): Um.......so + 253? return (msgSize + 254 - 1) / 254; // COBS overhead } From c7435fec56c5db608768f92fd5448127dc3a6216 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 28 Mar 2023 11:20:04 -0400 Subject: [PATCH 3/9] Removed redundant byte buffers. Reduced comments. Added .clang-format file --- .clang-format | 23 ++ .gitignore | 1 + .../generic_serial_cobs_transport.c | 318 +++++++++--------- zcm/util/buffer_utils.h | 59 ---- zcm/util/cobs.h | 163 ++++----- 5 files changed, 244 insertions(+), 320 deletions(-) create mode 100644 .clang-format delete mode 100644 zcm/util/buffer_utils.h diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..7e1e9305 --- /dev/null +++ b/.clang-format @@ -0,0 +1,23 @@ +BasedOnStyle: Google +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: true +ColumnLimit: 80 +DerivePointerAlignment: false +PointerAlignment: Left +IndentWidth: 4 +TabWidth: 4 +UseTab: Never +BreakBeforeBraces: Stroustrup +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 2 + CaseSensitive: true + - Regex: '^((<|")(gtest|gmock|isl|json)/)' + Priority: 3 + - Regex: '<[[:alnum:].]+>' + Priority: 4 + - Regex: '.*' + Priority: 1 + SortPriority: 0 diff --git a/.gitignore b/.gitignore index e63416c2..2a694a67 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,4 @@ build/ .lock* .waf* deps/ +.vscode/ diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 6783e172..850c5c33 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -1,8 +1,8 @@ #include "zcm/transport/cobs_serial/generic_serial_cobs_transport.h" + #include "zcm/transport.h" #include "zcm/transport/generic_serial_circ_buff.h" #include "zcm/transport/generic_serial_fletcher.h" -#include "zcm/util/buffer_utils.h" #include "zcm/util/cobs.h" #include "zcm/zcm.h" @@ -28,6 +28,8 @@ // sum2(*chan, *data) // 0x00 -- termination char #define FRAME_BYTES 8 +#define COBS_MAX_FRAME_OVERHEAD 1 +static const size_t minMessageSize = FRAME_BYTES + COBS_MAX_FRAME_OVERHEAD; typedef struct zcm_trans_cobs_serial_t { zcm_trans_t trans; // This must be first to preserve pointer casting @@ -37,9 +39,7 @@ typedef struct zcm_trans_cobs_serial_t { uint8_t recvChanName[ZCM_CHANNEL_MAXLEN + 1]; size_t mtu; uint8_t* sendMsgData; - uint8_t* sendMsgDataCobs; uint8_t* recvMsgData; - uint8_t* recvMsgDataCobs; size_t (*get)(uint8_t* data, size_t nData, void* usr); size_t (*put)(const uint8_t* data, size_t nData, void* usr); @@ -51,11 +51,99 @@ typedef struct zcm_trans_cobs_serial_t { static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt); -size_t serial_cobs_get_mtu(zcm_trans_cobs_serial_t *zt) -{ return zt->mtu; } +size_t serial_cobs_get_mtu(zcm_trans_cobs_serial_t* zt) { return zt->mtu; } + +/** + * @brief COBS encode \p length bytes from \p src to \p dest + * @param dest + * @param src + * @param length + * + * @return Encoded buffer length in bytes + * + * @note Does not encode termination character + */ +static size_t cobs_encode_zcm(circBuffer_t* dest, const uint8_t* src, + size_t length) +{ + uint8_t code = 1; + size_t stuffBytes = 1; + size_t bytesWritten = 0; + + for (const uint8_t* byte = src; length--; ++byte) { + if (*byte) { ++code; } + + if (!*byte || code == 0xff) { // zero or end of block, restart + cb_push_back(dest, code); + ++bytesWritten; + while (--code) { + cb_push_back(dest, src[bytesWritten - stuffBytes]); + ++bytesWritten; + } + if (*byte) { stuffBytes++; } + code = 1; + } + } + cb_push_back(dest, code); + ++bytesWritten; + while (--code) { + cb_push_back(dest, src[bytesWritten - stuffBytes]); + ++bytesWritten; + } + + return bytesWritten; +} + +/** + * @brief COBS decode \p length bytes from \p src to \p dest + * @param dest + * @param src + * @param length + * + * @return Decoded buffer length in bytes, excluding delimeter + * + * @note Stops decoding if delimiter byte is found + * @note Does not pop any values out of \p src + */ +static size_t cobs_decode_zcm(uint8_t* dest, circBuffer_t* src, size_t length) +{ + bool foundTerm = false; + uint8_t* decode = dest; + size_t stuffBytes = 0; + size_t bytesRead = 0; + uint8_t byte = cb_front(src, bytesRead++); + + for (uint8_t code = 0xff, block = 0; bytesRead < length; --block) { + if (block) { + *decode = byte; + decode++; + byte = cb_front(src, bytesRead++); + continue; + } + + if (code != 0xff) { + *decode = 0; + decode++; + } + else { + stuffBytes++; + } + + block = code = byte; + byte = cb_front(src, bytesRead++); + + if (!code) { + foundTerm = true; + bytesRead--; + break; + } + } + + return foundTerm ? bytesRead - stuffBytes : 0; +} -int -serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { +int serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) +{ size_t chanLen = strlen(msg.channel); size_t payloadLen = FRAME_BYTES + chanLen + msg.len; @@ -66,22 +154,17 @@ serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { return ZCM_EAGAIN; } - // RRR (Bendes): This process seems very inefficient. This feels like it - // should be a 0 copy function. Encode directly - // into zt->sendBuffer. I'm not seeing a reason to need 2 - // additional dynamic memory buffers - uint8_t* pMsgData = zt->sendMsgData; - // RRR (Bendes): Look at zcm_coretypes.h if you're looking for a function to - // do this for you. Generic serial just inlined it. I have no - // issue with inlining but if you want a function, use coretypes - // Copy channel and message length - // RRR (Bendes): Breach of SRP. Do the increment on its own line - // Very confusing to read *var++ = x - *pMsgData++ = chanLen; - pMsgData = bufferUint32(pMsgData, (uint32_t)msg.len); + *pMsgData = (uint8_t)chanLen; + pMsgData++; + + pMsgData[0] = (uint8_t)(msg.len & 0xFF); + pMsgData[1] = (uint8_t)((msg.len >> 8) & 0xFF); + pMsgData[2] = (uint8_t)((msg.len >> 16) & 0xFF); + pMsgData[3] = (uint8_t)((msg.len >> 24) & 0xFF); + pMsgData += 4; // Copy channel name into msgData memcpy(pMsgData, msg.channel, chanLen); @@ -93,94 +176,44 @@ serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) { // Calculate Fletcher-16 checksum and add to msgData uint16_t checksum = fletcher16(zt->sendMsgData, payloadLen - 3); - pMsgData = bufferUint16(pMsgData, checksum); + pMsgData[0] = (uint8_t)(checksum & 0xFF); + pMsgData[1] = (uint8_t)((checksum >> 8) & 0xFF); - // COBS encode the message, subtract 1 for termination char - size_t bytesEncoded = cobsEncode(zt->sendMsgData, payloadLen - 1, zt->sendMsgDataCobs); + size_t bytesEncoded = + cobs_encode_zcm(&zt->sendBuffer, zt->sendMsgData, payloadLen - 1); if (bytesEncoded <= payloadLen - 1) { - return ZCM_EAGAIN; // encoding failed for some reason - } - - // Push entire message into sendBuffer - for (int i = 0; i < bytesEncoded; ++i) { - cb_push_back(&zt->sendBuffer, zt->sendMsgDataCobs[i]); + return ZCM_EAGAIN; // encoding failed } - cb_push_back(&zt->sendBuffer, ZCM_COBS_SERIAL_TERM_CHAR); // terminator byte + cb_push_back(&zt->sendBuffer, ZCM_COBS_SERIAL_TERM_CHAR); return ZCM_EOK; } -int serial_cobs_recvmsg_enable(zcm_trans_cobs_serial_t *zt, const char *channel, bool enable) +int serial_cobs_recvmsg_enable(zcm_trans_cobs_serial_t* zt, const char* channel, + bool enable) { - // RRR (Bendes): Unneccessary comment. Also this isn't only to be used on - // a microprocessor. This is a transport that can be used on - // any system running zcm - // NOTE: not implemented because it is unlikely that a microprocessor is - // going to be hearing messages on a USB comms that it doesn't want - // to hear - return ZCM_EOK; + return ZCM_EOK; // not implemented } -int -serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { +int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, + int timeout) +{ uint64_t utime = zt->time(zt->time_usr); size_t incomingSize = cb_size(&zt->recvBuffer); - // RRR (Bendes): This is calculable at static time. No need to recalculate - // minMsgSize every time - size_t minMsgSize = FRAME_BYTES + cobsMaxOverhead(FRAME_BYTES); - if (incomingSize < minMsgSize) { - return ZCM_EAGAIN; - } - - // RRR (Bendes): Can't start at the back. You must return messages in the - // order in which you received them - - // Search for terminator bytes, starting at the back - // RRR (Bendes): Style in this repo is to refer to these as ...Idx not ...Addr - int termAddr = -1; - for (int i = incomingSize - 1; i >= minMsgSize; --i) { - if (cb_front(&zt->recvBuffer, i) == ZCM_COBS_SERIAL_TERM_CHAR) { - termAddr = i; // keep track of terminator closest to front - } - } - - // Check if terminator byte was found - // RRR (Bendes): Can be a one liner without curly braces - if (termAddr == -1) { - return ZCM_EAGAIN; - } - - // Pop CB from front to termAddr - // RRR (Bendes): As written, this can be multiple messages - for (int i = 0; i <= termAddr; ++i) { - zt->recvMsgDataCobs[i] = cb_front(&zt->recvBuffer, i); - } - cb_pop_front(&zt->recvBuffer, termAddr + 1); - - // RRR (Bendes): Why copy? Just decode straight out of the circular buffer. - // No benefit to spending time doing multiple copies + if (incomingSize < minMessageSize) return ZCM_EAGAIN; // COBS decode - // RRR (Bendes): No need to decode into a temporary buffer before processing - // it. Process as you decode. You can always fail at any point. - // That's why circular buffer is written the way it is where - // you can peek into the buffer instead of popping off of it - size_t decodedBytes = cobsDecode(zt->recvMsgDataCobs, termAddr, zt->recvMsgData); - if (decodedBytes >= termAddr) { - return ZCM_EAGAIN; // decoding failed, probably missing some of message - } + size_t bytesDecoded = + cobs_decode_zcm(zt->recvMsgData, &zt->recvBuffer, incomingSize); + if (!bytesDecoded) return ZCM_EAGAIN; + + cb_pop_front(&zt->recvBuffer, bytesDecoded + 1); // +1 for terminator // Extract channel and message sizes - uint8_t chanLen = 0; uint8_t* pMsgData = zt->recvMsgData; + uint8_t chanLen = *pMsgData; + pMsgData++; - // RRR (Bendes) SRP. Dont increment on the same line where you're doing - // something else. Don't force me to pull up this page to - // understand what your code is doing lol: - // https://en.cppreference.com/w/c/language/operator_precedence - chanLen = *pMsgData++; - // RRR (Bendes) Inconsistently using functions for bit manipulation vs - // inlining it. Do one or the other msg->len = pMsgData[0]; msg->len |= pMsgData[1] << 8; msg->len |= pMsgData[2] << 16; @@ -188,27 +221,16 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { pMsgData += 4; // Value rationality checks - // RRR (Bendes): join lines - if (chanLen > ZCM_CHANNEL_MAXLEN) - return ZCM_EAGAIN; - - // RRR (Bendes): join lines - if (msg->len > zt->mtu) - return ZCM_EAGAIN; - - // RRR (Bendes): join lines - if (termAddr != FRAME_BYTES + chanLen + msg->len) - return ZCM_EAGAIN; + if (chanLen > ZCM_CHANNEL_MAXLEN || msg->len > zt->mtu) return ZCM_EINVALID; // Calculate Fletcher-16 checksum and check against received uint16_t checksum = 0; uint16_t receivedCS = 0; - checksum = fletcher16(zt->recvMsgData, termAddr - 3); - receivedCS = zt->recvMsgData[termAddr - 3] | (zt->recvMsgData[termAddr - 2] << 8); - // RRR (Bendes): join lines. get rid of curlies - if (receivedCS != checksum) { - return ZCM_EINVALID; - } + checksum = fletcher16(zt->recvMsgData, bytesDecoded - 3); + receivedCS = zt->recvMsgData[bytesDecoded - 3] | + (zt->recvMsgData[bytesDecoded - 2] << 8); + + if (receivedCS != checksum) return ZCM_EINVALID; // Copy channel name memset(&zt->recvChanName, '\0', ZCM_CHANNEL_MAXLEN); @@ -223,14 +245,14 @@ serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, int timeout) { return ZCM_EOK; } -int serial_cobs_update_rx(zcm_trans_t *_zt) +int serial_cobs_update_rx(zcm_trans_t* _zt) { zcm_trans_cobs_serial_t* zt = cast(_zt); cb_flush_in(&zt->recvBuffer, zt->get, zt->put_get_usr); return ZCM_EOK; } -int serial_cobs_update_tx(zcm_trans_t *_zt) +int serial_cobs_update_tx(zcm_trans_t* _zt) { zcm_trans_cobs_serial_t* zt = cast(_zt); cb_flush_out(&zt->sendBuffer, zt->put, zt->put_get_usr); @@ -239,64 +261,60 @@ int serial_cobs_update_tx(zcm_trans_t *_zt) /********************** STATICS **********************/ // RRR (Bendes): Match style of repo you're in -static size_t -_serial_get_mtu(zcm_trans_t* zt) { +static size_t _serial_get_mtu(zcm_trans_t* zt) +{ return serial_cobs_get_mtu(cast(zt)); } // RRR (Bendes): Match style of repo you're in -static int -_serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { +static int _serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) +{ return serial_cobs_sendmsg(cast(zt), msg); } // RRR (Bendes): Match style of repo you're in -static int -_serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) { +static int _serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, + bool enable) +{ return serial_cobs_recvmsg_enable(cast(zt), channel, enable); } // RRR (Bendes): Match style of repo you're in -static int -_serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) { +static int _serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) +{ return serial_cobs_recvmsg(cast(zt), msg, timeout); } // RRR (Bendes): Match style of repo you're in -static int -_serial_update(zcm_trans_t* zt) { +static int _serial_update(zcm_trans_t* zt) +{ int rxRet = serial_cobs_update_rx(zt); int txRet = serial_cobs_update_tx(zt); return rxRet == ZCM_EOK ? txRet : rxRet; } static zcm_trans_methods_t methods = { - &_serial_get_mtu, - &_serial_sendmsg, - &_serial_recvmsg_enable, - &_serial_recvmsg, - &_serial_update, - &zcm_trans_generic_serial_cobs_destroy, + &_serial_get_mtu, &_serial_sendmsg, &_serial_recvmsg_enable, + &_serial_recvmsg, &_serial_update, &zcm_trans_generic_serial_cobs_destroy, }; // RRR (Bendes): Match style of repo you're in -static zcm_trans_cobs_serial_t* -cast(zcm_trans_t* zt) { +static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt) +{ assert(zt->vtbl == &methods); return (zcm_trans_cobs_serial_t*)zt; } // RRR (Bendes): Match style of repo you're in -zcm_trans_t* -zcm_trans_generic_serial_cobs_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) { - if (MTU == 0 || bufSize < FRAME_BYTES + MTU) - return NULL; +zcm_trans_t* zcm_trans_generic_serial_cobs_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) +{ + if (MTU == 0 || bufSize < FRAME_BYTES + MTU) return NULL; zcm_trans_cobs_serial_t* zt = malloc(sizeof(zcm_trans_cobs_serial_t)); - if (zt == NULL) - return NULL; + if (zt == NULL) return NULL; zt->mtu = MTU; // RRR (Bendes): This is excessive. This is a ton of repeated code. @@ -322,29 +340,9 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, return NULL; } - // Bytes needed to construct full COBS-encoded message - maxPayloadSize += cobsMaxOverhead(maxPayloadSize); - zt->recvMsgDataCobs = malloc(maxPayloadSize * sizeof(uint8_t)); - if (zt->recvMsgDataCobs == NULL) { - free(zt->recvMsgData); - free(zt->sendMsgData); - free(zt); - return NULL; - } - zt->sendMsgDataCobs = malloc(maxPayloadSize * sizeof(uint8_t)); - if (zt->sendMsgDataCobs == NULL) { - free(zt->recvMsgData); - free(zt->sendMsgData); - free(zt->recvMsgDataCobs); - free(zt); - return NULL; - } - if (!cb_init(&zt->sendBuffer, bufSize)) { free(zt->recvMsgData); free(zt->sendMsgData); - free(zt->recvMsgDataCobs); - free(zt->sendMsgDataCobs); free(zt); return NULL; } @@ -352,8 +350,6 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, cb_deinit(&zt->sendBuffer); free(zt->recvMsgData); free(zt->sendMsgData); - free(zt->recvMsgDataCobs); - free(zt->sendMsgDataCobs); free(zt); return NULL; } @@ -372,14 +368,12 @@ zcm_trans_generic_serial_cobs_create(size_t (*get)(uint8_t* data, size_t nData, } // RRR (Bendes): Match style of repo you're in -void -zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* _zt) { +void zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* _zt) +{ zcm_trans_cobs_serial_t* zt = cast(_zt); cb_deinit(&zt->recvBuffer); cb_deinit(&zt->sendBuffer); free(zt->recvMsgData); free(zt->sendMsgData); - free(zt->recvMsgDataCobs); - free(zt->sendMsgDataCobs); free(zt); } diff --git a/zcm/util/buffer_utils.h b/zcm/util/buffer_utils.h deleted file mode 100644 index c89f103e..00000000 --- a/zcm/util/buffer_utils.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef __ZCM_BUFFER_UTILS_H__ -#define __ZCM_BUFFER_UTILS_H__ - -#include - -#ifdef __cplusplus -extern "C" -{ -#endif - -// RRR (Bendes): This functionality is already provided via zcm_coretypes.h -// I don't believe this file should be needed - -/** Pull 1 uint8_t out of a uint32_t - @param var Value to break - @param byteNum Byte number to extract - - @return Extracted byte -*/ -static inline uint8_t breakUint32(uint32_t var, int byteNum) -{ - return (uint8_t)((uint32_t)(((var) >> ((byteNum) * 8)) & 0x00FF)); -} - -/** Break and buffer a uint16_t value - LSB first - @param pBuf Buffer to insert @val into - @param val Value to insert into @pBuf - - @return A pointer to the position immediately following the inserted value -*/ -static inline uint8_t* bufferUint16(uint8_t *pBuf, uint16_t val) -{ - *pBuf++ = val & 0xFF; - *pBuf++ = (val >> 8) & 0xFF; - - return (pBuf); -} - -/** Break and buffer a uint32_t value - LSB first - @param pBuf Buffer to insert @val into - @param val Value to insert into @pBuf - - @return A pointer to the position immediately following the inserted value -*/ -static inline uint8_t* bufferUint32(uint8_t *pBuf, uint32_t val) -{ - *pBuf++ = breakUint32(val, 0); - *pBuf++ = breakUint32(val, 1); - *pBuf++ = breakUint32(val, 2); - *pBuf++ = breakUint32(val, 3); - - return(pBuf); -} - -#ifdef __cplusplus -} -#endif - -#endif /* __ZCM_BUFFER_UTILS_H__ */ diff --git a/zcm/util/cobs.h b/zcm/util/cobs.h index 2d8003ef..bfdeeb7a 100644 --- a/zcm/util/cobs.h +++ b/zcm/util/cobs.h @@ -11,112 +11,78 @@ #define INLINE #endif -// RRR (Bendes): Try to match style of the repo you're working in. -// This repo has curly braces for functions alone on the next line. -// function definitions all on one line. Line length attempted to -// not exceed 80 characters, but definitely not to exceed 90 -// characters - -// RRR (Bendes): C style is that destintations go first in function args. -// Example: cobsEncode(dest, src, len) -/** COBS encode data to buffer - @param data Pointer to input data to encode - @param length Number of bytes to encode - @param buffer Pointer to encoded output buffer - - // RRR (Bendes): Next two lines are helpful comments. Above 4 are unhelpful clutter - // Use comments sparingly to communicate things that code - // can't do clearly alone. So for example I believe an assumption - // of this file is that buffer is at least "length" long. - // That should be communicated somewhere since it is not done - // so by the function definition. Another example: if buffer - // and data are not allowed to point at the same piece of memory, - // that should probably be communicated (although we typically - // just assume this). You can communicate that by a comment, or - // you could adjust the function definition to be - // cobsEncode(uint8_t* restrict buffer, const void* restrict data, size_t len) - // Doing so can also speed up the code. That's only valid in C, - // (c++ syntax is like __restrict__ or something like that) - // so you can use a similar #define like you did for INLINE - // above (something like RESTRICT). Just an example you don't - // need to do the restrict thing unless you want to - @return Encoded buffer length in bytes - @note Does not output delimiter byte -*/ -static INLINE size_t -cobsEncode(const void* data, size_t length, uint8_t* buffer) { - assert(data && buffer); // RRR (Bendes): Feels unnecessary - - // RRR (Bendes): Code should be self commenting. If your code nees this - // many comments, think about how to name your variables - // differently so the comments aren't necessary. Comments - // that repeat information clearly communicated by code - // are a direct violation of DRY. - // - uint8_t* encode = buffer; // Encoded byte pointer - uint8_t* codep = encode++; // Output code pointer - uint8_t code = 1; // Code value - - for (const uint8_t* byte = (const uint8_t*)data; length--; ++byte) { - // RRR (Bendes): Audit all of your comments and remove those that aren't - // necessary. If you need a comment because your code - // doesn't clearly communicate what the comment does, lean - // toward making the code clearer - not toward leaving a - // comment - if (*byte) // Byte not zero, write it +/** + * @brief COBS encode \p length bytes from \p src to \p dest + * @param dest + * @param src + * @param length + * + * @return Encoded buffer length in bytes + * + * @note Does not output delimiter byte + * @note \p dest must point to output buffer of length + * greater than or equal to \p src + */ +static INLINE size_t cobs_encode(uint8_t* dest, const uint8_t* src, + size_t length) +{ + uint8_t* encode = dest; + uint8_t* codep = encode++; + uint8_t code = 1; + + for (const uint8_t* byte = src; length--; ++byte) { + if (*byte) // not zero, write it *encode++ = *byte, ++code; - // RRR (Bendes): Curlys on same line as if with a space. See other - // files in repo for style examples - if (!*byte || code == 0xff) // Input is zero or block completed, restart - { - // RRR (Bendes): SRP dictates that we have any one line of code - // do 1 thing and 1 thing only. Split up your variable - // assignemnts on to separate lines - *codep = code, code = 1, codep = encode; - // RRR (Bendes): Match style: join lines in an if they fit on one line - if (!*byte || length) - ++encode; + if (!*byte || code == 0xff) { // zero or end of block, restart + *codep = code; + code = 1; + codep = encode; + + if (!*byte || length) ++encode; } } - // RRR (Bendes): Final time mentioning comments. Just assume the same - // review comment applies to every code comment across - // the whole PR - *codep = code; // Write final code value - return (size_t)(encode - buffer); + *codep = code; + + return (size_t)(encode - dest); } -/** COBS decode data from buffer - @param buffer Pointer to encoded input bytes - @param length Number of bytes to decode - @param data Pointer to decoded output data +/** + * @brief COBS decode \p length bytes from \p src to \p dest + * @param dest + * @param src + * @param length + * + * @return Number of bytes successfully decoded + * + * @note Stops decoding if delimiter byte is found + */ +static INLINE size_t cobs_decode(uint8_t* dest, const uint8_t* src, + size_t length) +{ + const uint8_t* byte = src; + uint8_t* decode = dest; + + for (uint8_t code = 0xff, block = 0; byte < src + length; --block) { + if (block) { + *decode = *byte; + decode++; + byte++; + continue; + } - @return Number of bytes successfully decoded - @note Stops decoding if delimiter byte is found -*/ -// RRR (Bendes): Why is data not a uint8_t* if this whole function assumes it is? -static INLINE size_t -cobsDecode(const uint8_t* buffer, size_t length, void* data) { - assert(buffer && data); // RRR (Bendes): Feels unnecessary - - const uint8_t* byte = buffer; // Encoded input byte pointer - uint8_t* decode = (uint8_t*)data; // Decoded output byte pointer - - for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block) { - // RRR (Bendes): If your else has curly braces, make sure your if does too - if (block) // Decode block byte - *decode++ = *byte++; - else { - if (code != 0xff) // Encoded zero, write it - *decode++ = 0; - block = code = *byte++; // Next block length - if (!code) // Delimiter code found - break; + if (code != 0xff) { + *decode = 0; + decode++; } + block = code = *byte; + byte++; + + if (!code) break; // found delimeter } - return (size_t)(decode - (uint8_t*)data); + return (size_t)(decode - dest); } /** Calculate maximum COBS encoding overhead @@ -124,10 +90,9 @@ cobsDecode(const uint8_t* buffer, size_t length, void* data) { @return Maximum number of overhead bytes */ -static INLINE size_t -cobsMaxOverhead(size_t msgSize) { - // RRR (Bendes): Um.......so + 253? - return (msgSize + 254 - 1) / 254; // COBS overhead +static INLINE size_t cobsMaxOverhead(size_t msgSize) +{ + return (msgSize + 253) / 254; } #endif /* _ZCM_TRANS_NONBLOCKING_COBS_H */ From 1b58b2cbacbf15e19358cfac8a55ff3770f2a44f Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 28 Mar 2023 13:14:57 -0400 Subject: [PATCH 4/9] Cleaned up zcm_trans_generic_serial_cobs_create. Added test for serial COBS transport --- .../cpp/transport/SerialCobsTransportTest.cpp | 94 +++++++++++++++++++ examples/cpp/transport/wscript | 4 + .../generic_serial_cobs_transport.c | 89 +++++++----------- zcm/transport/generic_serial_fletcher.h | 15 +-- zcm/util/cobs.h | 42 +++++---- zcm/wscript | 11 ++- 6 files changed, 170 insertions(+), 85 deletions(-) create mode 100644 examples/cpp/transport/SerialCobsTransportTest.cpp diff --git a/examples/cpp/transport/SerialCobsTransportTest.cpp b/examples/cpp/transport/SerialCobsTransportTest.cpp new file mode 100644 index 00000000..d1ada6e0 --- /dev/null +++ b/examples/cpp/transport/SerialCobsTransportTest.cpp @@ -0,0 +1,94 @@ +#include "types/example_t.hpp" +#include "zcm/transport/cobs_serial/generic_serial_cobs_transport.h" +#include "zcm/zcm-cpp.hpp" +#include + +#include +#include +#include + +using namespace std; +using namespace zcm; + +#define PUBLISH_DT (1e6) / (5) +#define MIN(A, B) ((A) < (B)) ? (A) : (B) +#define MAX_FIFO 12 + +queue fifo; + +static size_t get(uint8_t* data, size_t nData, void* usr) +{ + size_t n = MIN(MAX_FIFO, nData); + n = MIN(fifo.size(), n); + + for (size_t i = 0; i < n; ++i) { + data[i] = fifo.front(); + fifo.pop(); + } + + return n; +} + +static size_t put(const uint8_t* data, size_t nData, void* usr) +{ + size_t n = MIN(MAX_FIFO - fifo.size(), nData); + // cout << "Put " << n << " bytes" << endl; + + for (size_t i = 0; i < n; ++i) fifo.push(data[i]); + + return n; +} + +static uint64_t utime(void* usr) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return (uint64_t)tv.tv_sec * 1000000 + tv.tv_usec; +} + +class Handler { + int64_t lastHost = 0; + + public: + void handle(const ReceiveBuffer* rbuf, const string& chan, + const example_t* msg) + { + cout << "Message received" << endl; + + if (msg->timestamp <= lastHost || rbuf->recv_utime < lastHost) { + assert("ERROR: utime mismatch. This should never happen"); + } + lastHost = msg->timestamp; + } +}; + +int main(int argc, const char* argv[]) +{ + ZCM zcmLocal(zcm_trans_generic_serial_cobs_create(&get, &put, NULL, &utime, + NULL, 1024, 1024 * 5)); + + example_t example = {}; + example.num_ranges = 1; + example.ranges.resize(1); + example.ranges.at(0) = 1; + + Handler handler; + auto sub = zcmLocal.subscribe("EXAMPLE", &Handler::handle, &handler); + + uint64_t nextPublish = 0; + while (true) { + uint64_t now = utime(NULL); + if (now > nextPublish) { + cout << "Publishing" << endl; + example.timestamp = now; + zcmLocal.publish("EXAMPLE", &example); + nextPublish = now + PUBLISH_DT; + } + + zcmLocal.handleNonblock(); + } + + zcmLocal.unsubscribe(sub); + + return 0; +} diff --git a/examples/cpp/transport/wscript b/examples/cpp/transport/wscript index 238764b5..82c242c4 100644 --- a/examples/cpp/transport/wscript +++ b/examples/cpp/transport/wscript @@ -5,3 +5,7 @@ def build(ctx): ctx.program(target = 'generic-serial', use = 'default zcm examplezcmtypes_cpp', source = 'SerialTransportTest.cpp') + + ctx.program(target = 'generic-cobs-serial', + use = 'default zcm examplezcmtypes_cpp', + source = 'SerialCobsTransportTest.cpp') diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 850c5c33..c7ad4171 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -17,8 +17,6 @@ #define ASSERT(x) -// RRR (Bendes): See comments in other files about - // Framing (size = 8 + chanLen + data_len) // chanLen // data_len (4 bytes) @@ -36,8 +34,8 @@ typedef struct zcm_trans_cobs_serial_t { circBuffer_t sendBuffer; circBuffer_t recvBuffer; - uint8_t recvChanName[ZCM_CHANNEL_MAXLEN + 1]; size_t mtu; + uint8_t recvChanName[ZCM_CHANNEL_MAXLEN + 1]; uint8_t* sendMsgData; uint8_t* recvMsgData; @@ -54,14 +52,15 @@ static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt); size_t serial_cobs_get_mtu(zcm_trans_cobs_serial_t* zt) { return zt->mtu; } /** - * @brief COBS encode \p length bytes from \p src to \p dest - * @param dest - * @param src - * @param length + * @brief COBS encode \p length bytes from \p src to \p dest + * + * @param dest + * @param src + * @param length * - * @return Encoded buffer length in bytes + * @return Encoded buffer length in bytes * - * @note Does not encode termination character + * @note Does not encode termination character */ static size_t cobs_encode_zcm(circBuffer_t* dest, const uint8_t* src, size_t length) @@ -95,15 +94,16 @@ static size_t cobs_encode_zcm(circBuffer_t* dest, const uint8_t* src, } /** - * @brief COBS decode \p length bytes from \p src to \p dest - * @param dest - * @param src - * @param length + * @brief COBS decode \p length bytes from \p src to \p dest * - * @return Decoded buffer length in bytes, excluding delimeter + * @param dest + * @param src + * @param length * - * @note Stops decoding if delimiter byte is found - * @note Does not pop any values out of \p src + * @return Decoded buffer length in bytes, excluding delimeter + * + * @note Stops decoding if delimiter byte is found + * @note Does not pop any values out of \p src */ static size_t cobs_decode_zcm(uint8_t* dest, circBuffer_t* src, size_t length) { @@ -260,32 +260,27 @@ int serial_cobs_update_tx(zcm_trans_t* _zt) } /********************** STATICS **********************/ -// RRR (Bendes): Match style of repo you're in static size_t _serial_get_mtu(zcm_trans_t* zt) { return serial_cobs_get_mtu(cast(zt)); } -// RRR (Bendes): Match style of repo you're in static int _serial_sendmsg(zcm_trans_t* zt, zcm_msg_t msg) { return serial_cobs_sendmsg(cast(zt), msg); } -// RRR (Bendes): Match style of repo you're in static int _serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, bool enable) { return serial_cobs_recvmsg_enable(cast(zt), channel, enable); } -// RRR (Bendes): Match style of repo you're in static int _serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) { return serial_cobs_recvmsg(cast(zt), msg, timeout); } -// RRR (Bendes): Match style of repo you're in static int _serial_update(zcm_trans_t* zt) { int rxRet = serial_cobs_update_rx(zt); @@ -298,14 +293,12 @@ static zcm_trans_methods_t methods = { &_serial_recvmsg, &_serial_update, &zcm_trans_generic_serial_cobs_destroy, }; -// RRR (Bendes): Match style of repo you're in static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt) { assert(zt->vtbl == &methods); return (zcm_trans_cobs_serial_t*)zt; } -// RRR (Bendes): Match style of repo you're in zcm_trans_t* zcm_trans_generic_serial_cobs_create( size_t (*get)(uint8_t* data, size_t nData, void* usr), size_t (*put)(const uint8_t* data, size_t nData, void* usr), @@ -317,42 +310,20 @@ zcm_trans_t* zcm_trans_generic_serial_cobs_create( if (zt == NULL) return NULL; zt->mtu = MTU; - // RRR (Bendes): This is excessive. This is a ton of repeated code. - // Figure out a cleaner way to dealloc on failure or just - // use a goto. You can sacrifice speed here (null setting - // everything before allocation and checking them each on - // a failure of any). The constructor doesn't need to be - // doing minimum work. - - // RRR (Bendes): Not sure why so many buffers are needed + zt->sendMsgData = NULL; + zt->recvMsgData = NULL; + zt->recvBuffer.data = NULL; + zt->sendBuffer.data = NULL; - // Bytes needed to construct full message size_t maxPayloadSize = FRAME_BYTES + ZCM_CHANNEL_MAXLEN + zt->mtu; zt->recvMsgData = malloc(maxPayloadSize * sizeof(uint8_t)); - if (zt->recvMsgData == NULL) { - free(zt); - return NULL; - } + if (zt->recvMsgData == NULL) goto fail; + zt->sendMsgData = malloc(maxPayloadSize * sizeof(uint8_t)); - if (zt->sendMsgData == NULL) { - free(zt->recvMsgData); - free(zt); - return NULL; - } + if (zt->sendMsgData == NULL) goto fail; - if (!cb_init(&zt->sendBuffer, bufSize)) { - free(zt->recvMsgData); - free(zt->sendMsgData); - free(zt); - return NULL; - } - if (!cb_init(&zt->recvBuffer, bufSize)) { - cb_deinit(&zt->sendBuffer); - free(zt->recvMsgData); - free(zt->sendMsgData); - free(zt); - return NULL; - } + if (!cb_init(&zt->recvBuffer, bufSize)) goto fail; + if (!cb_init(&zt->sendBuffer, bufSize)) goto fail; zt->trans.trans_type = ZCM_NONBLOCKING; zt->trans.vtbl = &methods; @@ -365,9 +336,17 @@ zcm_trans_t* zcm_trans_generic_serial_cobs_create( zt->time_usr = time_usr; return (zcm_trans_t*)zt; + +fail: + if (zt->recvBuffer.data != NULL) cb_deinit(&zt->recvBuffer); + if (zt->sendBuffer.data != NULL) cb_deinit(&zt->sendBuffer); + if (zt->recvMsgData != NULL) free(zt->recvMsgData); + if (zt->sendMsgData != NULL) free(zt->sendMsgData); + + free(zt); + return NULL; } -// RRR (Bendes): Match style of repo you're in void zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* _zt) { zcm_trans_cobs_serial_t* zt = cast(_zt); diff --git a/zcm/transport/generic_serial_fletcher.h b/zcm/transport/generic_serial_fletcher.h index 4a2e0727..472e5c4b 100644 --- a/zcm/transport/generic_serial_fletcher.h +++ b/zcm/transport/generic_serial_fletcher.h @@ -20,13 +20,16 @@ static inline uint16_t fletcherUpdate(uint8_t b, uint16_t prevSum) return (sumHigh << 8) | sumLow; } -/*! - * @brief Calculate Fletcher-16 checksum +/** + * @brief Calculate Fletcher-16 checksum * - * @param data - pointed to array of bytes to calculate checksum for - * @param len - length of byte array + * @param data + * @param len + * + * @return Fletcher16 checksum */ // RRR (Bendes): why is this not deferring to the above? DRY principle :) +// RRR (Griff): because this one is more efficient static inline uint16_t fletcher16(const uint8_t* data, size_t len) { uint32_t c0, c1; @@ -35,9 +38,7 @@ static inline uint16_t fletcher16(const uint8_t* data, size_t len) /* n > 0 and n * (n+1) / 2 * (2^8-1) < (2^32-1). */ for (c0 = c1 = 0; len > 0;) { size_t blocklen = len; - if (blocklen > 5802) { - blocklen = 5802; - } + if (blocklen > 5802) blocklen = 5802; len -= blocklen; do { c0 = c0 + *data++; diff --git a/zcm/util/cobs.h b/zcm/util/cobs.h index bfdeeb7a..bb0b3ae3 100644 --- a/zcm/util/cobs.h +++ b/zcm/util/cobs.h @@ -12,16 +12,17 @@ #endif /** - * @brief COBS encode \p length bytes from \p src to \p dest - * @param dest - * @param src - * @param length + * @brief COBS encode \p length bytes from \p src to \p dest * - * @return Encoded buffer length in bytes + * @param dest + * @param src + * @param length * - * @note Does not output delimiter byte - * @note \p dest must point to output buffer of length - * greater than or equal to \p src + * @return Encoded buffer length in bytes + * + * @note Does not output delimiter byte + * @note \p dest must point to output buffer of length + * greater than or equal to \p src */ static INLINE size_t cobs_encode(uint8_t* dest, const uint8_t* src, size_t length) @@ -49,14 +50,15 @@ static INLINE size_t cobs_encode(uint8_t* dest, const uint8_t* src, } /** - * @brief COBS decode \p length bytes from \p src to \p dest - * @param dest - * @param src - * @param length + * @brief COBS decode \p length bytes from \p src to \p dest + * + * @param dest + * @param src + * @param length * - * @return Number of bytes successfully decoded + * @return Number of bytes successfully decoded * - * @note Stops decoding if delimiter byte is found + * @note Stops decoding if delimiter byte is found */ static INLINE size_t cobs_decode(uint8_t* dest, const uint8_t* src, size_t length) @@ -85,11 +87,13 @@ static INLINE size_t cobs_decode(uint8_t* dest, const uint8_t* src, return (size_t)(decode - dest); } -/** Calculate maximum COBS encoding overhead - @param msgSize Size of the pre-encoded message - - @return Maximum number of overhead bytes -*/ +/** + * @brief Calculate maximum COBS encoding overhead + * + * @param msgSize Size of the pre-encoded message + * + * @return Maximum number of overhead bytes + */ static INLINE size_t cobsMaxOverhead(size_t msgSize) { return (msgSize + 253) / 254; diff --git a/zcm/wscript b/zcm/wscript index d79484ce..be063b7e 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -50,7 +50,9 @@ def build(ctx): 'transport/generic_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', + 'transport/cobs_serial/generic_serial_cobs_transport.h', + 'transport/cobs_serial/generic_serial_cobs_transport.c'] if ctx.env.USING_THIRD_PARTY: embedSource.append('transport/third-party/embedded/**') @@ -104,7 +106,8 @@ def build(ctx): ctx.install_files('${PREFIX}/include/zcm/transport', ['transport/generic_serial_transport.h', 'transport/generic_serial_circ_buff.h', - 'transport/generic_serial_fletcher.h']) + 'transport/generic_serial_fletcher.h', + 'transport/cobs_serial/generic_serial_cobs_transport.h']) ctx.install_files('${PREFIX}/share/embedded', ['zcm-embed.tar.gz']) @@ -113,10 +116,10 @@ def build(ctx): ctx.recurse('json') if ctx.env.USING_JAVA: - ctx.recurse('java'); + ctx.recurse('java') if ctx.env.USING_NODEJS: - ctx.recurse('js'); + ctx.recurse('js') if ctx.env.USING_PYTHON: ctx.recurse('python') From db6b4e102f9b84e480758e57ddfa6f446fb3da36 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Tue, 28 Mar 2023 22:25:16 -0400 Subject: [PATCH 5/9] Handle recvBuffer with single message --- .../cobs_serial/generic_serial_cobs_transport.c | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index c7ad4171..3beeb6fe 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -107,13 +107,18 @@ static size_t cobs_encode_zcm(circBuffer_t* dest, const uint8_t* src, */ static size_t cobs_decode_zcm(uint8_t* dest, circBuffer_t* src, size_t length) { - bool foundTerm = false; - uint8_t* decode = dest; - size_t stuffBytes = 0; size_t bytesRead = 0; uint8_t byte = cb_front(src, bytesRead++); + if (!byte) { + cb_pop_front(src, 1); + return 0; + } - for (uint8_t code = 0xff, block = 0; bytesRead < length; --block) { + bool foundTerm = false; + size_t stuffBytes = 0; + uint8_t* decode = dest; + for (uint8_t code = 0xff, block = 0; (bytesRead - stuffBytes) < length; + --block) { if (block) { *decode = byte; decode++; @@ -206,6 +211,10 @@ int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, size_t bytesDecoded = cobs_decode_zcm(zt->recvMsgData, &zt->recvBuffer, incomingSize); if (!bytesDecoded) return ZCM_EAGAIN; + if (bytesDecoded < minMessageSize) { + cb_pop_front(&zt->recvBuffer, bytesDecoded); + return ZCM_EAGAIN; + } cb_pop_front(&zt->recvBuffer, bytesDecoded + 1); // +1 for terminator From e6d911fa475c2102ae85137b3ecc8019436d4716 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 29 Mar 2023 11:09:17 -0400 Subject: [PATCH 6/9] Send complementary Fletcher checkbytes rather than raw checksum --- .../generic_serial_cobs_transport.c | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 3beeb6fe..ed96b4ad 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -179,10 +179,12 @@ int serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) memcpy(pMsgData, msg.buf, msg.len); pMsgData += msg.len; - // Calculate Fletcher-16 checksum and add to msgData + // Calculate Fletcher-16 checksum and complementary bytes uint16_t checksum = fletcher16(zt->sendMsgData, payloadLen - 3); - pMsgData[0] = (uint8_t)(checksum & 0xFF); - pMsgData[1] = (uint8_t)((checksum >> 8) & 0xFF); + uint8_t sumLow = (uint8_t)(checksum & 0xFF); + uint8_t sumHigh = (uint8_t)((checksum >> 8) & 0xFF); + pMsgData[0] = 255 - ((sumLow + sumHigh) % 255); + pMsgData[1] = 255 - ((sumLow + pMsgData[0]) % 255); size_t bytesEncoded = cobs_encode_zcm(&zt->sendBuffer, zt->sendMsgData, payloadLen - 1); @@ -232,14 +234,10 @@ int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, // Value rationality checks if (chanLen > ZCM_CHANNEL_MAXLEN || msg->len > zt->mtu) return ZCM_EINVALID; - // Calculate Fletcher-16 checksum and check against received + // Calculate Fletcher-16 checksum for entire payload (including checkbytes) uint16_t checksum = 0; - uint16_t receivedCS = 0; - checksum = fletcher16(zt->recvMsgData, bytesDecoded - 3); - receivedCS = zt->recvMsgData[bytesDecoded - 3] | - (zt->recvMsgData[bytesDecoded - 2] << 8); - - if (receivedCS != checksum) return ZCM_EINVALID; + checksum = fletcher16(zt->recvMsgData, bytesDecoded - 1); + if (checksum != 0x0000) return ZCM_EINVALID; // Copy channel name memset(&zt->recvChanName, '\0', ZCM_CHANNEL_MAXLEN); From b163fafdacb959484df7ad9dfdfeadacd2329e75 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 29 Mar 2023 15:47:12 -0400 Subject: [PATCH 7/9] Rewrote cobs_decode_zcm to better handle malformed and incomplete messages --- .../generic_serial_cobs_transport.c | 53 +++++++++---------- 1 file changed, 25 insertions(+), 28 deletions(-) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index ed96b4ad..583b84a4 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -103,48 +103,51 @@ static size_t cobs_encode_zcm(circBuffer_t* dest, const uint8_t* src, * @return Decoded buffer length in bytes, excluding delimeter * * @note Stops decoding if delimiter byte is found - * @note Does not pop any values out of \p src + * @note Pops values out of \p src if message is complete or malformed, + * does not pop values if message is incomplete */ static size_t cobs_decode_zcm(uint8_t* dest, circBuffer_t* src, size_t length) { size_t bytesRead = 0; uint8_t byte = cb_front(src, bytesRead++); - if (!byte) { - cb_pop_front(src, 1); + if (byte == 0x00) { + cb_pop_front(src, bytesRead); return 0; } - bool foundTerm = false; size_t stuffBytes = 0; uint8_t* decode = dest; - for (uint8_t code = 0xff, block = 0; (bytesRead - stuffBytes) < length; - --block) { + for (uint8_t code = 0xff, block = 0; bytesRead < length; --block) { if (block) { - *decode = byte; - decode++; - byte = cb_front(src, bytesRead++); - continue; - } + if (byte == 0x00) { // packet malformed + cb_pop_front(src, bytesRead); + return 0; + } - if (code != 0xff) { - *decode = 0; + *decode = byte; decode++; } else { - stuffBytes++; + if (code == 0xff) { stuffBytes++; } + else { + *decode = 0; + decode++; + } + + block = code = byte; } - block = code = byte; byte = cb_front(src, bytesRead++); + } - if (!code) { - foundTerm = true; - bytesRead--; - break; - } + if (byte == 0x00) { // ended on terminator (complete) + stuffBytes++; + cb_pop_front(src, bytesRead); + return bytesRead - stuffBytes; } - return foundTerm ? bytesRead - stuffBytes : 0; + // ended on non-terminator (incomplete) + return 0; } int serial_cobs_sendmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t msg) @@ -212,13 +215,7 @@ int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, // COBS decode size_t bytesDecoded = cobs_decode_zcm(zt->recvMsgData, &zt->recvBuffer, incomingSize); - if (!bytesDecoded) return ZCM_EAGAIN; - if (bytesDecoded < minMessageSize) { - cb_pop_front(&zt->recvBuffer, bytesDecoded); - return ZCM_EAGAIN; - } - - cb_pop_front(&zt->recvBuffer, bytesDecoded + 1); // +1 for terminator + if (bytesDecoded < minMessageSize) return ZCM_EAGAIN; // Extract channel and message sizes uint8_t* pMsgData = zt->recvMsgData; From 3d510facd7b77fadc5331cfe935ef5bb6fa8985e Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 29 Mar 2023 16:29:30 -0400 Subject: [PATCH 8/9] Fix handling consecutive messages in recvBuffer --- zcm/transport/cobs_serial/generic_serial_cobs_transport.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 583b84a4..89188f67 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -135,6 +135,7 @@ static size_t cobs_decode_zcm(uint8_t* dest, circBuffer_t* src, size_t length) } block = code = byte; + if (code == 0x00) break; } byte = cb_front(src, bytesRead++); @@ -233,7 +234,7 @@ int serial_cobs_recvmsg(zcm_trans_cobs_serial_t* zt, zcm_msg_t* msg, // Calculate Fletcher-16 checksum for entire payload (including checkbytes) uint16_t checksum = 0; - checksum = fletcher16(zt->recvMsgData, bytesDecoded - 1); + checksum = fletcher16(zt->recvMsgData, bytesDecoded); if (checksum != 0x0000) return ZCM_EINVALID; // Copy channel name From 7524628978cb3f3095f1cbba81a6086f7e0aeba3 Mon Sep 17 00:00:00 2001 From: Joe Griffin Date: Wed, 17 Jul 2024 14:28:30 -0400 Subject: [PATCH 9/9] Register COBS-serial transport --- .../generic_serial_cobs_transport.c | 11 +- .../generic_serial_cobs_transport.h | 21 +- zcm/transport/transport_cobs_serial.cpp | 302 ++++++++++++++++++ zcm/transport/transport_serial.cpp | 197 ++++++------ zcm/transport/transport_serial.hpp | 33 ++ zcm/wscript | 1 + 6 files changed, 457 insertions(+), 108 deletions(-) create mode 100644 zcm/transport/transport_cobs_serial.cpp create mode 100644 zcm/transport/transport_serial.hpp diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c index 89188f67..d6c5abd7 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.c +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.c @@ -281,7 +281,7 @@ static int _serial_recvmsg_enable(zcm_trans_t* zt, const char* channel, return serial_cobs_recvmsg_enable(cast(zt), channel, enable); } -static int _serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, int timeout) +static int _serial_recvmsg(zcm_trans_t* zt, zcm_msg_t* msg, unsigned timeout) { return serial_cobs_recvmsg(cast(zt), msg, timeout); } @@ -294,8 +294,13 @@ static int _serial_update(zcm_trans_t* zt) } static zcm_trans_methods_t methods = { - &_serial_get_mtu, &_serial_sendmsg, &_serial_recvmsg_enable, - &_serial_recvmsg, &_serial_update, &zcm_trans_generic_serial_cobs_destroy, + &_serial_get_mtu, + &_serial_sendmsg, + &_serial_recvmsg_enable, + &_serial_recvmsg, + NULL, // drops + &_serial_update, + &zcm_trans_generic_serial_cobs_destroy, }; static zcm_trans_cobs_serial_t* cast(zcm_trans_t* zt) diff --git a/zcm/transport/cobs_serial/generic_serial_cobs_transport.h b/zcm/transport/cobs_serial/generic_serial_cobs_transport.h index bf5110f4..4b854755 100644 --- a/zcm/transport/cobs_serial/generic_serial_cobs_transport.h +++ b/zcm/transport/cobs_serial/generic_serial_cobs_transport.h @@ -5,22 +5,23 @@ extern "C" { #endif -#include - -#include "zcm/zcm.h" #include "zcm/transport.h" +#include "zcm/zcm.h" -zcm_trans_t *zcm_trans_generic_serial_cobs_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); +#include + +zcm_trans_t* zcm_trans_generic_serial_cobs_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); // frees all resources inside of zt and frees zt itself void zcm_trans_generic_serial_cobs_destroy(zcm_trans_t* zt); +int serial_cobs_update_rx(zcm_trans_t* _zt); +int serial_cobs_update_tx(zcm_trans_t* _zt); + #ifdef __cplusplus } #endif diff --git a/zcm/transport/transport_cobs_serial.cpp b/zcm/transport/transport_cobs_serial.cpp new file mode 100644 index 00000000..619fffab --- /dev/null +++ b/zcm/transport/transport_cobs_serial.cpp @@ -0,0 +1,302 @@ +#include "generic_serial_transport.h" +#include "util/TimeUtil.hpp" +#include "zcm/transport.h" +#include "zcm/transport/cobs_serial/generic_serial_cobs_transport.h" +#include "zcm/transport/transport_serial.hpp" +#include "zcm/transport_register.hpp" +#include "zcm/transport_registrar.h" +#include "zcm/util/debug.h" +#include "zcm/util/lockfile.h" +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; + +// TODO: This transport layer needs to be "hardened" to handle +// all of the possible errors and corner cases. Currently, it +// should work fine in most cases, but it might fail on some +// rare cases... + +// Define this the class name you want +#define ZCM_TRANS_CLASSNAME TransportCobsSerial +#define MTU (1 << 14) +#define ESCAPE_CHAR (0xcc) + +#define SERIAL_TIMEOUT_US 1e5 // u-seconds + +#define US_TO_MS(a) (a) / 1e3 + +using u8 = uint8_t; +using u16 = uint16_t; +using u32 = uint32_t; +using u64 = uint64_t; + +struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { + Serial 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 timeoutLeft; + + 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; + + // build 'options' + 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_generic_serial_cobs_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_generic_serial_cobs_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->timeoutLeft); + uint64_t diff = TimeUtil::utime() - startUtime; + me->timeoutLeft = me->timeoutLeft > diff ? me->timeoutLeft - 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) { return TimeUtil::utime(); } + + /********************** METHODS **********************/ + 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 { + // Note: No need to lock here ONLY because the internals of + // generic serial transport sendmsg only use the sendBuffer + // and touch no variables related to receiving + int ret = zcm_trans_sendmsg(this->gst, msg); + if (ret != ZCM_EOK) return ret; + return serial_cobs_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, int timeoutMs) + { + timeoutLeft = + timeoutMs > 0 ? timeoutMs * 1e3 : numeric_limits::max(); + + 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(); + + // Note: No need to lock here ONLY because the internals of + // generic serial transport recvmsg only use the recv + // related data members and touch no variables related to + // sending + int ret = zcm_trans_recvmsg(this->gst, msg, timeoutLeft); + if (ret == ZCM_EOK) return ret; + + uint64_t diff = TimeUtil::utime() - startUtime; + startUtime = TimeUtil::utime(); + // Note: timeoutLeft is calculated here because serial_update_rx + // needs it to be set properly so that the blocking read + // in `get` knows how long it has to exit + timeoutLeft = timeoutLeft > diff ? timeoutLeft - diff : 0; + + serial_cobs_update_rx(this->gst); + + diff = TimeUtil::utime() - startUtime; + timeoutLeft = timeoutLeft > diff ? timeoutLeft - diff : 0; + + } while (timeoutLeft > 0); + + return ZCM_EAGAIN; + } + } + + /********************** STATICS **********************/ + 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, // drops + NULL, // update + &ZCM_TRANS_CLASSNAME::_destroy, +}; + +static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) +{ + if (opt_errmsg) *opt_errmsg = NULL; // Feature unused in this transport + auto* trans = new ZCM_TRANS_CLASSNAME(url); + if (trans->good()) return trans; + + delete trans; + return nullptr; +} + +#ifdef USING_TRANS_SERIAL +// Register this transport with ZCM +const TransportRegister ZCM_TRANS_CLASSNAME::reg( + "serial-cobs", + "Transfer data via a serial connection using COBS encoding " + "(e.g. 'serial-cobs:///dev/ttyUSB0?baud=115200&hw_flow_control=true' or " + "'serial-cobs:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", + create); +#endif diff --git a/zcm/transport/transport_serial.cpp b/zcm/transport/transport_serial.cpp index b0cb8e06..175648a2 100644 --- a/zcm/transport/transport_serial.cpp +++ b/zcm/transport/transport_serial.cpp @@ -1,27 +1,23 @@ +#include "generic_serial_transport.h" +#include "util/TimeUtil.hpp" #include "zcm/transport.h" -#include "zcm/transport_registrar.h" #include "zcm/transport_register.hpp" -#include "zcm/util/lockfile.h" +#include "zcm/transport_registrar.h" #include "zcm/util/debug.h" - -#include "generic_serial_transport.h" - -#include "util/TimeUtil.hpp" - -#include -#include -#include -#include -#include +#include "zcm/util/lockfile.h" #include +#include +#include #include #include - +#include +#include #include #include #include -#include +#include +#include using namespace std; // TODO: This transport layer needs to be "hardened" to handle @@ -31,21 +27,20 @@ using namespace std; // Define this the class name you want #define ZCM_TRANS_CLASSNAME TransportSerial -#define MTU (1<<14) +#define MTU (1 << 14) #define ESCAPE_CHAR (0xcc) -#define SERIAL_TIMEOUT_US 1e5 // u-seconds +#define SERIAL_TIMEOUT_US 1e5 // u-seconds -#define US_TO_MS(a) (a)/1e3 +#define US_TO_MS(a) (a) / 1e3 -using u8 = uint8_t; +using u8 = uint8_t; using u16 = uint16_t; using u32 = uint32_t; using u64 = uint64_t; -struct Serial -{ - Serial(){} +struct Serial { + Serial() {} ~Serial() { close(); } bool open(const string& port, int baud, bool hwFlowControl); @@ -54,7 +49,8 @@ struct Serial int write(const u8* buf, size_t sz); int read(u8* buf, size_t sz, u64 timeoutUs); - // Returns 0 on invalid input baud otherwise returns termios constant baud value + // Returns 0 on invalid input baud otherwise returns termios constant baud + // value static int convertBaud(int baud); Serial(const Serial&) = delete; @@ -62,7 +58,7 @@ struct Serial Serial& operator=(const Serial&) = delete; Serial& operator=(Serial&&) = delete; - private: + private: string port; int fd = -1; lockfile_t* lf; @@ -71,17 +67,21 @@ struct Serial bool Serial::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 "); + 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()); + ZCM_DEBUG( + "failed to create lock file, refusing to open serial device (%s)", + port_.c_str()); return false; } this->port = port_; @@ -89,7 +89,8 @@ bool Serial::open(const string& port_, int baud, bool hwFlowControl) 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)); + ZCM_DEBUG("failed to open serial device (%s): %s", port.c_str(), + strerror(errno)); goto fail; } @@ -117,8 +118,8 @@ bool Serial::open(const string& port_, int baud, bool hwFlowControl) opts.c_cflag |= CS8; opts.c_cflag &= ~PARENB; if (hwFlowControl) opts.c_cflag |= CRTSCTS; - opts.c_cc[VTIME] = 1; - opts.c_cc[VMIN] = 30; + opts.c_cc[VTIME] = 1; + opts.c_cc[VMIN] = 30; // set the new termios config if (tcsetattr(fd, TCSANOW, &opts)) { @@ -130,7 +131,7 @@ bool Serial::open(const string& port_, int baud, bool hwFlowControl) return true; - fail: +fail: // Close the port if it was opened if (fd > 0) { const int saved_errno = errno; @@ -194,7 +195,8 @@ int Serial::read(u8* buf, size_t sz, u64 timeoutUs) int ret = ::read(fd, buf, sz); if (ret == -1) { ZCM_DEBUG("ERR: serial read failed: %s", strerror(errno)); - } else if (ret == 0) { + } + else if (ret == 0) { ZCM_DEBUG("ERR: serial device unplugged"); close(); assert(false && "ERR: serial device unplugged\n" && @@ -202,11 +204,13 @@ int Serial::read(u8* buf, size_t sz, u64 timeoutUs) return -3; } return ret; - } else { + } + else { ZCM_DEBUG("ERR: serial bytes not ready"); return -1; } - } else { + } + else { ZCM_DEBUG("ERR: serial read timed out"); return -2; } @@ -236,8 +240,7 @@ int Serial::convertBaud(int baud) } } -struct ZCM_TRANS_CLASSNAME : public zcm_trans_t -{ +struct ZCM_TRANS_CLASSNAME : public zcm_trans_t { Serial ser; int baud; @@ -277,7 +280,8 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t auto* baudStr = findOption("baud"); if (!baudStr) { fprintf(stderr, "Baud unspecified. Bypassing serial baud setup.\n"); - } else { + } + else { baud = atoi(baudStr->c_str()); if (baud == 0) { ZCM_DEBUG("expected integer argument for 'baud'"); @@ -288,11 +292,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t hwFlowControl = false; auto* hwFlowControlStr = findOption("hw_flow_control"); if (hwFlowControlStr) { - if (*hwFlowControlStr == "true") { - hwFlowControl = true; - } else if (*hwFlowControlStr == "false") { + if (*hwFlowControlStr == "true") { hwFlowControl = true; } + else if (*hwFlowControlStr == "false") { hwFlowControl = false; - } else { + } + else { ZCM_DEBUG("expected boolean argument for 'hw_flow_control'"); return; } @@ -301,11 +305,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t raw = false; auto* rawStr = findOption("raw"); if (rawStr) { - if (*rawStr == "true") { - raw = true; - } else if (*rawStr == "false") { + if (*rawStr == "true") { raw = true; } + else if (*rawStr == "false") { raw = false; - } else { + } + else { ZCM_DEBUG("expected boolean argument for 'raw'"); return; } @@ -313,9 +317,7 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t rawChan = ""; auto* rawChanStr = findOption("raw_channel"); - if (rawChanStr) { - rawChan = *rawChanStr; - } + if (rawChanStr) { rawChan = *rawChanStr; } rawSize = 1024; auto* rawSizeStr = findOption("raw_size"); @@ -333,13 +335,11 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t if (raw) { rawBuf.reset(new uint8_t[rawSize]); gst = nullptr; - } else { - gst = zcm_trans_generic_serial_create(&ZCM_TRANS_CLASSNAME::get, - &ZCM_TRANS_CLASSNAME::put, - this, - &ZCM_TRANS_CLASSNAME::timestamp_now, - nullptr, - MTU, MTU * 10); + } + else { + gst = zcm_trans_generic_serial_create( + &ZCM_TRANS_CLASSNAME::get, &ZCM_TRANS_CLASSNAME::put, this, + &ZCM_TRANS_CLASSNAME::timestamp_now, nullptr, MTU, MTU * 10); } } @@ -349,41 +349,38 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t if (gst) zcm_trans_generic_serial_destroy(gst); } - bool good() - { - return ser.isOpen(); - } + 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); + 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; + 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); + 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) - { return TimeUtil::utime(); } + static uint64_t timestamp_now(void* usr) { return TimeUtil::utime(); } /********************** METHODS **********************/ - size_t getMtu() - { return raw ? MTU : zcm_trans_get_mtu(this->gst); } + 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 { + } + else { // Note: No need to lock here ONLY because the internals of // generic serial transport sendmsg only use the sendBuffer // and touch no variables related to receiving @@ -394,7 +391,10 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t } int recvmsgEnable(const char* channel, bool enable) - { return raw ? ZCM_EOK : zcm_trans_recvmsg_enable(this->gst, channel, enable); } + { + return raw ? ZCM_EOK + : zcm_trans_recvmsg_enable(this->gst, channel, enable); + } int recvmsg(zcm_msg_t* msg, unsigned timeoutMs) { @@ -404,27 +404,30 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t size_t sz = get(rawBuf.get(), rawSize, this); if (sz == 0 || rawChan.empty()) return ZCM_EAGAIN; - msg->utime = timestamp_now(this); + msg->utime = timestamp_now(this); msg->channel = rawChan.c_str(); - msg->len = sz; - msg->buf = rawBuf.get(); + msg->len = sz; + msg->buf = rawBuf.get(); return ZCM_EOK; - } else { + } + else { do { uint64_t startUtime = TimeUtil::utime(); // Note: No need to lock here ONLY because the internals of - // generic serial transport recvmsg only use the recv related - // data members and touch no variables related to sending + // generic serial transport recvmsg only use the recv + // related data members and touch no variables related to + // sending int ret = zcm_trans_recvmsg(this->gst, msg, 0); if (ret == ZCM_EOK) return ret; uint64_t diff = TimeUtil::utime() - startUtime; startUtime = TimeUtil::utime(); - // Note: timeoutLeftUs is calculated here because serial_update_rx - // needs it to be set properly so that the blocking read in - // `get` knows how long it has to exit + // Note: timeoutLeftUs is calculated here because + // serial_update_rx + // needs it to be set properly so that the blocking read + // in `get` knows how long it has to exit timeoutLeftUs = timeoutLeftUs > diff ? timeoutLeftUs - diff : 0; serial_update_rx(this->gst); @@ -446,20 +449,24 @@ struct ZCM_TRANS_CLASSNAME : public zcm_trans_t return (ZCM_TRANS_CLASSNAME*)zt; } - static size_t _getMtu(zcm_trans_t* zt) - { return cast(zt)->getMtu(); } + 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); } + { + return cast(zt)->sendmsg(msg); + } static int _recvmsgEnable(zcm_trans_t* zt, const char* channel, bool enable) - { return cast(zt)->recvmsgEnable(channel, 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); } + { + return cast(zt)->recvmsg(msg, timeout); + } - static void _destroy(zcm_trans_t* zt) - { delete cast(zt); } + static void _destroy(zcm_trans_t* zt) { delete cast(zt); } static const TransportRegister reg; }; @@ -469,17 +476,16 @@ zcm_trans_methods_t ZCM_TRANS_CLASSNAME::methods = { &ZCM_TRANS_CLASSNAME::_sendmsg, &ZCM_TRANS_CLASSNAME::_recvmsgEnable, &ZCM_TRANS_CLASSNAME::_recvmsg, - NULL, // drops - NULL, // update + NULL, // drops + NULL, // update &ZCM_TRANS_CLASSNAME::_destroy, }; -static zcm_trans_t* create(zcm_url_t* url, char **opt_errmsg) +static zcm_trans_t* create(zcm_url_t* url, char** opt_errmsg) { - if (opt_errmsg) *opt_errmsg = NULL; // Feature unused in this transport + if (opt_errmsg) *opt_errmsg = NULL; // Feature unused in this transport auto* trans = new ZCM_TRANS_CLASSNAME(url); - if (trans->good()) - return trans; + if (trans->good()) return trans; delete trans; return nullptr; @@ -488,8 +494,9 @@ static zcm_trans_t* create(zcm_url_t* url, char **opt_errmsg) #ifdef USING_TRANS_SERIAL // 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 " - "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", + "serial", + "Transfer data via a serial connection " + "(e.g. 'serial:///dev/ttyUSB0?baud=115200&hw_flow_control=true' or " + "'serial:///dev/pts/10?raw=true&raw_channel=RAW_SERIAL')", create); #endif diff --git a/zcm/transport/transport_serial.hpp b/zcm/transport/transport_serial.hpp new file mode 100644 index 00000000..5e9dea9d --- /dev/null +++ b/zcm/transport/transport_serial.hpp @@ -0,0 +1,33 @@ +#include "zcm/util/lockfile.h" + +#include + +using u8 = uint8_t; +using u16 = uint16_t; +using u32 = uint32_t; +using u64 = uint64_t; + +struct Serial { + Serial() {} + ~Serial() { close(); } + + bool open(const std::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 timeoutMs); + // Returns 0 on invalid input baud otherwise returns termios constant baud + // value + static int convertBaud(int baud); + + Serial(const Serial&) = delete; + Serial(Serial&&) = delete; + Serial& operator=(const Serial&) = delete; + Serial& operator=(Serial&&) = delete; + + private: + std::string port; + int fd = -1; + lockfile_t* lf; +}; diff --git a/zcm/wscript b/zcm/wscript index be063b7e..9c091679 100644 --- a/zcm/wscript +++ b/zcm/wscript @@ -39,6 +39,7 @@ def build(ctx): 'util/*.c', 'util/*.cpp', 'tools/*.c', 'tools/*.cpp', 'transport/*.c', 'transport/*.cpp', + 'transport/cobs_serial/*.c', 'transport/udp/*.cpp', 'transport/lockfree/lf_*.c'], excl=srcExcludes))