From 44d9d410a13f98dd3ba65a911695b85f556ceb38 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 14 Feb 2022 17:28:16 +1100
Subject: [PATCH] GCS_MAVLink: fixed mavlink packet corruption with multiple
 threads

this ensures we don't try to send more data to a uart than is
available in the tx buffer
---
 libraries/GCS_MAVLink/GCS_Common.cpp  |  3 +++
 libraries/GCS_MAVLink/GCS_MAVLink.cpp | 29 ++++++++++++++++++++++-----
 libraries/GCS_MAVLink/GCS_MAVLink.h   |  5 +++--
 3 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 00a54e6146..b8a5284f5b 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1051,6 +1051,7 @@ bool GCS_MAVLINK::do_try_send_message(const ap_message id)
     if (telemetry_delayed()) {
         return false;
     }
+    WITH_SEMAPHORE(comm_chan_lock(chan));
 #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
     void *data = hal.scheduler->disable_interrupts_save();
     uint32_t start_send_message_us = AP_HAL::micros();
@@ -2058,6 +2059,8 @@ void GCS_MAVLINK::service_statustext(void)
     // want to move idx.
     const uint16_t payload_size = PAYLOAD_SIZE(chan, STATUSTEXT);
     for (uint8_t idx=0; idx<_statustext_queue.available(); ) {
+        WITH_SEMAPHORE(comm_chan_lock(chan));
+
         if (txspace() < payload_size) {
             break;
         }
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
index 86432feb46..84ad6ae28b 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
@@ -41,6 +41,7 @@ bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
 // per-channel lock
 static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
+static bool chan_discard[MAVLINK_COMM_NUM_BUFFERS];
 
 mavlink_system_t mavlink_system = {7,1};
 
@@ -90,7 +91,7 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
  */
 void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
 {
-    if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr) {
+    if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) {
         return;
     }
     if (gcs_alternative_active[chan]) {
@@ -109,16 +110,34 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
 
 /*
   lock a channel for send
+  if there is insufficient space to send size bytes then all bytes
+  written to the channel by the mavlink library will be discarded
+  while the lock is held.
  */
-void comm_send_lock(mavlink_channel_t chan)
+void comm_send_lock(mavlink_channel_t chan_m, uint16_t size)
 {
-    chan_locks[(uint8_t)chan].take_blocking();
+    const uint8_t chan = uint8_t(chan_m);
+    chan_locks[chan].take_blocking();
+    if (mavlink_comm_port[chan]->txspace() < size) {
+        chan_discard[chan] = true;
+    }
 }
 
 /*
   unlock a channel
  */
-void comm_send_unlock(mavlink_channel_t chan)
+void comm_send_unlock(mavlink_channel_t chan_m)
+{
+    const uint8_t chan = uint8_t(chan_m);
+    chan_discard[chan] = false;
+    chan_locks[chan].give();
+}
+
+/*
+  return reference to GCS channel lock, allowing for
+  HAVE_PAYLOAD_SPACE() to be run with a locked channel
+ */
+HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan)
 {
-    chan_locks[(uint8_t)chan].give();
+    return chan_locks[uint8_t(chan)];
 }
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h
index 45ddf37809..f1996d4fde 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.h
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.h
@@ -11,7 +11,7 @@
 
 #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_START_UART_SEND(chan, size) comm_send_lock(chan, size)
 #define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -70,7 +70,8 @@ uint16_t comm_get_txspace(mavlink_channel_t chan);
 #include "include/mavlink/v2.0/all/mavlink.h"
 
 // lock and unlock a channel, for multi-threaded mavlink send
-void comm_send_lock(mavlink_channel_t chan);
+void comm_send_lock(mavlink_channel_t chan, uint16_t size);
 void comm_send_unlock(mavlink_channel_t chan);
+HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan);
 
 #pragma GCC diagnostic pop