output modules simplify locking for mixer reset and load

- fixes the deadlock in px4io ioctl mixer reset
   - px4io Run() locks (CDev semaphore)
   - mixer load goes through px4io ioctl MIXERIOCRESET which calls MixingOutput::resetMixerThreadSafe()
   - MixingOutput::resetMixerThreadSafe() stores a Command::Type::resetMixer command in an atomic variable, schedules a work queue cycle, then sleep spins until the command is cleared
   - the execution of the cycle eventually calls back into PX4IO::updateOutputs(), which tries to lock (and waits forever)

src/drivers/dshot/DShot.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +35,8 @@
 
 #include <px4_arch/io_timer.h>
 
+#include <px4_platform_common/sem.hpp>
+
 char DShot::_telemetry_device[] {};
 px4::atomic_bool DShot::_request_telemetry_init{false};
 
@@ -487,6 +489,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void DShot::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -640,21 +644,21 @@ void DShot::update_params()
 
 int DShot::ioctl(file *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = OK;
 
 	PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}
@@ -664,8 +668,6 @@ int DShot::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	// if nobody wants it, let CDev have it
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,8 @@
 #include <board_pwm_out.h>
 #include <drivers/drv_hrt.h>
 
+#include <px4_platform_common/sem.hpp>
+
 using namespace pwm_out;
 
 LinuxPWMOut::LinuxPWMOut() :
@@ -124,6 +126,8 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
 
 void LinuxPWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -301,21 +305,21 @@ void LinuxPWMOut::update_params()
 
 int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = OK;
 
 	PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			update_params();
 			break;
 		}
@@ -325,8 +329,6 @@ int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);
 	}

src/drivers/pca9685_pwm_out/PCA9685.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +35,8 @@
 #include <cmath>
 #include "PCA9685.h"
 
+#include <px4_platform_common/sem.hpp>
+
 using namespace drv_pca9685_pwm;
 
 PCA9685::PCA9685(int bus, int addr):
@@ -255,4 +257,4 @@ void PCA9685::triggerRestart()
 		PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret);
 		return;
 	}
-}
+}

src/drivers/pca9685_pwm_out/main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -49,6 +49,8 @@
 
 #include "PCA9685.h"
 
+#include <px4_platform_common/sem.hpp>
+
 #define PCA9685_DEFAULT_IICBUS  1
 #define PCA9685_DEFAULT_ADDRESS (0x40)
 
@@ -374,6 +376,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned
 
 void PCA9685Wrapper::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -464,21 +468,20 @@ void PCA9685Wrapper::Run()
 
 int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 {
-	int ret = OK;
+	SmartLock lock_guard(_lock);
 
-	lock();
+	int ret = OK;
 
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}
@@ -501,8 +504,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filep, cmd, arg);
 	}

src/drivers/pwm_out/PWMOut.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,6 +33,8 @@
 
 #include "PWMOut.hpp"
 
+#include <px4_platform_common/sem.hpp>
+
 pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
 static px4::atomic_bool _require_arming[PWM_OUT_MAX_INSTANCES] {};
@@ -437,6 +439,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void PWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -688,6 +692,8 @@ void PWMOut::update_params()
 
 int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
@@ -704,8 +710,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case PWM_SERVO_ARM:
 		update_pwm_out_state(true);
@@ -912,14 +916,14 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			update_params();
 
 			break;
@@ -930,8 +934,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	return ret;
 }
 

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,6 +39,8 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 
+#include <px4_platform_common/sem.hpp>
+
 PWMSim::PWMSim(bool hil_mode_enabled) :
 	CDev(PWM_OUTPUT0_DEVICE_PATH),
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -56,6 +58,8 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
 void
 PWMSim::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -120,9 +124,9 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
 int
 PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
-	int ret = OK;
+	SmartLock lock_guard(_lock);
 
-	lock();
+	int ret = OK;
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
@@ -231,24 +235,21 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			break;
 		}
 
-
 	default:
 		ret = -ENOTTY;
 		break;
 	}
 
-	unlock();
-
 	return ret;
 }
 

src/drivers/px4io/px4io.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -379,8 +379,6 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	SmartLock lock_guard(_lock);
-
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
@@ -391,6 +389,8 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 int PX4IO::init()
 {
+	SmartLock lock_guard(_lock);
+
 	/* do regular cdev init */
 	int ret = CDev::init();
 
@@ -525,6 +525,8 @@ void PX4IO::updateFailsafe()
 
 void PX4IO::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -546,8 +548,6 @@ void PX4IO::Run()
 		_mixing_output.update();
 	}
 
-	SmartLock lock_guard(_lock);
-
 	if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
 		/* run at 50 */
 		_poll_last = hrt_absolute_time();
@@ -1698,7 +1698,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case MIXERIOCRESET:
 		PX4_DEBUG("MIXERIOCRESET");
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
@@ -1706,7 +1706,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}

src/drivers/tap_esc/TAP_ESC.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,6 +33,8 @@
 
 #include "TAP_ESC.hpp"
 
+#include <px4_platform_common/sem.hpp>
+
 TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
 	CDev(TAP_ESC_DEVICE_PATH),
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)),
@@ -325,6 +327,8 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
 
 void TAP_ESC::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -409,19 +413,19 @@ void TAP_ESC::Run()
 
 int TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
-	int ret = OK;
+	SmartLock lock_guard(_lock);
 
-	lock();
+	int ret = OK;
 
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			break;
 		}
 
@@ -431,8 +435,6 @@ int TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	return ret;
 }
 

src/drivers/uavcan/uavcan_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2017, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -946,7 +946,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
-	lock();
+	pthread_mutex_lock(&_node_mutex);
 
 	switch (cmd) {
 	case PWM_SERVO_SET_ARM_OK:
@@ -956,14 +956,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_interface_esc.mixingOutput().resetMixerThreadSafe();
+		_mixing_interface_esc.mixingOutput().resetMixer();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_interface_esc.mixingOutput().loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_interface_esc.mixingOutput().loadMixer(buf, buflen);
 		}
 		break;
 
@@ -972,7 +972,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
+	pthread_mutex_unlock(&_node_mutex);
 
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);