diff --git a/src/drivers/gnss/septentrio/sbf/decoder.cpp b/src/drivers/gnss/septentrio/sbf/decoder.cpp index 33f180a159..14e0682074 100644 --- a/src/drivers/gnss/septentrio/sbf/decoder.cpp +++ b/src/drivers/gnss/septentrio/sbf/decoder.cpp @@ -118,6 +118,7 @@ int Decoder::parse(Header *header) const int Decoder::parse(DOP *message) const { if (can_parse() && id() == BlockID::DOP) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(DOP)); return PX4_OK; } @@ -128,6 +129,7 @@ int Decoder::parse(DOP *message) const int Decoder::parse(PVTGeodetic *message) const { if (can_parse() && id() == BlockID::PVTGeodetic) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(PVTGeodetic)); return PX4_OK; } @@ -138,6 +140,7 @@ int Decoder::parse(PVTGeodetic *message) const int Decoder::parse(ReceiverStatus *message) const { if (can_parse() && id() == BlockID::ReceiverStatus) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(ReceiverStatus)); return PX4_OK; } @@ -148,6 +151,7 @@ int Decoder::parse(ReceiverStatus *message) const int Decoder::parse(QualityInd *message) const { if (can_parse() && id() == BlockID::QualityInd) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); // Safe to copy entire size of the message as it is smaller than the maximum expected SBF message size. // It's up to the user of the parsed message to ignore the invalid fields. memcpy(message, _message.payload, sizeof(QualityInd)); @@ -160,11 +164,16 @@ int Decoder::parse(QualityInd *message) const int Decoder::parse(RFStatus *message) const { if (can_parse() && id() == BlockID::PVTGeodetic) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band)); for (uint8_t i = 0; i < math::min(message->n, k_max_rfband_blocks); i++) { - memcpy(&message->rf_band[i], &_message.payload[sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i * - message->sb_length], sizeof(RFBand)); + const unsigned offset = sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i * + message->sb_length; + + if (offset + sizeof(RFBand) <= sizeof(_message.payload)) { + memcpy(&message->rf_band[i], &_message.payload[offset], sizeof(RFBand)); + } } return PX4_OK; @@ -176,6 +185,7 @@ int Decoder::parse(RFStatus *message) const int Decoder::parse(GALAuthStatus *message) const { if (can_parse() && id() == BlockID::GALAuthStatus) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(GALAuthStatus)); return PX4_OK; } @@ -186,6 +196,7 @@ int Decoder::parse(GALAuthStatus *message) const int Decoder::parse(VelCovGeodetic *message) const { if (can_parse() && id() == BlockID::VelCovGeodetic) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(VelCovGeodetic)); return PX4_OK; } @@ -196,11 +207,17 @@ int Decoder::parse(VelCovGeodetic *message) const int Decoder::parse(GEOIonoDelay *message) const { if (can_parse() && id() == BlockID::GEOIonoDelay) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc)); - for (size_t i = 0; i < math::min(message->n, (uint8_t)4); i++) { - memcpy(&message->idc[i], &_message.payload[sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i * - message->sb_length], sizeof(IDC)); + for (size_t i = 0; i < math::min(message->n, (uint8_t)(sizeof(GEOIonoDelay::idc) / sizeof(GEOIonoDelay::idc[0]))); + i++) { + const unsigned offset = sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i * + message->sb_length; + + if (offset + sizeof(IDC) <= sizeof(_message.payload)) { + memcpy(&message->idc[i], &_message.payload[offset], sizeof(IDC)); + } } return PX4_OK; @@ -212,6 +229,7 @@ int Decoder::parse(GEOIonoDelay *message) const int Decoder::parse(AttEuler *message) const { if (can_parse() && id() == BlockID::AttEuler) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(AttEuler)); return PX4_OK; } @@ -222,6 +240,7 @@ int Decoder::parse(AttEuler *message) const int Decoder::parse(AttCovEuler *message) const { if (can_parse() && id() == BlockID::AttCovEuler) { + static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(AttCovEuler)); return PX4_OK; }