From cad9ed8e13b1b5e7448f41a71837d0e52ca0ad42 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 13 Jan 2012 02:49:10 +0000 Subject: STM32 fixes for F4 32-bit timers git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@4300 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 7 +- nuttx/arch/arm/src/stm32/stm32_tim.c | 955 ++++++++++++++++++++--------------- nuttx/arch/arm/src/stm32/stm32_tim.h | 28 +- nuttx/drivers/input/tsc2007.c | 4 +- nuttx/include/stdlib.h | 8 +- nuttx/lib/stdio/lib_libvsprintf.c | 36 ++ 6 files changed, 622 insertions(+), 416 deletions(-) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 3a030612e..a6c69d13c 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -2312,7 +2312,7 @@ * configs/olimex-lpc1766stk/src/up_leds.c: Add new interfaces so that is CONFIG_ARCH_LEDS are not set, the LEDs may be controlled from application logic. - * configs/olimex-lpc1766stk/src/up_buttons.c: Add support form the buttons + * configs/olimex-lpc1766stk/src/up_buttons.c: Add support for the buttons on the Olimex LPC1766-STK board. * Makefile: Added 'apps_clean' and 'apps_distclean' target to simplify managing the state of the application directory while in the NuttX directory @@ -2361,4 +2361,7 @@ (Contributed by Mike Smith). * fs/fat/fs_fat32util.c: On a failure to recognize a FAT file system, the mount logic should return -EINVAL, not -ENODEV. - + * arch/arm/src/stm32/stm32_tim.c: Support for STM32 F4 32-bit timers + (Contributed by Mikhail Bychek) + * lib/stdio/lib_vsprintf.c: Add support for fixed-size fields with floating + point numbers (Contributed by Mikhail Bychek) diff --git a/nuttx/arch/arm/src/stm32/stm32_tim.c b/nuttx/arch/arm/src/stm32/stm32_tim.c index 0720ad9b1..39b0b8295 100644 --- a/nuttx/arch/arm/src/stm32/stm32_tim.c +++ b/nuttx/arch/arm/src/stm32/stm32_tim.c @@ -6,7 +6,7 @@ * * With modifications and updates by: * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -73,11 +73,11 @@ * is defined then the CONFIG_STM32_TIMn_PWM may also be defined to indicate that * the timer is intended to be used for pulsed output modulation. * - * - To control periodic ADC input sampling. If CONFIG_STM32_TIMn is defined then + * - To control periodic ADC input sampling. If CONFIG_STM32_TIMn is defined then * CONFIG_STM32_TIMn_ADC may also be defined to indicate that timer "n" is intended * to be used for that purpose. * - * - To control periodic DAC outputs. If CONFIG_STM32_TIMn is defined then + * - To control periodic DAC outputs. If CONFIG_STM32_TIMn is defined then * CONFIG_STM32_TIMn_DAC may also be defined to indicate that timer "n" is intended * to be used for that purpose. * @@ -139,75 +139,98 @@ * Private Types ************************************************************************************/ -/** TIM Device Structure - */ -struct stm32_tim_priv_s { - struct stm32_tim_ops_s *ops; - stm32_tim_mode_t mode; - uint32_t base; /** TIMn base address */ +/* TIM Device Structure */ + +struct stm32_tim_priv_s +{ + struct stm32_tim_ops_s *ops; + stm32_tim_mode_t mode; + uint32_t base; /* TIMn base address */ }; /************************************************************************************ * Private Functions ************************************************************************************/ -/** Get register value by offset */ -static inline uint16_t stm32_tim_getreg(FAR struct stm32_tim_dev_s *dev, uint8_t offset) +/* Get a 16-bit register value by offset */ + +static inline uint16_t stm32_getreg16(FAR struct stm32_tim_dev_s *dev, uint8_t offset) +{ + return getreg16(((struct stm32_tim_priv_s *)dev)->base + offset); +} + +/* Put a 16-bit register value by offset */ + +static inline void stm32_putreg16(FAR struct stm32_tim_dev_s *dev, uint8_t offset, uint16_t value) { - return getreg16( ((struct stm32_tim_priv_s *)dev)->base + offset); + putreg16(value, ((struct stm32_tim_priv_s *)dev)->base + offset); } -/** Put register value by offset */ -static inline void stm32_tim_putreg(FAR struct stm32_tim_dev_s *dev, uint8_t offset, uint16_t value) +/* Modify a 16-bit register value by offset */ + +static inline void stm32_modifyreg16(FAR struct stm32_tim_dev_s *dev, uint8_t offset, uint16_t clearbits, uint16_t setbits) { - //printf("putreg(%8x)=%4x\n", ((struct stm32_tim_priv_s *)dev)->base + offset, value ); - putreg16(value, ((struct stm32_tim_priv_s *)dev)->base + offset); + modifyreg16(((struct stm32_tim_priv_s *)dev)->base + offset, clearbits, setbits); } -/** Modify register value by offset */ -static inline void stm32_tim_modifyreg(FAR struct stm32_tim_dev_s *dev, uint8_t offset, uint16_t clearbits, uint16_t setbits) +/* Get a 32-bit register value by offset. This applies only for the STM32 F4 + * 32-bit registers (CNT, ARR, CRR1-4) in the 32-bit timers TIM2-5. + */ + +static inline uint32_t stm32_getreg32(FAR struct stm32_tim_dev_s *dev, uint8_t offset) { - modifyreg16( ((struct stm32_tim_priv_s *)dev)->base + offset, clearbits, setbits); + return getreg32(((struct stm32_tim_priv_s *)dev)->base + offset); +} + +/* Put a 32-bit register value by offset. This applies only for the STM32 F4 + * 32-bit registers (CNT, ARR, CRR1-4) in the 32-bit timers TIM2-5. + */ + +static inline void stm32_putreg32(FAR struct stm32_tim_dev_s *dev, uint8_t offset, uint32_t value) +{ + putreg32(value, ((struct stm32_tim_priv_s *)dev)->base + offset); } static void stm32_tim_reload_counter(FAR struct stm32_tim_dev_s *dev) { - uint16_t val = stm32_tim_getreg(dev, STM32_BTIM_EGR_OFFSET); - val |= ATIM_EGR_UG; - stm32_tim_putreg(dev, STM32_BTIM_EGR_OFFSET, val); + uint16_t val = stm32_getreg16(dev, STM32_BTIM_EGR_OFFSET); + val |= ATIM_EGR_UG; + stm32_putreg16(dev, STM32_BTIM_EGR_OFFSET, val); } static void stm32_tim_enable(FAR struct stm32_tim_dev_s *dev) { - uint16_t val = stm32_tim_getreg(dev, STM32_BTIM_CR1_OFFSET); - val |= ATIM_CR1_CEN; - stm32_tim_reload_counter(dev); - stm32_tim_putreg(dev, STM32_BTIM_CR1_OFFSET, val); + uint16_t val = stm32_getreg16(dev, STM32_BTIM_CR1_OFFSET); + val |= ATIM_CR1_CEN; + stm32_tim_reload_counter(dev); + stm32_putreg16(dev, STM32_BTIM_CR1_OFFSET, val); } static void stm32_tim_disable(FAR struct stm32_tim_dev_s *dev) { - uint16_t val = stm32_tim_getreg(dev, STM32_BTIM_CR1_OFFSET); - val &= ~ATIM_CR1_CEN; - stm32_tim_putreg(dev, STM32_BTIM_CR1_OFFSET, val); + uint16_t val = stm32_getreg16(dev, STM32_BTIM_CR1_OFFSET); + val &= ~ATIM_CR1_CEN; + stm32_putreg16(dev, STM32_BTIM_CR1_OFFSET, val); } -/** Reset timer into system default state, but do not affect output/input pins */ +/* Reset timer into system default state, but do not affect output/input pins */ static void stm32_tim_reset(FAR struct stm32_tim_dev_s *dev) { - ((struct stm32_tim_priv_s *)dev)->mode = STM32_TIM_MODE_DISABLED; - stm32_tim_disable(dev); + ((struct stm32_tim_priv_s *)dev)->mode = STM32_TIM_MODE_DISABLED; + stm32_tim_disable(dev); } static void stm32_tim_gpioconfig(uint32_t cfg, stm32_tim_channel_t mode) { - /** \todo Added support for input capture and bipolar dual outputs for TIM8 */ + /* TODO: Add support for input capture and bipolar dual outputs for TIM8 */ - if (mode & STM32_TIM_CH_MODE_MASK) { - stm32_configgpio(cfg); - } - else { - stm32_unconfiggpio(cfg); + if (mode & STM32_TIM_CH_MODE_MASK) + { + stm32_configgpio(cfg); + } + else + { + stm32_unconfiggpio(cfg); } } @@ -217,115 +240,148 @@ static void stm32_tim_gpioconfig(uint32_t cfg, stm32_tim_channel_t mode) static int stm32_tim_setclock(FAR struct stm32_tim_dev_s *dev, uint32_t freq) { - int prescaler; - - ASSERT(dev); - - /* Disable Timer? */ - if (freq == 0) { - stm32_tim_disable(dev); - return 0; + int prescaler; + + ASSERT(dev); + + /* Disable Timer? */ + + if (freq == 0) + { + stm32_tim_disable(dev); + return 0; } - -#if STM32_NATIM > 0 - if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM1_BASE || - ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM8_BASE) - prescaler = STM32_TIM18_FREQUENCY / freq; - else -#endif - prescaler = STM32_TIM27_FREQUENCY / freq; - - /* we need to decrement value for '1', but only, if we are allowed to - * not to cause underflow. Check for overflow. - */ - if (prescaler > 0) prescaler--; - if (prescaler > 0xFFFF) prescaler = 0xFFFF; - - stm32_tim_putreg(dev, STM32_BTIM_PSC_OFFSET, prescaler); - stm32_tim_enable(dev); - - return prescaler; + +#if STM32_NATIM > 0 + if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM1_BASE || + ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM8_BASE) + { + prescaler = STM32_TIM18_FREQUENCY / freq; + } + else +#endif + { + prescaler = STM32_TIM27_FREQUENCY / freq; + } + + /* We need to decrement value for '1', but only, if we are allowed to + * not to cause underflow. Check for overflow. + */ + + if (prescaler > 0) + { + prescaler--; + } + + if (prescaler > 0xffff) + { + prescaler = 0xffff; + } + + stm32_putreg16(dev, STM32_BTIM_PSC_OFFSET, prescaler); + stm32_tim_enable(dev); + + return prescaler; } -static void stm32_tim_setperiod(FAR struct stm32_tim_dev_s *dev, uint16_t period) +static void stm32_tim_setperiod(FAR struct stm32_tim_dev_s *dev, uint32_t period) { - ASSERT(dev); - stm32_tim_putreg(dev, STM32_BTIM_ARR_OFFSET, period); + ASSERT(dev); + stm32_putreg32(dev, STM32_BTIM_ARR_OFFSET, period); } static int stm32_tim_setisr(FAR struct stm32_tim_dev_s *dev, int (*handler)(int irq, void *context), int source) { - int vectorno; + int vectorno; + + ASSERT(dev); + ASSERT(source==0); - ASSERT(dev); - ASSERT(source==0); - - switch( ((struct stm32_tim_priv_s *)dev)->base ) { + switch (((struct stm32_tim_priv_s *)dev)->base) + { #if CONFIG_STM32_TIM2 - case STM32_TIM2_BASE: vectorno = STM32_IRQ_TIM2; break; + case STM32_TIM2_BASE: + vectorno = STM32_IRQ_TIM2; + break; #endif #if CONFIG_STM32_TIM3 - case STM32_TIM3_BASE: vectorno = STM32_IRQ_TIM3; break; + case STM32_TIM3_BASE: + vectorno = STM32_IRQ_TIM3; + break; #endif #if CONFIG_STM32_TIM4 - case STM32_TIM4_BASE: vectorno = STM32_IRQ_TIM4; break; + case STM32_TIM4_BASE: + vectorno = STM32_IRQ_TIM4; + break; #endif #if CONFIG_STM32_TIM5 - case STM32_TIM5_BASE: vectorno = STM32_IRQ_TIM5; break; + case STM32_TIM5_BASE: + vectorno = STM32_IRQ_TIM5; + break; #endif #if STM32_NBTIM > 0 #if CONFIG_STM32_TIM6 - case STM32_TIM6_BASE: vectorno = STM32_IRQ_TIM6; break; + case STM32_TIM6_BASE: + vectorno = STM32_IRQ_TIM6; + break; #endif #endif #if STM32_NBTIM > 1 #if CONFIG_STM32_TIM7 - case STM32_TIM7_BASE: vectorno = STM32_IRQ_TIM7; break; + case STM32_TIM7_BASE: + vectorno = STM32_IRQ_TIM7; + break; #endif #endif #if STM32_NATIM > 0 - /** \todo add support for multiple sources and callbacks */ + /* TODO: add support for multiple sources and callbacks */ #if CONFIG_STM32_TIM1 - case STM32_TIM1_BASE: vectorno = STM32_IRQ_TIM1UP; break; + case STM32_TIM1_BASE: + vectorno = STM32_IRQ_TIM1UP; + break; #endif #if CONFIG_STM32_TIM8 - case STM32_TIM8_BASE: vectorno = STM32_IRQ_TIM8UP; break; + case STM32_TIM8_BASE: + vectorno = STM32_IRQ_TIM8UP; + break; #endif #endif - default: return ERROR; + default: + return ERROR; } - - /* Disable interrupt when callback is removed */ - - if (!handler) { - up_disable_irq(vectorno); - irq_detach(vectorno); - return OK; + + /* Disable interrupt when callback is removed */ + + if (!handler) + { + up_disable_irq(vectorno); + irq_detach(vectorno); + return OK; } - - /* Otherwise set callback and enable interrupt */ - irq_attach(vectorno, handler); - up_enable_irq(vectorno); -// up_prioritize_irq(vectorno, NVIC_SYSH_PRIORITY_DEFAULT); - return OK; + /* Otherwise set callback and enable interrupt */ + + irq_attach(vectorno, handler); + up_enable_irq(vectorno); +//up_prioritize_irq(vectorno, NVIC_SYSH_PRIORITY_DEFAULT); + return OK; } static void stm32_tim_enableint(FAR struct stm32_tim_dev_s *dev, int source) { - ASSERT(dev); - stm32_tim_modifyreg(dev, STM32_BTIM_DIER_OFFSET, 0, ATIM_DIER_UIE); + ASSERT(dev); + stm32_modifyreg16(dev, STM32_BTIM_DIER_OFFSET, 0, ATIM_DIER_UIE); } static void stm32_tim_disableint(FAR struct stm32_tim_dev_s *dev, int source) { - ASSERT(dev); - stm32_tim_modifyreg(dev, STM32_BTIM_DIER_OFFSET, ATIM_DIER_UIE, 0); + ASSERT(dev); + stm32_modifyreg16(dev, STM32_BTIM_DIER_OFFSET, ATIM_DIER_UIE, 0); } static void stm32_tim_ackint(FAR struct stm32_tim_dev_s *dev, int source) { - stm32_tim_putreg(dev, STM32_BTIM_SR_OFFSET, ~ATIM_SR_UIF); + stm32_putreg16(dev, STM32_BTIM_SR_OFFSET, ~ATIM_SR_UIF); } /************************************************************************************ @@ -334,349 +390,437 @@ static void stm32_tim_ackint(FAR struct stm32_tim_dev_s *dev, int source) static int stm32_tim_setmode(FAR struct stm32_tim_dev_s *dev, stm32_tim_mode_t mode) { - uint16_t val = ATIM_CR1_CEN | ATIM_CR1_ARPE; - - ASSERT(dev); - - /* This function is not supported on basic timers. To enable or - * disable it, simply set its clock to valid frequency or zero. - */ - + uint16_t val = ATIM_CR1_CEN | ATIM_CR1_ARPE; + + ASSERT(dev); + + /* This function is not supported on basic timers. To enable or + * disable it, simply set its clock to valid frequency or zero. + */ + #if STM32_NBTIM > 0 - if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM6_BASE + if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM6_BASE #endif #if STM32_NBTIM > 1 - || ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM7_BASE + || ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM7_BASE #endif #if STM32_NBTIM > 0 - ) return ERROR; -#endif - - /* Decode operational modes */ - - switch(mode & STM32_TIM_MODE_MASK) { - - case STM32_TIM_MODE_DISABLED: - val = 0; - break; - - case STM32_TIM_MODE_DOWN: - val |= ATIM_CR1_DIR; - - case STM32_TIM_MODE_UP: - break; - - case STM32_TIM_MODE_UPDOWN: - val |= ATIM_CR1_CENTER1; - // Our default: Interrupts are generated on compare, when counting down - break; - - case STM32_TIM_MODE_PULSE: - val |= ATIM_CR1_OPM; - break; - - default: return ERROR; + ) + { + return ERROR; } - - stm32_tim_reload_counter(dev); - stm32_tim_putreg(dev, STM32_BTIM_CR1_OFFSET, val); - +#endif + + /* Decode operational modes */ + + switch (mode & STM32_TIM_MODE_MASK) + { + case STM32_TIM_MODE_DISABLED: + val = 0; + break; + + case STM32_TIM_MODE_DOWN: + val |= ATIM_CR1_DIR; + + case STM32_TIM_MODE_UP: + break; + + case STM32_TIM_MODE_UPDOWN: + val |= ATIM_CR1_CENTER1; + // Our default: Interrupts are generated on compare, when counting down + break; + + case STM32_TIM_MODE_PULSE: + val |= ATIM_CR1_OPM; + break; + + default: return ERROR; + } + + stm32_tim_reload_counter(dev); + stm32_putreg16(dev, STM32_BTIM_CR1_OFFSET, val); + #if STM32_NATIM > 0 - /* Advanced registers require Main Output Enable */ - + /* Advanced registers require Main Output Enable */ + if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM1_BASE || - ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM8_BASE) { - stm32_tim_modifyreg(dev, STM32_ATIM_BDTR_OFFSET, 0, ATIM_BDTR_MOE); - } + ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM8_BASE) + { + stm32_modifyreg16(dev, STM32_ATIM_BDTR_OFFSET, 0, ATIM_BDTR_MOE); + } #endif - - return OK; + + return OK; } static int stm32_tim_setchannel(FAR struct stm32_tim_dev_s *dev, uint8_t channel, stm32_tim_channel_t mode) { - uint16_t ccmr_val = 0; - uint16_t ccer_val = stm32_tim_getreg(dev, STM32_GTIM_CCER_OFFSET); - uint8_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET; - - ASSERT(dev); - - /* Further we use range as 0..3; if channel=0 it will also overflow here */ - - if (--channel > 4) return ERROR; - - /* Assume that channel is disabled and polarity is active high */ - - ccer_val &= ~(3 << (channel << 2)); - - /* This function is not supported on basic timers. To enable or - * disable it, simply set its clock to valid frequency or zero. - */ - + uint16_t ccmr_val = 0; + uint16_t ccer_val = stm32_getreg16(dev, STM32_GTIM_CCER_OFFSET); + uint8_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET; + + ASSERT(dev); + + /* Further we use range as 0..3; if channel=0 it will also overflow here */ + + if (--channel > 4) return ERROR; + + /* Assume that channel is disabled and polarity is active high */ + + ccer_val &= ~(3 << (channel << 2)); + + /* This function is not supported on basic timers. To enable or + * disable it, simply set its clock to valid frequency or zero. + */ + #if STM32_NBTIM > 0 - if ( ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM6_BASE + if (((struct stm32_tim_priv_s *)dev)->base == STM32_TIM6_BASE #endif #if STM32_NBTIM > 1 - || ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM7_BASE + || ((struct stm32_tim_priv_s *)dev)->base == STM32_TIM7_BASE #endif #if STM32_NBTIM > 0 - ) return ERROR; -#endif - - /* Decode configuration */ - - switch(mode & STM32_TIM_CH_MODE_MASK) { - - case STM32_TIM_CH_DISABLED: - break; - - case STM32_TIM_CH_OUTPWM: - ccmr_val = (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) + ATIM_CCMR1_OC1PE; - ccer_val |= ATIM_CCER_CC1E << (channel << 2); - break; - - default: - return ERROR; + ) + { + return ERROR; + } +#endif + + /* Decode configuration */ + + switch (mode & STM32_TIM_CH_MODE_MASK) + { + case STM32_TIM_CH_DISABLED: + break; + + case STM32_TIM_CH_OUTPWM: + ccmr_val = (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) + ATIM_CCMR1_OC1PE; + ccer_val |= ATIM_CCER_CC1E << (channel << 2); + break; + + default: + return ERROR; } - - /* Set polarity */ - - if (mode & STM32_TIM_CH_POLARITY_NEG) - ccer_val |= ATIM_CCER_CC1P << (channel << 2); - - /* define its position (shift) and get register offset */ - - if (channel & 1) ccmr_val <<= 8; - if (channel > 1) ccmr_offset = STM32_GTIM_CCMR2_OFFSET; - - stm32_tim_putreg(dev, ccmr_offset, ccmr_val); - stm32_tim_putreg(dev, STM32_GTIM_CCER_OFFSET, ccer_val); - - /* set GPIO */ - - switch( ((struct stm32_tim_priv_s *)dev)->base ) { + + /* Set polarity */ + + if (mode & STM32_TIM_CH_POLARITY_NEG) + { + ccer_val |= ATIM_CCER_CC1P << (channel << 2); + } + + /* Define its position (shift) and get register offset */ + + if (channel & 1) + { + ccmr_val <<= 8; + } + + if (channel > 1) + { + ccmr_offset = STM32_GTIM_CCMR2_OFFSET; + } + + stm32_putreg16(dev, ccmr_offset, ccmr_val); + stm32_putreg16(dev, STM32_GTIM_CCER_OFFSET, ccer_val); + + /* set GPIO */ + + switch (((struct stm32_tim_priv_s *)dev)->base) + { #if CONFIG_STM32_TIM2 - case STM32_TIM2_BASE: - switch(channel) { + case STM32_TIM2_BASE: + switch (channel) + { #if defined(GPIO_TIM2_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM2_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM2_CH1OUT, mode); + break; #endif #if defined(GPIO_TIM2_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM2_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM2_CH2OUT, mode); + break; #endif #if defined(GPIO_TIM2_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM2_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM2_CH3OUT, mode); + break; #endif #if defined(GPIO_TIM2_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM2_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM2_CH4OUT, mode); + break; #endif - default: return ERROR; - } - break; + default: + return ERROR; + } + break; #endif #if CONFIG_STM32_TIM3 - case STM32_TIM3_BASE: - switch(channel) { + case STM32_TIM3_BASE: + switch (channel) + { #if defined(GPIO_TIM3_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM3_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM3_CH1OUT, mode); + break; #endif #if defined(GPIO_TIM3_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM3_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM3_CH2OUT, mode); + break; #endif #if defined(GPIO_TIM3_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM3_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM3_CH3OUT, mode); + break; #endif #if defined(GPIO_TIM3_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM3_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM3_CH4OUT, mode); + break; #endif - default: return ERROR; - } - break; + default: + return ERROR; + } + break; #endif #if CONFIG_STM32_TIM4 - case STM32_TIM4_BASE: - switch(channel) { + case STM32_TIM4_BASE: + switch (channel) + { #if defined(GPIO_TIM4_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM4_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM4_CH1OUT, mode); + break; #endif #if defined(GPIO_TIM4_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM4_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM4_CH2OUT, mode); + break; #endif #if defined(GPIO_TIM4_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM4_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM4_CH3OUT, mode); + break; #endif #if defined(GPIO_TIM4_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM4_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM4_CH4OUT, mode); + break; #endif - default: return ERROR; - } - break; + default: return ERROR; + } + break; #endif #if CONFIG_STM32_TIM5 - case STM32_TIM5_BASE: - switch(channel) { + case STM32_TIM5_BASE: + switch (channel) + { #if defined(GPIO_TIM5_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM5_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM5_CH1OUT, mode); + break; #endif #if defined(GPIO_TIM5_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM5_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM5_CH2OUT, mode); + break; #endif #if defined(GPIO_TIM5_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM5_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM5_CH3OUT, mode); + break; #endif #if defined(GPIO_TIM5_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM5_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM5_CH4OUT, mode); + break; #endif - default: return ERROR; - } - break; + default: return ERROR; + } + break; #endif #if STM32_NATIM > 0 #if CONFIG_STM32_TIM1 - case STM32_TIM1_BASE: - switch(channel) { + case STM32_TIM1_BASE: + switch (channel) + { #if defined(GPIO_TIM1_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM1_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM1_CH1OUT, mode); break; #endif #if defined(GPIO_TIM1_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM1_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM1_CH2OUT, mode); break; #endif #if defined(GPIO_TIM1_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM1_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM1_CH3OUT, mode); break; #endif #if defined(GPIO_TIM1_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM1_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM1_CH4OUT, mode); break; #endif - default: return ERROR; - } - break; + default: return ERROR; + } + break; #endif #if CONFIG_STM32_TIM8 - case STM32_TIM8_BASE: - switch(channel) { + case STM32_TIM8_BASE: + switch (channel) + { #if defined(GPIO_TIM8_CH1OUT) - case 0: stm32_tim_gpioconfig(GPIO_TIM8_CH1OUT, mode); break; + case 0: + stm32_tim_gpioconfig(GPIO_TIM8_CH1OUT, mode); break; #endif #if defined(GPIO_TIM8_CH2OUT) - case 1: stm32_tim_gpioconfig(GPIO_TIM8_CH2OUT, mode); break; + case 1: + stm32_tim_gpioconfig(GPIO_TIM8_CH2OUT, mode); break; #endif #if defined(GPIO_TIM8_CH3OUT) - case 2: stm32_tim_gpioconfig(GPIO_TIM8_CH3OUT, mode); break; + case 2: + stm32_tim_gpioconfig(GPIO_TIM8_CH3OUT, mode); break; #endif #if defined(GPIO_TIM8_CH4OUT) - case 3: stm32_tim_gpioconfig(GPIO_TIM8_CH4OUT, mode); break; + case 3: + stm32_tim_gpioconfig(GPIO_TIM8_CH4OUT, mode); break; #endif - default: return ERROR; - } - break; + default: + return ERROR; + } + break; #endif #endif - default: return ERROR; + default: + return ERROR; } - - return OK; + + return OK; } -static int stm32_tim_setcompare(FAR struct stm32_tim_dev_s *dev, uint8_t channel, uint16_t compare) +static int stm32_tim_setcompare(FAR struct stm32_tim_dev_s *dev, uint8_t channel, uint32_t compare) { - ASSERT(dev); - - switch(channel) { - case 1: stm32_tim_putreg(dev, STM32_GTIM_CCR1_OFFSET, compare); break; - case 2: stm32_tim_putreg(dev, STM32_GTIM_CCR2_OFFSET, compare); break; - case 3: stm32_tim_putreg(dev, STM32_GTIM_CCR3_OFFSET, compare); break; - case 4: stm32_tim_putreg(dev, STM32_GTIM_CCR4_OFFSET, compare); break; - default: return ERROR; + ASSERT(dev); + + switch (channel) + { + case 1: + stm32_putreg32(dev, STM32_GTIM_CCR1_OFFSET, compare); + break; + case 2: + stm32_putreg32(dev, STM32_GTIM_CCR2_OFFSET, compare); + break; + case 3: + stm32_putreg32(dev, STM32_GTIM_CCR3_OFFSET, compare); + break; + case 4: + stm32_putreg32(dev, STM32_GTIM_CCR4_OFFSET, compare); + break; + default: + return ERROR; } - return OK; + return OK; } static int stm32_tim_getcapture(FAR struct stm32_tim_dev_s *dev, uint8_t channel) { - ASSERT(dev); - - switch(channel) { - case 1: return stm32_tim_getreg(dev, STM32_GTIM_CCR1_OFFSET); - case 2: return stm32_tim_getreg(dev, STM32_GTIM_CCR2_OFFSET); - case 3: return stm32_tim_getreg(dev, STM32_GTIM_CCR3_OFFSET); - case 4: return stm32_tim_getreg(dev, STM32_GTIM_CCR4_OFFSET); + ASSERT(dev); + + switch (channel) + { + case 1: + return stm32_getreg32(dev, STM32_GTIM_CCR1_OFFSET); + case 2: + return stm32_getreg32(dev, STM32_GTIM_CCR2_OFFSET); + case 3: + return stm32_getreg32(dev, STM32_GTIM_CCR3_OFFSET); + case 4: + return stm32_getreg32(dev, STM32_GTIM_CCR4_OFFSET); } - return ERROR; + return ERROR; } /************************************************************************************ * Advanced Functions ************************************************************************************/ -/** \todo Advanced functions for the STM32_ATIM */ - +/* TODO: Advanced functions for the STM32_ATIM */ /************************************************************************************ * Device Structures, Instantiation ************************************************************************************/ -struct stm32_tim_ops_s stm32_tim_ops = { - .setmode = &stm32_tim_setmode, - .setclock = &stm32_tim_setclock, - .setperiod = &stm32_tim_setperiod, - .setchannel = &stm32_tim_setchannel, - .setcompare = &stm32_tim_setcompare, - .getcapture = &stm32_tim_getcapture, - .setisr = &stm32_tim_setisr, - .enableint = &stm32_tim_enableint, - .disableint = &stm32_tim_disableint, - .ackint = &stm32_tim_ackint +struct stm32_tim_ops_s stm32_tim_ops = +{ + .setmode = &stm32_tim_setmode, + .setclock = &stm32_tim_setclock, + .setperiod = &stm32_tim_setperiod, + .setchannel = &stm32_tim_setchannel, + .setcompare = &stm32_tim_setcompare, + .getcapture = &stm32_tim_getcapture, + .setisr = &stm32_tim_setisr, + .enableint = &stm32_tim_enableint, + .disableint = &stm32_tim_disableint, + .ackint = &stm32_tim_ackint }; #if CONFIG_STM32_TIM2 -struct stm32_tim_priv_s stm32_tim2_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM2_BASE, +struct stm32_tim_priv_s stm32_tim2_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM2_BASE, }; #endif #if CONFIG_STM32_TIM3 -struct stm32_tim_priv_s stm32_tim3_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM3_BASE, +struct stm32_tim_priv_s stm32_tim3_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM3_BASE, }; #endif #if CONFIG_STM32_TIM4 -struct stm32_tim_priv_s stm32_tim4_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM4_BASE, +struct stm32_tim_priv_s stm32_tim4_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM4_BASE, }; #endif #if CONFIG_STM32_TIM5 -struct stm32_tim_priv_s stm32_tim5_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM5_BASE, +struct stm32_tim_priv_s stm32_tim5_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM5_BASE, }; #endif #if STM32_NBTIM > 0 #if CONFIG_STM32_TIM6 -struct stm32_tim_priv_s stm32_tim6_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM6_BASE, +struct stm32_tim_priv_s stm32_tim6_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM6_BASE, }; #endif #endif #if STM32_NBTIM > 1 #if CONFIG_STM32_TIM7 -struct stm32_tim_priv_s stm32_tim7_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM7_BASE, +struct stm32_tim_priv_s stm32_tim7_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM7_BASE, }; #endif #endif @@ -684,152 +828,175 @@ struct stm32_tim_priv_s stm32_tim7_priv = { #if STM32_NATIM > 0 #if CONFIG_STM32_TIM1 -struct stm32_tim_priv_s stm32_tim1_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM1_BASE, +struct stm32_tim_priv_s stm32_tim1_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM1_BASE, }; #endif #if CONFIG_STM32_TIM8 -struct stm32_tim_priv_s stm32_tim8_priv = { - .ops = &stm32_tim_ops, - .mode = STM32_TIM_MODE_UNUSED, - .base = STM32_TIM8_BASE, +struct stm32_tim_priv_s stm32_tim8_priv = +{ + .ops = &stm32_tim_ops, + .mode = STM32_TIM_MODE_UNUSED, + .base = STM32_TIM8_BASE, }; #endif #endif - /************************************************************************************ * Public Function - Initialization ************************************************************************************/ -FAR struct stm32_tim_dev_s * stm32_tim_init(int timer) +FAR struct stm32_tim_dev_s *stm32_tim_init(int timer) { - struct stm32_tim_dev_s * dev = NULL; - - /* Get structure and enable power */ - - switch(timer) { + struct stm32_tim_dev_s *dev = NULL; + + /* Get structure and enable power */ + + switch (timer) + { #if CONFIG_STM32_TIM2 - case 2: - dev = (struct stm32_tim_dev_s *)&stm32_tim2_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM2EN); - break; + case 2: + dev = (struct stm32_tim_dev_s *)&stm32_tim2_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM2EN); + break; #endif #if CONFIG_STM32_TIM3 - case 3: - dev = (struct stm32_tim_dev_s *)&stm32_tim3_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM3EN); - break; + case 3: + dev = (struct stm32_tim_dev_s *)&stm32_tim3_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM3EN); + break; #endif #if CONFIG_STM32_TIM4 - case 4: - dev = (struct stm32_tim_dev_s *)&stm32_tim4_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM4EN); - break; + case 4: + dev = (struct stm32_tim_dev_s *)&stm32_tim4_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM4EN); + break; #endif #if CONFIG_STM32_TIM5 - case 5: - dev = (struct stm32_tim_dev_s *)&stm32_tim5_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM5EN); - break; + case 5: + dev = (struct stm32_tim_dev_s *)&stm32_tim5_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM5EN); + break; #endif #if STM32_NBTIM > 0 #if CONFIG_STM32_TIM6 - case 6: - dev = (struct stm32_tim_dev_s *)&stm32_tim6_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM6EN); - break; + case 6: + dev = (struct stm32_tim_dev_s *)&stm32_tim6_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM6EN); + break; #endif #endif #if STM32_NBTIM > 1 #if CONFIG_STM32_TIM7 - case 7: - dev = (struct stm32_tim_dev_s *)&stm32_tim7_priv; - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM7EN); - break; + case 7: + dev = (struct stm32_tim_dev_s *)&stm32_tim7_priv; + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM7EN); + break; #endif #endif #if STM32_NATIM > 0 #if CONFIG_STM32_TIM1 - case 1: - dev = (struct stm32_tim_dev_s *)&stm32_tim1_priv; - modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); - break; + case 1: + dev = (struct stm32_tim_dev_s *)&stm32_tim1_priv; + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + break; #endif #if CONFIG_STM32_TIM8 - case 8: - dev = (struct stm32_tim_dev_s *)&stm32_tim8_priv; - modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM8EN); - break; + case 8: + dev = (struct stm32_tim_dev_s *)&stm32_tim8_priv; + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM8EN); + break; #endif #endif - default: return NULL; - } - - /* Is device already allocated */ - - if ( ((struct stm32_tim_priv_s *)dev)->mode != STM32_TIM_MODE_UNUSED) + default: return NULL; - - stm32_tim_reset(dev); - - return dev; + } + + /* Is device already allocated */ + + if (((struct stm32_tim_priv_s *)dev)->mode != STM32_TIM_MODE_UNUSED) + { + return NULL; + } + + stm32_tim_reset(dev); + + return dev; } +/* TODO: Detach interrupts, and close down all TIM Channels */ -/** \todo Detach interrupts, and close down all TIM Channels */ int stm32_tim_deinit(FAR struct stm32_tim_dev_s * dev) { - ASSERT(dev); - - /* Disable power */ - - switch( ((struct stm32_tim_priv_s *)dev)->base ) { + ASSERT(dev); + + /* Disable power */ + + switch (((struct stm32_tim_priv_s *)dev)->base) + { #if CONFIG_STM32_TIM2 - case STM32_TIM2_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM2EN, 0); break; + case STM32_TIM2_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM2EN, 0); + break; #endif #if CONFIG_STM32_TIM3 - case STM32_TIM3_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM3EN, 0); break; + case STM32_TIM3_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM3EN, 0); + break; #endif #if CONFIG_STM32_TIM4 - case STM32_TIM4_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM4EN, 0); break; + case STM32_TIM4_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM4EN, 0); + break; #endif #if CONFIG_STM32_TIM5 - case STM32_TIM5_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM5EN, 0); break; + case STM32_TIM5_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM5EN, 0); + break; #endif #if STM32_NBTIM > 0 #if CONFIG_STM32_TIM6 - case STM32_TIM6_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM6EN, 0); break; + case STM32_TIM6_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM6EN, 0); + break; #endif #endif #if STM32_NBTIM > 1 #if CONFIG_STM32_TIM7 - case STM32_TIM7_BASE: modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM7EN, 0); break; + case STM32_TIM7_BASE: + modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_TIM7EN, 0); + break; #endif #endif - + #if STM32_NATIM > 0 #if CONFIG_STM32_TIM1 - case STM32_TIM1_BASE: modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_TIM1EN, 0); break; + case STM32_TIM1_BASE: + modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_TIM1EN, 0); + break; #endif #if CONFIG_STM32_TIM8 - case STM32_TIM8_BASE: modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_TIM8EN, 0); break; + case STM32_TIM8_BASE: + modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_TIM8EN, 0); + break; #endif #endif - default: return ERROR; + default: + return ERROR; } - - /* Mark it as free */ - - ((struct stm32_tim_priv_s *)dev)->mode = STM32_TIM_MODE_UNUSED; - - return OK; + + /* Mark it as free */ + + ((struct stm32_tim_priv_s *)dev)->mode = STM32_TIM_MODE_UNUSED; + + return OK; } #endif /* defined(CONFIG_STM32_TIM1 || ... || TIM8) */ diff --git a/nuttx/arch/arm/src/stm32/stm32_tim.h b/nuttx/arch/arm/src/stm32/stm32_tim.h index 8f12b1875..081a3489f 100644 --- a/nuttx/arch/arm/src/stm32/stm32_tim.h +++ b/nuttx/arch/arm/src/stm32/stm32_tim.h @@ -6,7 +6,7 @@ * * With modifications and updates by: * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -92,7 +92,7 @@ struct stm32_tim_dev_s typedef enum { STM32_TIM_MODE_UNUSED = -1, - + /* One of the following */ STM32_TIM_MODE_MASK = 0x0310, @@ -101,7 +101,7 @@ typedef enum STM32_TIM_MODE_DOWN = 0x0110, STM32_TIM_MODE_UPDOWN = 0x0200, STM32_TIM_MODE_PULSE = 0x0300, - + /* One of the following */ STM32_TIM_MODE_CK_INT = 0x0000, @@ -116,9 +116,9 @@ typedef enum //STM32_TIM_MODE_CK_CH2 = 0x0002, //STM32_TIM_MODE_CK_CH3 = 0x0003, //STM32_TIM_MODE_CK_CH4 = 0x0004 - + /* Todo: external trigger block */ - + } stm32_tim_mode_t; /* TIM Channel Modes */ @@ -126,26 +126,26 @@ typedef enum typedef enum { STM32_TIM_CH_DISABLED = 0x00, - + /* Common configuration */ STM32_TIM_CH_POLARITY_POS = 0x00, STM32_TIM_CH_POLARITY_NEG = 0x01, - + /* MODES: */ STM32_TIM_CH_MODE_MASK = 0x06, - + /* Output Compare Modes */ STM32_TIM_CH_OUTPWM = 0x04, /** Enable standard PWM mode, active high when counter < compare */ //STM32_TIM_CH_OUTCOMPARE = 0x06, - + // TODO other modes ... as PWM capture, ENCODER and Hall Sensor //STM32_TIM_CH_INCAPTURE = 0x10, //STM32_TIM_CH_INPWM = 0x20 //STM32_TIM_CH_DRIVE_OC -- open collector mode - + } stm32_tim_channel_t; /* TIM Operations */ @@ -156,12 +156,12 @@ struct stm32_tim_ops_s int (*setmode)(FAR struct stm32_tim_dev_s *dev, stm32_tim_mode_t mode); int (*setclock)(FAR struct stm32_tim_dev_s *dev, uint32_t freq); - void (*setperiod)(FAR struct stm32_tim_dev_s *dev, uint16_t period); - + void (*setperiod)(FAR struct stm32_tim_dev_s *dev, uint32_t period); + /* General and Advanced Timers Adds */ - + int (*setchannel)(FAR struct stm32_tim_dev_s *dev, uint8_t channel, stm32_tim_channel_t mode); - int (*setcompare)(FAR struct stm32_tim_dev_s *dev, uint8_t channel, uint16_t compare); + int (*setcompare)(FAR struct stm32_tim_dev_s *dev, uint8_t channel, uint32_t compare); int (*getcapture)(FAR struct stm32_tim_dev_s *dev, uint8_t channel); int (*setisr)(FAR struct stm32_tim_dev_s *dev, int (*handler)(int irq, void *context), int source); diff --git a/nuttx/drivers/input/tsc2007.c b/nuttx/drivers/input/tsc2007.c index 3afe926fe..52f8652a7 100644 --- a/nuttx/drivers/input/tsc2007.c +++ b/nuttx/drivers/input/tsc2007.c @@ -257,13 +257,13 @@ static void tsc2007_notify(FAR struct tsc2007_dev_s *priv) if (priv->nwaiters > 0) { /* After posting this semaphore, we need to exit because the TSC2007 - * is no longer avaialable. + * is no longer available. */ sem_post(&priv->waitsem); } - /* If there are threads waiting on poll() for TSC2007 data to become availabe, + /* If there are threads waiting on poll() for TSC2007 data to become available, * then wake them up now. NOTE: we wake up all waiting threads because we * do not know that they are going to do. If they all try to read the data, * then some make end up blocking after all. diff --git a/nuttx/include/stdlib.h b/nuttx/include/stdlib.h index febdd7025..b9b9346b5 100644 --- a/nuttx/include/stdlib.h +++ b/nuttx/include/stdlib.h @@ -144,12 +144,12 @@ EXTERN unsigned long long strtoull(const char *, char **, int); #endif EXTERN double_t strtod(const char *, char **); -#define atoi(nptr) strtol((nptr), NULL, 10); -#define atol(nptr) strtol((nptr), NULL, 10); +#define atoi(nptr) strtol((nptr), NULL, 10) +#define atol(nptr) strtol((nptr), NULL, 10) #ifdef CONFIG_HAVE_LONG_LONG -#define atoll(nptr) strtoll((nptr), NULL, 10); +#define atoll(nptr) strtoll((nptr), NULL, 10) #endif -#define atof(nptr) strtod((nptr), NULL); +#define atof(nptr) strtod((nptr), NULL) /* Memory Management */ diff --git a/nuttx/lib/stdio/lib_libvsprintf.c b/nuttx/lib/stdio/lib_libvsprintf.c index 3bc112e92..341c055c6 100644 --- a/nuttx/lib/stdio/lib_libvsprintf.c +++ b/nuttx/lib/stdio/lib_libvsprintf.c @@ -529,6 +529,21 @@ static int getusize(uint8_t fmt, uint8_t flags, unsigned int n) utoascii(&nulloutstream, fmt, flags, n); return nulloutstream.nput; } + +/**************************************************************************** + * Name: getdblsize + ****************************************************************************/ + +#ifdef CONFIG_LIBC_FLOATINGPOINT +static int getdblsize(uint8_t fmt, int trunc, uint8_t flags, double n) +{ + struct lib_outstream_s nulloutstream; + lib_nulloutstream(&nulloutstream); + + lib_dtoa(&nulloutstream, fmt, trunc, flags, n); + return nulloutstream.nput; +} +#endif #endif /* CONFIG_NOPRINTF_FIELDWIDTH */ #ifdef CONFIG_LONG_IS_NOT_INT @@ -1535,9 +1550,30 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a else if (strchr("eEfgG", FMT_CHAR)) { double dblval = va_arg(ap, double); + +#ifndef CONFIG_NOPRINTF_FIELDWIDTH + int dblsize; + + /* Get the width of the output */ + + dblsize = getdblsize(FMT_CHAR, trunc, flags, dblval); + + /* Perform left field justification actions */ + + prejustify(obj, fmt, flags, width, dblsize); +#endif + + /* Output the number */ + lib_dtoa(obj, FMT_CHAR, trunc, flags, dblval); + +#ifndef CONFIG_NOPRINTF_FIELDWIDTH + /* Perform right field justification actions */ + + postjustify(obj, fmt, flags, width, dblsize); } #endif +#endif /* CONFIG_LIBC_FLOATINGPOINT */ } return obj->nput; -- cgit v1.2.3