From 048dfee68a694d31f40d5ee7cb6d2b80841df5e5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Sep 2017 15:40:39 +1000 Subject: [PATCH] AP_GPS: make SBF driver more robust this fixes a crash bug in the SBF GPS driver where a badly formed packet could cause crc16_ccitt to reference invalid memory --- libraries/AP_GPS/AP_GPS_SBF.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 8f35a054e9..86223f1a8a 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -132,14 +132,22 @@ AP_GPS_SBF::parse(uint8_t temp) } break; case sbf_msg_parser_t::DATA: - if (sbf_msg.read >= sizeof(sbf_msg.data)) { - Debug("parse overflow length=%u\n", (unsigned)sbf_msg.read); - sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; - break; + if (sbf_msg.read < sizeof(sbf_msg.data)) { + sbf_msg.data.bytes[sbf_msg.read] = temp; } - sbf_msg.data.bytes[sbf_msg.read] = temp; sbf_msg.read++; if (sbf_msg.read >= (sbf_msg.length - 8)) { + if (sbf_msg.read > sizeof(sbf_msg.data)) { + // not interested in these large messages + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + break; + } + if (sbf_msg.length < 8) { + Debug("length error %u\n", (unsigned)sbf_msg.length); + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + crc_error_counter++; + break; + } uint16_t crc = crc16_ccitt((uint8_t*)&sbf_msg.blockid, 2, 0); crc = crc16_ccitt((uint8_t*)&sbf_msg.length, 2, crc); crc = crc16_ccitt((uint8_t*)&sbf_msg.data, sbf_msg.length - 8, crc);