diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp
index 8a7e69a526..f0a36da96f 100644
--- a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp
+++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp
@@ -306,7 +306,7 @@ led_pwm_servo_init(void)
 	for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
 #if defined(BOARD_HAS_SHARED_PWM_TIMERS)
 		// Use the io_timer init to mark it initialized
-		io_timer_init_timer(i);
+		io_timer_init_timer(i, IOTimerChanMode_LED);
 #endif
 		led_pwm_timer_init(i);
 	}
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
index 6286c7959a..457bd98c2b 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
@@ -90,6 +90,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 {
 	unsigned buffer_offset = 0;
 
+	for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) {
+		dshot_handler[timer_index].init = false;
+	}
+
 	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
 		if (io_timers[timer].base == 0) { // no more timers configured
 			break;
@@ -108,6 +112,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 
 	/* Init channels */
 	int ret_val = OK;
+	int channels_init_mask = 0;
 
 	for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) {
 		if (channel_mask & (1 << channel)) {
@@ -117,16 +122,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 				continue;
 			}
 
-			// First free any that were not DShot mode before
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
-
 			ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL);
+			channel_mask &= ~(1 << channel);
 
 			if (OK == ret_val) {
-				channel_mask &= ~(1 << channel);
 				dshot_handler[timer].init = true;
+				channels_init_mask |= 1 << channel;
+
+			} else if (ret_val == -EBUSY) {
+				/* either timer or channel already used - this is not fatal */
+				ret_val = 0;
 			}
 		}
 	}
@@ -146,7 +151,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 		}
 	}
 
-	return ret_val;
+	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
 void up_dshot_trigger(void)
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
index bda44530bb..c476e2c8df 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
+++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
@@ -74,6 +74,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_OneShot = 4,
 	IOTimerChanMode_Trigger = 5,
 	IOTimerChanMode_Dshot   = 6,
+	IOTimerChanMode_LED     = 7,
+	IOTimerChanMode_Other   = 8,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 
@@ -129,28 +131,33 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL
 __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
 __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
 
-__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
-
 __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 				   channel_handler_t channel_handler, void *context);
 
-__EXPORT int io_timer_init_timer(unsigned timer);
+__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode);
 
-__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
+__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate);
 __EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
 				 io_timer_channel_allocation_t masks);
-__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
 __EXPORT uint16_t io_channel_get_ccr(unsigned channel);
 __EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
 __EXPORT uint32_t io_timer_get_group(unsigned timer);
 __EXPORT int io_timer_validate_channel_index(unsigned channel);
-__EXPORT int io_timer_is_channel_free(unsigned channel);
-__EXPORT int io_timer_free_channel(unsigned channel);
+__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode);
+__EXPORT int io_timer_unallocate_channel(unsigned channel);
 __EXPORT int io_timer_get_channel_mode(unsigned channel);
 __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
 __EXPORT extern void io_timer_trigger(void);
 __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
 
+/**
+ * Reserve a timer
+ * @return 0 on success (if not used yet, or already set to the mode)
+ */
+__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode);
+
+__EXPORT int io_timer_unallocate_timer(unsigned timer);
+
 __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
 
 /**
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c
index 21989a1d44..528862dcd0 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c
@@ -162,10 +162,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt
 
 		} else {
 
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
-
 			input_capture_bind(channel, callback, context);
 
 			rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context);
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
index 922029e146..121ec9629d 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
@@ -149,12 +149,12 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 /* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA  */
 #define TIM_DMABURSTLENGTH_4TRANSFERS	0x00000300U
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0 };
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0 };
 
 typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
 
-static io_timer_allocation_t once = 0;
+io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { };
 
 #if defined(BOARD_HAS_CAPTURE)
 
@@ -280,25 +280,34 @@ static inline int validate_timer_index(unsigned timer)
 	return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
 }
 
-static inline int is_timer_uninitalized(unsigned timer)
+int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode)
 {
-	int rv = 0;
+	int ret = -EINVAL;
 
-	if (once & 1 << timer) {
-		rv = -EBUSY;
+	if (validate_timer_index(timer) == 0) {
+		// check if timer is unused or already set to the mode we want
+		if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) {
+			timer_allocations[timer] = mode;
+			ret = 0;
+
+		} else {
+			ret = -EBUSY;
+		}
 	}
 
-	return rv;
+	return ret;
 }
 
