kinetis: io_timer: reserve pins & timers on first use

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

@@ -64,6 +64,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;
 
@@ -122,21 +125,29 @@ __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);
+
+/**
+ * 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.
@@ -148,5 +159,4 @@ __EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel);
  */
 __EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel);
 
-
 __END_DECLS

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

@@ -104,11 +104,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) {
-		channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
+	if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) {
+		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 = _REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF;
 
@@ -169,10 +169,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/kinetis/io_pins/io_timer.c

@@ -152,12 +152,14 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 
 #define CnSC_PWMIN_INIT 0 // TBD
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   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] = { };
+
+/* Stats and handlers are only useful for Capture */
 
 typedef struct channel_stat_t {
 	uint32_t 			isr_cout;
@@ -269,25 +271,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 (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;
 
-	if (once & 1 << timer) {
-		rv = -EBUSY;
+		} 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)
@@ -324,24 +335,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;
@@ -380,7 +373,6 @@ uint32_t io_timer_channel_get_as_pwm_input(unsigned channel)
 	return timer_io_channels[channel].gpio_in;
 }
 
-
 int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
 {
 	if (mode < IOTimerChanModeSize) {
@@ -429,21 +421,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);
 
@@ -456,24 +453,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;
@@ -482,7 +461,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);
 		}
 	}
 
@@ -568,18 +547,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)
 {
-	/* Do this only once per timer */
+	if (validate_timer_index(timer) != 0) {
+		return -EINVAL;
+	}
 
-	int rv = is_timer_uninitalized(timer);
+	io_timer_channel_mode_t previous_mode = timer_allocations[timer];
+	int rv = io_timer_allocate_timer(timer, mode);
 
-	if (rv == 0) {
+	/* Do this only once per timer */
+	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 */
 
 		uint32_t regval = _REG(io_timers[timer].clock_register);
@@ -650,62 +631,54 @@ 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 or Oneshot */
+	/* 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;
+	}
 
-	if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
-			 channel_allocations[IOTimerChanMode_OneShot] |
-			 channel_allocations[IOTimerChanMode_NotUsed])) ==
-	    channels) {
+	/* Get the channel bits that belong to the timer and are in PWM or OneShot mode */
 
-		/* Change only a timer that is owned by pwm or one shot */
+	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 ?*/
+	/* Request to use OneShot ?*/
 
-		if (rate == 0) {
+	if (PWM_RATE_ONESHOT == rate) {
 
-			/* 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(timer);
-			}
+		/* Request to use OneShot
+		 */
+		int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot);
 
-		} else {
+		/* Did the allocation change */
+		if (changed_channels) {
+			io_timer_set_oneshot_mode(timer);
+		}
 
-			/* Request to use PWM
-			 *
-			 * We are here because  ALL these channels were either PWM or Oneshot
-			 * Now they need to be PWM
-			 */
+	} else {
 
-			if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
-				io_timer_set_PWM_mode(timer);
-			}
+		/* Request to use PWM
+		 */
+		int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut);
 
-			timer_set_rate(timer, rate);
+		if (changed_channels) {
+			io_timer_set_PWM_mode(timer);
 		}
 
-		rv = OK;
+		timer_set_rate(timer, rate);
 	}
 
-	return rv;
+	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 = timer_io_channels[channel].gpio_in;
 	uint32_t clearbits = CnSC_RESET;
 	uint32_t setbits = CnSC_CAPTURE_INIT;
@@ -733,25 +706,33 @@ 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);
 
-	/* Valid channel should now be reserved in new mode */
+	if (rv == 0) {
+		/* Try to reserve & initialize the timer - it will only do it once */
 
-	if (rv >= 0) {
+		rv = io_timer_init_timer(timer, mode);
 
-		/* Blindly try to initialize the timer - it will only do it once */
+		if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) {
+			/* free the channel if it was not used before */
+			io_timer_unallocate_channel(channel);
+		}
+	}
 
-		io_timer_init_timer(channels_timer(channel));
+	/* Valid channel should now be reserved in new mode */
 
-		irqstate_t flags = px4_enter_critical_section();
+	if (rv == 0) {
 
 		/* Set up IO */
 		if (gpio) {
 			px4_arch_configgpio(gpio);
 		}
 
-
-		unsigned timer = channels_timer(channel);
+		/* configure the channel */
 
 		/* configure the channel */
 
@@ -767,9 +748,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/kinetis/io_pins/pwm_servo.c

@@ -75,34 +75,42 @@ 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
+			ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
+			channel_mask &= ~(1 << channel);
+
+			if (OK == ret_val) {
+				channels_init_mask |= 1 << channel;
 
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
+			} else if (ret_val == -EBUSY) {
+				/* either timer or channel already used - this is not fatal */
+				ret_val = 0;
 			}
-
-			io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
-			channel_mask &= ~(1 << channel);
 		}
 	}
 
-	return OK;
+	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
 void up_pwm_servo_deinit(uint32_t channel_mask)
@@ -132,7 +140,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,18 +148,12 @@ 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)
 {
-	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/kinetis/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++) {
-		if (channel_mask & (1 << channel)) {
+	int ret_val = OK;
+	int channels_init_mask = 0;
 
-			// First free any that were not trigger mode before
-			if (-EBUSY == io_timer_is_channel_free(channel)) {
-				io_timer_free_channel(channel);
-			}
+	for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
+		if (channel_mask & (1 << 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()