fix(mavlink): protect mission shared static state with a mutex

MavlinkMissionManager keeps several pieces of mission state as static
class members so all instances share one view of the active mission:
_mission_dataman_id, _fence_dataman_id, _safepoint_dataman_id,
_current_seq, _count[], _crc32[]. Multiple instances' receiver
threads (and their send() ticks) read and write these concurrently,
which TSan flags.

Add a static pthread_mutex_t _shared_state_mutex (recursive, so the
nested call inside update_geofence_count -> update_active_mission is
fine) initialized once via pthread_once, and take it around every
read-modify-write or read-then-publish path:

  - update_active_mission
  - update_geofence_count
  - update_safepoint_count
  - send() (covers _current_seq / _count / _crc32 access)
  - check_active_mission (the publisher of the dataman bump)

Mutex declaration is in the public section so the pthread_once
initializer can reach it; it remains a class-level static.

src/modules/mavlink/mavlink_mission.cpp

@@ -66,6 +66,8 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
 uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 };
 int32_t MavlinkMissionManager::_current_seq = 0;
 bool MavlinkMissionManager::_transfer_in_progress = false;
+
+pthread_mutex_t MavlinkMissionManager::_shared_state_mutex;
 constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
 
 #define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
@@ -73,9 +75,22 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
 		 (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
 		 (_msg.target_component == MAV_COMP_ID_ALL)))
 
+static pthread_once_t s_mission_mutex_once = PTHREAD_ONCE_INIT;
+
+static void init_mission_shared_mutex()
+{
+	pthread_mutexattr_t attr;
+	pthread_mutexattr_init(&attr);
+	pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+	pthread_mutex_init(&MavlinkMissionManager::_shared_state_mutex, &attr);
+	pthread_mutexattr_destroy(&attr);
+}
+
 MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
 	_mavlink(mavlink)
 {
+	pthread_once(&s_mission_mutex_once, init_mission_shared_mutex);
+
 	if (!_dataman_init) {
 		_dataman_init = true;
 
@@ -157,6 +172,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1
 		bool write_to_dataman)
 {
 	/* update active mission state */
+	pthread_mutex_lock(&_shared_state_mutex);
 	_mission_dataman_id = mission_dataman_id;
 	_my_mission_dataman_id = _mission_dataman_id;
 	_count[MAV_MISSION_TYPE_MISSION] = count;
@@ -175,6 +191,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1
 	mission.safe_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
 	mission.land_start_index = _land_start_marker;
 	mission.land_index = _land_marker;
+	pthread_mutex_unlock(&_shared_state_mutex);
 
 	if (write_to_dataman) {
 		bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),
@@ -191,6 +208,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1
 int
 MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32)
 {
+	pthread_mutex_lock(&_shared_state_mutex);
 	_fence_dataman_id = fence_dataman_id;
 	_my_fence_dataman_id = fence_dataman_id;
 
@@ -215,18 +233,22 @@ MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigne
 				     "Mission: Unable to write to storage");
 		}
 
+		pthread_mutex_unlock(&_shared_state_mutex);
 		return PX4_ERROR;
 	}
 
+	// update_active_mission takes the same (recursive) mutex
 	update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq,
 			      _crc32[MAV_MISSION_TYPE_MISSION],
 			      false);
+	pthread_mutex_unlock(&_shared_state_mutex);
 	return PX4_OK;
 }
 
 int
 MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32)
 {
+	pthread_mutex_lock(&_shared_state_mutex);
 	_safepoint_dataman_id = safepoint_dataman_id;
 	_my_safepoint_dataman_id = safepoint_dataman_id;
 
@@ -251,12 +273,14 @@ MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, un
 				     "Mission: Unable to write to storage");
 		}
 
+		pthread_mutex_unlock(&_shared_state_mutex);
 		return PX4_ERROR;
 	}
 
 	update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq,
 			      _crc32[MAV_MISSION_TYPE_MISSION],
 			      false);
+	pthread_mutex_unlock(&_shared_state_mutex);
 	return PX4_OK;
 }
 
@@ -516,6 +540,10 @@ MavlinkMissionManager::send()
 		return;
 	}
 
+	// Hold _shared_state_mutex across accesses to the shared statics
+	// (_current_seq, _count[], _crc32[]) that check_active_mission() on
+	// other instances may be writing concurrently.
+	pthread_mutex_lock(&_shared_state_mutex);
 
 	if (_mission_result_sub.update()) {
 		const mission_result_s &mission_result = _mission_result_sub.get();
@@ -609,6 +637,8 @@ MavlinkMissionManager::send()
 		_time_last_sent = 0;
 		_time_last_recv = 0;
 	}
+
+	pthread_mutex_unlock(&_shared_state_mutex);
 }
 
 void
@@ -1977,6 +2007,11 @@ void MavlinkMissionManager::check_active_mission()
 	if (_mission_sub.updated()) {
 		_mission_sub.update();
 
+		// Hold _shared_state_mutex around the check-then-update on _crc32[],
+		// _count[], _mission_dataman_id etc. Multiple receiver threads call
+		// this concurrently and would race on these shared statics.
+		pthread_mutex_lock(&_shared_state_mutex);
+
 		if ((_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE])
 		    || (_my_fence_dataman_id != (dm_item_t) _mission_sub.get().fence_dataman_id)) {
 			load_geofence_stats();
@@ -1992,6 +2027,8 @@ void MavlinkMissionManager::check_active_mission()
 			PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
 			init_offboard_mission(_mission_sub.get());
 		}
+
+		pthread_mutex_unlock(&_shared_state_mutex);
 	}
 }
 

src/modules/mavlink/mavlink_mission.h

@@ -131,6 +131,14 @@ private:
 	static uint32_t		_crc32[3];				///< Checksum of items in (active) mission for each MAV_MISSION_TYPE
 	static int32_t		_current_seq;				///< Current item sequence in active mission
 
+public:
+	// Serializes access to the shared static state above. Multiple Mavlink
+	// instances' receiver threads concurrently detect mission changes and
+	// write these statics; without this lock they race on writes.
+	// Public so that the pthread_once initializer can reach it.
+	static pthread_mutex_t	_shared_state_mutex;
+private:
+
 	int32_t			_last_reached{-1};			///< Last reached waypoint in active mission (-1 means nothing reached)
 
 	dm_item_t		_transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1};	///< Dataman storage ID for current transmission