1
0
Fork 0
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:
jflyper 2019-06-11 10:38:06 +09:00
parent 2a6e94d030
commit 8ceaed1072
8 changed files with 155 additions and 17 deletions

View file

@ -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;
}

View file

@ -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

View file

@ -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);

View file

@ -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);
}

View file

@ -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)

View file

@ -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
View 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
View 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