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.

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);
 	}

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)

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);
 
 /**

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);

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;

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 */

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()

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