diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index a2372744a9..ce870cd0ac 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -276,7 +276,7 @@ bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us) return false; } - if (_frame.length + CRSF_HEADER_LEN < _frame_ofs) { + if (_frame.length < CRSF_FRAME_LENGTH_MIN) { // invalid short frame return false; } diff --git a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp index 8510695225..9c6a08371c 100644 --- a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp +++ b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp @@ -23,6 +23,12 @@ #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#include +#include +#endif + void setup(); void loop(); @@ -280,6 +286,39 @@ static bool test_protocol_bytesonly(const char *name, uint32_t baudrate, return ret; } +/* + test with random data + */ +static void test_random(void) +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + const uint32_t baudrates[] = { 115200, 100000, 416666, 420000 }; + const uint32_t test_bytes = 1000000; + int fd = open("/dev/urandom", O_RDONLY); + if (fd == -1) { + printf("Can't open /dev/urandom\n"); + return; + } + uint8_t *buf = (uint8_t *)malloc(test_bytes); + for (const auto b : baudrates) { + printf("Testing random with baud %u\n", unsigned(b)); + rcprot = new AP_RCProtocol(); + rcprot->init(); + if (::read(fd, buf, test_bytes) != test_bytes) { + printf("Failed to read from /dev/urandom\n"); + break; + } + for (uint32_t i=0; iprocess_byte(buf[i], b); + } + delete rcprot; + rcprot = nullptr; + } + free(buf); + close(fd); +#endif +} + //Main loop where the action takes place #pragma GCC diagnostic error "-Wframe-larger-than=2000" void loop() @@ -479,6 +518,11 @@ void loop() test_protocol("FPORT2_16CH", 115200, fport2_16ch_bytes, sizeof(fport2_16ch_bytes), fport2_16ch_output, ARRAY_SIZE(fport2_16ch_output), 3, 0, true); test_protocol("FPORT2_24CH", 115200, fport2_24ch_bytes, sizeof(fport2_24ch_bytes), fport2_24ch_output, ARRAY_SIZE(fport2_24ch_output), 3, 0, true); + /* + now test with random data to ensure we don't have any logic bugs that can cause a crash of the parser + */ + test_random(); + if (test_count++ == 10) { if (test_failures == 0) { printf("Test PASSED\n");