fix: CRSF frame parsing (#5243)
Co-authored-by: LupusTheCanine <tymoteusz.2000.0+GitHub@gmail.com>
This commit is contained in:
Raphael Coeffic
co-authored by
LupusTheCanine
LupusTheCanine
parent
21389006dd
commit
061f39fd79
@@ -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 = {
|
||||
|
||||
@@ -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<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE>();
|
||||
assert(luaInputTelemetryFifo != nullptr);
|
||||
} else {
|
||||
luaInputTelemetryFifo->clear();
|
||||
}
|
||||
}
|
||||
|
||||
template<unsigned Len>
|
||||
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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user