MAVLink signing (WIP)

libs/mavlink/include/mavlink/v2.0

@@ -1 +1 @@
-Subproject commit 706c8aabcf8a06fe61ae12c9f23a6848edd53579
+Subproject commit 1cc62dc73e8484e1c5d8554684ae393b7a1ff367

src/QGCApplication.cc

@@ -168,6 +168,7 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting)
     , _bluetoothAvailable(false)
 {
     _app = this;
+    memset(&m_mavlink_status, 0, sizeof(m_mavlink_status));
 
     // This prevents usage of QQuickWidget to fail since it doesn't support native widget siblings
 #ifndef __android__

src/Settings/App.SettingsGroup.json

@@ -162,5 +162,17 @@
     "shortDescription": "Default firmware type for flashing",
     "type":             "uint32",
     "defaultValue":     12
+},
+{
+    "name":             "Mavlink2Signing",
+    "shortDescription": "Use MAVLink 2.0 Signing",
+    "type":             "bool",
+    "defaultValue":     false
+},
+{
+    "name":             "Mavlink2SigningKey",
+    "shortDescription": "MAVLink 2.0 signing key",
+    "type":             "string",
+    "defaultValue":     ""
 }
 ]

src/Settings/AppSettings.cc

@@ -36,6 +36,8 @@ const char* AppSettings::autoLoadMissionsName =                         "AutoLoa
 const char* AppSettings::mapboxTokenName =                              "MapboxToken";
 const char* AppSettings::esriTokenName =                                "EsriToken";
 const char* AppSettings::defaultFirmwareTypeName =                      "DefaultFirmwareType";
+const char* AppSettings::mavlink2SigningName =                          "Mavlink2Signing";
+const char* AppSettings::mavlink2SigningKeyName =                       "Mavlink2SigningKey";
 
 const char* AppSettings::parameterFileExtension =   "params";
 const char* AppSettings::planFileExtension =        "plan";
@@ -54,26 +56,28 @@ const char* AppSettings::videoDirectory =           "Video";
 
 AppSettings::AppSettings(QObject* parent)
     : SettingsGroup(appSettingsGroupName, QString() /* root settings group */, parent)
-    , _offlineEditingFirmwareTypeFact(NULL)
+    , _offlineEditingFirmwareTypeFact       (NULL)
-    , _offlineEditingVehicleTypeFact(NULL)
+    , _offlineEditingVehicleTypeFact        (NULL)
-    , _offlineEditingCruiseSpeedFact(NULL)
+    , _offlineEditingCruiseSpeedFact        (NULL)
-    , _offlineEditingHoverSpeedFact(NULL)
+    , _offlineEditingHoverSpeedFact         (NULL)
-    , _offlineEditingAscentSpeedFact(NULL)
+    , _offlineEditingAscentSpeedFact        (NULL)
-    , _offlineEditingDescentSpeedFact(NULL)
+    , _offlineEditingDescentSpeedFact       (NULL)
-    , _batteryPercentRemainingAnnounceFact(NULL)
+    , _batteryPercentRemainingAnnounceFact  (NULL)
-    , _defaultMissionItemAltitudeFact(NULL)
+    , _defaultMissionItemAltitudeFact       (NULL)
-    , _telemetrySaveFact(NULL)
+    , _telemetrySaveFact                    (NULL)
-    , _telemetrySaveNotArmedFact(NULL)
+    , _telemetrySaveNotArmedFact            (NULL)
-    , _audioMutedFact(NULL)
+    , _audioMutedFact                       (NULL)
-    , _virtualJoystickFact(NULL)
+    , _virtualJoystickFact                  (NULL)
-    , _appFontPointSizeFact(NULL)
+    , _appFontPointSizeFact                 (NULL)
-    , _indoorPaletteFact(NULL)
+    , _indoorPaletteFact                    (NULL)
-    , _showLargeCompassFact(NULL)
+    , _showLargeCompassFact                 (NULL)
-    , _savePathFact(NULL)
+    , _savePathFact                         (NULL)
-    , _autoLoadMissionsFact(NULL)
+    , _autoLoadMissionsFact                 (NULL)
-    , _mapboxTokenFact(NULL)
+    , _mapboxTokenFact                      (NULL)
-    , _esriTokenFact(NULL)
+    , _esriTokenFact                        (NULL)
-    , _defaultFirmwareTypeFact(NULL)
+    , _defaultFirmwareTypeFact              (NULL)
+    , _mavlink2Signing                      (NULL)
+    , _mavlink2SigningKey                   (NULL)
 {
     QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
     qmlRegisterUncreatableType<AppSettings>("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only");
@@ -388,3 +392,21 @@ Fact* AppSettings::defaultFirmwareType(void)
 
     return _defaultFirmwareTypeFact;
 }
