Bring in UDP fixes from master

src/comm/UDPLink.cc

@@ -68,6 +68,37 @@ static QString get_ip_address(const QString& address)
     return QString("");
 }
 
+static bool is_ip_local(const QHostAddress& add)
+{
+    // In simulation and testing setups the vehicle and the GCS can be
+    // running on the same host. This leads to packets arriving through
+    // the local network or the loopback adapter, which makes it look
+    // like the vehicle is connected through two different links,
+    // complicating routing.
+    //
+    // We detect this case and force all traffic to a simulated instance
+    // onto the local loopback interface.
+    // Run through all IPv4 interfaces and check if their canonical
+    // IP address in string representation matches the source IP address
+    foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) {
+        if (address == add) {
+            // This is a local address of the same host
+            return true;
+        }
+    }
+    return false;
+}
+
+static bool contains_target(const QList<UDPCLient*> list, const QHostAddress& address, quint16 port)
+{
+    foreach(UDPCLient* target, list) {
+        if(target->address == address && target->port == port) {
+            return true;
+        }
+    }
+    return false;
+}
+
 UDPLink::UDPLink(SharedLinkConfigurationPointer& config)
     : LinkInterface(config)
     #if defined(QGC_ZEROCONF_ENABLED)
@@ -89,6 +120,9 @@ UDPLink::~UDPLink()
     _disconnect();
     // Tell the thread to exit
     _running = false;
+    // Clear client list
+    qDeleteAll(_sessionTargets);
+    _sessionTargets.clear();
     quit();
     // Wait for it to exit
     wait();
@@ -124,37 +158,27 @@ QString UDPLink::getName() const
     return _udpConfig->name();
 }
 
-void UDPLink::addHost(const QString& host)
-{
-    _udpConfig->addHost(host);
-}
-
-void UDPLink::removeHost(const QString& host)
-{
-    _udpConfig->removeHost(host);
-}
-
 void UDPLink::_writeBytes(const QByteArray data)
 {
     if (!_socket)
         return;
-
-    QStringList goneHosts;
+    // Send to all manually targeted systems
+    foreach(UDPCLient* target, _udpConfig->targetHosts()) {
+        // Skip it if it's part of the session clients below
+        if(!contains_target(_sessionTargets, target->address, target->port)) {
+            _writeDataGram(data, target);
+        }
+    }
     // Send to all connected systems
-    QString host;
-    int port;
-    if(_udpConfig->firstHost(host, port)) {
-        do {
-            QHostAddress currentHost(host);
-            if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) {
-                // This host is gone. Add to list to be removed
-                // We should keep track of hosts that were manually added (static) and
-                // hosts that were added because we heard from them (dynamic). Only
-                // dynamic hosts should be removed and even then, after a few tries, not
-                // the first failure. In the mean time, we don't remove anything.
-                if(REMOVE_GONE_HOSTS) {
-                    goneHosts.append(host);
+    foreach(UDPCLient* target, _sessionTargets) {
+        _writeDataGram(data, target);
     }
+}
+
+void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target)
+{
+    if(_socket->writeDatagram(data, target->address, target->port) < 0) {
+        qWarning() << "Error writing to" << target->address << target->port;
     } else {
         // Only log rate if data actually got sent. Not sure about this as
         // "host not there" takes time too regardless of size of data. In fact,
@@ -162,12 +186,6 @@ void UDPLink::_writeBytes(const QByteArray data)
         // unit sent by UDP.
         _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
     }
-        } while (_udpConfig->nextHost(host, port));
-        //-- Remove hosts that are no longer there
-        foreach (const QString& ghost, goneHosts) {
-            _udpConfig->removeHost(ghost);
-        }
-    }
 }
 
 /**
@@ -190,11 +208,19 @@ void UDPLink::readBytes()
             databuffer.clear();
         }
         _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
-        // TODO This doesn't validade the sender. Anything sending UDP packets to this port gets
+        // TODO: This doesn't validade the sender. Anything sending UDP packets to this port gets
         // added to the list and will start receiving datagrams from here. Even a port scanner
         // would trigger this.
         // Add host to broadcast list if not yet present, or update its port
-        _udpConfig->addHost(sender.toString(), (int)senderPort);
+        QHostAddress asender = sender;
+        if(is_ip_local(sender)) {
+            asender = QHostAddress(QString("127.0.0.1"));
+        }
+        if(!contains_target(_sessionTargets, asender, senderPort)) {
+            qDebug() << "Adding target" << asender << senderPort;
+            UDPCLient* target = new UDPCLient(asender, senderPort);
+            _sessionTargets.append(target);
+        }
     }
     //-- Send whatever is left
     if(databuffer.size()) {
@@ -336,42 +362,49 @@ UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name
     _localPort = settings->udpListenPort()->rawValue().toInt();
     QString targetHostIP = settings->udpTargetHostIP()->rawValue().toString();
     if (!targetHostIP.isEmpty()) {
-        addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toInt());
+        addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toUInt());
     }
 }
 
 UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
 {
-    _localPort = source->localPort();
-    QString host;
-    int port;
-    _hostList.clear();
-    if(source->firstHost(host, port)) {
-        do {
-            addHost(host, port);
-        } while(source->nextHost(host, port));
-    }
+    _copyFrom(source);
+}
+
+UDPConfiguration::~UDPConfiguration()
+{
+    _clearTargetHosts();
 }
 
 void UDPConfiguration::copyFrom(LinkConfiguration *source)
 {
     LinkConfiguration::copyFrom(source);
+    _copyFrom(source);
+}
+
+void UDPConfiguration::_copyFrom(LinkConfiguration *source)
+{
     UDPConfiguration* usource = dynamic_cast<UDPConfiguration*>(source);
     if (usource) {
         _localPort = usource->localPort();
-        _hosts.clear();
-        QString host;
-        int port;
-        if(usource->firstHost(host, port)) {
-            do {
-                addHost(host, port);
-            } while(usource->nextHost(host, port));
+        _clearTargetHosts();
+        foreach(UDPCLient* target, usource->targetHosts()) {
+            if(!contains_target(_targetHosts, target->address, target->port)) {
+                UDPCLient* newTarget = new UDPCLient(target);
+                _targetHosts.append(newTarget);
+            }
         }
     } else {
         qWarning() << "Internal error";
     }
 }
 
+void UDPConfiguration::_clearTargetHosts()
+{
+    qDeleteAll(_targetHosts);
+    _targetHosts.clear();
+}
+
 /**
  * @param host Hostname in standard formatt, e.g. localhost:14551 or 192.168.1.1:14551
  */
