io_timer: reserve pins & timers on first use

This allows modules to do a first-come-first-serve pin/timer reservation
on bootup.
E.g. camera trigger reserves any of the pins, and then PWM/DShot output
will just use the rest of the available pins.

src/drivers/camera_trigger/interfaces/src/gpio.cpp

@@ -51,15 +51,35 @@ CameraInterfaceGPIO::CameraInterfaceGPIO()
 	setup();
 }
 
+CameraInterfaceGPIO::~CameraInterfaceGPIO()
+{
+	unsigned channel = 0;
+
+	while (_allocated_channels != 0) {
+		if (((1 << channel) & _allocated_channels)) {
+			io_timer_unallocate_channel(channel);
+			_allocated_channels &= ~(1u << channel);
+		}
+
+		++channel;
+	}
+}
+
 void CameraInterfaceGPIO::setup()
 {
+	_allocated_channels = 0;
+
 	for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
 		// Pin range is from 0 to num_gpios - 1
 		if (_pins[i] >= 0 && t < (int)arraySize(_triggers)) {
 			uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]);
-			_triggers[t++] = gpio;
-			px4_arch_configgpio(gpio);
-			px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
+
+			if (io_timer_allocate_channel(_pins[i], IOTimerChanMode_Trigger) == 0) {
+				_allocated_channels |= 1 << _pins[i];
+				_triggers[t++] = gpio;
+				px4_arch_configgpio(gpio);
+				px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
+			}
 		}
 	}
 }

src/drivers/camera_trigger/interfaces/src/gpio.h

@@ -49,7 +49,7 @@ class CameraInterfaceGPIO : public CameraInterface
 {
 public:
 	CameraInterfaceGPIO();
-	virtual ~CameraInterfaceGPIO() = default;
+	virtual ~CameraInterfaceGPIO();
 
 	void trigger(bool trigger_on_true);
 
@@ -65,6 +65,7 @@ private:
 	bool _trigger_invert{false};
 
 	uint32_t _triggers[num_gpios] {};
+	uint32_t _allocated_channels{0};
 };
 
 #endif /* ifdef __PX4_NUTTX */

src/drivers/camera_trigger/interfaces/src/pwm.cpp

@@ -68,7 +68,22 @@ void CameraInterfacePWM::setup()
 	}
 
 	// Initialize and arm channels
-	up_pwm_trigger_init(pin_bitmask);
+	int ret = up_pwm_trigger_init(pin_bitmask);
+
+	if (ret < 0) {
+		PX4_ERR("up_pwm_trigger_init failed (%i)", ret);
+		pin_bitmask = 0;
+
+	} else {
+		pin_bitmask = ret;
+	}
+
+	// Clear pins that could not be initialized
+	for (unsigned i = 0; i < arraySize(_pins); i++) {
+		if (_pins[i] >= 0 && ((1 << _pins[i]) & pin_bitmask) == 0) {
+			_pins[i] = -1;
+		}
+	}
 
 	// Set neutral pulsewidths
 	for (unsigned i = 0; i < arraySize(_pins); i++) {

src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp

@@ -69,7 +69,12 @@ void CameraInterfaceSeagull::setup()
 
 			// Initialize the interface
 			uint32_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
-			up_pwm_trigger_init(pin_bitmask);
+			int ret = up_pwm_trigger_init(pin_bitmask);
+
+			if (ret != (int)pin_bitmask) {
+				PX4_WARN("up_pwm_trigger_init failed (%i)", ret);
+				continue;
+			}
 
 			// Set both interface pins to disarmed
 			int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);

src/drivers/drv_pwm_output.h