+
+Fact* AppSettings::mavlink2Signing(void)
+{
+    if (!_mavlink2Signing) {
+        _mavlink2Signing = _createSettingsFact(mavlink2SigningName);
+    }
+
+    return _mavlink2Signing;
+}
+
+Fact* AppSettings::mavlink2SigningKey(void)
+{
+    if (!_mavlink2SigningKey) {
+        _mavlink2SigningKey = _createSettingsFact(mavlink2SigningKeyName);
+    }
+
+    return _mavlink2SigningKey;
+}

src/Settings/AppSettings.h

@@ -40,6 +40,8 @@ public:
     Q_PROPERTY(Fact* mapboxToken                        READ mapboxToken                        CONSTANT)
     Q_PROPERTY(Fact* esriToken                          READ esriToken                          CONSTANT)
     Q_PROPERTY(Fact* defaultFirmwareType                READ defaultFirmwareType                CONSTANT)
+    Q_PROPERTY(Fact* mavlink2Signing                    READ mavlink2Signing                    CONSTANT)
+    Q_PROPERTY(Fact* mavlink2SigningKey                 READ mavlink2SigningKey                 CONSTANT)
 
     Q_PROPERTY(QString missionSavePath      READ missionSavePath    NOTIFY savePathsChanged)
     Q_PROPERTY(QString parameterSavePath    READ parameterSavePath  NOTIFY savePathsChanged)
@@ -74,6 +76,8 @@ public:
     Fact* mapboxToken                       (void);
     Fact* esriToken                         (void);
     Fact* defaultFirmwareType               (void);
+    Fact* mavlink2Signing                   (void);
+    Fact* mavlink2SigningKey                (void);
 
     QString missionSavePath     (void);
     QString parameterSavePath   (void);
@@ -106,6 +110,8 @@ public:
     static const char* mapboxTokenName;
     static const char* esriTokenName;
     static const char* defaultFirmwareTypeName;
+    static const char* mavlink2SigningName;
+    static const char* mavlink2SigningKeyName;
 
     // Application wide file extensions
     static const char* parameterFileExtension;
@@ -152,6 +158,8 @@ private:
     SettingsFact* _mapboxTokenFact;
     SettingsFact* _esriTokenFact;
     SettingsFact* _defaultFirmwareTypeFact;
+    SettingsFact* _mavlink2Signing;
+    SettingsFact* _mavlink2SigningKey;
 };
 
 #endif

src/comm/LinkInterface.cc