-static inline void set_timer_initalized(unsigned timer)
+int io_timer_unallocate_timer(unsigned timer)
 {
-	once |= 1 << timer;
-}
+	int ret = -EINVAL;
 
-static inline void set_timer_deinitalized(unsigned timer)
-{
-	once &= ~(1 << timer);
+	if (validate_timer_index(timer) == 0) {
+		timer_allocations[timer] = IOTimerChanMode_NotUsed;
+		ret = 0;
+	}
+
+	return ret;
 }
 
 static inline int channels_timer(unsigned channel)
@@ -334,24 +343,6 @@ static uint32_t get_timer_channels(unsigned timer)
 	return channels_cache[timer];
 }
 
-static inline int is_channels_timer_uninitalized(unsigned channel)
-{
-	return is_timer_uninitalized(channels_timer(channel));
-}
-
-int io_timer_is_channel_free(unsigned channel)
-{
-	int rv = io_timer_validate_channel_index(channel);
-
-	if (rv == 0) {
-		if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
-			rv = -EBUSY;
-		}
-	}
-
-	return rv;
-}
-
 int io_timer_validate_channel_index(unsigned channel)
 {
 	int rv = -EINVAL;
@@ -444,21 +435,22 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode
 	return before ^ channels;
 }
 
-static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
+__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 {
-	int rv = io_timer_is_channel_free(channel);
+	int existing_mode = io_timer_get_channel_mode(channel);
 
-	if (rv == 0) {
+	if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) {
 		io_timer_channel_allocation_t bit = 1 << channel;
 		channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
 		channel_allocations[mode] |= bit;
+		return 0;
 	}
 
-	return rv;
+	return -EBUSY;
 }
 
 
-static inline int free_channel_resource(unsigned channel)
+int io_timer_unallocate_channel(unsigned channel)
 {
 	int mode = io_timer_get_channel_mode(channel);
 
@@ -471,24 +463,6 @@ static inline int free_channel_resource(unsigned channel)
 	return mode;
 }
 
-int io_timer_free_channel(unsigned channel)
-{
-	if (io_timer_validate_channel_index(channel) != 0) {
-		return -EINVAL;
-	}
-
-	int mode = io_timer_get_channel_mode(channel);
-
-	if (mode > IOTimerChanMode_NotUsed) {
-		io_timer_set_enable(false, mode, 1 << channel);
-		free_channel_resource(channel);
-
-	}
-
-	return 0;
-}
-
-
 static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 {
 	int rv = -EINVAL;
@@ -497,7 +471,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 		rv = io_timer_validate_channel_index(channel);
 
 		if (rv == 0) {
-			rv = allocate_channel_resource(channel, mode);
+			rv = io_timer_allocate_channel(channel, mode);
 		}
 	}
 
@@ -633,18 +607,20 @@ void io_timer_trigger(void)
 	}
 }
 
-int io_timer_init_timer(unsigned timer)
+int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 {
+	if (validate_timer_index(timer) != 0) {
+		return -EINVAL;
+	}
+
+	io_timer_channel_mode_t previous_mode = timer_allocations[timer];
+	int rv = io_timer_allocate_timer(timer, mode);
+
 	/* Do this only once per timer */
-
-	int rv = is_timer_uninitalized(timer);
-
-	if (rv == 0) {
+	if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) {
 
 		irqstate_t flags = px4_enter_critical_section();
 
-		set_timer_initalized(timer);
-
 		/* enable the timer clock before we try to talk to it */
 
 		modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit);
@@ -721,70 +697,53 @@ int io_timer_init_timer(unsigned timer)
 }
 
 
