/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /* * @file drv_pwm_servo.c * * Servo driver supporting PWM servos connected to STM32 timer blocks. * * Works with any of the 'generic' or 'advanced' STM32 timers that * have output pins, does not require an interrupt. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "drv_io_timer.h" #include #include #include #include #include #include #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) #define MAX_CHANNELS_PER_TIMER 4 #define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) #define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) #define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) #define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) #define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) #define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) #define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) #define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) #define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) #define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) #define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) #define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) #define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) #define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) #define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) #define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) #define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) #define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) #define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) #define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF) #define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF) #define CCMR_C1_RESET 0x00ff #define CCMR_C1_NUM_BITS 8 #define CCER_C1_NUM_BITS 4 #define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \ (GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \ (GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT) #define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE #define CCMR_C1_PWMIN_INIT 0 // TBD // NotUsed PWMOut PWMIn Capture io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0 , 0 , 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ static io_timer_allocation_t once = 0; typedef struct channel_stat_t { uint32_t isr_cout; uint32_t overflows; } channel_stat_t; static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; static struct channel_handler_entry { channel_handler_t callback; void *context; } channel_handlers[MAX_TIMER_IO_CHANNELS]; static int io_timer_handler(uint16_t timer_index) { /* Read the count at the time of the interrupt */ uint16_t count = rCNT(timer_index); /* Read the HRT at the time of the interrupt */ hrt_abstime now = hrt_absolute_time(); const io_timers_t *tmr = &io_timers[timer_index]; /* What is pending and enabled? */ uint16_t statusr = rSR(timer_index); uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF; /* Iterate over the timer_io_channels table */ for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { uint16_t masks = timer_io_channels[chan_index].masks; /* Do we have an enabled channel */ if (enabled & masks) { if (statusr & masks & GTIM_SR_CCIF) { io_timer_channel_stats[chan_index].isr_cout++; /* Call the client to read the CCxR etc and clear the CCxIF */ if (channel_handlers[chan_index].callback) { channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, chan_index, &timer_io_channels[chan_index], now , count); } } if (statusr & masks & GTIM_SR_CCOF) { /* Error we has a second edge before we cleared CCxR */ io_timer_channel_stats[chan_index].overflows++; } } } /* Clear all the SR bits for interrupt enabled channels only */ rSR(timer_index) = ~(statusr & (enabled | enabled << 8)); return 0; } int io_timer_handler0(int irq, void *context) { return io_timer_handler(0); } int io_timer_handler1(int irq, void *context) { return io_timer_handler(1); } int io_timer_handler2(int irq, void *context) { return io_timer_handler(2); } int io_timer_handler3(int irq, void *context) { return io_timer_handler(3); } 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 rv = 0; if (once & 1 << timer) { rv = -EBUSY; } return rv; } static inline void set_timer_initalized(unsigned timer) { once |= 1 << timer; } static inline void set_timer_deinitalized(unsigned timer) { once &= ~(1 << timer); } static inline int channels_timer(unsigned channel) { return timer_io_channels[channel].timer_index; } static uint32_t get_timer_channels(unsigned timer) { uint32_t channels = 0; if (validate_timer_index(timer) == 0) { const io_timers_t *tmr = &io_timers[timer]; /* Gather the channel bit that belong to the timer */ for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) { channels |= 1 << chan_index; } } return channels; } 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; if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { unsigned timer = timer_io_channels[channel].timer_index; /* test timer for validity */ if ((io_timers[timer].base != 0) && (timer_io_channels[channel].gpio_out != 0) && (timer_io_channels[channel].gpio_in != 0)) { rv = 0; } } return rv; } int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { return channel_allocations[mode]; } return 0; } int io_timer_get_channel_mode(unsigned channel) { io_timer_channel_allocation_t bit = 1 << channel; for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { if (bit & channel_allocations[mode]) { return mode; } } return -1; } static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) { int rv = io_timer_is_channel_free(channel); if (rv == 0) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; } return rv; } static inline int free_channel_resource(unsigned channel) { int mode = io_timer_get_channel_mode(channel); if (mode > IOTimerChanMode_NotUsed) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[mode] &= ~bit; channel_allocations[IOTimerChanMode_NotUsed] |= bit; } 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; if (mode != IOTimerChanMode_NotUsed) { rv = io_timer_validate_channel_index(channel); if (rv == 0) { rv = allocate_channel_resource(channel, mode); } } return rv; } static int timer_set_rate(unsigned timer, unsigned rate) { #if defined(PX4_IO_TIMER_ALTERNATE_RATE) /* Override the rate to a constant that could be provided by the board */ rARR(timer) = PX4_IO_TIMER_ALTERNATE_RATE; #else /* configure the timer to update at the desired rate */ rARR(timer) = 1000000 / rate; #endif /* generate an update event; reloads the counter and all registers */ rEGR(timer) = GTIM_EGR_UG; return 0; } int io_timer_init_timer(unsigned timer) { /* Do this only once per timer */ 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 */ modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit); /* disable and configure the timer */ rCR1(timer) = 0; rCR2(timer) = 0; rSMCR(timer) = 0; rDIER(timer) = 0; rCCER(timer) = 0; rCCMR1(timer) = 0; rCCMR2(timer) = 0; rCCR1(timer) = 0; rCCR2(timer) = 0; rCCR3(timer) = 0; rCCR4(timer) = 0; rCCER(timer) = 0; rDCR(timer) = 0; if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) { /* master output enable = on */ rBDTR(timer) = ATIM_BDTR_MOE; } /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN * then configure the timer to free-run at 1MHz. * Otherwize, other frequencies are attainable by adjusting .clock_freq accordingly. */ rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1; /* * Note we do the Standard PWM Out init here * default to updating at 50Hz */ timer_set_rate(timer, 50); /* * Note that the timer is left disabled with IRQ subs installed * and active but DEIR bits are not set. */ irq_attach(io_timers[timer].vectorno, io_timers[timer].handler); up_enable_irq(io_timers[timer].vectorno); px4_leave_critical_section(flags); } return rv; } int io_timer_set_rate(unsigned timer, unsigned rate) { /* Gather the channel bit that belong to the timer */ uint32_t channels = get_timer_channels(timer); /* Check ownership of PWM out */ if ((channels & channel_allocations[IOTimerChanMode_PWMOut]) != 0) { /* Change only a timer that is owned by pwm */ timer_set_rate(timer, rate); } return 0; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { uint32_t gpio = 0; uint32_t clearbits = CCMR_C1_RESET; uint32_t setbits = CCMR_C1_CAPTURE_INIT; uint32_t ccer_setbits = GTIM_CCER_CC1E; uint32_t dier_setbits = GTIM_DIER_CC1IE; /* figure out the GPIO config first */ switch (mode) { case IOTimerChanMode_PWMOut: ccer_setbits = 0; dier_setbits = 0; setbits = CCMR_C1_PWMOUT_INIT; break; case IOTimerChanMode_PWMIn: setbits = CCMR_C1_PWMIN_INIT; gpio = timer_io_channels[channel].gpio_in; break; case IOTimerChanMode_Capture: setbits = CCMR_C1_CAPTURE_INIT; gpio = timer_io_channels[channel].gpio_in; break; case IOTimerChanMode_NotUsed: setbits = 0; break; default: return -EINVAL; } int rv = allocate_channel(channel, mode); /* Valid channel should now be reserved in new mode */ if (rv >= 0) { /* Blindly try to initialize the time - it will only do it once */ io_timer_init_timer(channels_timer(channel)); irqstate_t flags = px4_enter_critical_section(); /* Set up IO */ if (gpio) { px4_arch_configgpio(gpio); } unsigned timer = channels_timer(channel); /* configure the channel */ uint32_t shifts = timer_io_channels[channel].timer_channel - 1; /* Map shifts timer channel 1-4 to 0-3 */ uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t)); uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t)); clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS; uint16_t rvalue = REG(timer, ccmr_offset); rvalue &= ~clearbits; rvalue |= setbits; REG(timer, ccmr_offset) = rvalue; /* * The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode * As soon as CCxS (in SSMRx becomes different from 00, the channel is configured * in input and the TIMx_CCR1 register becomes read-only. * so the next line does nothing in capture mode and initializes an PWM out to * 0 */ REG(timer, ccr_offset) = 0; /* on PWM Out ccer_setbits is 0 */ clearbits = (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS); setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS); rvalue = rCCER(timer); rvalue &= ~clearbits; rvalue |= setbits; rCCER(timer) = rvalue; channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; rDIER(timer) |= dier_setbits << shifts; px4_leave_critical_section(flags); } return rv; } int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks) { struct action_cache_t { uint32_t ccer_clearbits; uint32_t ccer_setbits; uint32_t dier_setbits; uint32_t dier_clearbits; uint32_t base; uint32_t gpio[MAX_CHANNELS_PER_TIMER]; } action_cache[MAX_IO_TIMERS]; memset(action_cache, 0, sizeof(action_cache)); uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0; uint32_t ccer_bit = state ? GTIM_CCER_CC1E : 0; switch (mode) { case IOTimerChanMode_NotUsed: case IOTimerChanMode_PWMOut: dier_bit = 0; break; case IOTimerChanMode_PWMIn: case IOTimerChanMode_Capture: break; default: return -EINVAL; } /* Was the request for all channels in this mode ?*/ if (masks == IO_TIMER_ALL_MODES_CHANNELS) { /* Yes - we provide them */ masks = channel_allocations[mode]; } else { /* No - caller provided mask */ /* Only allow the channels in that mode to be affected */ masks &= channel_allocations[mode]; } /* Pre calculate all the changes */ for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { if (masks & (1 << chan_index)) { masks &= ~(1 << chan_index); uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1; uint32_t timer = channels_timer(chan_index); action_cache[timer].base = io_timers[timer].base; action_cache[timer].ccer_clearbits |= GTIM_CCER_CC1E << (shifts * CCER_C1_NUM_BITS); action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS); action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts; action_cache[timer].dier_setbits |= dier_bit << shifts; if (state && mode == IOTimerChanMode_PWMOut) { action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; } } } irqstate_t flags = px4_enter_critical_section(); for (unsigned actions = 0; actions < arraySize(action_cache) && action_cache[actions].base != 0 ; actions++) { uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET); rvalue &= ~action_cache[actions].ccer_clearbits; rvalue |= action_cache[actions].ccer_setbits; _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue; uint32_t after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET); rvalue &= ~action_cache[actions].dier_clearbits; rvalue |= action_cache[actions].dier_setbits; _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue; /* Any On ?*/ if (after != 0) { /* force an update to preload all registers */ rEGR(actions) = GTIM_EGR_UG; for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) { if (action_cache[actions].gpio[chan]) { px4_arch_configgpio(action_cache[actions].gpio[chan]); action_cache[actions].gpio[chan] = 0; } } /* arm requires the timer be enabled */ rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; } else { rCR1(actions) = 0; } } px4_leave_critical_section(flags); return 0; } int io_timer_set_ccr(unsigned channel, uint16_t value) { int rv = io_timer_validate_channel_index(channel); if (rv == 0) { if (io_timer_get_channel_mode(channel) != IOTimerChanMode_PWMOut) { rv = -EIO; } else { /* configure the channel */ #ifdef BOARD_PWM_DRIVE_ACTIVE_LOW unsigned period = rARR(channels_timer(channel)); value = period - value; #endif if (value > 0) { value--; } REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value; } } return rv; } uint16_t io_channel_get_ccr(unsigned channel) { uint16_t value = 0; if (io_timer_validate_channel_index(channel) == 0 && io_timer_get_channel_mode(channel) == IOTimerChanMode_PWMOut) { value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) + 1; #ifdef BOARD_PWM_DRIVE_ACTIVE_LOW unsigned period = rARR(channels_timer(channel)); value = period - value; #endif } return value; } uint32_t io_timer_get_group(unsigned timer) { return get_timer_channels(timer); }