@@ -380,104 +413,48 @@ void UDPConfiguration::addHost(const QString host)
     // Handle x.x.x.x:p
     if (host.contains(":"))
     {
-        addHost(host.split(":").first(), host.split(":").last().toInt());
+        addHost(host.split(":").first(), host.split(":").last().toUInt());
     }
     // If no port, use default
     else
     {
-        addHost(host, (int)_localPort);
+        addHost(host, _localPort);
     }
 }
 
-void UDPConfiguration::addHost(const QString& host, int port)
+void UDPConfiguration::addHost(const QString& host, quint16 port)
 {
-    bool changed = false;
-    QMutexLocker locker(&_confMutex);
-    if(_hosts.contains(host)) {
-        if(_hosts[host] != port) {
-            _hosts[host] = port;
-            changed = true;
-        }
-    } else {
     QString ipAdd = get_ip_address(host);
     if(ipAdd.isEmpty()) {
         qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
     } else {
-            // In simulation and testing setups the vehicle and the GCS can be
-            // running on the same host. This leads to packets arriving through
-            // the local network or the loopback adapter, which makes it look
-            // like the vehicle is connected through two different links,
-            // complicating routing.
-            //
-            // We detect this case and force all traffic to a simulated instance
-            // onto the local loopback interface.
-            bool not_local = true;
-            // Run through all IPv4 interfaces and check if their canonical
-            // IP address in string representation matches the source IP address
-            foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) {
-                if (address.protocol() == QAbstractSocket::IPv4Protocol) {
-                    if (ipAdd.endsWith(address.toString())) {
-                        // This is a local address of the same host
-                        not_local = false;
-                    }
-                }
-            }
-            if (not_local) {
-                // This is a normal remote host, add it using its IPv4 address
-                _hosts[ipAdd] = port;
-                //qDebug() << "UDP:" << "Adding Host:" << ipAdd << ":" << port;
-            } else {
-                // It is localhost, so talk to it through the IPv4 loopback interface
-                _hosts["127.0.0.1"] = port;
-            }
-            changed = true;
-        }
-    }
-    if(changed) {
+        QHostAddress address(ipAdd);
+        if(!contains_target(_targetHosts, address, port)) {
+            UDPCLient* newTarget = new UDPCLient(address, port);
+            _targetHosts.append(newTarget);
             _updateHostList();
         }
+    }
 }
 
 void UDPConfiguration::removeHost(const QString host)
 {
-    QMutexLocker locker(&_confMutex);
-    QString tHost = host;
-    if (tHost.contains(":")) {
-        tHost = tHost.split(":").first();
-    }
-    tHost = tHost.trimmed();
-    QMap<QString, int>::iterator i = _hosts.find(tHost);
-    if(i != _hosts.end()) {
-        //qDebug() << "UDP:" << "Removed host:" << host;
-        _hosts.erase(i);
-    } else {
-        qWarning() << "UDP:" << "Could not remove unknown host:" << host;
-    }
+    if (host.contains(":"))
+    {
+        QHostAddress address = QHostAddress(get_ip_address(host.split(":").first()));
+        quint16 port = host.split(":").last().toUInt();
+        for(int i = 0; i < _targetHosts.size(); i++) {
+            UDPCLient* target = _targetHosts.at(i);
+            if(target->address == address && target->port == port) {
+                _targetHosts.removeAt(i);
+                delete target;
                 _updateHostList();
-}
-
-bool UDPConfiguration::firstHost(QString& host, int& port)
-{
-    _confMutex.lock();
-    _it = _hosts.begin();
-    if(_it == _hosts.end()) {
-        _confMutex.unlock();
-        return false;
+                return;
             }
-    _confMutex.unlock();
-    return nextHost(host, port);
-}
-
-bool UDPConfiguration::nextHost(QString& host, int& port)
-{
-    QMutexLocker locker(&_confMutex);
-    if(_it != _hosts.end()) {
-        host = _it.key();
-        port = _it.value();
-        _it++;
-        return true;
         }
-    return false;
+    }
+    qWarning() << "UDP:" << "Could not remove unknown host:" << host;
+    _updateHostList();
 }
 
 void UDPConfiguration::setLocalPort(quint16 port)
@@ -487,31 +464,23 @@ void UDPConfiguration::setLocalPort(quint16 port)
 
 void UDPConfiguration::saveSettings(QSettings& settings, const QString& root)
 {
-    _confMutex.lock();
     settings.beginGroup(root);
     settings.setValue("port", (int)_localPort);
-    settings.setValue("hostCount", _hosts.count());
-    int index = 0;
-    QMap<QString, int>::const_iterator it = _hosts.begin();
-    while(it != _hosts.end()) {
-        QString hkey = QString("host%1").arg(index);
-        settings.setValue(hkey, it.key());
-        QString pkey = QString("port%1").arg(index);
-        settings.setValue(pkey, it.value());
-        it++;
-        index++;
+    settings.setValue("hostCount", _targetHosts.size());
+    for(int i = 0; i < _targetHosts.size(); i++) {
+        UDPCLient* target = _targetHosts.at(i);
+        QString hkey = QString("host%1").arg(i);
+        settings.setValue(hkey, target->address.toString());
+        QString pkey = QString("port%1").arg(i);
+        settings.setValue(pkey, target->port);
     }
     settings.endGroup();
-    _confMutex.unlock();
 }
 
 void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
 {
     AutoConnectSettings* acSettings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
-
-    _confMutex.lock();
-    _hosts.clear();
-    _confMutex.unlock();
+    _clearTargetHosts();
     settings.beginGroup(root);
     _localPort = (quint16)settings.value("port", acSettings->udpListenPort()->rawValue().toInt()).toUInt();
     int hostCount = settings.value("hostCount", 0).toInt();
@@ -519,7 +488,7 @@ void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
         QString hkey = QString("host%1").arg(i);
         QString pkey = QString("port%1").arg(i);
         if(settings.contains(hkey) && settings.contains(pkey)) {
-            addHost(settings.value(hkey).toString(), settings.value(pkey).toInt());
+            addHost(settings.value(hkey).toString(), settings.value(pkey).toUInt());
         }
     }
     settings.endGroup();
@@ -539,11 +508,10 @@ void UDPConfiguration::updateSettings()
 void UDPConfiguration::_updateHostList()
 {
     _hostList.clear();
-    QMap<QString, int>::const_iterator it = _hosts.begin();
-    while(it != _hosts.end()) {
-        QString host = QString("%1").arg(it.key()) + ":" + QString("%1").arg(it.value());
-        _hostList += host;
-        it++;
+    for(int i = 0; i < _targetHosts.size(); i++) {
+        UDPCLient* target = _targetHosts.at(i);
+        QString host = QString("%1").arg(target->address.toString()) + ":" + QString("%1").arg(target->port);
+        _hostList << host;
     }
     emit hostListChanged();
 }

src/comm/UDPLink.h

@@ -34,10 +34,23 @@
 #include "QGCConfig.h"
 #include "LinkManager.h"
 
+class UDPCLient {
+public:
+    UDPCLient(const QHostAddress& address_, quint16 port_)
+        : address(address_)
+        , port(port_)
+    {}
+    UDPCLient(const UDPCLient* other)
+        : address(other->address)
+        , port(other->port)
+    {}
+    QHostAddress    address;
+    quint16         port;
+};
+
 class UDPConfiguration : public LinkConfiguration
 {
     Q_OBJECT
-
 public:
 
     Q_PROPERTY(quint16      localPort   READ localPort  WRITE setLocalPort  NOTIFY localPortChanged)
@@ -61,30 +74,7 @@ public:
      */
     UDPConfiguration(UDPConfiguration* source);
 
-    /*!
-     * @brief Begin iteration through the list of target hosts
-     *
-     * @param[out] host Host name
-     * @param[out] port Port number
-     * @return Returns false if list is empty
-     */
-    bool firstHost      (QString& host, int& port);
-
-    /*!
-     * @brief Continues iteration through the list of target hosts
-     *
-     * @param[out] host Host name
-     * @param[out] port Port number
-     * @return Returns false if reached the end of the list (in which case, both host and port are unchanged)
-     */
-    bool nextHost       (QString& host, int& port);
-
-    /*!
-     * @brief Get the number of target hosts
-     *
-     * @return Number of hosts in list
-     */
-    int hostCount       () { return _hosts.count(); }
+    ~UDPConfiguration();
 
     /*!
      * @brief The UDP port we bind to
@@ -106,7 +96,7 @@ public:
      * @param[in] host Host name, e.g. localhost or 192.168.1.1
      * @param[in] port Port number
      */
-    void addHost        (const QString& host, int port);
+    void addHost        (const QString& host, quint16 port);
 
     /*!
      * @brief Remove a target host from the list
@@ -127,6 +117,8 @@ public:
      */
     QStringList hostList    () { return _hostList; }
 
+    const QList<UDPCLient*> targetHosts() { return _targetHosts; }
+
     /// From LinkConfiguration
     LinkType    type                 () { return LinkConfiguration::TypeUdp; }
     void        copyFrom             (LinkConfiguration* source);
@@ -142,11 +134,11 @@ signals:
 
 private:
     void _updateHostList    ();
+    void _clearTargetHosts  ();
+    void _copyFrom          (LinkConfiguration *source);
 
 private:
-    QMutex _confMutex;
-    QMap<QString, int>::iterator _it;
-    QMap<QString, int> _hosts;  ///< ("host", port)
+    QList<UDPCLient*>   _targetHosts;
     QStringList         _hostList;      ///< Exposed to QML
     quint16             _localPort;
 };
@@ -159,32 +151,28 @@ class UDPLink : public LinkInterface
     friend class LinkManager;
 
 public:
-    void requestReset() { }
-    bool isConnected() const;
-    QString getName() const;
+    void    requestReset            () override { }
+    bool    isConnected             () const override;
+    QString getName                 () const override;
 
     // Extensive statistics for scientific purposes
-    qint64 getConnectionSpeed() const;
-    qint64 getCurrentInDataRate() const;
-    qint64 getCurrentOutDataRate() const;
+    qint64  getConnectionSpeed      () const override;
+    qint64  getCurrentInDataRate    () const;
+    qint64  getCurrentOutDataRate   () const;
 
-    void run();
+    // Thread
+    void    run                     () override;
 
     // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
     // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
-    bool connect(void);
-    bool disconnect(void);
+    bool    connect                 (void);
+    bool    disconnect              (void);
 
 public slots:
-    /*! @brief Add a new host to broadcast messages to */
-    void addHost    (const QString& host);
-    /*! @brief Remove a host from broadcasting messages to */
-    void removeHost (const QString& host);
-
-    void readBytes();
+    void    readBytes               ();
 
 private slots:
-    void _writeBytes(const QByteArray data);
+    void    _writeBytes             (const QByteArray data) override;
 
 private:
     // Links are only created/destroyed by LinkManager so constructor/destructor is not public
@@ -192,14 +180,14 @@ private:
     ~UDPLink();
 
     // From LinkInterface
-    virtual bool _connect(void);
-    virtual void _disconnect(void);
+    bool    _connect                (void) override;
+    void    _disconnect             (void) override;
 
-    bool _hardwareConnect();
-    void _restartConnection();
-
-    void _registerZeroconf(uint16_t port, const std::string& regType);
-    void _deregisterZeroconf();
+    bool    _hardwareConnect        ();
+    void    _restartConnection      ();
+    void    _registerZeroconf       (uint16_t port, const std::string& regType);
+    void    _deregisterZeroconf     ();
+    void    _writeDataGram          (const QByteArray data, const UDPCLient* target);
 
 #if defined(QGC_ZEROCONF_ENABLED)
     DNSServiceRef  _dnssServiceRef;
@@ -209,6 +197,8 @@ private:
     QUdpSocket*         _socket;
     UDPConfiguration*   _udpConfig;
     bool                _connectState;
+    QList<UDPCLient*>   _sessionTargets;
+
 };
 
 #endif // UDPLINK_H