From d7c2034b53c6ac79c163fe14b92b6cecc4a0a0cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 27 Dec 2020 19:33:49 +0100 Subject: [PATCH] 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::List, void, void (GeoFenceController::*)(Vehicle*)>::call(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:152) ==690288== by 0x5B7797: void QtPrivate::FunctionPointer::call, void>(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:185) ==690288== by 0x5B7325: QtPrivate::QSlotObject, 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::List, void, void (Vehicle::*)(LinkInterface*, __mavlink_message)>::call(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:152) ==690288== by 0x725990: void QtPrivate::FunctionPointer::call, void>(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:185) ==690288== by 0x723B4B: QtPrivate::QSlotObject, 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::List, void, void (MAVLinkProtocol::*)(LinkInterface*, QByteArray)>::call(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:152) ==690288== by 0x74BAB4: void QtPrivate::FunctionPointer::call, void>(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:185) ==690288== by 0x74B3B7: QtPrivate::QSlotObject, 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 | 1 + src/TerrainTile.cc | 4 ++-- src/Vehicle/FTPManager.cc | 10 +++++----- src/Vehicle/FTPManager.h | 2 +- src/Vehicle/Vehicle.cc | 4 +--- src/Vehicle/Vehicle.h | 2 +- src/comm/BluetoothLink.h | 7 ++----- src/comm/LinkManager.cc | 41 +++++++++++++++++++++-------------------- src/comm/LinkManager.h | 2 +- src/comm/LogReplayLink.h | 7 ++----- src/comm/MAVLinkProtocol.cc | 10 +++++++++- src/comm/MockLink.h | 2 +- src/comm/SerialLink.h | 9 +++------ src/comm/TCPLink.h | 8 ++------ src/comm/UDPLink.h | 8 ++------ 15 files changed, 54 insertions(+), 63 deletions(-) diff --git a/src/FactSystem/FactGroup.h b/src/FactSystem/FactGroup.h index 114fb44..c6ab817 100644 --- a/src/FactSystem/FactGroup.h +++ b/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& factGroups() const { return _nameToFactGroupMap; } /// Allows a FactGroup to parse incoming messages and fill in values virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message); diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index bc2503c..654847f 100644 --- a/src/TerrainTile.cc +++ b/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; } } diff --git a/src/Vehicle/FTPManager.cc b/src/Vehicle/FTPManager.cc index f2359b8..c98e8bc 100644 --- a/src/Vehicle/FTPManager.cc +++ b/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); diff --git a/src/Vehicle/FTPManager.h b/src/Vehicle/FTPManager.h index ad48fd0..00ee97c 100644 --- a/src/Vehicle/FTPManager.h +++ b/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 _rgStateMachine; DownloadState_t _downloadState; QTimer _ackOrNakTimeoutTimer; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6219233..66bf6e8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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; ihandleMessage(this, message); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index b970f7f..fedb6da 100644 --- a/src/Vehicle/Vehicle.h +++ b/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; diff --git a/src/comm/BluetoothLink.h b/src/comm/BluetoothLink.h index 114e1d2..3d54685 100644 --- a/src/comm/BluetoothLink.h +++ b/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; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 87a5907..2f7b045 100644 --- a/src/comm/LinkManager.cc +++ b/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(config, isPX4Flow); break; #else Q_UNUSED(isPX4Flow) #endif case LinkConfiguration::TypeUdp: - link = new UDPLink(config); + link = std::make_shared(config); break; case LinkConfiguration::TypeTcp: - link = new TCPLink(config); + link = std::make_shared(config); break; #ifdef QGC_ENABLE_BLUETOOTH case LinkConfiguration::TypeBluetooth: - link = new BluetoothLink(config); + link = std::make_shared(config); break; #endif case LinkConfiguration::TypeLogReplay: - link = new LogReplayLink(config); + link = std::make_shared(config); break; #ifdef QT_DEBUG case LinkConfiguration::TypeMock: - link = new MockLink(config); + link = std::make_shared(config); break; #endif case LinkConfiguration::TypeLast: break; } - QScopedPointer 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); diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index e8df136..0416a26 100644 --- a/src/comm/LinkManager.h +++ b/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); diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index f5b2a60..ae6bae3 100644 --- a/src/comm/LogReplayLink.h +++ b/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; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index ecb33ba..02f104e 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/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)); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 7d778a9..cc19bc0 100644 --- a/src/comm/MockLink.h +++ b/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; } diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index d5d5e74..8c34fe0 100644 --- a/src/comm/SerialLink.h +++ b/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; diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index 93544ee..0b618b9 100644 --- a/src/comm/TCPLink.h +++ b/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; diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 6995a99..9ad57c2 100644 --- a/src/comm/UDPLink.h +++ b/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;