Validate RTCM from UDP before forwarding

src/GPS/NTRIP/NTRIPManager.cc

@@ -55,7 +55,8 @@ NTRIPManager::NTRIPManager(QObject* parent)
         auto applyUdpInputSettings = [this, settings]() {
             const quint16 uin_port = static_cast<quint16>(
                 settings->rtcmUdpInputPort()->rawValue().toUInt());
-                _rtcmUdpInput->setPort(uin_port);
+            _rtcmUdpInput->setPort(uin_port);
+            _rtcmUdpInput->setValidation(settings->rtcmUdpValidate()->rawValue().toBool());
             if (settings->rtcmUdpInputEnabled()->rawValue().toBool()) {
                 _rtcmUdpInput->start();
             } else {

src/GPS/NTRIP/RTCMParser.h

@@ -16,6 +16,7 @@ class RTCMParser
     uint16_t messageId() const;
     const uint8_t* crcBytes() const { return _crcBytes; }
     static constexpr int kCrcSize = 3;
+    static constexpr int kHeaderSize = 3;
 
     bool validateCrc() const;
     static uint32_t crc24q(const uint8_t* data, size_t len);
@@ -29,7 +30,6 @@ class RTCMParser
     };
 
     static constexpr uint16_t kMaxPayloadLength = 1023;
-    static constexpr int kHeaderSize = 3;
 
     State _state;
     uint8_t _buffer[kHeaderSize + kMaxPayloadLength];

src/GPS/NTRIP/RTCMUdpInput.cc

@@ -18,6 +18,7 @@ RTCMUdpInput::~RTCMUdpInput()
 bool RTCMUdpInput::start()
 {
     stop();
+    _rtcmParser.reset();
 
     if (!_socket.bind(QHostAddress::AnyIPv4, _port)) {
         qCWarning(RTCMUdpInputLog) << "Failed to bind UDP socket on port" << _port
@@ -62,7 +63,7 @@ void RTCMUdpInput::_readDatagrams()
     while (_socket.hasPendingDatagrams()) {
         const qint64 size = _socket.pendingDatagramSize();
         if (size <= 0) {
-            (void) _socket.readDatagram(nullptr, 0); // discard malformed
+            (void) _socket.readDatagram(nullptr, 0);  // discard malformed
             continue;
         }
 
@@ -77,7 +78,52 @@ void RTCMUdpInput::_readDatagrams()
             data.resize(static_cast<qsizetype>(read));
         }
 
-        qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << read << "bytes";
-        emit rtcmDataReceived(data);
+        if (!_validateRtcm) {
+            qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << read << "bytes";
+            emit rtcmDataReceived(data);
+            continue;
+        }
+
+        QByteArray validData;
+        int framesFound   = 0;
+        int framesDropped = 0;
+
+        for (qsizetype i = 0; i < data.size(); ++i) {
+            if (!_rtcmParser.addByte(static_cast<uint8_t>(data[i]))) {
+                continue;
+            }
+
+            if (_rtcmParser.validateCrc()) {
+                const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize;
+                validData.append(reinterpret_cast<const char*>(_rtcmParser.message()), RTCMParser::kHeaderSize + _rtcmParser.messageLength());
+                validData.append(reinterpret_cast<const char*>(_rtcmParser.crcBytes()), RTCMParser::kCrcSize);
+                _validBytes += frameSize;
+                framesFound++;
+                qCDebug(RTCMUdpInputLog) << "RTCM message" << _rtcmParser.messageId() << frameSize << "bytes";
+            } else {
+                const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize;
+                qCWarning(RTCMUdpInputLog) << "Dropped RTCM message" << _rtcmParser.messageId() << "- CRC mismatch";
+                _invalidBytes += frameSize;
+                framesDropped++;
+            }
+
+            _rtcmParser.reset();
+        }
+
+        qCDebug(RTCMUdpInputLog) << "Datagram" << read << "bytes -"
+                                 << "framesFound:" << framesFound
+                                 << "framesDropped:" << framesDropped
+                                 << "validData:" << validData.size() << "bytes";
+
+        if (!validData.isEmpty()) {
+            emit rtcmDataReceived(validData);
+        }
+
+        const quint64 totalBytes = _validBytes + _invalidBytes;
+        if (totalBytes > 0) {
+            const double dropPct = 100.0 * _invalidBytes / totalBytes;
+            qCDebug(RTCMUdpInputLog) << QString("RTCM byte stats: %1 valid, %2 invalid, %3% dropped")
+                                            .arg(_validBytes).arg(_invalidBytes).arg(dropPct, 0, 'f', 1);
+        }
     }
 }

src/GPS/NTRIP/RTCMUdpInput.h

@@ -4,6 +4,7 @@
 #include <QtCore/QObject>
 #include <QtNetwork/QHostAddress>
 #include <QtNetwork/QUdpSocket>
+#include "RTCMParser.h"
 
 Q_DECLARE_LOGGING_CATEGORY(RTCMUdpInputLog)
 
@@ -41,12 +42,16 @@ class RTCMUdpInput : public QObject
     /// Unbind the socket and stop accepting datagrams.
     void stop();
 
-    bool   isRunning() const { return _running; }
+    bool isRunning() const { return _running; }
     quint16 port()     const { return _port; }
 
     /// Change the listen port. If already running, restarts automatically.
     void setPort(quint16 port);
 
+    /// Enable/disable validation of RTCM data.
+    /// With this enabled, only valid RTCM packets are converted to MAVLink.
+    void setValidation(const bool validate) { _validateRtcm = validate; }
+
 signals:
     /// Emitted once per received datagram with the raw RTCM payload.
     /// Connect directly to RTCMMavlink::RTCMDataUpdate (same thread).
@@ -60,6 +65,10 @@ private slots:
 
 private:
     QUdpSocket _socket;
-    quint16    _port;
-    bool       _running = false;
+    quint16 _port;
+    bool _running = false;
+    bool _validateRtcm = false;
+    RTCMParser _rtcmParser;
+    quint64 _validBytes = 0;
+    quint64 _invalidBytes = 0;
 };

