diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c old mode 100755 new mode 100644 index 703242d10dd..152e489dd16 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -820,11 +820,14 @@ static bool gpsParseFrameUBLOX(void) if(_buffer.navsig.numSigs > 0) { - for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) + // A multi band receiver can report more signals than we have room for, those were + // already dropped while the frame was received. + const int numSigs = MIN(_buffer.navsig.numSigs, UBLOX_MAX_SIGNALS); + for(int i=0; i < numSigs; ++i) { memcpy(&satelites[i], &_buffer.navsig.sig[i], sizeof(ubx_nav_sig_info)); } - for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) + for(int i = numSigs; i < UBLOX_MAX_SIGNALS; ++i) { satelites[i].svId = 0xFF; // no used satelites[i].gnssId = 0xFF; @@ -897,11 +900,20 @@ static bool gpsNewFrameUBLOX(uint8_t data) _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length |= (uint16_t)(data << 8); - if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) { - // we can't receive the whole packet, just log the error and start searching for the next packet. - gpsStats.errors++; - _step = 0; - break; + if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE) { + // Multi band receivers report more signals than fit in the buffer, so UBX-NAV-SIG + // and UBX-NAV-SAT are read to the end and checksummed anyway. Only the first + // MAX_UBLOX_PAYLOAD_SIZE bytes are kept, the signals that do not fit are dropped. + // That keeps the parser in sync and the rest of the frame usable. Anything else + // this long is garbage, just log the error and search for the next packet. + const bool truncatable = (_class == CLASS_NAV) && + (_msg_id == MSG_NAV_SIG || _msg_id == MSG_NAV_SAT) && + (_payload_length <= UBLOX_MAX_ACCEPTED_PAYLOAD_SIZE); + if (!truncatable) { + gpsStats.errors++; + _step = 0; + break; + } } // prepare to receive payload _payload_counter = 0; @@ -914,6 +926,17 @@ static bool gpsNewFrameUBLOX(uint8_t data) if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) { _buffer.bytes[_payload_counter] = data; } + if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE && _payload_counter == 7) { + // Both NAV-SIG and NAV-SAT have an eight-byte header with the record count at byte 5. + // Reject inconsistent lengths before a damaged header consumes subsequent fixes. + const uint16_t recordSize = _msg_id == MSG_NAV_SIG ? sizeof(ubx_nav_sig_info) : sizeof(ubx_nav_svinfo_channel); + const uint16_t expectedLength = 8 + _buffer.bytes[5] * recordSize; + if (_payload_length != expectedLength) { + gpsStats.errors++; + _step = 0; + break; + } + } // NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received. if (_payload_counter == _payload_length - 1) { _step++; @@ -942,6 +965,12 @@ static bool gpsNewFrameUBLOX(uint8_t data) break; } + // An oversized payload was truncated while being received, so the frame handlers must + // not look at anything beyond what is actually in the buffer. + if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE) { + _payload_length = MAX_UBLOX_PAYLOAD_SIZE; + } + if (gpsParseFrameUBLOX()) { parsed = true; } diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 75f10901035..5d36aead6b9 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -31,15 +31,22 @@ extern "C" { #define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 +// Number of UBX-NAV-SIG signals that are stored. Multi band receivers report two to three signals +// per satellite and can send more than this, the surplus signals are dropped while receiving. #ifndef UBLOX_MAX_SIGNALS #define UBLOX_MAX_SIGNALS 64 #endif #define MAX_UBLOX_PAYLOAD_SIZE ((UBLOX_MAX_SIGNALS * 16) + 8) // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS * 16 + 8 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE +// Longest UBX-NAV-SIG / UBX-NAV-SAT payload that is still read to the end (and checksummed) when it +// does not fit in the buffer. numSigs and numSvs are U1, so this covers every such frame a receiver +// can send. Anything longer is treated as garbage and the parser resyncs immediately. +#define UBLOX_MAX_ACCEPTED_PAYLOAD_SIZE ((255 * 16) + 8) #define UBLOX_SBAS_MESSAGE_LENGTH 16 #define GPS_CAPA_INTERVAL 5000 STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); +STATIC_ASSERT(UBLOX_MAX_ACCEPTED_PAYLOAD_SIZE >= MAX_UBLOX_PAYLOAD_SIZE, ubx_accepted_size_too_small); #define UBX_DYNMODEL_PORTABLE 0 #define UBX_DYNMODEL_STATIONARY 2 @@ -270,9 +277,13 @@ typedef struct { uint8_t version; // We support version 0 uint8_t numSigs; // number of signals uint16_t reserved; - ubx_nav_sig_info sig[UBLOX_MAX_SIGNALS]; // 32 signals + ubx_nav_sig_info sig[UBLOX_MAX_SIGNALS]; // UBLOX_MAX_SIGNALS signals } __attribute__((packed)) ubx_nav_sig; +// The receive buffer has to be able to hold a complete UBLOX_MAX_SIGNALS frame, otherwise the copy +// loop in gpsParseFrameUBLOX() would read past the data that was actually received. +STATIC_ASSERT(sizeof(ubx_nav_sig) <= UBLOX_BUFFER_SIZE, ubx_nav_sig_exceeds_buffer); + #define MAX_GNSS 7 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)