mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 19:40:31 +03:00
[H7] Add pseudo peripheral TIMUP for TIMx_UP DMA management
This commit is contained in:
parent
2a6e94d030
commit
8ceaed1072
8 changed files with 155 additions and 17 deletions
|
@ -140,6 +140,7 @@ uint8_t cliMode = 0;
|
|||
#include "pg/serial_uart.h"
|
||||
#include "pg/sdio.h"
|
||||
#include "pg/timerio.h"
|
||||
#include "pg/timerup.h"
|
||||
#include "pg/usb.h"
|
||||
#include "pg/vtx_table.h"
|
||||
|
||||
|
@ -4868,6 +4869,7 @@ typedef struct dmaoptEntry_s {
|
|||
uint8_t stride;
|
||||
uint8_t offset;
|
||||
uint8_t maxIndex;
|
||||
uint32_t presenceMask;
|
||||
} dmaoptEntry_t;
|
||||
|
||||
// Handy macros for keeping the table tidy.
|
||||
|
@ -4876,21 +4878,26 @@ typedef struct dmaoptEntry_s {
|
|||
// DEFW : Wider stride case; array of structs.
|
||||
|
||||
#define DEFS(device, peripheral, pgn, type, member) \
|
||||
{ device, peripheral, pgn, 0, offsetof(type, member), 0 }
|
||||
{ device, peripheral, pgn, 0, offsetof(type, member), 0, 0 }
|
||||
|
||||
#define DEFA(device, peripheral, pgn, type, member, max) \
|
||||
{ device, peripheral, pgn, sizeof(uint8_t), offsetof(type, member), max }
|
||||
#define DEFA(device, peripheral, pgn, type, member, max, mask) \
|
||||
{ device, peripheral, pgn, sizeof(uint8_t), offsetof(type, member), max, mask }
|
||||
|
||||
#define DEFW(device, peripheral, pgn, type, member, max) \
|
||||
{ device, peripheral, pgn, sizeof(type), offsetof(type, member), max }
|
||||
#define DEFW(device, peripheral, pgn, type, member, max, mask) \
|
||||
{ device, peripheral, pgn, sizeof(type), offsetof(type, member), max, mask }
|
||||
|
||||
#define MASK_IGNORED (0)
|
||||
|
||||
dmaoptEntry_t dmaoptEntryTable[] = {
|
||||
DEFW("SPI_TX", DMA_PERIPH_SPI_TX, PG_SPI_PIN_CONFIG, spiPinConfig_t, txDmaopt, SPIDEV_COUNT),
|
||||
DEFW("SPI_RX", DMA_PERIPH_SPI_RX, PG_SPI_PIN_CONFIG, spiPinConfig_t, rxDmaopt, SPIDEV_COUNT),
|
||||
DEFA("ADC", DMA_PERIPH_ADC, PG_ADC_CONFIG, adcConfig_t, dmaopt, ADCDEV_COUNT),
|
||||
DEFW("SPI_TX", DMA_PERIPH_SPI_TX, PG_SPI_PIN_CONFIG, spiPinConfig_t, txDmaopt, SPIDEV_COUNT, MASK_IGNORED),
|
||||
DEFW("SPI_RX", DMA_PERIPH_SPI_RX, PG_SPI_PIN_CONFIG, spiPinConfig_t, rxDmaopt, SPIDEV_COUNT, MASK_IGNORED),
|
||||
DEFA("ADC", DMA_PERIPH_ADC, PG_ADC_CONFIG, adcConfig_t, dmaopt, ADCDEV_COUNT, MASK_IGNORED),
|
||||
DEFS("SDIO", DMA_PERIPH_SDIO, PG_SDIO_CONFIG, sdioConfig_t, dmaopt),
|
||||
DEFW("UART_TX", DMA_PERIPH_UART_TX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, txDmaopt, UARTDEV_CONFIG_MAX),
|
||||
DEFW("UART_RX", DMA_PERIPH_UART_RX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, rxDmaopt, UARTDEV_CONFIG_MAX),
|
||||
DEFW("UART_TX", DMA_PERIPH_UART_TX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, txDmaopt, UARTDEV_CONFIG_MAX, MASK_IGNORED),
|
||||
DEFW("UART_RX", DMA_PERIPH_UART_RX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, rxDmaopt, UARTDEV_CONFIG_MAX, MASK_IGNORED),
|
||||
#ifdef STM32H7
|
||||
DEFW("TIMUP", DMA_PERIPH_TIMUP, PG_TIMER_UP_CONFIG, timerUpConfig_t, dmaopt, HARDWARE_TIMER_DEFINITION_COUNT, TIMUP_TIMERS),
|
||||
#endif
|
||||
};
|
||||
|
||||
#undef DEFS
|
||||
|
@ -5128,7 +5135,7 @@ static void cliDmaopt(char *cmdline)
|
|||
pch = strtok_r(NULL, " ", &saveptr);
|
||||
if (entry) {
|
||||
index = atoi(pch) - 1;
|
||||
if (index < 0 || index >= entry->maxIndex) {
|
||||
if (index < 0 || index >= entry->maxIndex || !((entry->presenceMask != MASK_IGNORED) && (entry->presenceMask & BIT(index + 1)))) {
|
||||
cliPrintErrorLinef("BAD INDEX: '%s'", pch ? pch : "");
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -59,6 +59,7 @@ typedef struct dmaTimerMapping_s {
|
|||
#define REQMAP_SGL(periph) { DMA_PERIPH_ ## periph, 0, DMA_REQUEST_ ## periph }
|
||||
#define REQMAP(periph, device) { DMA_PERIPH_ ## periph, periph ## DEV_ ## device, DMA_REQUEST_ ## periph ## device }
|
||||
#define REQMAP_DIR(periph, device, dir) { DMA_PERIPH_ ## periph ## _ ## dir, periph ## DEV_ ## device, DMA_REQUEST_ ## periph ## device ## _ ## dir }
|
||||
#define REQMAP_TIMUP(periph, timno) { DMA_PERIPH_TIMUP, timno - 1, DMA_REQUEST_ ## TIM ## timno ## _UP }
|
||||
|
||||
// Resolve UART/UART mess
|
||||
#define DMA_REQUEST_UART1_RX DMA_REQUEST_USART1_RX
|
||||
|
@ -114,8 +115,24 @@ static const dmaPeripheralMapping_t dmaPeripheralMapping[] = {
|
|||
REQMAP_DIR(UART, 8, TX),
|
||||
REQMAP_DIR(UART, 8, RX),
|
||||
#endif
|
||||
|
||||
#ifdef USE_TIMER
|
||||
// Pseudo peripheral for TIMx_UP channel
|
||||
REQMAP_TIMUP(TIMUP, 1),
|
||||
REQMAP_TIMUP(TIMUP, 2),
|
||||
REQMAP_TIMUP(TIMUP, 3),
|
||||
REQMAP_TIMUP(TIMUP, 4),
|
||||
REQMAP_TIMUP(TIMUP, 5),
|
||||
REQMAP_TIMUP(TIMUP, 6),
|
||||
REQMAP_TIMUP(TIMUP, 7),
|
||||
REQMAP_TIMUP(TIMUP, 8),
|
||||
REQMAP_TIMUP(TIMUP, 15),
|
||||
REQMAP_TIMUP(TIMUP, 16),
|
||||
REQMAP_TIMUP(TIMUP, 17),
|
||||
#endif
|
||||
};
|
||||
|
||||
#undef REQMAP_TIMUP
|
||||
#undef REQMAP
|
||||
#undef REQMAP_SGL
|
||||
#undef REQMAP_DIR
|
||||
|
@ -473,4 +490,19 @@ dmaoptValue_t dmaGetOptionByTimer(const timerHardware_t *timer)
|
|||
|
||||
return DMA_OPT_UNUSED;
|
||||
}
|
||||
|
||||
#if defined(STM32H7)
|
||||
// A variant of dmaGetOptionByTimer that looks for matching dmaTimUPRef
|
||||
dmaoptValue_t dmaGetUpOptionByTimer(const timerHardware_t *timer)
|
||||
{
|
||||
for (unsigned opt = 0; opt < ARRAYLEN(dmaChannelSpec); opt++) {
|
||||
if (timer->dmaTimUPRef == dmaChannelSpec[opt].ref) {
|
||||
return (dmaoptValue_t)opt;
|
||||
}
|
||||
}
|
||||
|
||||
return DMA_OPT_UNUSED;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // USE_DMA_SPEC
|
||||
|
|
|
@ -49,6 +49,7 @@ typedef enum {
|
|||
DMA_PERIPH_SDIO,
|
||||
DMA_PERIPH_UART_TX,
|
||||
DMA_PERIPH_UART_RX,
|
||||
DMA_PERIPH_TIMUP,
|
||||
} dmaPeripheral_e;
|
||||
|
||||
typedef int8_t dmaoptValue_t;
|
||||
|
@ -68,3 +69,4 @@ const dmaChannelSpec_t *dmaGetChannelSpecByPeripheral(dmaPeripheral_e device, ui
|
|||
const dmaChannelSpec_t *dmaGetChannelSpecByTimerValue(TIM_TypeDef *tim, uint8_t channel, dmaoptValue_t dmaopt);
|
||||
const dmaChannelSpec_t *dmaGetChannelSpecByTimer(const timerHardware_t *timer);
|
||||
dmaoptValue_t dmaGetOptionByTimer(const timerHardware_t *timer);
|
||||
dmaoptValue_t dmaGetUpOptionByTimer(const timerHardware_t *timer);
|
||||
|
|
|
@ -34,6 +34,7 @@
|
|||
#include "drivers/nvic.h"
|
||||
#include "dma.h"
|
||||
#include "rcc.h"
|
||||
#include "pg/timerup.h"
|
||||
|
||||
static HAL_StatusTypeDef result;
|
||||
|
||||
|
@ -252,8 +253,16 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
|||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
if (useBurstDshot) {
|
||||
#ifdef USE_DMA_SPEC
|
||||
uint8_t timnum = timerGetTIMNumber(timerHardware->tim);
|
||||
dmaoptValue_t dmaopt = timerUpConfig(timnum - 1)->dmaopt;
|
||||
const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_TIMUP, timnum - 1, dmaopt);
|
||||
dmaRef = dmaChannelSpec->ref;
|
||||
dmaChannel = dmaChannelSpec->channel;
|
||||
#else
|
||||
dmaRef = timerHardware->dmaTimUPRef;
|
||||
dmaChannel = timerHardware->dmaTimUPChannel;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -369,8 +378,8 @@ P - High - High -
|
|||
memset(motor->timer->dmaBurstBuffer, 0, DSHOT_DMA_BUFFER_SIZE * 4 * sizeof(uint32_t));
|
||||
|
||||
/* Set hdma_tim instance */
|
||||
motor->timer->hdma_tim.Instance = timerHardware->dmaTimUPRef;
|
||||
motor->timer->hdma_tim.Init.Request = timerHardware->dmaTimUPChannel;
|
||||
motor->timer->hdma_tim.Instance = dmaRef;
|
||||
motor->timer->hdma_tim.Init.Request = dmaChannel;
|
||||
|
||||
/* Link hdma_tim to hdma[TIM_DMA_ID_UPDATE] (update) */
|
||||
__HAL_LINKDMA(&motor->timer->timHandle, hdma[TIM_DMA_ID_UPDATE], motor->timer->hdma_tim);
|
||||
|
@ -413,14 +422,14 @@ P - High - High -
|
|||
return;
|
||||
}
|
||||
|
||||
dmaIdentifier_e identifier = dmaGetIdentifier(dmaRef);
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
if (useBurstDshot) {
|
||||
dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
|
||||
dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), timerIndex);
|
||||
dmaInit(identifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
|
||||
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), timerIndex);
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
dmaIdentifier_e identifier = dmaGetIdentifier(dmaRef);
|
||||
dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
|
||||
}
|
||||
|
|
|
@ -170,6 +170,7 @@ typedef enum {
|
|||
#define HARDWARE_TIMER_DEFINITION_COUNT 14
|
||||
#elif defined(STM32H7)
|
||||
#define HARDWARE_TIMER_DEFINITION_COUNT 17
|
||||
#define TIMUP_TIMERS ( BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) | BIT(15) | BIT(16) | BIT(17) )
|
||||
#endif
|
||||
|
||||
#define MHZ_TO_HZ(x) ((x) * 1000000)
|
||||
|
@ -178,6 +179,7 @@ typedef enum {
|
|||
extern const timerHardware_t timerHardware[];
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(USE_TIMER_MGMT)
|
||||
#if defined(STM32F4)
|
||||
|
||||
|
|
|
@ -145,7 +145,8 @@
|
|||
#define PG_VTX_TABLE_CONFIG 546
|
||||
#define PG_STATS_CONFIG 547
|
||||
#define PG_QUADSPI_CONFIG 548
|
||||
#define PG_BETAFLIGHT_END 548
|
||||
#define PG_TIMER_UP_CONFIG 549 // used to store dmaopt for TIMx_UP channel
|
||||
#define PG_BETAFLIGHT_END 549
|
||||
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
|
|
49
src/main/pg/timerup.c
Normal file
49
src/main/pg/timerup.c
Normal file
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#if defined(USE_TIMER_MGMT) && defined(STM32H7)
|
||||
|
||||
#include "drivers/dma_reqmap.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#include "timerup.h"
|
||||
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(timerUpConfig_t, HARDWARE_TIMER_DEFINITION_COUNT, timerUpConfig, PG_TIMER_UP_CONFIG, 0);
|
||||
|
||||
void pgResetFn_timerUpConfig(timerUpConfig_t *config)
|
||||
{
|
||||
for (unsigned timno = 0; timno < HARDWARE_TIMER_DEFINITION_COUNT; timno++) {
|
||||
config[timno].dmaopt = DMA_OPT_UNUSED;
|
||||
}
|
||||
|
||||
#if !defined(USE_UNIFIED_TARGET)
|
||||
// Scan target timerHardware and extract dma option for TIMUP
|
||||
for (unsigned i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
||||
const timerHardware_t *timhw = &timerHardware[i];
|
||||
uint8_t timnum = timerGetTIMNumber(timhw->tim) - 1;
|
||||
config[timnum].dmaopt = dmaGetUpOptionByTimer(timhw);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
36
src/main/pg/timerup.h
Normal file
36
src/main/pg/timerup.h
Normal file
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/timer.h" // For HARDWARE_TIMER_DEFINITION_COUNT
|
||||
|
||||
#ifdef USE_TIMER_MGMT
|
||||
|
||||
typedef struct timerUpConfig_s {
|
||||
int8_t dmaopt;
|
||||
} timerUpConfig_t;
|
||||
|
||||
PG_DECLARE_ARRAY(timerUpConfig_t, HARDWARE_TIMER_DEFINITION_COUNT, timerUpConfig);
|
||||
#endif
|
Loading…
Add table
Add a link
Reference in a new issue