From afbaa71bfca62d4ed0f9bfe327ec688ad63261dd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 19 Jun 2025 23:03:01 +0200
Subject: [PATCH] fix mavlink: add mutex for mavlink shell (#25082)

There was a race condition when closing the shell:
- the main thread checks if _mavlink_shell is not nullptr (which is true)
- the receiver thread closes the shell, which clears _mavlink_shell
- the main thread continues with _mavlink_shell->available()
---
 src/modules/mavlink/mavlink_main.cpp     | 46 ++++++++++++++++--------
 src/modules/mavlink/mavlink_main.h       |  4 +++
 src/modules/mavlink/mavlink_receiver.cpp |  2 ++
 3 files changed, 37 insertions(+), 15 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a08b6f8690..f364a57487 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2236,6 +2236,7 @@ Mavlink::task_main(int argc, char *argv[])
 		return PX4_ERROR;
 	}
 
+	pthread_mutex_init(&_mavlink_shell_mutex, nullptr);
 	pthread_mutex_init(&_message_buffer_mutex, nullptr);
 	pthread_mutex_init(&_send_mutex, nullptr);
 	pthread_mutex_init(&_radio_status_mutex, nullptr);
@@ -2374,21 +2375,7 @@ Mavlink::task_main(int argc, char *argv[])
 		handleStatus();
 		handleCommands();
 		handleAndGetCurrentCommandAck();
-
-		/* check for shell output */
-		if (_mavlink_shell && _mavlink_shell->available() > 0) {
-			if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
-				mavlink_serial_control_t msg;
-				msg.baudrate = 0;
-				msg.flags = SERIAL_CONTROL_FLAG_REPLY;
-				msg.timeout = 0;
-				msg.device = SERIAL_CONTROL_DEV_SHELL;
-				msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
-				msg.target_system = _mavlink_shell->targetSysid();
-				msg.target_component = _mavlink_shell->targetCompid();
-				mavlink_msg_serial_control_send_struct(get_channel(), &msg);
-			}
-		}
+		handleMavlinkShellOutput();
 
 		check_requested_subscriptions();
 
@@ -2521,6 +2508,7 @@ Mavlink::task_main(int argc, char *argv[])
 		_mavlink_ulog = nullptr;
 	}
 
+	pthread_mutex_destroy(&_mavlink_shell_mutex);
 	pthread_mutex_destroy(&_send_mutex);
 	pthread_mutex_destroy(&_radio_status_mutex);
 	pthread_mutex_destroy(&_message_buffer_mutex);
@@ -2563,6 +2551,34 @@ void Mavlink::handleStatus()
 	}
 }
 
+void Mavlink::handleMavlinkShellOutput()
+{
+	if (_mavlink_shell) { // First do a fast check before taking the lock
+		mavlink_serial_control_t msg;
+		msg.count = 0;
+		{
+			const LockGuard lg{_mavlink_shell_mutex};
+
+			if (_mavlink_shell && _mavlink_shell->available() > 0) {
+				if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+					msg.baudrate = 0;
+					msg.flags = SERIAL_CONTROL_FLAG_REPLY;
+					msg.timeout = 0;
+					msg.device = SERIAL_CONTROL_DEV_SHELL;
+					msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
+					msg.target_system = _mavlink_shell->targetSysid();
+					msg.target_component = _mavlink_shell->targetCompid();
+				}
+			}
+		}
+
+		// Send message without lock
+		if (msg.count > 0) {
+			mavlink_msg_serial_control_send_struct(get_channel(), &msg);
+		}
+	}
+}
+
 void Mavlink::handleCommands()
 {
 	if (_mode == MAVLINK_MODE_IRIDIUM) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e8e75bc8e1..304bd06d58 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -485,6 +485,7 @@ public:
 	/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
 	 *  Returns nullptr if shell cannot be created */
 	MavlinkShell		*get_shell();
+	pthread_mutex_t		&get_shell_mutex() { return _mavlink_shell_mutex; }
 	/** close the Mavlink shell if it is open */
 	void			close_shell();
 
@@ -571,6 +572,7 @@ private:
 	List<MavlinkStream *>		_streams;
 
 	MavlinkShell		*_mavlink_shell{nullptr};
+	pthread_mutex_t		_mavlink_shell_mutex{};
 	MavlinkULog		*_mavlink_ulog{nullptr};
 	static events::EventBuffer	*_event_buffer;
 	events::SendProtocol		_events{*_event_buffer, *this};
@@ -716,6 +718,8 @@ private:
 
 	void handleStatus();
 
+	void handleMavlinkShellOutput();
+
 	/**
 	 * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID
 	 *
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 11a47d07b8..e68353eabd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -62,6 +62,7 @@
 #include "mavlink_receiver.h"
 
 #include <lib/drivers/device/Device.hpp> // For DeviceId union
+#include <containers/LockGuard.hpp>
 
 #ifdef CONFIG_NET
 #define MAVLINK_RECEIVER_NET_ADDED_STACK 1360
@@ -1846,6 +1847,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
 		return;
 	}
 
+	const LockGuard lg{_mavlink.get_shell_mutex()};
 	MavlinkShell *shell = _mavlink.get_shell();
 
 	if (shell) {