From 061f39fd79a22df54f78857f4ed58e61acc4e91d Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Mon, 22 Jul 2024 01:13:04 +0200 Subject: [PATCH] fix: CRSF frame parsing (#5243) Co-authored-by: LupusTheCanine --- radio/src/pulses/crossfire.cpp | 122 +++++++++-------- radio/src/tests/crossfire.cpp | 234 +++++++++++++++++++++++++++++++++ 2 files changed, 298 insertions(+), 58 deletions(-) diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 87748c06b..808ac0b4c 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/radio/src/pulses/crossfire.cpp @@ -154,6 +154,7 @@ static bool _checkFrameCRC(uint8_t* rxBuffer) { uint8_t len = rxBuffer[1]; uint8_t crc = crc8(&rxBuffer[2], len - 1); + // TRACE("[XF] crc = %d; pkt = %d", crc, rxBuffer[len + 1]); return (crc == rxBuffer[len + 1]); } @@ -175,76 +176,42 @@ static void crossfireSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, u drv->sendBuffer(drv_ctx, buffer, p_buf - buffer); } -static bool _lenIsSane(uint8_t len) +static bool _lenIsSane(uint32_t len) { // packet len must be at least 3 bytes (type + payload + crc) // and 2 bytes < MAX (hdr + len) return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE - 1); } -static void crossfireProcessFrame(void* ctx, uint8_t* frame, uint8_t frame_len, - uint8_t* buf, uint8_t* p_len) +static bool _validHdr(uint8_t* buf) { - if (frame_len < MIN_FRAME_LEN) return; - - // De-Fragmentation: - // It is assumed here that a continuation chunk - // will not belong to multiple CRSF frames. - - uint8_t& len = *p_len; - if (len > 0) { - uint8_t unfrag_len = buf[1] + 2; - uint8_t defrag_len = len + frame_len; - - if (defrag_len <= unfrag_len && defrag_len <= TELEMETRY_RX_PACKET_SIZE) { - // If we're not going to overshoot - // the intended frame length, - // let's reassemble it - memcpy(buf + len, frame, frame_len); - len = defrag_len; - frame_len = 0; - - // frame complete? - if (defrag_len < unfrag_len) { - TRACE("[XF] frag cont frame (%d < %d)", defrag_len, unfrag_len); - return; - } else { - TRACE("[XF] frame complete"); - } - } else { - TRACE("[XF] overshoot (%d > %d)", defrag_len, unfrag_len); - } - } - - if (frame_len > 0) { - memcpy(buf, frame, frame_len); - len = frame_len; - - uint8_t unfrag_len = buf[1] + 2; - if (!_lenIsSane(unfrag_len)) { - TRACE("[XF] pkt len error (%d)", unfrag_len); - len = 0; - return; - } - - if (len < unfrag_len) { - TRACE("[XF] frag frame (%d < %d)", len, unfrag_len); - return; - } - } + return buf[0] == RADIO_ADDRESS || buf[0] == UART_SYNC; +} +static uint8_t* _processFrames(void* ctx, uint8_t* buf, uint8_t& len) +{ uint8_t* p_buf = buf; while (len >= MIN_FRAME_LEN) { - uint8_t pkt_len = p_buf[1] + 2; - if (pkt_len > len) { - TRACE("[XF] length error (%d > %d)", pkt_len, len); - len = 0; - return; + + if (!_validHdr(p_buf)) { + TRACE("[XF] skipping invalid start bytes"); + do { p_buf++; len--; } while(len > 0 && !_validHdr(p_buf)); + if (len < MIN_FRAME_LEN) break; } - if (p_buf[0] != RADIO_ADDRESS && p_buf[0] != UART_SYNC) { - TRACE("[XF] address 0x%02X error", p_buf[0]); - } else if (!_checkFrameCRC(p_buf)) { + uint32_t pkt_len = p_buf[1] + 2; + if (!_lenIsSane(pkt_len)) { + TRACE("[XF] pkt len error (%d)", pkt_len); + len = 0; + break; + } + + if (pkt_len > (uint32_t)len) { + // incomplete packet + break; + } + + if (!_checkFrameCRC(p_buf)) { TRACE("[XF] CRC error "); } else { #if defined(BLUETOOTH) @@ -262,6 +229,45 @@ static void crossfireProcessFrame(void* ctx, uint8_t* frame, uint8_t frame_len, p_buf += pkt_len; len -= pkt_len; } + + return p_buf; +} + +static void crossfireProcessFrame(void* ctx, uint8_t* frame, uint8_t frame_len, + uint8_t* buf, uint8_t* p_len) +{ + if (frame_len < MIN_FRAME_LEN) return; + + uint8_t& len = *p_len; + if (len == 0) { + // buffer is empty: no re-assembly + if (!_validHdr(frame)) { + TRACE("[XF] invalid frame start"); + return; + } + + // process frames directly out of RX buffer + uint8_t* p_buf = _processFrames(ctx, frame, frame_len); + if (frame_len > 0) { + memcpy(buf, p_buf, frame_len); + len = frame_len; + } + } else { + uint32_t defrag_len = (uint32_t)len + (uint32_t)frame_len; + if (defrag_len > TELEMETRY_RX_PACKET_SIZE) { + TRACE("[XF] overflow (%d > %d)", defrag_len, TELEMETRY_RX_PACKET_SIZE); + frame_len = TELEMETRY_RX_PACKET_SIZE - len; + defrag_len = (uint32_t)len + (uint32_t)frame_len; + } + + memcpy(buf + len, frame, frame_len); + len = (uint8_t)defrag_len; + + uint8_t* p_buf = _processFrames(ctx, buf, len); + if ((len > 0) && (p_buf != buf)) { + memmove(buf, p_buf, len); + } + } } static const etx_serial_init crsfSerialParams = { diff --git a/radio/src/tests/crossfire.cpp b/radio/src/tests/crossfire.cpp index 9cf855ffc..07a91357f 100644 --- a/radio/src/tests/crossfire.cpp +++ b/radio/src/tests/crossfire.cpp @@ -19,9 +19,12 @@ * GNU General Public License for more details. */ +#include "gtest/gtest.h" #include "gtests.h" +#include "telemetry/telemetry.h" #if defined(CROSSFIRE) + uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses); TEST(Crossfire, createCrossfireChannelsFrame) { @@ -44,5 +47,236 @@ TEST(Crossfire, crc8) uint8_t crc = crc8(&frame[2], frame[1]-1); ASSERT_EQ(frame[frame[1]+1], crc); } + +#if defined(HARDWARE_EXTERNAL_MODULE) +#include "pulses/crossfire.h" + +struct crsf_frame_test { + void* ctx = nullptr; + + uint8_t buffer[TELEMETRY_RX_PACKET_SIZE]; + uint8_t len = 0; + + crsf_frame_test() + { + ctx = CrossfireDriver.init(EXTERNAL_MODULE); + if (!luaInputTelemetryFifo) { + luaInputTelemetryFifo = + new Fifo(); + assert(luaInputTelemetryFifo != nullptr); + } else { + luaInputTelemetryFifo->clear(); + } + } + + template + void process(uint8_t (&frame)[Len]) { + CrossfireDriver.processFrame(ctx, frame, Len, buffer, &len); + } + + ~crsf_frame_test() + { + if (ctx != nullptr) { + CrossfireDriver.deinit(ctx); + } + } +}; + +static uint8_t incomplete_frame[] = { + // first frame + 0xEA, 0x14, 0xFF, 0x11, 0xFD, 0x05, 0x00, 0x00, 0x13, 0x01, 0x01, + 0x2A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x5D, 0x98, 0xB4, + // second frame + 0xEA, 0x21, 0xFF, 0x1E, 0xFD, 0x12, 0x00, 0x00, 0x14, 0x01, 0x01, + 0x4A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x8F, 0xC2, 0x35, 0x3F, 0xFD, 0xC7, 0xD1, 0x3C, 0x4C, 0x01, 0x92, + 0x3F, 0x31, + // incomplete third frame + 0xEA, 0x1F, 0xFF, 0x1C, 0xFD, 0x10, 0x00, +}; + +static uint8_t cont_frame[] = { + 0x00, 0x15, 0x01, 0x01, 0x24, 0x00, 0x00, 0x35, 0x7D, 0xF2, 0x40, + 0xE8, 0x03, 0xE8, 0x03, 0xDC, 0x05, 0xDC, 0x05, 0xE1, 0x05, 0xE1, + 0x05, 0x2A, 0xFE, 0x5F, + //next trailing packet start + 0xEA, 0x0A, 0x0B +}; + +TEST(Crossfire, frameParser_incompleteFrames) +{ + crsf_frame_test ft; + if (!ft.ctx) return; + + ft.process(incomplete_frame); + EXPECT_EQ(ft.len, 7); + EXPECT_EQ(ft.buffer[0], 0xEA); + EXPECT_EQ(ft.buffer[1], 0x1F); + + ft.process(cont_frame); + EXPECT_EQ(ft.len, 3); + EXPECT_EQ(ft.buffer[0], 0xEA); + EXPECT_EQ(ft.buffer[1], 0x0A); + + uint8_t* lua_buffer = luaInputTelemetryFifo->buffer(); + EXPECT_EQ(luaInputTelemetryFifo->size(), 0x14 + 0x21 + 0x1F); + + unsigned offset = 0; + EXPECT_EQ(lua_buffer[offset], 0x14); + offset += 0x14; + + EXPECT_EQ(lua_buffer[offset], 0x21); + offset += 0x21; + + EXPECT_EQ(lua_buffer[offset], 0x1F); + EXPECT_EQ(lua_buffer[offset + 0x1F - 1], 0xFE); +} + +static uint8_t length_error[] = { + 0x2A, 0xFE, 0x5F, 0x00, +}; + +static uint8_t length_error2[] = { + // first frame + 0xEA, 0x09, 0xFF, 0x11, 0xFD, 0x05, 0x00, 0x00, 0x13, 0x01, 0x8C, + // 2nd incomplete frame + 0xEA, 0xFE, 0x5F, 0x00, +}; + +static uint8_t length_error3[] = { + // first frame + 0xEA, 0x09, 0xFF, 0x11, 0xFD, 0x05, 0x00, 0x00, 0x13, 0x01, 0x8C, + // invalid: invalid frame start + 0x2A, 0x09, 0x5F, 0x00, + // 2nd valid frame + 0xEA, 0x09, 0xFF, 0x11, 0xFD, 0x05, 0x00, 0x00, 0x13, 0x01, 0x8C, +}; + + +TEST(Crossfire, frameParser_length) +{ + crsf_frame_test ft; + if (!ft.ctx) return; + + uint8_t* lua_buffer = luaInputTelemetryFifo->buffer(); + + // Check that a frame that is too big is rejected even if incomplete + ft.process(length_error); + EXPECT_EQ(ft.len, 0); + + // Check that a frame that is too big is rejected if positioned + // after a complete frame + ft.process(length_error2); + EXPECT_EQ(ft.len, 0); + + // the first complete frame should have been processed + EXPECT_EQ(luaInputTelemetryFifo->size(), 0x09); + + ft.process(length_error3); + EXPECT_EQ(ft.len, 0); + + // only the first frame has been processed, as the rest + // of the input buffer is thrown away due to length error + EXPECT_EQ(luaInputTelemetryFifo->size(), 0x09 + 0x09 + 0x09); + + // check all 3 frames + unsigned offset = 0; + for (int i = 0; i < 3; i++) { + EXPECT_EQ(lua_buffer[offset], 0x09); + EXPECT_EQ(lua_buffer[offset + 0x09 - 1], 0x01); + offset += 0x09; + } +} + +static uint8_t invalid_frames[] = { + // first frame + 0xEA, 0x14, 0xFF, 0x11, 0xFD, 0x05, 0x00, 0x00, 0x13, 0x01, 0x01, + 0x2A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x5D, 0x98, 0xB4, + // random bytes + 0x00, 0x35, 0xA4, + // second frame + 0xEA, 0x21, 0xFF, 0x1E, 0xFD, 0x12, 0x00, 0x00, 0x14, 0x01, 0x01, + 0x4A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x8F, 0xC2, 0x35, 0x3F, 0xFD, 0xC7, 0xD1, 0x3C, 0x4C, 0x01, 0x92, + 0x3F, 0x31, + // invalid CRC frame + 0xEA, 0x02, 0x00, 0x01, + // third frame + 0xEA, 0x1F, 0xFF, 0x1C, 0xFD, 0x10, 0x00,0x00, 0x15, 0x01, 0x01, 0x24, 0x00, 0x00, 0x35, 0x7D, 0xF2, 0x40, + 0xE8, 0x03, 0xE8, 0x03, 0xDC, 0x05, 0xDC, 0x05, 0xE1, 0x05, 0xE1, + 0x05, 0x2A, 0xFE, 0x5F, +}; + +TEST(Crossfire, frameParser_badFrames) +{ + //check if frameParser correctly skips bad frames (too long, bad CRC) and does't lose following packets + crsf_frame_test ft; + if (!ft.ctx) return; + + ft.process(invalid_frames); + EXPECT_EQ(ft.len,0); + + uint8_t* lua_buffer = luaInputTelemetryFifo->buffer(); + EXPECT_EQ(luaInputTelemetryFifo->size(), 0x14 + 0x21 + 0x1F); + + unsigned offset = 0; + EXPECT_EQ(lua_buffer[offset], 0x14); + offset += 0x14; + + EXPECT_EQ(lua_buffer[offset], 0x21); + offset += 0x21; + + EXPECT_EQ(lua_buffer[offset], 0x1F); + EXPECT_EQ(lua_buffer[offset + 0x1F - 1], 0xFE); +} + +static uint8_t jumboFrame1[]={ + 0xEA, 0x3E, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFB +}; +static uint8_t jumboFrame2[]={ //jumbo frame 2 + 0x8D, 0xEA, 0x3D, 0xFE, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x57 +}; + +TEST(Crossfire, frameParser_multipleJumboFrames) +{ + crsf_frame_test ft; + if (!ft.ctx) return; + + ft.process(jumboFrame1); + EXPECT_EQ(ft.len, 63); + EXPECT_EQ(ft.buffer[0], 0xEA); + EXPECT_EQ(ft.buffer[1], 0x3E); + + ft.process(jumboFrame2); + EXPECT_EQ(ft.len,0); + + uint8_t* lua_buffer = luaInputTelemetryFifo->buffer(); + EXPECT_EQ(luaInputTelemetryFifo->size(), 62 + 61); + + unsigned offset = 0; + EXPECT_EQ(lua_buffer[offset], 0x3E); + EXPECT_EQ(lua_buffer[offset + 0x3E - 1], 0xFB); + + EXPECT_EQ(lua_buffer[offset], 0x3E); + offset += 0x3E; + + EXPECT_EQ(lua_buffer[offset], 0x3D); + EXPECT_EQ(lua_buffer[offset + 0x3D - 1], 0xF0); +} +#endif // HARDWARE_EXTERNAL_MODULE #endif