@@ -310,8 +310,9 @@ typedef enum {
  *
  * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
  *			This allows some of the channels to remain configured
- *			as GPIOs or as another function.
- * @return		OK on success.
+ *			as GPIOs or as another function. Already used channels/timers
+ *			will not be configured as PWM.
+ * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
 
@@ -402,9 +403,9 @@ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
  *
  * @param channel_mask		Bitmask of channels (LSB = channel 0) to enable.
  *				This allows some of the channels to remain configured
- *				as GPIOs or as another function.
+ *				as GPIOs or as another function. Already used channels/timers will not be configured as DShot
  * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
- * @return OK on success.
+ * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
 

src/drivers/drv_pwm_trigger.h

@@ -49,8 +49,9 @@ __BEGIN_DECLS
  *
  * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
  *			This allows some of the channels to remain configured
- *			as GPIOs or as another function.
- * @return		OK on success.
+ *			as GPIOs or as another function. Already used channels/timers
+ *			will not be configured as PWM.
+ * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int	up_pwm_trigger_init(uint32_t channel_mask);
 
@@ -78,4 +79,4 @@ __EXPORT extern void	up_pwm_trigger_arm(bool armed);
  */
 __EXPORT extern int	up_pwm_trigger_set(unsigned channel, uint16_t value);
 
-__END_DECLS
+__END_DECLS

src/drivers/dshot/DShot.cpp

@@ -334,11 +334,12 @@ void DShot::enable_dshot_outputs(const bool enabled)
 
 		int ret = up_dshot_init(_output_mask, dshot_frequency);
 
-		if (ret != 0) {
+		if (ret < 0) {
 			PX4_ERR("up_dshot_init failed (%i)", ret);
 			return;
 		}
 
+		_output_mask = ret;
 		_outputs_initialized = true;
 	}
 
@@ -1380,6 +1381,7 @@ int DShot::print_status()
 	}
 
 	PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no");
+	PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
 	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
 	perf_print_counter(_cycle_perf);
 	_mixing_output.printStatus();

src/drivers/pwm_input/pwm_input.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "pwm_input.h"
+#include <px4_arch/io_timer.h>
 
 int
 PWMIN::task_spawn(int argc, char *argv[])
@@ -65,6 +66,25 @@ PWMIN::start()
 void
 PWMIN::timer_init(void)
 {
+	/* TODO
+	 * - use gpio+irq directly instead of timer (if accurate enough)
+	 * - make pin configurable
+	 */
+
+	/* reserve the pin + timer */
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
+		if ((GPIO_PWM_IN & (GPIO_PORT_MASK | GPIO_PIN_MASK)) ==
+		    (timer_io_channels[i].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK))) {
+			int ret1 = io_timer_allocate_channel(i, IOTimerChanMode_PWMIn);
+			int ret2 = io_timer_allocate_timer(timer_io_channels[i].timer_index, IOTimerChanMode_PWMIn);
+
+			if (ret1 != 0 || ret2 != 0) {
+				PX4_ERR("timer/channel alloc failed (%i %i)", ret1, ret2);
+				return;
+			}
+		}
+	}
+
 	/* run with interrupts disabled in case the timer is already
 	 * setup. We don't want it firing while we are doing the setup */
 	irqstate_t flags = px4_enter_critical_section();

src/drivers/pwm_out/PWMOut.cpp

@@ -564,14 +564,25 @@ bool PWMOut::update_pwm_out_state(bool on)
 
 		// Initialize the PWM output state for all instances
 		// this is re-done once per instance, but harmless
-		up_pwm_servo_init(pwm_mask_new);
+		int ret = up_pwm_servo_init(pwm_mask_new);
 
-		// Set rate is not affecting non-masked channels, so can be called
-		// individually
-		set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
+		if (ret >= 0) {
+			for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
+				if (_objects[i].load()) {
+					_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
+				}
+			}
 
-		_pwm_initialized = true;
-		_all_instances_ready.fetch_add(1);
+			// Set rate is not affecting non-masked channels, so can be called
+			// individually
+			set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
+
+			_pwm_initialized = true;
+			_all_instances_ready.fetch_add(1);
+
+		} else {
+			PX4_ERR("up_pwm_servo_init failed (%i)", ret);
+		}
 	}
 
 	up_pwm_servo_arm(on, _pwm_mask);

src/drivers/pwm_out/PWMOut.hpp

@@ -158,11 +158,12 @@ public:
 	virtual int	init();
 
 	int		set_mode(Mode mode);
-	Mode		get_mode() { return _mode; }
-	uint32_t	get_pwm_mask() { return _pwm_mask; }
-	uint32_t	get_alt_rate_channels() { return _pwm_alt_rate_channels; }
-	unsigned	get_alt_rate() { return _pwm_alt_rate; }
-	unsigned	get_default_rate() { return _pwm_default_rate; }
+	Mode		get_mode() const { return _mode; }
+	uint32_t	get_pwm_mask() const { return _pwm_mask; }
+	void		set_pwm_mask(uint32_t mask) { _pwm_mask = mask; }
+	uint32_t	get_alt_rate_channels() const { return _pwm_alt_rate_channels; }
+	unsigned	get_alt_rate() const { return _pwm_alt_rate; }
+	unsigned	get_default_rate() const { return _pwm_default_rate; }
 	void		request_mode(Mode new_mode);
 
 	static int	set_i2c_bus_clock(unsigned bus, unsigned clock_hz);