From a846b063f96d39b5f346eca447a431b433de6778 Mon Sep 17 00:00:00 2001 From: portersky <24420859+portersky@users.noreply.github.com> Date: Sun, 14 Jun 2026 20:51:57 +0200 Subject: [PATCH] feat: implement frame parse and streaming reader cel_crsf_frame_parse() parses ELRS USB format frames: [addr][length][type][payload...][crc] cel_crsf_stream_* provides incremental parsing from a byte stream: skips invalid sync bytes, discards bad CRC frames, buffers partial frames across feed calls. --- celrs/crsf.c | 36 ++++++--- celrs/crsf_stream.c | 73 +++++++++++------ tests/CMakeLists.txt | 8 ++ tests/test_crsf.c | 85 +++++++++++++++++++- tests/test_crsf_stream.c | 169 +++++++++++++++++++++++++++++++++++++++ 5 files changed, 335 insertions(+), 36 deletions(-) create mode 100644 tests/test_crsf_stream.c diff --git a/celrs/crsf.c b/celrs/crsf.c index 51ce4e3..ced13cc 100644 --- a/celrs/crsf.c +++ b/celrs/crsf.c @@ -85,16 +85,32 @@ size_t cel_crsf_build_param_write_frame(uint8_t* dst, uint8_t index, int cel_crsf_frame_parse(cel_crsf_frame* frame, uint8_t const* buf, size_t len) { - /* TODO: parse ELRS-format frame [addr][length][type][payload...][crc]. - * Validate: - * - buf has at least 4 bytes (addr + length + type + crc minimum) - * - total frame = 2 (addr+length) + length bytes - * - CRC over buf[2..2+length-1] matches buf[2+length-1] - * Copy type and payload into frame struct. */ - (void)frame; - (void)buf; - (void)len; - return -1; + if (frame == NULL || buf == NULL) return -1; + /* Minimum: addr(1) + length(1) + type(1) + crc(1) = 4 bytes */ + if (len < 4) return -1; + + uint8_t addr = buf[0]; + uint8_t length = buf[1]; + size_t total = 2 + length; + if (len < total) return -1; + + uint8_t type = buf[2]; + size_t payload_len = length > 1 ? length - 2 : 0; + uint8_t crc_recv = buf[total - 1]; + + /* CRC over type + payload */ + uint8_t crc_calc = cel_crsf_crc(buf + 2, 1 + payload_len); + if (crc_calc != crc_recv) return -1; + + frame->addr = addr; + frame->length = length; + frame->type = type; + frame->payload_len = (uint8_t)payload_len; + if (payload_len > 0) { + memcpy(frame->payload, buf + 3, payload_len); + } + frame->crc = crc_recv; + return 0; } /* --------------------------------------------------------------------------- */ diff --git a/celrs/crsf_stream.c b/celrs/crsf_stream.c index 4db3318..94131d1 100644 --- a/celrs/crsf_stream.c +++ b/celrs/crsf_stream.c @@ -15,44 +15,67 @@ static int is_valid_addr(uint8_t addr) { } struct cel_crsf_stream { - uint8_t buf[260]; /* internal buffer (max frame = 2 + 255 + 1 = 258) */ + uint8_t buf[260]; /* max frame = 2 + 255 + 1 = 258 */ size_t buf_len; }; cel_crsf_stream* cel_crsf_stream_create(void) { - /* TODO: allocate and zero-initialize the stream struct. */ - return NULL; + cel_crsf_stream* s = calloc(1, sizeof(*s)); + return s; } void cel_crsf_stream_destroy(cel_crsf_stream* stream) { - /* TODO: free the stream struct. */ - (void)stream; + free(stream); } int cel_crsf_stream_feed(cel_crsf_stream* stream, uint8_t const* data, size_t len, cel_crsf_frame* out, size_t out_capacity) { - /* TODO: append data to internal buffer, scan for complete frames. - * Algorithm: - * 1. Copy incoming bytes into internal buffer. - * 2. While buffer has >= 4 bytes: - * a. If buf[0] is not a valid address byte, pop(0) and continue. - * b. Read length = buf[1]. - * c. Total frame = 2 + length. - * d. If buffer < total, break (need more data). - * e. Parse frame from buf[0..total-1]. - * f. On success (CRC valid), copy into out[] and advance buffer. - * g. On failure (CRC invalid), discard frame and continue. - * 3. Return number of valid frames parsed, or -1 on error. */ - (void)stream; - (void)data; - (void)len; - (void)out; - (void)out_capacity; - return 0; + if (stream == NULL) return -1; + if (out == NULL) return -1; + if (data == NULL && len > 0) return -1; + if (data == NULL && len == 0) { + /* Drain remaining frames from buffer without new data */ + len = 0; + } + + /* Append new data to buffer */ + if (len > 0) { + if (stream->buf_len + len > sizeof(stream->buf)) { + /* Buffer overflow — discard everything */ + stream->buf_len = 0; + return -1; + } + memcpy(stream->buf + stream->buf_len, data, len); + stream->buf_len += len; + } + + int count = 0; + while (stream->buf_len >= 4 && count < (int)out_capacity) { + /* Skip invalid sync bytes */ + if (!is_valid_addr(stream->buf[0])) { + memmove(stream->buf, stream->buf + 1, stream->buf_len - 1); + stream->buf_len--; + continue; + } + + uint8_t length = stream->buf[1]; + size_t total = 2 + length; + if (stream->buf_len < total) break; /* need more data */ + + /* Try to parse frame */ + cel_crsf_frame frame; + if (cel_crsf_frame_parse(&frame, stream->buf, total) == 0) { + out[count++] = frame; + } + /* Discard frame (valid or not) */ + memmove(stream->buf, stream->buf + total, stream->buf_len - total); + stream->buf_len -= total; + } + + return count; } void cel_crsf_stream_reset(cel_crsf_stream* stream) { - /* TODO: zero buf_len to discard any partial frame. */ - (void)stream; + stream->buf_len = 0; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index dd188fb..5dd0729 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -38,6 +38,14 @@ target_compile_features(test_crsf PRIVATE c_std_23) add_test(NAME test_crsf COMMAND test_crsf) list(APPEND TEST_TARGETS test_crsf) +# CRSF stream tests +add_executable(test_crsf_stream test_crsf_stream.c) +target_include_directories(test_crsf_stream PRIVATE "${CMAKE_SOURCE_DIR}") +target_link_libraries(test_crsf_stream PRIVATE celrs_crsf Unity::Unity) +target_compile_features(test_crsf_stream PRIVATE c_std_23) +add_test(NAME test_crsf_stream COMMAND test_crsf_stream) +list(APPEND TEST_TARGETS test_crsf_stream) + # Serial tests — mocks the platform backend (serial_internal.h) add_executable(test_serial test_serial.c) target_include_directories(test_serial PRIVATE "${CMAKE_SOURCE_DIR}") diff --git a/tests/test_crsf.c b/tests/test_crsf.c index 330c872..e1199d5 100644 --- a/tests/test_crsf.c +++ b/tests/test_crsf.c @@ -18,7 +18,6 @@ void test_crc_single_byte(void) { } void test_crc_known_value(void) { - /* Verify CRC is deterministic */ uint8_t data[6] = {0x10, 0x80, 0x03, 0x02, 0x80, 0x01}; uint8_t crc = cel_crsf_crc(data, 6); TEST_ASSERT_TRUE(crc != 0); @@ -39,6 +38,83 @@ void test_channel_clamp_mid(void) { TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, cel_crsf_channel_clamp(CEL_CRSF_CH_MID)); } +/* Frame parse tests — ELRS format: [addr][length][type][payload][crc] */ + +/* Build a valid test frame with known CRC */ +static void build_test_frame(uint8_t* dst, uint8_t addr, uint8_t type, + uint8_t const* payload, uint8_t payload_len) { + uint8_t length = 1 + payload_len + 1; /* type + payload + crc */ + dst[0] = addr; + dst[1] = length; + dst[2] = type; + memcpy(dst + 3, payload, payload_len); + uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len); + dst[2 + length - 1] = crc; +} + +void test_parse_valid_frame(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_frame frame; + TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf))); + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type); + TEST_ASSERT_EQUAL_UINT8(2, frame.payload_len); + TEST_ASSERT_EQUAL_UINT8(0x80, frame.payload[0]); + TEST_ASSERT_EQUAL_UINT8(0x01, frame.payload[1]); +} + +void test_parse_null_frame(void) { + uint8_t buf[8]; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(NULL, buf, 8)); +} + +void test_parse_null_buf(void) { + cel_crsf_frame frame; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, NULL, 8)); +} + +void test_parse_too_short(void) { + cel_crsf_frame frame; + uint8_t buf[2] = {0xC8, 0x03}; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, 2)); +} + +void test_parse_bad_crc(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + buf[5] ^= 0xFF; /* corrupt the CRC */ + + cel_crsf_frame frame; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, sizeof(buf))); +} + +void test_parse_empty_payload(void) { + uint8_t buf[32]; + build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_HEARTBEAT, NULL, 0); + + cel_crsf_frame frame; + TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf))); + TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type); + TEST_ASSERT_EQUAL_UINT8(0, frame.payload_len); +} + +void test_parse_module_addr(void) { + uint8_t buf[32]; + uint8_t payload[4] = {0xAA, 0xBB, 0xCC, 0xDD}; + build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_GPS, payload, 4); + + cel_crsf_frame frame; + TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf))); + TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frame.type); + TEST_ASSERT_EQUAL_UINT8(4, frame.payload_len); +} + int main(void) { UNITY_BEGIN(); RUN_TEST(test_crc_empty); @@ -47,5 +123,12 @@ int main(void) { RUN_TEST(test_channel_clamp_min); RUN_TEST(test_channel_clamp_max); RUN_TEST(test_channel_clamp_mid); + RUN_TEST(test_parse_valid_frame); + RUN_TEST(test_parse_null_frame); + RUN_TEST(test_parse_null_buf); + RUN_TEST(test_parse_too_short); + RUN_TEST(test_parse_bad_crc); + RUN_TEST(test_parse_empty_payload); + RUN_TEST(test_parse_module_addr); return UNITY_END(); } diff --git a/tests/test_crsf_stream.c b/tests/test_crsf_stream.c new file mode 100644 index 0000000..c325af9 --- /dev/null +++ b/tests/test_crsf_stream.c @@ -0,0 +1,169 @@ +#include "unity.h" +#include "celrs/crsf.h" +#include + +void setUp(void) {} +void tearDown(void) {} + +/* Helper: build a valid test frame, return total bytes */ +static size_t build_frame(uint8_t* dst, uint8_t addr, uint8_t type, + uint8_t const* payload, uint8_t payload_len) { + uint8_t length = 1 + payload_len + 1; /* type + payload + crc */ + dst[0] = addr; + dst[1] = length; + dst[2] = type; + memcpy(dst + 3, payload, payload_len); + uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len); + dst[2 + length - 1] = crc; + return 2 + length; +} + +/* Stream tests */ + +void test_stream_create_destroy(void) { + cel_crsf_stream* s = cel_crsf_stream_create(); + TEST_ASSERT_NOT_NULL(s); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_empty(void) { + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + int n = cel_crsf_stream_feed(s, NULL, 0, frames, 4); + TEST_ASSERT_EQUAL_INT(0, n); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_single_frame(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + int n = cel_crsf_stream_feed(s, buf, len, frames, 4); + TEST_ASSERT_EQUAL_INT(1, n); + TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type); + TEST_ASSERT_EQUAL_UINT8(2, frames[0].payload_len); + TEST_ASSERT_EQUAL_UINT8(0x80, frames[0].payload[0]); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_incremental(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + size_t total = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + + /* Feed first 3 bytes — not enough for a complete frame */ + int n = cel_crsf_stream_feed(s, buf, 3, frames, 4); + TEST_ASSERT_EQUAL_INT(0, n); + + /* Feed remaining bytes — now complete */ + n = cel_crsf_stream_feed(s, buf + 3, total - 3, frames, 4); + TEST_ASSERT_EQUAL_INT(1, n); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_multiple_frames(void) { + uint8_t buf[64]; + uint8_t p1[2] = {0x80, 0x01}; + uint8_t p2[3] = {0xAA, 0xBB, 0xCC}; + + size_t len1 = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, p1, 2); + size_t len2 = build_frame(buf + len1, 0xEE, CEL_CRSF_TYPE_GPS, p2, 3); + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + int n = cel_crsf_stream_feed(s, buf, len1 + len2, frames, 4); + TEST_ASSERT_EQUAL_INT(2, n); + TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type); + TEST_ASSERT_EQUAL_UINT8(0xEE, frames[1].addr); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frames[1].type); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_skip_bad_sync(void) { + uint8_t buf[48]; + /* Garbage bytes before valid frame */ + buf[0] = 0xFF; + buf[1] = 0xFE; + buf[2] = 0xFD; + + uint8_t payload[2] = {0x80, 0x01}; + size_t len = build_frame(buf + 3, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + int n = cel_crsf_stream_feed(s, buf, 3 + len, frames, 4); + TEST_ASSERT_EQUAL_INT(1, n); + TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr); + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_discard_bad_crc(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + buf[len - 1] ^= 0xFF; /* corrupt CRC */ + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[4]; + int n = cel_crsf_stream_feed(s, buf, len, frames, 4); + TEST_ASSERT_EQUAL_INT(0, n); /* bad frame discarded */ + cel_crsf_stream_destroy(s); +} + +void test_stream_feed_overflow(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_stream* s = cel_crsf_stream_create(); + cel_crsf_frame frames[1]; + /* Feed two frames but only have room for one */ + size_t total = len * 2; + memcpy(buf + len, buf, len); + int n = cel_crsf_stream_feed(s, buf, total, frames, 1); + TEST_ASSERT_EQUAL_INT(1, n); + /* Second frame should still be in buffer */ + n = cel_crsf_stream_feed(s, NULL, 0, frames, 1); + TEST_ASSERT_EQUAL_INT(1, n); + cel_crsf_stream_destroy(s); +} + +void test_stream_reset(void) { + uint8_t buf[32]; + uint8_t payload[2] = {0x80, 0x01}; + size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2); + + cel_crsf_stream* s = cel_crsf_stream_create(); + /* Feed partial frame */ + cel_crsf_frame frames[4]; + cel_crsf_stream_feed(s, buf, 3, frames, 4); + /* Reset should discard partial */ + cel_crsf_stream_reset(s); + /* Feed complete frame — should parse normally */ + int n = cel_crsf_stream_feed(s, buf, len, frames, 4); + TEST_ASSERT_EQUAL_INT(1, n); + cel_crsf_stream_destroy(s); +} + +int main(void) { + UNITY_BEGIN(); + RUN_TEST(test_stream_create_destroy); + RUN_TEST(test_stream_feed_empty); + RUN_TEST(test_stream_feed_single_frame); + RUN_TEST(test_stream_feed_incremental); + RUN_TEST(test_stream_feed_multiple_frames); + RUN_TEST(test_stream_feed_skip_bad_sync); + RUN_TEST(test_stream_feed_discard_bad_crc); + RUN_TEST(test_stream_feed_overflow); + RUN_TEST(test_stream_reset); + return UNITY_END(); +}