From 048a52ebc068e1ec2d4e760df3f1fb083307e1e6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 6 Aug 2018 15:51:38 +1000
Subject: [PATCH] GCS_MAVLink: make mavlink send from multiple threads safe

this takes a lock to prevent interleaving of mavlink msgs from
multiple threads
---
 libraries/GCS_MAVLink/GCS.cpp         |  1 -
 libraries/GCS_MAVLink/GCS.h           |  7 ++++++-
 libraries/GCS_MAVLink/GCS_Common.cpp  |  6 ++++++
 libraries/GCS_MAVLink/GCS_MAVLink.cpp | 20 ++++++++++++++++++++
 libraries/GCS_MAVLink/GCS_MAVLink.h   |  7 +++++++
 5 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index 4a2be865fc..862deda4ee 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -67,5 +67,4 @@ bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protoco
     return true;
 }
 
-
 #undef FOR_EACH_ACTIVE_CHANNEL
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index ed7a77e1bc..eadce413a3 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -648,7 +648,7 @@ public:
 
     // get the VFR_HUD throttle
     int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; }
-    
+
 private:
 
     static GCS *_singleton;
@@ -664,6 +664,11 @@ private:
     static const uint8_t _status_capacity = 30;
 #endif
 
+    // a lock for the statustext queue, to make it safe to use send_text()
+    // from multiple threads
+    HAL_Semaphore _statustext_sem;
+
+    // queue of outgoing statustext messages
     ObjectArray<statustext_t> _statustext_queue{_status_capacity};
 
     // true if we are running short on time in our main loop
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 39bc056969..42df90da44 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1281,6 +1281,8 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha
     statustext.msg.severity = severity;
     strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
 
+    _statustext_sem.take_blocking();
+    
     // The force push will ensure comm links do not block other comm links forever if they fail.
     // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
     // block but not until the buffer fills up.
@@ -1293,6 +1295,8 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha
     if (notify) {
         notify->send_text(text);
     }
+
+    _statustext_sem.give();
 }
 
 /*
@@ -1359,7 +1363,9 @@ void GCS::retry_deferred()
             chan(i).retry_deferred();
         }
     }
+    _statustext_sem.take_blocking();
     service_statustext();
+    _statustext_sem.give();
 }
 
 void GCS::data_stream_send()
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
index f4bcc292d5..a3ffdec3fb 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
@@ -26,6 +26,7 @@ This provides some support code and variables for MAVLink enabled sketches
 #include <AP_GPS/AP_GPS.h>
 #include <AP_HAL/AP_HAL.h>
 
+extern const AP_HAL::HAL& hal;
 
 #ifdef MAVLINK_SEPARATE_HELPERS
 // Shut up warnings about missing declarations; TODO: should be fixed on
@@ -39,6 +40,9 @@ This provides some support code and variables for MAVLink enabled sketches
 AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
 bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
+// per-channel lock
+static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
+
 mavlink_system_t mavlink_system = {7,1};
 
 // mask of serial ports disabled to allow for SERIAL_CONTROL
@@ -134,3 +138,19 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
     }
     mavlink_comm_port[chan]->write(buf, len);
 }
+
+/*
+  lock a channel for send
+ */
+void comm_send_lock(mavlink_channel_t chan)
+{
+    chan_locks[(uint8_t)chan].take_blocking();
+}
+
+/*
+  unlock a channel
+ */
+void comm_send_unlock(mavlink_channel_t chan)
+{
+    chan_locks[(uint8_t)chan].give();
+}
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h
index 93324f1b33..112f9e4312 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.h
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.h
@@ -11,6 +11,9 @@
 
 #define MAVLINK_SEND_UART_BYTES(chan, buf, len) comm_send_buffer(chan, buf, len)
 
+#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan)
+#define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 // allow extra mavlink channels in SITL for:
 //    Vicon
@@ -72,4 +75,8 @@ uint16_t comm_get_txspace(mavlink_channel_t chan);
 // return a MAVLink variable type given a AP_Param type
 uint8_t mav_var_type(enum ap_var_type t);
 
+// lock and unlock a channel, for multi-threaded mavlink send
+void comm_send_lock(mavlink_channel_t chan);
+void comm_send_unlock(mavlink_channel_t chan);
+
 #pragma GCC diagnostic pop