-int io_timer_set_rate(unsigned timer, unsigned rate)
+int io_timer_set_pwm_rate(unsigned timer, unsigned rate)
 {
-	int rv = EBUSY;
-
-	/* Get the channel bits that belong to the timer */
-
-	uint32_t channels = get_timer_channels(timer);
-
-	/* Check that all channels are either in PWM, Oneshot or Dshot*/
-
-	if ((channels & (channel_allocations[IOTimerChanMode_Dshot] |
-			 channel_allocations[IOTimerChanMode_PWMOut] |
-			 channel_allocations[IOTimerChanMode_OneShot] |
-			 channel_allocations[IOTimerChanMode_NotUsed])) ==
-	    channels) {
-
-		/* Change only a timer that is owned by pwm or one shot */
-
-		/* Request to use OneShot ?*/
-
-		if (PWM_RATE_ONESHOT == rate) {
-
-			/* Request to use OneShot
-			 *
-			 * We are here because ALL these channels were either PWM or Dshot
-			 * Now they need to be Oneshot
-			 */
-
-			int changePWMOut = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot);
-			int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_OneShot);
-			int changedChannels = changePWMOut | changeDshot;
-
-			/* Did the allocation change */
-			if (changedChannels) {
-				io_timer_set_oneshot_mode(timer);
-			}
-
-		} else {
-
-			/* Request to use PWM
-			 *
-			 * We are here because  ALL these channels were either Oneshot or Dshot
-			 * Now they need to be PWM
-			 */
-
-			int changeOneShot = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut);
-			int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_PWMOut);
-
-			if (changeOneShot || changeDshot) {
-				io_timer_set_PWM_mode(timer);
-			}
-
-			timer_set_rate(timer, rate);
-		}
-
-		rv = OK;
+	/* Change only a timer that is owned by pwm or one shot */
+	if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) {
+		return -EINVAL;
 	}
 
-	return rv;
+	/* Get the channel bits that belong to the timer and are in PWM or OneShot mode */
+
+	uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) |
+			    io_timer_get_mode_channels(IOTimerChanMode_PWMOut));
+
+	/* Request to use OneShot ?*/
+
+	if (PWM_RATE_ONESHOT == rate) {
+
+		/* Request to use OneShot
+		 */
+		int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot);
+
+		/* Did the allocation change */
+		if (changed_channels) {
+			io_timer_set_oneshot_mode(timer);
+		}
+
+	} else {
+
+		/* Request to use PWM
+		 */
+		int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut);
+
+		if (changed_channels) {
+			io_timer_set_PWM_mode(timer);
+		}
+
+		timer_set_rate(timer, rate);
+	}
+
+	return OK;
 }
 
 int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 			  channel_handler_t channel_handler, void *context)
 {
+	if (io_timer_validate_channel_index(channel) != 0) {
+		return -EINVAL;
+	}
 
 	uint32_t gpio = 0;
 	uint32_t clearbits = CCMR_C1_RESET;
@@ -826,15 +785,24 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 		return -EINVAL;
 	}
 
+	int previous_mode = io_timer_get_channel_mode(channel);
 	int rv = allocate_channel(channel, mode);
+	unsigned timer = channels_timer(channel);
+
+	if (rv == 0) {
+		/* Try to reserve & initialize the timer - it will only do it once */
+
+		rv = io_timer_init_timer(timer, mode);
+
+		if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) {
+			/* free the channel if it was not used before */
+			io_timer_unallocate_channel(channel);
+		}
+	}
 
 	/* Valid channel should now be reserved in new mode */
 
-	if (rv >= 0) {
-
-		/* Blindly try to initialize the timer - it will only do it once */
-
-		io_timer_init_timer(channels_timer(channel));
+	if (rv == 0) {
 
 		irqstate_t flags = px4_enter_critical_section();
 
@@ -843,10 +811,6 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 			px4_arch_configgpio(gpio);
 		}
 
-
-		unsigned timer = channels_timer(channel);
-
-
 		/* configure the channel */
 
 		uint32_t shifts = timer_io_channels[channel].timer_channel - 1;
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
index fa5304a15a..40397fb4a7 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
@@ -82,7 +82,8 @@ int up_pwm_servo_init(uint32_t channel_mask)
 
 	for (unsigned channel = 0; current != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {
 		if (current & (1 << channel)) {
-			io_timer_free_channel(channel);
+			io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel);
+			io_timer_unallocate_channel(channel);
 			current &= ~(1 << channel);
 		}
 	}
@@ -90,23 +91,28 @@ int up_pwm_servo_init(uint32_t channel_mask)
 
 	/* Now allocate the new set */
 
+	int ret_val = OK;
+	int channels_init_mask = 0;
+
 	for (unsigned channel = 0; channel_mask != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {
 		if (channel_mask & (1 << channel)) {
 
-			/* First free any that were not PWM mode before */
-
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
-
 			/* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */
 
-			io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
+			ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
 			channel_mask &= ~(1 << channel);
+
+			if (OK == ret_val) {
+				channels_init_mask |= 1 << channel;
+
+			} else if (ret_val == -EBUSY) {
+				/* either timer or channel already used - this is not fatal */
+				ret_val = 0;
+			}
 		}
 	}
 
-	return OK;
+	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
 void up_pwm_servo_deinit(uint32_t channel_mask)
@@ -132,7 +138,7 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 		}
 	}
 
