Fix Segfault on vehicle reboot (#9267)

* LinkManager::createConnectedLink: use make_shared to avoid separate allocation

Also avoids some memory leaks when returning early.

* fix LinkManager: set setAutoConnect()

* fix MAVLinkProtocol: use a weak_ptr to check if the link stays valid

It is not enough to check if the link is valid only on entering the method.

If message handling called VehicleLinkManager::closeVehicle(), the link
was removed, and possibly deleted. If more mavlink messages were in the
same buffer, the link was accessed again, triggering a segfault.

This happened for example when rebooting the vehicle from parameter editor.

* Vehicle: fix uninitialized variable _capabilityBits

From valgrind:
==690288== Conditional jump or move depends on uninitialised value(s)
==690288==    at 0x5B5F18: GeoFenceController::supported() const (GeoFenceController.cc:495)
==690288==    by 0x5B309E: GeoFenceController::_managerVehicleChanged(Vehicle*) (GeoFenceController.cc:125)
==690288==    by 0x5B79AF: QtPrivate::FunctorCall<QtPrivate::IndexesList<0>, QtPrivate::List<Vehicle*>, void, void (GeoFenceController::*)(Vehicle*)>::call(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:152)
==690288==    by 0x5B7797: void QtPrivate::FunctionPointer<void (GeoFenceController::*)(Vehicle*)>::call<QtPrivate::List<Vehicle*>, void>(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:185)
==690288==    by 0x5B7325: QtPrivate::QSlotObject<void (GeoFenceController::*)(Vehicle*), QtPrivate::List<Vehicle*>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288==    by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)
==690288==    by 0x913C9C: PlanMasterController::managerVehicleChanged(Vehicle*) (moc_PlanMasterController.cpp:544)
==690288==    by 0x5FF00B: PlanMasterController::_activeVehicleChanged(Vehicle*) (PlanMasterController.cc:160)
==690288==    by 0x5FE9A4: PlanMasterController::startStaticActiveVehicle(Vehicle*, bool) (PlanMasterController.cc:116)
==690288==    by 0x912EB9: PlanMasterController::qt_static_metacall(QObject*, QMetaObject::Call, int, void**) (moc_PlanMasterController.cpp:267)
==690288==    by 0x913A2A: PlanMasterController::qt_metacall(QMetaObject::Call, int, void**) (moc_PlanMasterController.cpp:473)
==690288==    by 0x880E5C8: QQmlObjectOrGadget::metacall(QMetaObject::Call, int, void**) const (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Qml.so.5.12.5)

* FTPManager: fix uninitialized variable & message filter logic

From Valgrind:
==690288== Conditional jump or move depends on uninitialised value(s)
==690288==    at 0x6E74E6: FTPManager::_mavlinkMessageReceived(__mavlink_message const&) (FTPManager.cc:108)
==690288==    by 0x70D164: Vehicle::_mavlinkMessageReceived(LinkInterface*, __mavlink_message) (Vehicle.cc:603)
==690288==    by 0x726F53: QtPrivate::FunctorCall<QtPrivate::IndexesList<0, 1>, QtPrivate::List<LinkInterface*, __mavlink_message>, void, void (Vehicle::*)(LinkInterface*, __mavlink_message)>::call(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:152)
==690288==    by 0x725990: void QtPrivate::FunctionPointer<void (Vehicle::*)(LinkInterface*, __mavlink_message)>::call<QtPrivate::List<LinkInterface*, __mavlink_message>, void>(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:185)
==690288==    by 0x723B4B: QtPrivate::QSlotObject<void (Vehicle::*)(LinkInterface*, __mavlink_message), QtPrivate::List<LinkInterface*, __mavlink_message>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288==    by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)
==690288==    by 0x95DE46: MAVLinkProtocol::messageReceived(LinkInterface*, __mavlink_message) (moc_MAVLinkProtocol.cpp:346)
==690288==    by 0x757522: MAVLinkProtocol::receiveBytes(LinkInterface*, QByteArray) (MAVLinkProtocol.cc:362)
==690288==    by 0x74C71E: QtPrivate::FunctorCall<QtPrivate::IndexesList<0, 1>, QtPrivate::List<LinkInterface*, QByteArray>, void, void (MAVLinkProtocol::*)(LinkInterface*, QByteArray)>::call(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:152)
==690288==    by 0x74BAB4: void QtPrivate::FunctionPointer<void (MAVLinkProtocol::*)(LinkInterface*, QByteArray)>::call<QtPrivate::List<LinkInterface*, QByteArray>, void>(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:185)
==690288==    by 0x74B3B7: QtPrivate::QSlotObject<void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), QtPrivate::List<LinkInterface*, QByteArray>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288==    by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)

