From 8fa3b3cbdd1c87caf886dd7f4b7f03abfedcf369 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 26 Sep 2017 15:06:46 +0200
Subject: [PATCH] udp_connection: make remote IP/port theadsafe

---
 core/udp_connection.cpp | 84 ++++++++++++++++++++++-------------------
 core/udp_connection.h   |  6 ++-
 2 files changed, 49 insertions(+), 41 deletions(-)

diff --git a/core/udp_connection.cpp b/core/udp_connection.cpp
index 0b9ec6ca81..c1609db743 100644
--- a/core/udp_connection.cpp
+++ b/core/udp_connection.cpp
@@ -27,8 +27,7 @@ namespace dronecore {
 UdpConnection::UdpConnection(DroneCoreImpl *parent,
                              int local_port_number) :
     Connection(parent),
-    _local_port_number(local_port_number),
-    _should_exit(false)
+    _local_port_number(local_port_number)
 {
     if (_local_port_number == 0) {
         _local_port_number = DEFAULT_UDP_LOCAL_PORT;
@@ -132,23 +131,28 @@ DroneCore::ConnectionResult UdpConnection::stop()
 
 bool UdpConnection::send_message(const mavlink_message_t &message)
 {
-    if (_remote_ip.empty()) {
-        LogErr() << "Remote IP unknown";
-        return false;
-    }
+    struct sockaddr_in dest_addr;
 
-    if (_remote_port_number == 0) {
-        LogErr() << "Remote port unknown";
-        return false;
-    }
+    {
+        std::lock_guard<std::mutex> lock(_remote_mutex);
 
-    struct sockaddr_in dest_addr;
-    memset((char *)&dest_addr, 0, sizeof(dest_addr));
-    dest_addr.sin_family = AF_INET;
+        if (_remote_ip.empty()) {
+            LogErr() << "Remote IP unknown";
+            return false;
+        }
+
+        if (_remote_port_number == 0) {
+            LogErr() << "Remote port unknown";
+            return false;
+        }
 
-    inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);
+        memset((char *)&dest_addr, 0, sizeof(dest_addr));
+        dest_addr.sin_family = AF_INET;
 
-    dest_addr.sin_port = htons(_remote_port_number);
+        inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);
+
+        dest_addr.sin_port = htons(_remote_port_number);
+    }
 
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);
@@ -192,38 +196,40 @@ void UdpConnection::receive(UdpConnection *parent)
             continue;
         }
 
-        int new_remote_port_number = ntohs(src_addr.sin_port);
-        std::string new_remote_ip(inet_ntoa(src_addr.sin_addr));
+        {
+            std::lock_guard<std::mutex> lock(parent->_remote_mutex);
 
-        // TODO make calls to remote threadsafe.
+            int new_remote_port_number = ntohs(src_addr.sin_port);
+            std::string new_remote_ip(inet_ntoa(src_addr.sin_addr));
 
-        if (parent->_remote_ip.empty()) {
+            if (parent->_remote_ip.empty()) {
 
-            if (parent->_remote_port_number == 0 ||
-                parent->_remote_port_number == new_remote_port_number) {
-                // Set IP if we don't know it yet.
-                parent->_remote_ip = new_remote_ip;
-                parent->_remote_port_number = new_remote_port_number;
+                if (parent->_remote_port_number == 0 ||
+                    parent->_remote_port_number == new_remote_port_number) {
+                    // Set IP if we don't know it yet.
+                    parent->_remote_ip = new_remote_ip;
+                    parent->_remote_port_number = new_remote_port_number;
 
-                LogInfo() << "Partner IP: " << parent->_remote_ip
-                          << ":" << parent->_remote_port_number;
+                    LogInfo() << "Partner IP: " << parent->_remote_ip
+                              << ":" << parent->_remote_port_number;
 
-            } else {
+                } else {
 
-                LogWarn() << "Ignoring message from remote port " << new_remote_port_number
-                          << " instead of " << parent->_remote_port_number;
-                continue;
-            }
+                    LogWarn() << "Ignoring message from remote port " << new_remote_port_number
+                              << " instead of " << parent->_remote_port_number;
+                    continue;
+                }
 
-        } else if (parent->_remote_ip.compare(new_remote_ip) != 0) {
-            LogWarn() << "Ignoring message from IP: " << new_remote_ip;
-            continue;
-
-        } else {
-            if (parent->_remote_port_number != new_remote_port_number) {
-                LogWarn() << "Ignoring message from remote port " << new_remote_port_number
-                          << " instead of " << parent->_remote_port_number;
+            } else if (parent->_remote_ip.compare(new_remote_ip) != 0) {
+                LogWarn() << "Ignoring message from IP: " << new_remote_ip;
                 continue;
+
+            } else {
+                if (parent->_remote_port_number != new_remote_port_number) {
+                    LogWarn() << "Ignoring message from remote port " << new_remote_port_number
+                              << " instead of " << parent->_remote_port_number;
+                    continue;
+                }
             }
         }
 
diff --git a/core/udp_connection.h b/core/udp_connection.h
index e0eed8280e..a49a5b8382 100644
--- a/core/udp_connection.h
+++ b/core/udp_connection.h
@@ -34,12 +34,14 @@ class UdpConnection : public Connection
     static constexpr int DEFAULT_UDP_LOCAL_PORT = 14540;
 
     int _local_port_number;
+
+    std::mutex _remote_mutex = {};
     std::string _remote_ip = {};
     int _remote_port_number = 0;
-    std::mutex _mutex = {};
+
     int _socket_fd = -1;
     std::thread *_recv_thread = nullptr;
-    std::atomic_bool _should_exit;
+    std::atomic_bool _should_exit {false};
 };
 
 } // namespace dronecore