diff --git a/src/main/drivers/at32/timer_at32bsp.c b/src/main/drivers/at32/timer_at32bsp.c index d14c39e31c..0f059a9b23 100644 --- a/src/main/drivers/at32/timer_at32bsp.c +++ b/src/main/drivers/at32/timer_at32bsp.c @@ -696,11 +696,9 @@ void timerInit(void) } } -// finish configuring timers after allocation phase -// start timers -void timerStart(void) +void timerStart(tmr_type *tim) { - + tmr_counter_enable(tim, TRUE); } /** diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index d6f2d82307..3e53ffc186 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -111,7 +111,7 @@ typedef struct { escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS]; -extern timerHardware_t* serialTimerHardware; +//extern timerHardware_t* serialTimerHardware; const struct serialPortVTable escSerialVTable[]; @@ -135,11 +135,10 @@ enum { #define STOP_BIT_MASK (1 << 0) #define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1)) -// XXX No TIM_DeInit equivalent in HAL driver??? #ifdef USE_HAL_DRIVER static void TIM_DeInit(TIM_TypeDef *tim) { - UNUSED(tim); + LL_TIM_DeInit(tim); } #endif @@ -183,7 +182,7 @@ static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg) } IOInit(IOGetByTag(tag), OWNER_MOTOR, 0); -#ifdef STM32F7 +#if defined(STM32F7) || defined(STM32H7) IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction); #else IOConfigGPIO(IOGetByTag(tag), cfg); @@ -194,7 +193,7 @@ static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) { escSerialGPIOConfig(timerHardwarePtr, IOCFG_AF_PP_UP); timerChClearCCFlag(timerHardwarePtr); - timerChITConfig(timerHardwarePtr,ENABLE); + timerChITConfig(timerHardwarePtr, ENABLE); } static bool isTimerPeriodTooLarge(uint32_t timerPeriod) @@ -211,7 +210,7 @@ static bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance) static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr) { escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP); - timerChITConfig(timerHardwarePtr,DISABLE); + timerChITConfig(timerHardwarePtr, DISABLE); } static void processTxStateBL(escSerial_t *escSerial) @@ -346,7 +345,7 @@ static void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture processRxStateBL(escSerial); } -static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud) +static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t portIndex, uint32_t baud) { uint32_t clock = SystemCoreClock/2; uint32_t timerPeriod; @@ -363,9 +362,10 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 } } while (isTimerPeriodTooLarge(timerPeriod)); - timerConfigure(timerHardwarePtr, timerPeriod, clock); - timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL); - timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); + timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, clock); + timerChCCHandlerInit(&escSerialPorts[portIndex].timerCb, onSerialTimerBL); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[portIndex].timerCb, NULL); + timerStart(timerHardwarePtr->tim); } static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) @@ -421,10 +421,11 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2); + timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, SystemCoreClock / 2); timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); + timerStart(timerHardwarePtr->tim); } #ifdef USE_ESCSERIAL_SIMONK @@ -545,9 +546,10 @@ static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint { uint32_t timerPeriod = 34; TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1)); + timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, MHZ_TO_HZ(1)); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); + timerStart(timerHardwarePtr->tim); } static void extractAndStoreRxByteEsc(escSerial_t *escSerial) @@ -634,10 +636,11 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1)); + timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, MHZ_TO_HZ(1)); timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); + timerStart(timerHardwarePtr->tim); } #endif @@ -678,6 +681,9 @@ static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSeria #ifdef USE_HAL_DRIVER escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim); #endif + // Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below + // this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first. + timerConfigure(escSerial->rxTimerHardware, 0xffff, 1); } escSerial->mode = mode; @@ -690,6 +696,10 @@ static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSeria escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim); #endif + // Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below + // this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first. + timerConfigure(escSerial->txTimerHardware, 0xffff, 1); + escSerial->port.vTable = escSerialVTable; escSerial->port.baudRate = baud; escSerial->port.mode = MODE_RXTX; @@ -941,9 +951,7 @@ bool escEnablePassthrough(serialPort_t *escPassthroughPort, const motorDevConfig uint8_t motor_output = escIndex; LED0_OFF; LED1_OFF; - //StopPwmAllMotors(); - // XXX Review effect of motor refactor - //pwmDisableMotors(); + motorDisable(); passPort = escPassthroughPort; diff --git a/src/main/drivers/stm32/timer_hal.c b/src/main/drivers/stm32/timer_hal.c index 25cafafb49..f3862a6f3f 100644 --- a/src/main/drivers/stm32/timer_hal.c +++ b/src/main/drivers/stm32/timer_hal.c @@ -773,16 +773,24 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) break; } case __builtin_clz(TIM_IT_CC1): - timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1); + if (timerConfig->edgeCallback[0]) { + timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1); + } break; case __builtin_clz(TIM_IT_CC2): - timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2); + if (timerConfig->edgeCallback[1]) { + timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2); + } break; case __builtin_clz(TIM_IT_CC3): - timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3); + if (timerConfig->edgeCallback[2]) { + timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3); + } break; case __builtin_clz(TIM_IT_CC4): - timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4); + if (timerConfig->edgeCallback[3]) { + timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4); + } break; } } @@ -1043,35 +1051,12 @@ void timerInit(void) } } -// finish configuring timers after allocation phase -// start timers -// TODO - Work in progress - initialization routine must be modified/verified to start correctly without timers -void timerStart(void) +void timerStart(TIM_TypeDef *tim) { -#if 0 - for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { - int priority = -1; - int irq = -1; - for (unsigned hwc = 0; hwc < TIMER_CHANNEL_COUNT; hwc++) { - if ((timerChannelInfo[hwc].type != TYPE_FREE) && (TIMER_HARDWARE[hwc].tim == usedTimers[timer])) { - // TODO - move IRQ to timer info - irq = TIMER_HARDWARE[hwc].irq; - } - } - // TODO - aggregate required timer paramaters - configTimeBase(usedTimers[timer], 0, 1); - TIM_Cmd(usedTimers[timer], ENABLE); - if (priority >= 0) { // maybe none of the channels was configured - NVIC_InitTypeDef NVIC_InitStructure; + TIM_HandleTypeDef* handle = timerFindTimerHandle(tim); + if (handle == NULL) return; - NVIC_InitStructure.NVIC_IRQChannel = irq; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SPLIT_PRIORITY_BASE(priority); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_SPLIT_PRIORITY_SUB(priority); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - } - } -#endif + __HAL_TIM_ENABLE(handle); } /** diff --git a/src/main/drivers/stm32/timer_stdperiph.c b/src/main/drivers/stm32/timer_stdperiph.c index ec0e008c2d..4b809c81aa 100644 --- a/src/main/drivers/stm32/timer_stdperiph.c +++ b/src/main/drivers/stm32/timer_stdperiph.c @@ -321,6 +321,11 @@ void timerNVICConfigure(uint8_t irq) NVIC_Init(&NVIC_InitStructure); } +void timerReconfigureTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) +{ + configTimeBase(tim, period, hz); +} + void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; @@ -851,34 +856,9 @@ void timerInit(void) } } -// finish configuring timers after allocation phase -// start timers -// TODO - Work in progress - initialization routine must be modified/verified to start correctly without timers -void timerStart(void) +void timerStart(TIM_TypeDef *tim) { -#if 0 - for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { - int priority = -1; - int irq = -1; - for (unsigned hwc = 0; hwc < TIMER_CHANNEL_COUNT; hwc++) - if ((timerChannelInfo[hwc].type != TYPE_FREE) && (TIMER_HARDWARE[hwc].tim == usedTimers[timer])) { - // TODO - move IRQ to timer info - irq = TIMER_HARDWARE[hwc].irq; - } - // TODO - aggregate required timer parameters - configTimeBase(usedTimers[timer], 0, 1); - TIM_Cmd(usedTimers[timer], ENABLE); - if (priority >= 0) { // maybe none of the channels was configured - NVIC_InitTypeDef NVIC_InitStructure; - - NVIC_InitStructure.NVIC_IRQChannel = irq; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SPLIT_PRIORITY_BASE(priority); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_SPLIT_PRIORITY_SUB(priority); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - } - } -#endif + TIM_Cmd(tim, ENABLE); } /** diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index c7231b83d8..9854aec4b8 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -134,7 +134,13 @@ void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz); // Initialisation // void timerInit(void); -void timerStart(void); + +// +// per-timer +// + +// once-upon-a-time all the timers were started on boot, now they are started when needed. +void timerStart(TIM_TypeDef *tim); // // per-channel diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 14e0917e7e..cd055c638b 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -855,12 +855,6 @@ void init(void) #endif // VTX_CONTROL -#ifdef USE_TIMER - // start all timers - // TODO - not implemented yet - timerStart(); -#endif - batteryInit(); // always needs doing, regardless of features. #ifdef USE_RCDEVICE diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 4302bf20aa..a89f3bdd2d 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -358,10 +358,6 @@ void timerInit(void) printf("[timer]Init...\n"); } -void timerStart(void) -{ -} - void failureMode(failureMode_e mode) { printf("[failureMode]!!! %d\n", mode); diff --git a/src/main/target/STM32F7X2/target.h b/src/main/target/STM32F7X2/target.h index 62930a6be8..a12432653a 100644 --- a/src/main/target/STM32F7X2/target.h +++ b/src/main/target/STM32F7X2/target.h @@ -70,10 +70,6 @@ #define USE_USB_DETECT -#ifdef USE_ESCSERIAL -#undef USE_ESCSERIAL -#endif - #define USE_ADC #define USE_EXTI #define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors