Comms: Prepare TCP for Threading Changes

src/Comms/TCPLink.cc

@@ -9,196 +9,269 @@
 
 #include "TCPLink.h"
 #include "DeviceInfo.h"
+#include "QGCLoggingCategory.h"
 
-#include <QtCore/QList>
+#include <QtCore/QTimer>
 #include <QtNetwork/QTcpSocket>
 #include <QtTest/QSignalSpy>
 
-TCPLink::TCPLink(SharedLinkConfigurationPtr& config)
-    : LinkInterface(config)
-    , _tcpConfig(qobject_cast<const TCPConfiguration*>(config.get()))
-    , _socket(nullptr)
-    , _socketIsConnected(false)
-{
-    Q_ASSERT(_tcpConfig);
-}
+QGC_LOGGING_CATEGORY(TCPLinkLog, "qgc.comms.tcplink")
 
-TCPLink::~TCPLink()
-{
-    disconnect();
+namespace {
+    constexpr int SEND_BUFFER_SIZE = 1024; // >= MAVLINK_MAX_PACKET_LEN
+    constexpr int RECEIVE_BUFFER_SIZE = 1024; // >= MAVLINK_MAX_PACKET_LEN
+    constexpr int READ_BUFFER_SIZE = 1024; // >= MAVLINK_MAX_PACKET_LEN
+    constexpr int CONNECT_TIMEOUT_MS = 1000;
+    constexpr int TYPE_OF_SERVICE = 32; // Optional: Set ToS for low delay
+    constexpr int MAX_RECONNECTION_ATTEMPTS = 5;
 }
 
-#ifdef TCPLINK_READWRITE_DEBUG
-void TCPLink::_writeDebugBytes(const QByteArray data)
+TCPLink::TCPLink(SharedLinkConfigurationPtr &config, QObject *parent)
+    : LinkInterface(config, parent)
+    , _tcpConfig(qobject_cast<const TCPConfiguration*>(config.get()))
+    , _socket(new QTcpSocket())
 {
-    QString bytes;
-    QString ascii;
-    for (int i=0, size = data.size(); i<size; i++)
-    {
-        unsigned char v = data[i];
-        bytes.append(QString::asprintf("%02x ", v));
-        if (data[i] > 31 && data[i] < 127)
-        {
-            ascii.append(data[i]);
-        }
-        else
-        {
-            ascii.append(219);
-        }
+    // qCDebug(TCPLinkLog) << Q_FUNC_INFO << this;
+
+    Q_CHECK_PTR(_tcpConfig);
+    if (!_tcpConfig) {
+        qCWarning(TCPLinkLog) << "Invalid TCPConfiguration provided.";
+        emit communicationError(
+            tr("Configuration Error"),
+            tr("Link %1: Invalid TCP configuration.").arg(config->name())
+        );
+        return;
     }
-    qDebug() << "Sent" << size << "bytes to" << _tcpConfig->host() << ":" << _tcpConfig->port() << "data:";
-    qDebug() << bytes;
-    qDebug() << "ASCII:" << ascii;
-}
-#endif
 
-void TCPLink::_writeBytes(const QByteArray &data)
-{
-#ifdef TCPLINK_READWRITE_DEBUG
-    _writeDebugBytes(data);
-#endif
+    _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption, SEND_BUFFER_SIZE);
+    _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, RECEIVE_BUFFER_SIZE);
+    _socket->setReadBufferSize(READ_BUFFER_SIZE);
+    _socket->setSocketOption(QAbstractSocket::LowDelayOption, 1);
+    _socket->setSocketOption(QAbstractSocket::KeepAliveOption, 1);
+    // _socket->setSocketOption(QAbstractSocket::TypeOfServiceOption, TYPE_OF_SERVICE);
 
-    if (_socket) {
-        _socket->write(data);
-        emit bytesSent(this, data);
-    }
+    (void) QObject::connect(_socket, &QTcpSocket::connected, this, [this]() {
+        _isConnected = true;
+        _reconnectionAttempts = 0;
+        qCDebug(TCPLinkLog) << "TCP connected to" << _tcpConfig->host() << ":" << _tcpConfig->port();
+        emit connected();
+    }, Qt::AutoConnection);
+
+    (void) QObject::connect(_socket, &QTcpSocket::disconnected, this, [this]() {
+        _isConnected = false;
+        qCDebug(TCPLinkLog) << "TCP disconnected from" << _tcpConfig->host() << ":" << _tcpConfig->port();
+        emit disconnected();
+        // TODO: Uncomment after threading changes
+        // _attemptReconnection();
+    }, Qt::AutoConnection);
+
+    (void) QObject::connect(_socket, &QTcpSocket::readyRead, this, &TCPLink::_readBytes, Qt::AutoConnection);
+
+    (void) QObject::connect(_socket, &QTcpSocket::errorOccurred, this, [this](QTcpSocket::SocketError error) {
+        qCWarning(TCPLinkLog) << "TCP Link Error:" << error << _socket->errorString();
+        emit communicationError(
+            tr("TCP Link Error"),
+            tr("Link %1: %2.").arg(_tcpConfig->name(), _socket->errorString())
+        );
+    }, Qt::AutoConnection);
+
+#ifdef QT_DEBUG
+    (void) QObject::connect(_socket, &QTcpSocket::stateChanged, this, [](QTcpSocket::SocketState state) {
+        qCDebug(TCPLinkLog) << "TCP State Changed:" << state;
+    }, Qt::AutoConnection);
+
+    (void) QObject::connect(_socket, &QTcpSocket::hostFound, this, []() {
+        qCDebug(TCPLinkLog) << "TCP Host Found";
+    }, Qt::AutoConnection);
+#endif
 }
 
