mavlink: fix deadlock on USB disconnect/reconnect (#26297)

Commit 5fe82aa added mutex protection in ~Mavlink() to fix a race
condition when start_helper() deleted an instance without holding the
mutex. However, this caused a deadlock because stop_command() and
destroy_all_instances() already hold mavlink_module_mutex when calling
delete, and the mutex is non-recursive.

Fix by moving instance cleanup to the callers:
- All callers now hold the mutex and remove the instance from
  mavlink_module_instances BEFORE calling delete
- The destructor no longer touches mavlink_module_instances
- Event handoff remains in destructor (works because `this` is already
  removed from the list when destructor runs)

This hopefully fixes the original race condition while avoiding the
deadlock that caused USB mavlink to hang on reconnect.
This commit is contained in:
Julian Oes 2026-01-20 10:25:50 +13:00 committed by GitHub
parent 25de111a4a
commit 02103b9100
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194

View File

@ -162,13 +162,6 @@ Mavlink::~Mavlink()
} while (running());
}
if (_instance_id >= 0) {
{
LockGuard lg{mavlink_module_mutex};
mavlink_module_instances[_instance_id] = nullptr;
}
mavlink_instance_count.fetch_sub(1);
}
// if this instance was responsible for checking events then select a new mavlink instance
if (check_events()) {
@ -381,8 +374,13 @@ Mavlink::destroy_all_instances()
LockGuard lg{mavlink_module_mutex};
// we know all threads have exited, so it's safe to delete objects.
for (Mavlink *inst_to_del : mavlink_module_instances) {
delete inst_to_del;
for (int i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
if (mavlink_module_instances[i] != nullptr) {
Mavlink *inst = mavlink_module_instances[i];
mavlink_module_instances[i] = nullptr;
mavlink_instance_count.fetch_sub(1);
delete inst;
}
}
}
@ -2921,6 +2919,14 @@ int Mavlink::start_helper(int argc, char *argv[])
res = instance->task_main(argc, argv);
if (res != PX4_OK) {
LockGuard lg{mavlink_module_mutex};
int instance_id = instance->get_instance_id();
if (instance_id >= 0) {
mavlink_module_instances[instance_id] = nullptr;
mavlink_instance_count.fetch_sub(1);
}
delete instance;
}
}
@ -3235,8 +3241,9 @@ Mavlink::stop_command(int argc, char *argv[])
for (int mavlink_instance = 0; mavlink_instance < MAVLINK_COMM_NUM_BUFFERS; mavlink_instance++) {
if (mavlink_module_instances[mavlink_instance] == inst) {
delete inst;
mavlink_module_instances[mavlink_instance] = nullptr;
mavlink_instance_count.fetch_sub(1);
delete inst;
return PX4_OK;
}
}