imxrt: io_timer: reserve pins & timers on first use

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h

@@ -60,6 +60,9 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_Capture = 3,
 	IOTimerChanMode_OneShot = 4,
 	IOTimerChanMode_Trigger = 5,
+	IOTimerChanMode_Dshot   = 6,
+	IOTimerChanMode_LED     = 7,
+	IOTimerChanMode_Other   = 8,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 
@@ -123,27 +126,32 @@ __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_allocate_channel(unsigned channel, io_timer_channel_mode_t mode);
-__EXPORT int io_timer_free_channel(unsigned channel);
+__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);
 
+/**
+ * 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);
+
 /**
  * Returns the pin configuration for a specific channel, to be used as GPIO output.
  * 0 is returned if the channel is not valid.

platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c

@@ -87,11 +87,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
 {
 	channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
 
-	if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
+	if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) {
-		channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
+		channel_stats[chan_index].latency = (isrs_rcnt - capture);
 	}
 
-	channel_stats[chan_index].chan_in_edges_out++;
+	channel_stats[chan_index].edges++;
 	channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
 	uint32_t overflow = 0;//_REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF;
 
@@ -132,7 +132,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt
 	int rv = io_timer_validate_channel_index(channel);
 
 	if (rv == 0) {
-
 		if (edge == Disabled) {
 
 			io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel);
@@ -140,10 +139,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/nxp/imxrt/io_pins/io_timer.c

@@ -149,12 +149,14 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 #define rFCTRL20(_tim)         REG(_tim,  0, IMXRT_FLEXPWM_FCTRL20_OFFSET)       /* Fault Control 2 Register */
 
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0 };
+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] = { };
+
+/* Stats and handlers are only useful for Capture */
 
 typedef struct channel_stat_t {
 	uint32_t 			isr_cout;
@@ -213,22 +215,40 @@ int io_timer_handler7(int irq, void *context, void *arg)
 	return io_timer_handler(7);
 }
 
-static inline int is_timer_uninitalized(unsigned timer)
+static inline int validate_timer_index(unsigned timer)
 {
-	int rv = 0;
+	return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
+}
 
-	if (once & 1 << timer) {
+int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode)
-		rv = -EBUSY;
+{
+	int ret = -EINVAL;
+
+	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;
-}
 
+	if (validate_timer_index(timer) == 0) {
+		timer_allocations[timer] = IOTimerChanMode_NotUsed;
+		ret = 0;
+	}
+
+	return ret;
+}
 
 static inline int channels_timer(unsigned channel)
 {
@@ -240,19 +260,6 @@ static uint32_t get_channel_mask(unsigned channel)
 	return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0;
 }
 
-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;
@@ -337,21 +344,26 @@ 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);
+	irqstate_t flags = px4_enter_critical_section();
+	int existing_mode = io_timer_get_channel_mode(channel);
+	int ret = -EBUSY;
 
-	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;
+		ret = 0;
 	}
 
-	return rv;
+	px4_leave_critical_section(flags);
+
+	return ret;
 }
 
 
-static inline int free_channel_resource(unsigned channel)
+int io_timer_unallocate_channel(unsigned channel)
 {
 	int mode = io_timer_get_channel_mode(channel);
 
@@ -364,24 +376,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;
@@ -390,7 +384,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);
 		}
 	}
 
@@ -479,18 +473,20 @@ void io_timer_trigger(void)
 	px4_leave_critical_section(flags);
 }
 
-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 */
-
+	if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) {
-	int rv = is_timer_uninitalized(timer);
-
-	if (rv == 0) {
 
 		irqstate_t flags = px4_enter_critical_section();
 
-		set_timer_initalized(timer);
-
 		/* enable the timer clock before we try to talk to it */
 
 		switch (io_timers[timer].base) {
@@ -545,62 +541,54 @@ int io_timer_init_timer(unsigned timer)
 }
 
 
-int io_timer_set_rate(unsigned channel, unsigned rate)
+int io_timer_set_pwm_rate(unsigned timer, unsigned rate)
 {
-	int rv = EBUSY;
+	/* Change only a timer that is owned by pwm or one shot */
-
+	if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) {
-	/* Get the channel bits that belong to the channel */
+		return -EINVAL;
-
-	uint32_t channels = get_channel_mask(channel);
-
-	/* Check that all channels are either in PWM or Oneshot */
-
-	if ((channels & (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 (rate == 0) {
-
-			/* Request to use OneShot
-			 *
-			 * We are here because ALL these channels were either PWM or Oneshot
-			 * Now they need to be Oneshot
-			 */
-
-			/* Did the allocation change */
-			if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
-				io_timer_set_oneshot_mode(channel);
-			}
-
-		} else {
-
-			/* Request to use PWM
-			 *
-			 * We are here because  ALL these channels were either PWM or Oneshot
-			 * Now they need to be PWM
-			 */
-
-			if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
-				io_timer_set_PWM_mode(channel);
-			}
-
-			timer_set_rate(channel, rate);
-		}
-
-		rv = OK;
 	}
 
-	return rv;
+	/* Get the channel bits that belong to the timer and are in PWM or OneShot mode */
+
+	uint32_t channels = get_channel_mask(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;
 
 	/* figure out the GPIO config first */
@@ -625,19 +613,26 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 		return -EINVAL;
 	}
 
+	irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config
+
+	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) {
+	if (rv == 0) {
-
-		unsigned timer = channels_timer(channel);
-
-		/* Blindly try to initialize the timer - it will only do it once */
-
-		io_timer_init_timer(timer);
-
-		irqstate_t flags = px4_enter_critical_section();
 
 		/* Set up IO */
 		if (gpio) {
@@ -650,9 +645,10 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 
 		channel_handlers[channel].callback = channel_handler;
 		channel_handlers[channel].context = context;
-		px4_leave_critical_section(flags);
 	}
 
+	px4_leave_critical_section(flags);
+
 	return rv;
 }
 

platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c

@@ -74,34 +74,44 @@ servo_position_t up_pwm_servo_get(unsigned channel)
 int up_pwm_servo_init(uint32_t channel_mask)
 {
 	/* Init channels */
-	uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
+	uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) |
+			   io_timer_get_mode_channels(IOTimerChanMode_OneShot);
 
 	// First free the current set of PWMs
 
 	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);
 		}
 	}
 
-	// Now allocate the new set
+
+	/* 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)) {
+			ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
-				io_timer_free_channel(channel);
-			}
-
-			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)
@@ -131,7 +141,7 @@ int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)
 		}
 	}
 
-	return io_timer_set_rate(channel, rate);
+	return io_timer_set_pwm_rate(channel, rate);
 }
 
 void up_pwm_update(void)
@@ -139,18 +149,12 @@ void up_pwm_update(void)
 	io_timer_trigger();
 }
 
-int up_pwm_servo_set_rate(unsigned rate)
-{
-	for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; i++) {
-		up_pwm_servo_set_rate_group_update(i, rate);
-	}
-
-	return 0;
-}
-
 uint32_t up_pwm_servo_get_rate_group(unsigned group)
 {
-	return io_timer_get_group(group);
+	/* only return the set of channels in the group which we own */
+	return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) |
+		io_timer_get_mode_channels(IOTimerChanMode_OneShot)) &
+	       io_timer_get_group(group);
 }
 
 void

platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c

@@ -64,23 +64,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
+			ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
-
-			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()