src/Settings/NTRIP.SettingsGroup.json

@@ -108,6 +108,14 @@
             "max": 65535,
             "decimalPlaces": 0,
             "label": "UDP RTCM input port"
+        },
+        {
+            "name": "rtcmUdpValidate",
+            "shortDesc": "UDP RTCM enable validation",
+            "longDesc": "Enable validation of incoming data as RTCM and drop garbage (improves security).",
+            "type": "bool",
+            "default": true,
+            "label": "UDP RTCM enable validation"
         }
     ]
 }

src/Settings/NTRIPSettings.cc

@@ -15,3 +15,4 @@ DECLARE_SETTINGSFACT(NTRIPSettings, ntripUdpTargetAddress)
 DECLARE_SETTINGSFACT(NTRIPSettings, ntripUdpTargetPort)
 DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpInputEnabled)
 DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpInputPort)
+DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpValidate)

src/Settings/NTRIPSettings.h

@@ -24,4 +24,5 @@ class NTRIPSettings : public SettingsGroup
     DEFINE_SETTINGFACT(ntripUdpTargetPort)
     DEFINE_SETTINGFACT(rtcmUdpInputEnabled)
     DEFINE_SETTINGFACT(rtcmUdpInputPort)
+    DEFINE_SETTINGFACT(rtcmUdpValidate)
 };

test/GPS/RTCMParserTest.cc

@@ -322,4 +322,41 @@ void RTCMParserTest::_testParserCorruptedPreamble()
     QVERIFY(parser.validateCrc());
 }
 
+void RTCMParserTest::_testParserRecoveryAfterBadCrc()
+{
+    QByteArray msg1 = GpsTestHelpers::buildRtcmFrame(1005, 4);
+    QByteArray msg2 = GpsTestHelpers::buildRtcmFrame(1077, 8);
+    QByteArray msg3 = GpsTestHelpers::buildRtcmFrame(1087, 2);
+
+    // Corrupt the CRC of the middle frame
+    msg2[msg2.size() - 1] = static_cast<char>(msg2[msg2.size() - 1] ^ 0xFF);
+
+    QByteArray stream = msg1 + msg2 + msg3;
+
+    RTCMParser parser;
+    int validCount = 0;
+    int invalidCount = 0;
+    QVector<uint16_t> validIds;
+
+    for (int i = 0; i < stream.size(); i++) {
+        if (!parser.addByte(static_cast<uint8_t>(stream[i]))) {
+            continue;
+        }
+
+        if (parser.validateCrc()) {
+            validIds.append(parser.messageId());
+            validCount++;
+        } else {
+            invalidCount++;
+        }
+
+        parser.reset();
+    }
+
+    QCOMPARE(validCount, 2);
+    QCOMPARE(invalidCount, 1);
+    QCOMPARE(validIds[0], static_cast<uint16_t>(1005));
+    QCOMPARE(validIds[1], static_cast<uint16_t>(1087));
+}
+
 UT_REGISTER_TEST(RTCMParserTest, TestLabel::Unit)

test/GPS/RTCMParserTest.h

@@ -27,4 +27,5 @@ private slots:
     void _testParserMaxLength();
     void _testParserTruncatedFrame();
     void _testParserCorruptedPreamble();
+    void _testParserRecoveryAfterBadCrc();
 };