-void TCPLink::_readBytes()
+TCPLink::~TCPLink()
 {
-    if (_socket) {
-        qint64 byteCount = _socket->bytesAvailable();
-        if (byteCount)
-        {
-            QByteArray buffer;
-            buffer.resize(byteCount);
-            _socket->read(buffer.data(), buffer.size());
-            emit bytesReceived(this, buffer);
-#ifdef TCPLINK_READWRITE_DEBUG
-            writeDebugBytes(buffer.data(), buffer.size());
-#endif
+    if (_socket->isOpen()) {
+        _socket->disconnectFromHost();
+        if (_socket->state() != QAbstractSocket::UnconnectedState) {
+            _socket->waitForDisconnected(CONNECT_TIMEOUT_MS);
         }
     }
+
+    _socket->deleteLater();
+
+    // qCDebug(TCPLinkLog) << Q_FUNC_INFO << this;
 }
 
-void TCPLink::disconnect(void)
+void TCPLink::disconnect()
 {
-    if (_socket) {
-        // This prevents stale signal from calling the link after it has been deleted
-        QObject::disconnect(_socket, &QIODevice::readyRead, this, &TCPLink::_readBytes);
-        _socketIsConnected = false;
-        _socket->disconnectFromHost(); // Disconnect tcp
-        _socket->deleteLater(); // Make sure delete happens on correct thread
-        _socket = nullptr;
+    if (isConnected()) {
+        _socket->disconnectFromHost();
+    } else {
         emit disconnected();
     }
 }
 
-bool TCPLink::_connect(void)
+bool TCPLink::_connect()
 {
-    if (_socket) {
-        qWarning() << "connect called while already connected";
+    if (isConnected()) {
+        qCWarning(TCPLinkLog) << "Already connected to" << _tcpConfig->host() << ":" << _tcpConfig->port();
         return true;
     }
 
-    return _hardwareConnect();
-}
-
-bool TCPLink::_hardwareConnect()
-{
-    Q_ASSERT(_socket == nullptr);
-    _socket = new QTcpSocket();
-    QObject::connect(_socket, &QIODevice::readyRead, this, &TCPLink::_readBytes);
-
-    QSignalSpy errorSpy(_socket, &QAbstractSocket::errorOccurred);
-    QObject::connect(_socket, &QAbstractSocket::errorOccurred, this, &TCPLink::_socketError);
+    QSignalSpy errorSpy(_socket, &QTcpSocket::errorOccurred);
 
+    qCDebug(TCPLinkLog) << "Attempting to connect to host:" << _tcpConfig->host() << "port:" << _tcpConfig->port();
     _socket->connectToHost(_tcpConfig->host(), _tcpConfig->port());
 
-    // Give the socket a second to connect to the other side otherwise error out
-    if (!_socket->waitForConnected(1000))
-    {
-        // Whether a failed connection emits an error signal or not is platform specific.
-        // So in cases where it is not emitted, we emit one ourselves.
+    // TODO: Switch Blocking to Signals after Threading Changes
+    if (!_socket->waitForConnected(CONNECT_TIMEOUT_MS)) {
+        qCWarning(TCPLinkLog) << "Connection to" << _tcpConfig->host() << ":" << _tcpConfig->port() << "failed:" << _socket->errorString();
         if (errorSpy.count() == 0) {
-            emit communicationError(tr("Link Error"), tr("Error on link %1. Connection failed").arg(_config->name()));
+            emit communicationError(
+                tr("TCP Link Connect Error"),
+                tr("Link %1: %2.").arg(_tcpConfig->name(), tr("Connection Failed: %1").arg(_socket->errorString()))
+            );
         }
-        delete _socket;
-        _socket = nullptr;
         return false;
     }
-    _socketIsConnected = true;
-    emit connected();
+
+    qCDebug(TCPLinkLog) << "Successfully connected to" << _tcpConfig->host() << ":" << _tcpConfig->port();
     return true;
 }
 
-void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
+void TCPLink::_writeBytes(const QByteArray &bytes)
 {
-    Q_UNUSED(socketError);
-    emit communicationError(tr("Link Error"), tr("Error on link %1. Error on socket: %2.").arg(_config->name()).arg(_socket->errorString()));
+    if (!_socket->isValid()) {
+        return;
+    }
+
+    static const QString title = tr("TCP Link Write Error");
+
+    if (!isConnected()) {
+        emit communicationError(
+            title,
+            tr("Link %1: Could Not Send Data - Link is Disconnected!").arg(_tcpConfig->name())
+        );
+        return;
+    }
+
+    const qint64 bytesWritten = _socket->write(bytes);
+    if (bytesWritten < 0) {
+        emit communicationError(
+            title,
+            tr("Link %1: Could Not Send Data - Write Failed: %2").arg(_tcpConfig->name(), _socket->errorString())
+        );
+        return;
+    }
+
+    if (bytesWritten < bytes.size()) {
+        qCWarning(TCPLinkLog) << "Wrote" << bytesWritten << "Out of" << bytes.size() << "total bytes";
+        const QByteArray remainingBytes = bytes.mid(bytesWritten);
+        writeBytesThreadSafe(remainingBytes.constData(), remainingBytes.size());
+    }
+
+    emit bytesSent(this, bytes);
 }
 
-/**
- * @brief Check if connection is active.
- *
- * @return True if link is connected, false otherwise.
- **/
-bool TCPLink::isConnected() const
+void TCPLink::_readBytes()
 {
-    return _socketIsConnected;
+    if (!_socket->isValid()) {
+        return;
+    }
+
+    if (!isConnected()) {
+        emit communicationError(
+            tr("TCP Link Read Error"),
+            tr("Link %1: Could Not Read Data - Link is Disconnected!").arg(_tcpConfig->name())
+        );
+        return;
+    }
+
+    const QByteArray buffer = _socket->readAll();
+
+    if (buffer.isEmpty()) {
+        emit communicationError(
+            tr("TCP Link Read Error"),
+            tr("Link %1: Could Not Read Data - No Data Available!").arg(_tcpConfig->name())
+        );
+        return;
+    }
+
+    emit bytesReceived(this, buffer);
+}
+
+void TCPLink::_attemptReconnection()
+{
+    if (_reconnectionAttempts >= MAX_RECONNECTION_ATTEMPTS) {
+        qCWarning(TCPLinkLog) << "Max reconnection attempts reached for" << _tcpConfig->host() << ":" << _tcpConfig->port();
+        emit communicationError(tr("TCP Link Reconnect Error"), tr("Link %1: Maximum reconnection attempts reached.").arg(_tcpConfig->name()));
+        return;
+    }
+
+    _reconnectionAttempts++;
+    const int delay = qPow(2, _reconnectionAttempts) * 1000; // Exponential backoff
+    qCDebug(TCPLinkLog) << "Attempting reconnection #" << _reconnectionAttempts << "in" << delay << "ms";
+    QTimer::singleShot(delay, this, [this]() {
+        _connect();
+    });
 }
 
 bool TCPLink::isSecureConnection()
 {
     return QGCDeviceInfo::isNetworkWired();
 }
 
-//--------------------------------------------------------------------------
-//-- TCPConfiguration
+/*===========================================================================*/
 
-TCPConfiguration::TCPConfiguration(const QString& name) : LinkConfiguration(name)
+TCPConfiguration::TCPConfiguration(const QString &name, QObject *parent)
+    : LinkConfiguration(name, parent)
 {
-    _port    = QGC_TCP_PORT;
-    _host    = QLatin1String("0.0.0.0");
+    // qCDebug(TCPLinkLog) << Q_FUNC_INFO << this;
 }
 
-TCPConfiguration::TCPConfiguration(const TCPConfiguration* source) : LinkConfiguration(source)
+TCPConfiguration::TCPConfiguration(const TCPConfiguration *copy, QObject *parent)
+    : LinkConfiguration(copy, parent)
+    , _host(copy->host())
+    , _port(copy->port())
 {
-    _port    = source->port();
-    _host    = source->host();
-}
+    // qCDebug(TCPLinkLog) << Q_FUNC_INFO << this;
 
-void TCPConfiguration::copyFrom(const LinkConfiguration *source)
-{
-    LinkConfiguration::copyFrom(source);
-    const TCPConfiguration* usource = qobject_cast<const TCPConfiguration*>(source);
-    Q_ASSERT(usource != nullptr);
-    _port    = usource->port();
-    _host = usource->host();
+    Q_CHECK_PTR(copy);
+
+    LinkConfiguration::copyFrom(copy);
 }
 
-void TCPConfiguration::setPort(quint16 port)
+TCPConfiguration::~TCPConfiguration()
 {
-    _port = port;
+    // qCDebug(TCPLinkLog) << Q_FUNC_INFO << this;
 }
 
-void TCPConfiguration::setHost(const QString host)
+void TCPConfiguration::copyFrom(const LinkConfiguration *source)
 {
-    _host = host;
+    Q_CHECK_PTR(source);
+    LinkConfiguration::copyFrom(source);
+
+    const TCPConfiguration* const tcpSource = qobject_cast<const TCPConfiguration*>(source);
+    Q_CHECK_PTR(tcpSource);
+
+    setHost(tcpSource->host());
+    setPort(tcpSource->port());
 }
 
-void TCPConfiguration::saveSettings(QSettings& settings, const QString& root)
+void TCPConfiguration::loadSettings(QSettings &settings, const QString &root)
 {
     settings.beginGroup(root);
-    settings.setValue("port", (int)_port);
-    settings.setValue("host", _host);
+
+    setHost(settings.value(QStringLiteral("host"), host()).toString());
+    setPort(static_cast<quint16>(settings.value(QStringLiteral("port"), port()).toUInt()));
+
     settings.endGroup();
 }
 
-void TCPConfiguration::loadSettings(QSettings& settings, const QString& root)
+void TCPConfiguration::saveSettings(QSettings &settings, const QString &root)
 {
     settings.beginGroup(root);
-    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
-    _host = settings.value("host", _host).toString();
+
+    settings.setValue(QStringLiteral("host"), host());
+    settings.setValue(QStringLiteral("port"), port());
+
     settings.endGroup();
 }