-	return io_timer_set_rate(group, rate);
+	return io_timer_set_pwm_rate(group, rate);
 }
 
 void up_pwm_update(void)
@@ -140,15 +146,6 @@ void up_pwm_update(void)
 	io_timer_trigger();
 }
 
-int up_pwm_servo_set_rate(unsigned rate)
-{
-	for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
-		up_pwm_servo_set_rate_group_update(i, rate);
-	}
-
-	return 0;
-}
-
 uint32_t up_pwm_servo_get_rate_group(unsigned group)
 {
 	/* only return the set of channels in the group which we own */
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c
index bec12b5c9d..d716a8a022 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c
@@ -66,23 +66,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value)
 int up_pwm_trigger_init(uint32_t channel_mask)
 {
 	/* Init channels */
-	for (unsigned channel = 0; channel_mask != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {
+	int ret_val = OK;
+	int channels_init_mask = 0;
+
+	for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
 		if (channel_mask & (1 << channel)) {
 
-			// First free any that were not trigger mode before
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
-
-			io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
+			ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
 			channel_mask &= ~(1 << channel);
+
+			if (OK == ret_val) {
+				channels_init_mask |= 1 << channel;
+
+			} else if (ret_val == -EBUSY) {
+				/* either timer or channel already used - this is not fatal */
+				ret_val = 0;
+			}
 		}
 	}
 
 	/* Enable the timers */
-	up_pwm_trigger_arm(true);
+	if (ret_val == OK) {
+		up_pwm_trigger_arm(true);
+	}
 
-	return OK;
+	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
 void up_pwm_trigger_deinit()
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp
index d4b79f4746..cfe55b1868 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp
+++ b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp
@@ -94,8 +94,6 @@
 #define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
 
-extern int             io_timer_init_timer(unsigned timer);
-
 static void             led_pwm_channel_init(unsigned channel);
 
 int led_pwm_servo_set(unsigned channel, uint8_t  value);
@@ -297,7 +295,12 @@ led_pwm_servo_init(void)
 	/* do basic timer initialisation first */
 	for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
 #if defined(BOARD_HAS_SHARED_PWM_TIMERS)
-		io_timer_init_timer(i);
+		int ret = io_timer_init_timer(i, IOTimerChanMode_LED);
+
+		if (ret != 0) {
+			return ret;
+		}
+
 #else
 		led_pwm_timer_init_timer(i);
 #endif
diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp
index df0ab47104..80d080310e 100644
--- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp
+++ b/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);
+			}
 		}
 	}
 }
diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h
index 9ef78eb471..da81fa20e6 100644
--- a/src/drivers/camera_trigger/interfaces/src/gpio.h
+++ b/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 */
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index b3bb582810..9b54dd7b0b 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/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++) {
diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
index ae4cd18ffa..0f8b5f534c 100644
--- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
+++ b/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);
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index aa44f4d508..ea2a60e157 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/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);
 
diff --git a/src/drivers/drv_pwm_trigger.h b/src/drivers/drv_pwm_trigger.h
index f066e3750b..2abbc7d9c0 100644
--- a/src/drivers/drv_pwm_trigger.h
+++ b/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
\ No newline at end of file
+__END_DECLS
diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
index d925058cf6..ebcba34f7a 100644
--- a/src/drivers/dshot/DShot.cpp
+++ b/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();
diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp
index bdb798c291..d29eafb9b3 100644
--- a/src/drivers/pwm_input/pwm_input.cpp
+++ b/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();
diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index 4aea51383e..33805a2dd9 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/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);
diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp
index 9b84d5115e..c58655bf45 100644
--- a/src/drivers/pwm_out/PWMOut.hpp
+++ b/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);