* FTPManager: fix uninitialized Request

- Parts of the header were uninit
- zero init the payload allows for mavlink trimming

* Fix ~TerrainTile: use delete[] for arrays

* FactGroup: fix expensive & unnecessary object creation and map lookups

Every mavlink message reception was leading to creation of a list and
key lookup for each of the keys. This can easily be avoided and results
in noticeable reduction of CPU load (I'm seeing overall around 4% with
a pixhawk on USB).

The same pattern exists elsewhere as well

src/FactSystem/FactGroup.h

@@ -49,6 +49,7 @@ public:
     QStringList factNames           (void) const { return _factNames; }
     QStringList factGroupNames      (void) const { return _nameToFactGroupMap.keys(); }
     bool        telemetryAvailable  (void) const { return _telemetryAvailable; }
+    const QMap<QString, FactGroup*>& factGroups() const { return _nameToFactGroupMap; }
 
     /// Allows a FactGroup to parse incoming messages and fill in values
     virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message);

src/TerrainTile.cc

@@ -47,9 +47,9 @@ TerrainTile::~TerrainTile()
 {
     if (_data) {
         for (int i = 0; i < _gridSizeLat; i++) {
-            delete _data[i];
+            delete[] _data[i];
         }
-        delete _data;
+        delete[] _data;
         _data = nullptr;
     }
 }

src/Vehicle/FTPManager.cc

@@ -105,7 +105,7 @@ void FTPManager::_downloadComplete(const QString& errorMsg)
 
 void FTPManager::_mavlinkMessageReceived(const mavlink_message_t& message)
 {
-    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL && message.compid != _ftpCompId) {
+    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL || message.compid != _ftpCompId) {
         return;
     }
 