@@ -28,6 +28,7 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
     , _enableRateCollection(false)
     , _decodedFirstMavlinkPacket(false)
 {
+    memset(&signing, 0, sizeof(mavlink_signing_t));
     _config->setLink(this);
 
     // Initialize everything for the data rate calculation buffers.

src/comm/LinkInterface.h

@@ -124,6 +124,9 @@ public:
     bool connect(void);
     bool disconnect(void);
 
+    /// Signing structure for mavlink 2 signing
+    mavlink_signing_t signing;
+
 public slots:
 
     /**

src/comm/MAVLinkProtocol.cc

@@ -26,6 +26,7 @@
 #include <QMetaType>
 #include <QDir>
 #include <QFileInfo>
+#include <QCryptographicHash>
 
 #include "MAVLinkProtocol.h"
 #include "UASInterface.h"
@@ -46,6 +47,16 @@ QGC_LOGGING_CATEGORY(MAVLinkProtocolLog, "MAVLinkProtocolLog")
 const char* MAVLinkProtocol::_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file
 const char* MAVLinkProtocol::_logFileExtension = "mavlink";             ///< Extension for log files
 
+static mavlink_signing_streams_t signing_streams;
+
+extern "C" {
+static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t msgId)
+{
+    Q_UNUSED(status);
+    return msgId == MAVLINK_MSG_ID_RADIO_STATUS;
+}
+}
+
 /**
  * The default constructor will create a new MAVLink object sending heartbeats at
  * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
@@ -67,6 +78,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox)
     memset(&totalErrorCounter, 0, sizeof(totalErrorCounter));
     memset(&currReceiveCounter, 0, sizeof(currReceiveCounter));
     memset(&currLossCounter, 0, sizeof(currLossCounter));
+    memset(&signing_streams, 0, sizeof(signing_streams));
 }
 
 MAVLinkProtocol::~MAVLinkProtocol()
@@ -81,6 +93,7 @@ void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox)
 
    _linkMgr =               _toolbox->linkManager();
    _multiVehicleManager =   _toolbox->multiVehicleManager();
+   _appSettings =           _toolbox->settingsManager()->appSettings();
 
    qRegisterMetaType<mavlink_message_t>("mavlink_message_t");
 
@@ -161,16 +174,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 
 //    receiveMutex.lock();
     mavlink_message_t message;
-    mavlink_status_t status;
 
     int mavlinkChannel = link->mavlinkChannel();
+    // the channel mavlink status is needed in other to be able to parse the signed packages
+    mavlink_status_t* mavlinkStatus =  mavlink_get_channel_status(mavlinkChannel);
 
     static int nonmavlinkCount = 0;
     static bool checkedUserNonMavlink = false;
     static bool warnedUserNonMavlink = false;
 
     for (int position = 0; position < b.size(); position++) {
-        unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
+        unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, mavlinkStatus);
 
         if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
         {
@@ -194,10 +208,48 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         if (decodeState == 1)
         {
             if (!link->decodedFirstMavlinkPacket()) {
-                mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
                 if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
                     qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
                     mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+                    // Set all links to v2
+                    // setVersion needs to be re-instated on merge
+                    // setVersion(200);
+                    // Setup mavlink 2 signing
+                    //-- Check if signing is enabled
+                    if (_appSettings->mavlink2Signing()->rawValue().toBool()) {
+                        mavlink_setup_signing_t setupSigning;
+                        //-- Get saved signature seed
+                        QString key = _appSettings->mavlink2SigningKey()->rawValue().toString();
+                        if(key.isEmpty()) {
+                            qWarning() << "MAVLink signing key is empty";
+                        }
+                        setupSigning.target_system = message.sysid;
+                        setupSigning.target_component = message.compid;
+                        setupSigning.initial_timestamp = QDateTime(QDate(2015, 1, 1)).msecsTo(QDateTime::currentDateTimeUtc()) * 100;
+                        memcpy(setupSigning.secret_key, QCryptographicHash::hash(key.toUtf8(), QCryptographicHash::Sha256).data(), 32);
+
+                        mavlink_message_t msg;
+                        mavlink_msg_setup_signing_encode(
+                                    getSystemId(),
+                                    getComponentId(),
+                                    &msg,
+                                    &setupSigning);
+                        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+                        int len = mavlink_msg_to_send_buffer(buffer, &msg);
+                        // Send twice in case of lossy connection
+                        link->writeBytesSafe((const char*)buffer, len);
+                        link->writeBytesSafe((const char*)buffer, len);
+
+                        mavlink_signing_t& signing = link->signing;
+                        memcpy(signing.secret_key, setupSigning.secret_key, 32);
+                        signing.link_id = (uint8_t)mavlinkChannel;
+                        signing.timestamp = setupSigning.initial_timestamp;
+                        signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING;
+                        signing.accept_unsigned_callback = accept_unsigned_callback;
+
+                        mavlinkStatus->signing = &signing;
+                        mavlinkStatus->signing_streams = &signing_streams;
+                    }
                 }
                 link->setDecodedFirstMavlinkPacket(true);
             }
@@ -362,7 +414,7 @@ void MAVLinkProtocol::_startLogging(void)
     }
 #ifdef __mobile__
     //-- Mobile build don't write to /tmp unless told to do so
-    if (!_app->toolbox()->settingsManager()->appSettings()->telemetrySave()->rawValue().toBool()) {
+    if (!_appSettings->telemetrySave()->rawValue().toBool()) {
         return;
     }
 #endif
@@ -390,8 +442,8 @@ void MAVLinkProtocol::_stopLogging(void)
 {
     if (_tempLogFile.isOpen()) {
         if (_closeLogFile()) {
-            if ((_vehicleWasArmed || _app->toolbox()->settingsManager()->appSettings()->telemetrySaveNotArmed()->rawValue().toBool()) &&
+            if ((_vehicleWasArmed || _appSettings->telemetrySaveNotArmed()->rawValue().toBool()) &&
-                _app->toolbox()->settingsManager()->appSettings()->telemetrySave()->rawValue().toBool()) {
+                _appSettings->telemetrySave()->rawValue().toBool()) {
                 emit saveTelemetryLog(_tempLogFile.fileName());
             } else {
                 QFile::remove(_tempLogFile.fileName());
@@ -439,4 +491,3 @@ void MAVLinkProtocol::deleteTempLogFiles(void)
         QFile::remove(fileInfo.filePath());
     }
 }
-

src/comm/MAVLinkProtocol.h

@@ -28,6 +28,7 @@
 class LinkManager;
 class MultiVehicleManager;
 class QGCApplication;
+class AppSettings;
 
 Q_DECLARE_LOGGING_CATEGORY(MAVLinkProtocolLog)
 
@@ -180,6 +181,7 @@ private:
 
     LinkManager*            _linkMgr;
     MultiVehicleManager*    _multiVehicleManager;
+    AppSettings*            _appSettings;
 };
 
 #endif // MAVLINKPROTOCOL_H_

src/comm/MockLink.cc

@@ -860,7 +860,10 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
                                       &commandAck,
                                       request.command,
                                       commandResult,
-                                      0);
+                                      0,
+                                      0,    // int32_t result_param2
+                                      0,    // uint8_t target_system
+                                      0);   // uint8_t target_component
     respondWithMavlinkMessage(commandAck);
 }
 

src/ui/preferences/MavlinkSettings.qml

@@ -14,11 +14,12 @@ import QtQuick.Controls.Styles  1.4
 import QtQuick.Dialogs          1.2
 
 import QGroundControl                       1.0
-import QGroundControl.FactSystem            1.0
 import QGroundControl.Controls              1.0
-import QGroundControl.ScreenTools           1.0
+import QGroundControl.FactControls          1.0
+import QGroundControl.FactSystem            1.0
 import QGroundControl.MultiVehicleManager   1.0
 import QGroundControl.Palette               1.0
+import QGroundControl.ScreenTools           1.0
 
 Rectangle {
     id:             __mavlinkRoot
@@ -149,6 +150,28 @@ Rectangle {
                             QGroundControl.isVersionCheckEnabled = checked
                         }
                     }
+                    //-----------------------------------------------------------------
+                    //-- Mavlink 2 signing key
+                    Row {
+                        spacing:  ScreenTools.defaultFontPixelWidth
+                        FactCheckBox {
+                            id:                 signingKeyCheckBox
+                            fact:               _mavlink2Signing
+                            text:               qsTr("MAVLink 2 Signing Key:")
+                            width:              _labelWidth
+                            visible:            _mavlink2Signing.visible
+                            anchors.verticalCenter: parent.verticalCenter
+                            property Fact _mavlink2Signing: QGroundControl.settingsManager.appSettings.mavlink2Signing
+                        }
+                        FactTextField {
+                            id:                 signingKeyField
+                            fact:               _mavlink2SigningKey
+                            enabled:            signingKeyCheckBox.checked
+                            visible:            _mavlink2SigningKey.visible
+                            anchors.verticalCenter: parent.verticalCenter
+                            property Fact _mavlink2SigningKey: QGroundControl.settingsManager.appSettings.mavlink2SigningKey
+                        }
+                    }
                 }
             }
             //-----------------------------------------------------------------