@@ -182,7 +182,7 @@ QString FTPManager::_errorMsgFromNak(const MavlinkFTP::Request* nak)
 
 void FTPManager::_openFileROBegin(void)
 {
-    MavlinkFTP::Request request;
+    MavlinkFTP::Request request{};
     request.hdr.session = 0;
     request.hdr.opcode  = MavlinkFTP::kCmdOpenFileRO;
     request.hdr.offset  = 0;
@@ -241,7 +241,7 @@ void FTPManager::_burstReadFileWorker(bool firstRequest)
 {
     qCDebug(FTPManagerLog) << "_burstReadFileWorker: starting burst at offset:firstRequest:retryCount" << _downloadState.expectedOffset << firstRequest << _downloadState.retryCount;
 
-    MavlinkFTP::Request request;
+    MavlinkFTP::Request request{};
     request.hdr.session = _downloadState.sessionId;
     request.hdr.opcode  = MavlinkFTP::kCmdBurstReadFile;
     request.hdr.offset  = _downloadState.expectedOffset;
@@ -357,7 +357,7 @@ void FTPManager::_burstReadFileTimeout(void)
 void FTPManager::_fillMissingBlocksWorker(bool firstRequest)
 {
     if (_downloadState.rgMissingData.count()) {
-        MavlinkFTP::Request request;
+        MavlinkFTP::Request request{};
         MissingData_t&      missingData = _downloadState.rgMissingData.first();
 
         uint32_t cBytesToRead = qMin((uint32_t)sizeof(request.data), missingData.cBytesMissing);
@@ -483,7 +483,7 @@ void FTPManager::_fillMissingBlocksTimeout(void)
 
 void FTPManager::_resetSessionsBegin(void)
 {
-    MavlinkFTP::Request request;
+    MavlinkFTP::Request request{};
     request.hdr.opcode  = MavlinkFTP::kCmdResetSessions;
     request.hdr.size    = 0;
     _sendRequestExpectAck(&request);

src/Vehicle/FTPManager.h

@@ -126,7 +126,7 @@ private:
     bool    _parseURI                   (const QString& uri, QString& parsedURI, uint8_t& compId);
 
     Vehicle*                _vehicle;
-    uint8_t                 _ftpCompId;
+    uint8_t                 _ftpCompId = MAV_COMP_ID_AUTOPILOT1;
     QList<StateFunctions_t> _rgStateMachine;
     DownloadState_t         _downloadState;
     QTimer                  _ackOrNakTimeoutTimer;

src/Vehicle/Vehicle.cc

@@ -611,9 +611,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     VehicleBatteryFactGroup::handleMessageForFactGroupCreation(this, message);
 
     // Let the fact groups take a whack at the mavlink traffic
-    QStringList groupNames = factGroupNames();
-    for (int i=0; i<groupNames.count(); i++) {
-        FactGroup* factGroup = getFactGroup(groupNames[i]);
+    for (FactGroup* factGroup : factGroups()) {
         factGroup->handleMessage(this, message);
     }
 

src/Vehicle/Vehicle.h

@@ -1008,7 +1008,7 @@ private:
     unsigned        _mavlinkProtocolRequestMaxProtoVersion  = 0;
     unsigned        _maxProtoVersion                        = 0;
     bool            _capabilityBitsKnown                    = false;
-    uint64_t        _capabilityBits;
+    uint64_t        _capabilityBits                         = 0;
     bool            _receivingAttitudeQuaternion = false;
     CheckList       _checkListState                         = CheckListNotSetup;
     bool            _readyToFlyAvailable                    = false;

src/comm/BluetoothLink.h

@@ -119,11 +119,9 @@ class BluetoothLink : public LinkInterface
 {
     Q_OBJECT
 
-    friend class BluetoothConfiguration;
-    friend class LinkManager;
-
 public:
-    ~BluetoothLink();
+    BluetoothLink(SharedLinkConfigurationPtr& config);
+    virtual ~BluetoothLink();
 
     // Overrides from QThread
     void run(void) override;
@@ -147,7 +145,6 @@ private slots:
     void _writeBytes(const QByteArray bytes) override;
 
 private:
-    BluetoothLink(SharedLinkConfigurationPtr& config);
 
     // LinkInterface overrides
     bool _connect(void) override;

src/comm/LinkManager.cc

@@ -103,41 +103,39 @@ void LinkManager::createConnectedLink(LinkConfiguration* config)
 
 bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr& config, bool isPX4Flow)
 {
-    LinkInterface* link = nullptr;
+    SharedLinkInterfacePtr link = nullptr;
 
     switch(config->type()) {
 #ifndef NO_SERIAL_LINK
     case LinkConfiguration::TypeSerial:
-        link = new SerialLink(config, isPX4Flow);
+        link = std::make_shared<SerialLink>(config, isPX4Flow);
         break;
 #else
     Q_UNUSED(isPX4Flow)
 #endif
     case LinkConfiguration::TypeUdp:
-        link = new UDPLink(config);
+        link = std::make_shared<UDPLink>(config);
         break;
     case LinkConfiguration::TypeTcp:
-        link = new TCPLink(config);
+        link = std::make_shared<TCPLink>(config);
         break;
 #ifdef QGC_ENABLE_BLUETOOTH
     case LinkConfiguration::TypeBluetooth:
-        link = new BluetoothLink(config);
+        link = std::make_shared<BluetoothLink>(config);
         break;
 #endif
     case LinkConfiguration::TypeLogReplay:
-        link = new LogReplayLink(config);
+        link = std::make_shared<LogReplayLink>(config);
         break;
 #ifdef QT_DEBUG
     case LinkConfiguration::TypeMock:
-        link = new MockLink(config);
+        link = std::make_shared<MockLink>(config);
         break;
 #endif
     case LinkConfiguration::TypeLast:
         break;
     }
 
-    QScopedPointer<LinkInterface> scopedLink(link);
-
     if (link) {
 
         int mavlinkChannel = _reserveMavlinkChannel();
@@ -148,24 +146,25 @@ bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr& config, bool i
             return false;
         }
 
-        SharedLinkInterfacePtr sharedLink(link);
-        _rgLinks.append(sharedLink);
-        config->setLink(sharedLink);
+        _rgLinks.append(link);
+        config->setLink(link);
 
-        connect(link, &LinkInterface::communicationError,  _app,                &QGCApplication::criticalMessageBoxOnMainThread);
-        connect(link, &LinkInterface::bytesReceived,       _mavlinkProtocol,    &MAVLinkProtocol::receiveBytes);
-        connect(link, &LinkInterface::bytesSent,           _mavlinkProtocol,    &MAVLinkProtocol::logSentBytes);
-        connect(link, &LinkInterface::disconnected,        this,                &LinkManager::_linkDisconnected);
+        connect(link.get(), &LinkInterface::communicationError,  _app,                &QGCApplication::criticalMessageBoxOnMainThread);
+        connect(link.get(), &LinkInterface::bytesReceived,       _mavlinkProtocol,    &MAVLinkProtocol::receiveBytes);
+        connect(link.get(), &LinkInterface::bytesSent,           _mavlinkProtocol,    &MAVLinkProtocol::logSentBytes);
+        connect(link.get(), &LinkInterface::disconnected,        this,                &LinkManager::_linkDisconnected);
 
-        _mavlinkProtocol->resetMetadataForLink(link);
+        _mavlinkProtocol->resetMetadataForLink(link.get());
         _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion());
 
         if (!link->_connect()) {
             return false;
         }
+
+        return true;
     }
 
-    return scopedLink.take() ? true : false;
+    return false;
 }
 
 SharedLinkInterfacePtr LinkManager::mavlinkForwardingLink()
@@ -213,7 +212,7 @@ void LinkManager::_linkDisconnected(void)
     }
 }
 
-SharedLinkInterfacePtr LinkManager::sharedLinkInterfacePointerForLink(LinkInterface* link)
+SharedLinkInterfacePtr LinkManager::sharedLinkInterfacePointerForLink(LinkInterface* link, bool ignoreNull)
 {
     for (int i=0; i<_rgLinks.count(); i++) {
         if (_rgLinks[i].get() == link) {
@@ -221,7 +220,8 @@ SharedLinkInterfacePtr LinkManager::sharedLinkInterfacePointerForLink(LinkInterf
         }
     }
 
-    qWarning() << "LinkManager::sharedLinkInterfaceForLink returning nullptr";
+    if (!ignoreNull)
+        qWarning() << "LinkManager::sharedLinkInterfaceForLink returning nullptr";
     return SharedLinkInterfacePtr(nullptr);
 }
 
@@ -568,6 +568,7 @@ void LinkManager::_updateAutoConnectLinks(void)
                         pSerialConfig->setBaud      (boardType == QGCSerialPortInfo::BoardTypeSiKRadio ? 57600 : 115200);
                         pSerialConfig->setDynamic   (true);
                         pSerialConfig->setPortName  (portInfo.systemLocation());
+                        pSerialConfig->setAutoConnect(true);
 
                         SharedLinkConfigurationPtr  sharedConfig(pSerialConfig);
                         createConnectedLink(sharedConfig, boardType == QGCSerialPortInfo::BoardTypePX4Flow);

src/comm/LinkManager.h

@@ -117,7 +117,7 @@ public:
 
     /// If you are going to hold a reference to a LinkInterface* in your object you must reference count it
     /// by using this method to get access to the shared pointer.
-    SharedLinkInterfacePtr sharedLinkInterfacePointerForLink(LinkInterface* link);
+    SharedLinkInterfacePtr sharedLinkInterfacePointerForLink(LinkInterface* link, bool ignoreNull=false);
 
     bool containsLink(LinkInterface* link);
 

src/comm/LogReplayLink.h

@@ -52,10 +52,9 @@ class LogReplayLink : public LinkInterface
 {
     Q_OBJECT
 
-    friend class LinkManager;
-
 public:
-    ~LogReplayLink();
+    LogReplayLink(SharedLinkConfigurationPtr& config);
+    virtual ~LogReplayLink();
 
     /// @return true: log is currently playing, false: log playback is paused
     bool isPlaying(void) { return _readTickTimer.isActive(); }
@@ -96,8 +95,6 @@ private slots:
     void _setPlaybackSpeed  (qreal playbackSpeed);
 
 private:
-    // Links are only created/destroyed by LinkManager so constructor/destructor is not public
-    LogReplayLink(SharedLinkConfigurationPtr& config);
 
     // LinkInterface overrides
     bool _connect(void) override;

src/comm/MAVLinkProtocol.cc

@@ -197,7 +197,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
     // Since receiveBytes signals cross threads we can end up with signals in the queue
     // that come through after the link is disconnected. For these we just drop the data
     // since the link is closed.
-    if (!_linkMgr->containsLink(link)) {
+    WeakLinkInterfacePtr linkPtr = _linkMgr->sharedLinkInterfacePointerForLink(link, true);
+    if (linkPtr.expired()) {
         return;
     }
 
@@ -359,6 +360,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             // kind of inefficient, but no issue for a groundstation pc.
             // It buys as reentrancy for the whole code over all threads
             emit messageReceived(link, _message);
+
+            // Anyone handling the message could close the connection, which deletes the link,
+            // so we check if it's expired
+            if (linkPtr.expired()) {
+                break;
+            }
+
             // Reset message parsing
             memset(&_status,  0, sizeof(_status));
             memset(&_message, 0, sizeof(_message));

src/comm/MockLink.h

@@ -95,7 +95,7 @@ class MockLink : public LinkInterface
 
 public:
     MockLink(SharedLinkConfigurationPtr& config);
-    ~MockLink(void);
+    virtual ~MockLink();
 
     int             vehicleId           (void)                                          { return _vehicleSystemId; }
     MAV_AUTOPILOT   getFirmwareType     (void)                                          { return _firmwareType; }

src/comm/SerialLink.h

@@ -109,10 +109,10 @@ class SerialLink : public LinkInterface
 {
     Q_OBJECT
 
-    friend class SerialConfiguration;
-    friend class LinkManager;
-
 public:
+    SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow = false);
+    virtual ~SerialLink();
+
     // LinkInterface overrides
     bool isConnected(void) const override;
     void disconnect (void) override;
@@ -130,9 +130,6 @@ private slots:
     void _readBytes     (void);
 
 private:
-    // Links are only created/destroyed by LinkManager so constructor/destructor is not public
-    SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow = false);
-    ~SerialLink();
 
     // LinkInterface overrides
     bool _connect(void) override;

src/comm/TCPLink.h

@@ -70,12 +70,9 @@ class TCPLink : public LinkInterface
 {
     Q_OBJECT
 
-    friend class TCPLinkTest;
-    friend class TCPConfiguration;
-    friend class LinkManager;
-
 public:
-    ~TCPLink();
+    TCPLink(SharedLinkConfigurationPtr& config);
+    virtual ~TCPLink();
 
     QTcpSocket* getSocket           (void) { return _socket; }
     void        signalBytesWritten  (void);
@@ -101,7 +98,6 @@ private slots:
     void _writeBytes(const QByteArray data) override;
 
 private:
-    TCPLink(SharedLinkConfigurationPtr& config);
 
     // LinkInterface overrides
     bool _connect(void) override;

src/comm/UDPLink.h

@@ -96,11 +96,9 @@ class UDPLink : public LinkInterface
 {
     Q_OBJECT
 
-    friend class UDPConfiguration;
-    friend class LinkManager;
-
 public:
-    ~UDPLink();
+    UDPLink(SharedLinkConfigurationPtr& config);
+    virtual ~UDPLink();
 
     // LinkInterface overrides
     bool isConnected(void) const override;
@@ -117,8 +115,6 @@ private slots:
     void _writeBytes(const QByteArray data) override;
 
 private:
-    // Links are only created/destroyed by LinkManager so constructor/destructor is not public
-    UDPLink(SharedLinkConfigurationPtr& config);
 
     // LinkInterface overrides
     bool _connect(void) override;