diff --git a/mk/source.mk b/mk/source.mk index a9a8ed0e49..9b981c05cb 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -112,7 +112,6 @@ COMMON_SRC = \ drivers/resource.c \ drivers/serial.c \ drivers/serial_impl.c \ - drivers/serial_uart_hw.c \ drivers/sound_beeper.c \ drivers/stack_check.c \ drivers/timer_common.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9a9a749093..ebc5b894e4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1081,7 +1081,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_IMU_SMALL_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) }, { PARAM_NAME_IMU_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) }, #ifdef USE_MAG - { PARAM_NAME_IMU_MAG_DECLINATION, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3599 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) }, + { PARAM_NAME_IMU_MAG_DECLINATION, VAR_INT16 | MASTER_VALUE, .config.minmaxUnsigned = { -300, 300 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) }, #endif // PG_ARMING_CONFIG diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 3c4801146a..1a35baad44 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -62,16 +62,16 @@ typedef struct busDevice_s { } busType_u; bool useDMA; uint8_t deviceCount; +#ifdef USE_DMA dmaChannelDescriptor_t *dmaTx; dmaChannelDescriptor_t *dmaRx; -#ifdef USE_DMA // Use a reference here as this saves RAM for unused descriptors #if defined(USE_FULL_LL_DRIVER) - LL_DMA_InitTypeDef *initTx; - LL_DMA_InitTypeDef *initRx; + LL_DMA_InitTypeDef *dmaInitTx; + LL_DMA_InitTypeDef *dmaInitRx; #else - DMA_InitTypeDef *initTx; - DMA_InitTypeDef *initRx; + DMA_InitTypeDef *dmaInitTx; + DMA_InitTypeDef *dmaInitRx; #endif #endif // USE_DMA volatile struct busSegment_s* volatile curSegment; @@ -97,11 +97,11 @@ typedef struct extDevice_s { #ifdef USE_DMA // Cache the init structure for the next DMA transfer to reduce inter-segment delay #if defined(USE_FULL_LL_DRIVER) - LL_DMA_InitTypeDef initTx; - LL_DMA_InitTypeDef initRx; + LL_DMA_InitTypeDef dmaInitTx; + LL_DMA_InitTypeDef dmaInitRx; #else - DMA_InitTypeDef initTx; - DMA_InitTypeDef initRx; + DMA_InitTypeDef dmaInitTx; + DMA_InitTypeDef dmaInitRx; #endif #endif // USE_DMA // Support disabling DMA on a per device basis diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index f9ebf24713..ae82bbcd63 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -365,8 +365,10 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device) bus->busType = BUS_TYPE_SPI; bus->useDMA = false; bus->deviceCount = 1; - bus->initTx = &dev->initTx; - bus->initRx = &dev->initRx; +#ifdef USE_DMA + bus->dmaInitTx = &dev->dmaInitTx; + bus->dmaInitRx = &dev->dmaInitRx; +#endif return true; } diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index d4c530d889..4e99137a6e 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -50,19 +50,7 @@ #include "pg/serial_uart.h" -#if defined(STM32H7) -#define UART_TX_BUFFER_ATTRIBUTE DMA_RAM // D2 SRAM -#define UART_RX_BUFFER_ATTRIBUTE DMA_RAM // D2 SRAM -#elif defined(STM32G4) -#define UART_TX_BUFFER_ATTRIBUTE DMA_RAM_W // SRAM MPU NOT_BUFFERABLE -#define UART_RX_BUFFER_ATTRIBUTE DMA_RAM_R // SRAM MPU NOT CACHABLE -#elif defined(STM32F7) -#define UART_TX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT // DTCM RAM -#define UART_RX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT // DTCM RAM -#elif defined(STM32F4) || defined(AT32F4) || defined(APM32F4) -#define UART_TX_BUFFER_ATTRIBUTE // NONE -#define UART_RX_BUFFER_ATTRIBUTE // NONE -#else +#if !defined(UART_TX_BUFFER_ATTRIBUTE) || !defined(UART_RX_BUFFER_ATTRIBUTE) #error Undefined UART_{TX,RX}_BUFFER_ATTRIBUTE for this MCU #endif @@ -377,13 +365,7 @@ static void uartWrite(serialPort_t *instance, uint8_t ch) } else #endif { -#if defined(USE_HAL_DRIVER) - __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); -#elif defined(USE_ATBSP_DRIVER) - usart_interrupt_enable(uartPort->USARTx, USART_TDBE_INT, TRUE); -#else - USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE); -#endif + uartEnableTxInterrupt(uartPort); } } @@ -442,13 +424,7 @@ static void uartEndWrite(serialPort_t *instance) } else #endif { -#if defined(USE_HAL_DRIVER) - __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); -#elif defined(USE_ATBSP_DRIVER) - usart_interrupt_enable(uartPort->USARTx, USART_TDBE_INT, TRUE); -#else - USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE); -#endif + uartEnableTxInterrupt(uartPort); } } @@ -469,98 +445,6 @@ const struct serialPortVTable uartVTable[] = { } }; -// TODO - move to serial_uart_hw.c -#ifdef USE_DMA -void uartConfigureDma(uartDevice_t *uartdev) -{ - uartPort_t *uartPort = &(uartdev->port); - const uartHardware_t *hardware = uartdev->hardware; - -#ifdef USE_DMA_SPEC - const serialPortIdentifier_e uartPortIdentifier = hardware->identifier; - const uartDeviceIdx_e uartDeviceIdx = uartDeviceIdxFromIdentifier(uartPortIdentifier); - if (uartDeviceIdx == UARTDEV_INVALID) { - return; - } - const int resourceIdx = serialResourceIndex(uartPortIdentifier); - const int ownerIndex = serialOwnerIndex(uartPortIdentifier); - const resourceOwner_e ownerTxRx = serialOwnerTxRx(uartPortIdentifier); // rx is always +1 - - const dmaChannelSpec_t *dmaChannelSpec; - const serialUartConfig_t *cfg = serialUartConfig(resourceIdx); - if (!cfg) { - return; - } - if (cfg->txDmaopt != DMA_OPT_UNUSED) { - dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_TX, uartDeviceIdx, cfg->txDmaopt); - if (dmaChannelSpec) { - uartPort->txDMAResource = dmaChannelSpec->ref; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) - uartPort->txDMAChannel = dmaChannelSpec->channel; -#elif defined(AT32F4) - uartPort->txDMAMuxId = dmaChannelSpec->dmaMuxId; -#endif - } - } - - if (cfg->rxDmaopt != DMA_OPT_UNUSED) { - dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_RX, uartDeviceIdx, cfg->txDmaopt); - if (dmaChannelSpec) { - uartPort->rxDMAResource = dmaChannelSpec->ref; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) - uartPort->rxDMAChannel = dmaChannelSpec->channel; -#elif defined(AT32F4) - uartPort->rxDMAMuxId = dmaChannelSpec->dmaMuxId; -#endif - } - } -#else /* USE_DMA_SPEC */ - // Non USE_DMA_SPEC does not support configurable ON/OFF of UART DMA - - if (hardware->rxDMAResource) { - uartPort->rxDMAResource = hardware->rxDMAResource; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) - uartPort->rxDMAChannel = hardware->rxDMAChannel; -#elif defined(AT32F4) - uartPort->rxDMAMuxId = hardware->rxDMAMuxId; -#endif - } - - if (hardware->txDMAResource) { - uartPort->txDMAResource = hardware->txDMAResource; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) - uartPort->txDMAChannel = hardware->txDMAChannel; -#elif defined(AT32F4) - uartPort->txDMAMuxId = hardware->txDMAMuxId; -#endif - } -#endif /* USE_DMA_SPEC */ - - if (uartPort->txDMAResource) { - const dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->txDMAResource); - if (dmaAllocate(identifier, ownerTxRx, ownerIndex)) { - dmaEnable(identifier); -#if defined(AT32F4) - dmaMuxEnable(identifier, uartPort->txDMAMuxId); -#endif - dmaSetHandler(identifier, uartDmaIrqHandler, hardware->txPriority, (uint32_t)uartdev); - uartPort->txDMAPeripheralBaseAddr = (uint32_t)&UART_REG_TXD(hardware->reg); - } - } - - if (uartPort->rxDMAResource) { - const dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->rxDMAResource); - if (dmaAllocate(identifier, ownerTxRx + 1, ownerIndex)) { - dmaEnable(identifier); -#if defined(AT32F4) - dmaMuxEnable(identifier, uartPort->rxDMAMuxId); -#endif - uartPort->rxDMAPeripheralBaseAddr = (uint32_t)&UART_REG_RXD(hardware->reg); - } - } -} -#endif - #define UART_IRQHandler(type, number, dev) \ FAST_IRQ_HANDLER void type ## number ## _IRQHandler(void) \ { \ diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index 94b679e505..42e199c5ad 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -31,20 +31,8 @@ // Various serial routines return the buffer occupied size as uint8_t which would need to be extended in order to // increase size further. -// define some common UART features -#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F43x) -#define UART_TRAIT_AF_PIN 1 // pin AF mode is configured for each pin individually -#else -#define UART_TRAIT_AF_PORT 1 // all pins on given uart use same AF -#endif +#include "platform.h" -#if !defined(STM32F4) || !defined(APM32F4) // all others support pinswap -#define UART_TRAIT_PINSWAP 1 -#endif - -#if defined(STM32F4) - -#define UARTHARDWARE_MAX_PINS 4 #ifndef UART_RX_BUFFER_SIZE #define UART_RX_BUFFER_SIZE 256 #endif @@ -56,77 +44,8 @@ #endif #endif -#elif defined(STM32F7) - -#define UARTHARDWARE_MAX_PINS 4 -#ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 256 -#endif -#ifndef UART_TX_BUFFER_SIZE -#ifdef USE_MSP_DISPLAYPORT -#define UART_TX_BUFFER_SIZE 1280 -#else -#define UART_TX_BUFFER_SIZE 256 -#endif -#endif - -#elif defined(STM32H7) - -#define UARTHARDWARE_MAX_PINS 5 -#ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 256 -#endif -#ifndef UART_TX_BUFFER_SIZE -#ifdef USE_MSP_DISPLAYPORT -#define UART_TX_BUFFER_SIZE 1280 -#else -#define UART_TX_BUFFER_SIZE 256 -#endif -#endif - -#elif defined(STM32G4) - -#define UARTHARDWARE_MAX_PINS 3 -#ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 256 -#endif -#ifndef UART_TX_BUFFER_SIZE -#ifdef USE_MSP_DISPLAYPORT -#define UART_TX_BUFFER_SIZE 1280 -#else -#define UART_TX_BUFFER_SIZE 256 -#endif -#endif - -#elif defined(AT32F4) - -#define UARTHARDWARE_MAX_PINS 5 -#ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 256 -#endif -#ifndef UART_TX_BUFFER_SIZE -#ifdef USE_MSP_DISPLAYPORT -#define UART_TX_BUFFER_SIZE 1280 -#else -#define UART_TX_BUFFER_SIZE 256 -#endif -#endif -#elif defined(APM32F4) - -#define UARTHARDWARE_MAX_PINS 4 -#ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 256 -#endif -#ifndef UART_TX_BUFFER_SIZE -#ifdef USE_MSP_DISPLAYPORT -#define UART_TX_BUFFER_SIZE 1280 -#else -#define UART_TX_BUFFER_SIZE 256 -#endif -#endif - -#else -#error unknown MCU family +#if !UART_TRAIT_AF_PIN && !UART_TRAIT_AF_PORT +#error "Must specify either AF mode for MCU" #endif // compressed index of UART/LPUART. Direct index into uartDevice[] @@ -188,10 +107,10 @@ typedef struct uartHardware_s { // For H7 and G4 , {tx|rx}DMAChannel are DMAMUX input index for peripherals (DMA_REQUEST_xxx); H7:RM0433 Table 110, G4:RM0440 Table 80. // For F4 and F7, these are 32-bit channel identifiers (DMA_CHANNEL_x) // For at32f435/7 DmaChannel is the dmamux, need to call dmamuxenable using dmamuxid -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) +#if DMA_TRAIT_CHANNEL uint32_t txDMAChannel; uint32_t rxDMAChannel; -#elif defined(AT32F4) +#elif DMA_TRAIT_MUX uint32_t txDMAMuxId;//for dmaspec->dmamux and using dmaMuxEnable(dmax,muxid) uint32_t rxDMAMuxId; #endif @@ -257,6 +176,7 @@ uartPort_t *serialUART(uartDevice_t *uart, uint32_t baudRate, portMode_e mode, p void uartConfigureExternalPinInversion(uartPort_t *uartPort); void uartIrqHandler(uartPort_t *s); +void uartEnableTxInterrupt(uartPort_t *uartPort); void uartReconfigure(uartPort_t *uartPort); @@ -267,20 +187,6 @@ void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor); bool checkUsartTxOutput(uartPort_t *s); void uartTxMonitor(uartPort_t *s); -#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) -#define UART_REG_RXD(base) ((base)->RDR) -#define UART_REG_TXD(base) ((base)->TDR) -#elif defined(STM32F4) -#define UART_REG_RXD(base) ((base)->DR) -#define UART_REG_TXD(base) ((base)->DR) -#elif defined(AT32F43x) -#define UART_REG_RXD(base) ((base)->dt) -#define UART_REG_TXD(base) ((base)->dt) -#elif defined(APM32F4) -#define UART_REG_RXD(base) ((base)->DATA) -#define UART_REG_TXD(base) ((base)->DATA) -#endif - #define UART_BUFFER(type, n, rxtx) type volatile uint8_t uart ## n ## rxtx ## xBuffer[UART_ ## rxtx ## X_BUFFER_SIZE] #define UART_BUFFERS_EXTERN(n) \ diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 1a9bc30494..daeaa2ce22 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -22,19 +22,6 @@ #include "drivers/serial.h" -#if defined(STM32F7) -#include "common/maths.h" - -#include "usbd_cdc.h" - -extern USBD_HandleTypeDef USBD_Device; - -#elif defined(STM32H7) || defined(STM32G4) -#include "usbd_cdc.h" - -extern USBD_HandleTypeDef USBD_Device; -#endif - typedef struct { serialPort_t port; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 930a8ee9e2..43624e06ea 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -262,6 +262,7 @@ #define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki" #define PARAM_NAME_IMU_SMALL_ANGLE "small_angle" #define PARAM_NAME_IMU_PROCESS_DENOM "imu_process_denom" + #ifdef USE_MAG #define PARAM_NAME_IMU_MAG_DECLINATION "mag_declination" #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e353ae3b84..014052cf56 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -125,7 +125,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .imu_dcm_ki = 0, // 0.003 * 10000 .small_angle = DEFAULT_SMALL_ANGLE, .imu_process_denom = 2, - .mag_declination = 0 + .mag_declination = 0, ); static void imuQuaternionComputeProducts(quaternion_t *quat, quaternionProducts *quatProd) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 3ab4f3cbd6..ef0d297233 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -63,7 +63,7 @@ typedef struct imuConfig_s { uint16_t imu_dcm_ki; // DCM filter integral gain ( x 10000) uint8_t small_angle; uint8_t imu_process_denom; - uint16_t mag_declination; // Magnetic declination in degrees * 10 + int16_t mag_declination; // Magnetic declination in degrees * 10 } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); @@ -92,5 +92,4 @@ void imuSetHasNewData(uint32_t dt); bool imuQuaternionHeadfreeOffsetSet(void); void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v); -bool shouldInitializeGPSHeading(void); bool isUpright(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index d0bf9e563a..891a5cf7da 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1847,7 +1847,33 @@ case MSP_NAME: sbufWriteU8(dst, gyroDeviceConfig(0)->alignment); sbufWriteU8(dst, ALIGN_DEFAULT); #endif + // Added in MSP API 1.47 + switch (gyroConfig()->gyro_to_use) { +#ifdef USE_MULTI_GYRO + case GYRO_CONFIG_USE_GYRO_2: + sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.roll); + sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.pitch); + sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.yaw); + break; +#endif + case GYRO_CONFIG_USE_GYRO_BOTH: + // for dual-gyro in "BOTH" mode we only read/write gyro 0 + default: + sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.roll); + sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.pitch); + sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.yaw); + break; + } +#ifdef USE_MAG + sbufWriteU16(dst, compassConfig()->mag_customAlignment.roll); + sbufWriteU16(dst, compassConfig()->mag_customAlignment.pitch); + sbufWriteU16(dst, compassConfig()->mag_customAlignment.yaw); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif break; } case MSP_ADVANCED_CONFIG: @@ -3024,7 +3050,36 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #else gyroDeviceConfigMutable(0)->alignment = gyroAlignment; #endif - + } + // Added in API 1.47 + if (sbufBytesRemaining(src) >= 6) { + switch (gyroConfig()->gyro_to_use) { +#ifdef USE_MULTI_GYRO + case GYRO_CONFIG_USE_GYRO_2: + gyroDeviceConfigMutable(1)->customAlignment.roll = sbufReadU16(src); + gyroDeviceConfigMutable(1)->customAlignment.pitch = sbufReadU16(src); + gyroDeviceConfigMutable(1)->customAlignment.yaw = sbufReadU16(src); + break; +#endif + case GYRO_CONFIG_USE_GYRO_BOTH: + // For dual-gyro in "BOTH" mode we'll only update gyro 0 + default: + gyroDeviceConfigMutable(0)->customAlignment.roll = sbufReadU16(src); + gyroDeviceConfigMutable(0)->customAlignment.pitch = sbufReadU16(src); + gyroDeviceConfigMutable(0)->customAlignment.yaw = sbufReadU16(src); + break; + } + } + if (sbufBytesRemaining(src) >= 6) { +#ifdef USE_MAG + compassConfigMutable()->mag_customAlignment.roll = sbufReadU16(src); + compassConfigMutable()->mag_customAlignment.pitch = sbufReadU16(src); + compassConfigMutable()->mag_customAlignment.yaw = sbufReadU16(src); +#else + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif } break; } diff --git a/src/platform/APM32/bus_spi_apm32.c b/src/platform/APM32/bus_spi_apm32.c index 831796331f..8f09a3489a 100644 --- a/src/platform/APM32/bus_spi_apm32.c +++ b/src/platform/APM32/bus_spi_apm32.c @@ -99,31 +99,31 @@ void spiInitDevice(SPIDevice device) void spiInternalResetDescriptors(busDevice_t *bus) { - DDL_DMA_InitTypeDef *initTx = bus->initTx; + DDL_DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; - DDL_DMA_StructInit(initTx); + DDL_DMA_StructInit(dmaInitTx); - initTx->Channel = bus->dmaTx->channel; - initTx->Mode = DDL_DMA_MODE_NORMAL; - initTx->Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH; - initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA; - initTx->Priority = DDL_DMA_PRIORITY_LOW; - initTx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT; - initTx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE; - initTx->MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_BYTE; + dmaInitTx->Channel = bus->dmaTx->channel; + dmaInitTx->Mode = DDL_DMA_MODE_NORMAL; + dmaInitTx->Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH; + dmaInitTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA; + dmaInitTx->Priority = DDL_DMA_PRIORITY_LOW; + dmaInitTx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT; + dmaInitTx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE; + dmaInitTx->MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_BYTE; if (bus->dmaRx) { - DDL_DMA_InitTypeDef *initRx = bus->initRx; + DDL_DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; - DDL_DMA_StructInit(initRx); + DDL_DMA_StructInit(dmaInitRx); - initRx->Channel = bus->dmaRx->channel; - initRx->Mode = DDL_DMA_MODE_NORMAL; - initRx->Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY; - initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA; - initRx->Priority = DDL_DMA_PRIORITY_LOW; - initRx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT; - initRx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE; + dmaInitRx->Channel = bus->dmaRx->channel; + dmaInitRx->Mode = DDL_DMA_MODE_NORMAL; + dmaInitRx->Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY; + dmaInitRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA; + dmaInitRx->Priority = DDL_DMA_PRIORITY_LOW; + dmaInitRx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT; + dmaInitRx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE; } } @@ -175,34 +175,34 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) int len = segment->len; uint8_t *txData = segment->u.buffers.txData; - DDL_DMA_InitTypeDef *initTx = bus->initTx; + DDL_DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; if (txData) { - initTx->MemoryOrM2MDstAddress = (uint32_t)txData; - initTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT; + dmaInitTx->MemoryOrM2MDstAddress = (uint32_t)txData; + dmaInitTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT; } else { dummyTxByte = 0xff; - initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte; - initTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT; + dmaInitTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte; + dmaInitTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT; } - initTx->NbData = len; + dmaInitTx->NbData = len; if (dev->bus->dmaRx) { uint8_t *rxData = segment->u.buffers.rxData; - DDL_DMA_InitTypeDef *initRx = bus->initRx; + DDL_DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; if (rxData) { /* Flush the D cache for the start and end of the receive buffer as * the cache will be invalidated after the transfer and any valid data * just before/after must be in memory at that point */ - initRx->MemoryOrM2MDstAddress = (uint32_t)rxData; - initRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT; + dmaInitRx->MemoryOrM2MDstAddress = (uint32_t)rxData; + dmaInitRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT; } else { - initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte; - initRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT; + dmaInitRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte; + dmaInitRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT; } - initRx->NbData = len; + dmaInitRx->NbData = len; } } @@ -235,8 +235,8 @@ void spiInternalStartDMA(const extDevice_t *dev) DDL_EX_DMA_EnableIT_TC(streamRegsRx); // Update streams - DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); - DDL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->dmaInitTx); + DDL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->dmaInitRx); /* Note from AN4031 * @@ -265,7 +265,7 @@ void spiInternalStartDMA(const extDevice_t *dev) DDL_EX_DMA_EnableIT_TC(streamRegsTx); // Update streams - DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); + DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->dmaInitTx); /* Note from AN4031 * diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk index 4b3a3cee21..5203bf6781 100644 --- a/src/platform/APM32/mk/APM32F4.mk +++ b/src/platform/APM32/mk/APM32F4.mk @@ -180,6 +180,7 @@ MCU_COMMON_SRC = \ drivers/bus_spi_config.c \ common/stm32/bus_spi_hw.c \ common/stm32/bus_spi_pinconfig.c \ + common/stm32/serial_uart_hw.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ drivers/serial_uart_pinconfig.c \ @@ -205,6 +206,7 @@ MSC_SRC = \ msc/usbd_storage_sdio.c SPEED_OPTIMISED_SRC += \ + common/stm32/bus_spi_hw.c \ common/stm32/system.c SIZE_OPTIMISED_SRC += \ diff --git a/src/platform/APM32/platform_mcu.h b/src/platform/APM32/platform_mcu.h index 8e0aa79749..71833d55ce 100644 --- a/src/platform/APM32/platform_mcu.h +++ b/src/platform/APM32/platform_mcu.h @@ -187,6 +187,18 @@ #define USE_TX_IRQ_HANDLER +#define UART_TX_BUFFER_ATTRIBUTE /* NONE */ +#define UART_RX_BUFFER_ATTRIBUTE /* NONE */ + +#define UART_TRAIT_AF_PORT 1 + +#define UARTHARDWARE_MAX_PINS 4 + +#define UART_REG_RXD(base) ((base)->DATA) +#define UART_REG_TXD(base) ((base)->DATA) + +#define DMA_TRAIT_CHANNEL 1 + #endif #if defined(APM32F4) diff --git a/src/platform/AT32/bus_spi_at32bsp.c b/src/platform/AT32/bus_spi_at32bsp.c index d8978298db..b76303d5af 100644 --- a/src/platform/AT32/bus_spi_at32bsp.c +++ b/src/platform/AT32/bus_spi_at32bsp.c @@ -100,29 +100,29 @@ void spiInitDevice(SPIDevice device) void spiInternalResetDescriptors(busDevice_t *bus) { - dma_init_type *initTx = bus->initTx; + dma_init_type *dmaInitTx = bus->dmaInitTx; - dma_default_para_init(initTx); + dma_default_para_init(dmaInitTx); - initTx->direction=DMA_DIR_MEMORY_TO_PERIPHERAL; - initTx->loop_mode_enable=FALSE; - initTx->peripheral_base_addr=(uint32_t)&bus->busType_u.spi.instance->dt ; - initTx->priority =DMA_PRIORITY_LOW; - initTx->peripheral_inc_enable =FALSE; - initTx->peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; - initTx->memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; + dmaInitTx->direction=DMA_DIR_MEMORY_TO_PERIPHERAL; + dmaInitTx->loop_mode_enable=FALSE; + dmaInitTx->peripheral_base_addr=(uint32_t)&bus->busType_u.spi.instance->dt ; + dmaInitTx->priority =DMA_PRIORITY_LOW; + dmaInitTx->peripheral_inc_enable =FALSE; + dmaInitTx->peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; + dmaInitTx->memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; if (bus->dmaRx) { - dma_init_type *initRx = bus->initRx; + dma_init_type *dmaInitRx = bus->dmaInitRx; - dma_default_para_init(initRx); + dma_default_para_init(dmaInitRx); - initRx->direction = DMA_DIR_PERIPHERAL_TO_MEMORY; - initRx->loop_mode_enable = FALSE; - initRx->peripheral_base_addr = (uint32_t)&bus->busType_u.spi.instance->dt; - initRx->priority = DMA_PRIORITY_LOW; - initRx->peripheral_inc_enable = FALSE; - initRx->peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; + dmaInitRx->direction = DMA_DIR_PERIPHERAL_TO_MEMORY; + dmaInitRx->loop_mode_enable = FALSE; + dmaInitRx->peripheral_base_addr = (uint32_t)&bus->busType_u.spi.instance->dt; + dmaInitRx->priority = DMA_PRIORITY_LOW; + dmaInitRx->peripheral_inc_enable = FALSE; + dmaInitRx->peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; } } @@ -175,31 +175,31 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) int len = segment->len; uint8_t *txData = segment->u.buffers.txData; - dma_init_type *initTx = bus->initTx; + dma_init_type *dmaInitTx = bus->dmaInitTx; if (txData) { - initTx->memory_base_addr = (uint32_t)txData; - initTx->memory_inc_enable =TRUE; + dmaInitTx->memory_base_addr = (uint32_t)txData; + dmaInitTx->memory_inc_enable =TRUE; } else { dummyTxByte = 0xff; - initTx->memory_base_addr = (uint32_t)&dummyTxByte; - initTx->memory_inc_enable =FALSE; + dmaInitTx->memory_base_addr = (uint32_t)&dummyTxByte; + dmaInitTx->memory_inc_enable =FALSE; } - initTx->buffer_size =len; + dmaInitTx->buffer_size =len; if (dev->bus->dmaRx) { uint8_t *rxData = segment->u.buffers.rxData; - dma_init_type *initRx = bus->initRx; + dma_init_type *dmaInitRx = bus->dmaInitRx; if (rxData) { - initRx->memory_base_addr= (uint32_t)rxData; - initRx->memory_inc_enable = TRUE; + dmaInitRx->memory_base_addr= (uint32_t)rxData; + dmaInitRx->memory_inc_enable = TRUE; } else { - initRx->memory_base_addr = (uint32_t)&dummyRxByte; - initRx->memory_inc_enable = FALSE; + dmaInitRx->memory_base_addr = (uint32_t)&dummyRxByte; + dmaInitRx->memory_inc_enable = FALSE; } - initRx->buffer_size = len; + dmaInitRx->buffer_size = len; } } @@ -229,8 +229,8 @@ void spiInternalStartDMA(const extDevice_t *dev) xDMA_ITConfig(streamRegsRx, DMA_IT_TCIF, TRUE); // Update streams - xDMA_Init(streamRegsTx, dev->bus->initTx); - xDMA_Init(streamRegsRx, dev->bus->initRx); + xDMA_Init(streamRegsTx, dev->bus->dmaInitTx); + xDMA_Init(streamRegsRx, dev->bus->dmaInitRx); // Enable streams xDMA_Cmd(streamRegsRx, TRUE); @@ -253,7 +253,7 @@ void spiInternalStartDMA(const extDevice_t *dev) xDMA_Cmd(streamRegsTx, FALSE); // Update stream - xDMA_Init(streamRegsTx, dev->bus->initTx); + xDMA_Init(streamRegsTx, dev->bus->dmaInitTx); // Enable stream xDMA_Cmd(streamRegsTx, TRUE); diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk index 0c0ff8be39..759c36ad60 100644 --- a/src/platform/AT32/mk/AT32F4.mk +++ b/src/platform/AT32/mk/AT32F4.mk @@ -122,6 +122,7 @@ MCU_COMMON_SRC = \ drivers/bus_spi_config.c \ common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_hw.c \ + common/stm32/serial_uart_hw.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ drivers/serial_uart_pinconfig.c \ diff --git a/src/platform/AT32/platform_mcu.h b/src/platform/AT32/platform_mcu.h index 9a42c16cfc..97ee59aa1d 100644 --- a/src/platform/AT32/platform_mcu.h +++ b/src/platform/AT32/platform_mcu.h @@ -134,12 +134,25 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #define SPI_IO_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, GPIO_PULL_NONE) #define SPI_IO_CS_HIGH_CFG IO_CONFIG(GPIO_MODE_INPUT, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, GPIO_PULL_UP) -#define SPIDEV_COUNT 4 +#define SPIDEV_COUNT 4 #define CHECK_SPI_RX_DATA_AVAILABLE(instance) LL_SPI_IsActiveFlag_RXNE(instance) #define SPI_RX_DATA_REGISTER(base) ((base)->DR) -#define MAX_SPI_PIN_SEL 4 +#define MAX_SPI_PIN_SEL 4 + +#define UART_TX_BUFFER_ATTRIBUTE // NONE +#define UART_RX_BUFFER_ATTRIBUTE // NONE + +#define UART_TRAIT_AF_PIN 1 +#define UART_TRAIT_PINSWAP 1 + +#define UARTHARDWARE_MAX_PINS 5 + +#define UART_REG_RXD(base) ((base)->dt) +#define UART_REG_TXD(base) ((base)->dt) + +#define DMA_TRAIT_MUX 1 #endif diff --git a/src/platform/STM32/bus_spi_ll.c b/src/platform/STM32/bus_spi_ll.c index da00e15d3f..77707ad260 100644 --- a/src/platform/STM32/bus_spi_ll.c +++ b/src/platform/STM32/bus_spi_ll.c @@ -146,45 +146,45 @@ void spiInitDevice(SPIDevice device) void spiInternalResetDescriptors(busDevice_t *bus) { - LL_DMA_InitTypeDef *initTx = bus->initTx; + LL_DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; - LL_DMA_StructInit(initTx); + LL_DMA_StructInit(dmaInitTx); #if defined(STM32G4) || defined(STM32H7) - initTx->PeriphRequest = bus->dmaTx->channel; + dmaInitTx->PeriphRequest = bus->dmaTx->channel; #else - initTx->Channel = bus->dmaTx->channel; + dmaInitTx->Channel = bus->dmaTx->channel; #endif - initTx->Mode = LL_DMA_MODE_NORMAL; - initTx->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + dmaInitTx->Mode = LL_DMA_MODE_NORMAL; + dmaInitTx->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; #if defined(STM32H7) - initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->TXDR; + dmaInitTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->TXDR; #else - initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; + dmaInitTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; #endif - initTx->Priority = LL_DMA_PRIORITY_LOW; - initTx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - initTx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; - initTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + dmaInitTx->Priority = LL_DMA_PRIORITY_LOW; + dmaInitTx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + dmaInitTx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + dmaInitTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; if (bus->dmaRx) { - LL_DMA_InitTypeDef *initRx = bus->initRx; + LL_DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; - LL_DMA_StructInit(initRx); + LL_DMA_StructInit(dmaInitRx); #if defined(STM32G4) || defined(STM32H7) - initRx->PeriphRequest = bus->dmaRx->channel; + dmaInitRx->PeriphRequest = bus->dmaRx->channel; #else - initRx->Channel = bus->dmaRx->channel; + dmaInitRx->Channel = bus->dmaRx->channel; #endif - initRx->Mode = LL_DMA_MODE_NORMAL; - initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; + dmaInitRx->Mode = LL_DMA_MODE_NORMAL; + dmaInitRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; #if defined(STM32H7) - initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR; + dmaInitRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR; #else - initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; + dmaInitRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR; #endif - initRx->Priority = LL_DMA_PRIORITY_LOW; - initRx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + dmaInitRx->Priority = LL_DMA_PRIORITY_LOW; + dmaInitRx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + dmaInitRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; } } @@ -285,7 +285,7 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) int len = segment->len; uint8_t *txData = segment->u.buffers.txData; - LL_DMA_InitTypeDef *initTx = bus->initTx; + LL_DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; if (txData) { #ifdef __DCACHE_PRESENT @@ -301,19 +301,19 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) (((uint32_t)txData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK); } #endif // __DCACHE_PRESENT - initTx->MemoryOrM2MDstAddress = (uint32_t)txData; - initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + dmaInitTx->MemoryOrM2MDstAddress = (uint32_t)txData; + dmaInitTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; } else { - initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte; - initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; + dmaInitTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte; + dmaInitTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; } - initTx->NbData = len; + dmaInitTx->NbData = len; #if !defined(STM32G4) && !defined(STM32H7) if (dev->bus->dmaRx) { #endif uint8_t *rxData = segment->u.buffers.rxData; - LL_DMA_InitTypeDef *initRx = bus->initRx; + LL_DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; if (rxData) { /* Flush the D cache for the start and end of the receive buffer as @@ -333,13 +333,13 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) (((uint32_t)rxData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK); } #endif // __DCACHE_PRESENT - initRx->MemoryOrM2MDstAddress = (uint32_t)rxData; - initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + dmaInitRx->MemoryOrM2MDstAddress = (uint32_t)rxData; + dmaInitRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; } else { - initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte; - initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; + dmaInitRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte; + dmaInitRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; } - initRx->NbData = len; + dmaInitRx->NbData = len; #if !defined(STM32G4) && !defined(STM32H7) } #endif @@ -373,8 +373,8 @@ void spiInternalStartDMA(const extDevice_t *dev) LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream); // Update channels - LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); - LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->dmaInitTx); + LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->dmaInitRx); // Enable channels LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream); @@ -395,8 +395,8 @@ void spiInternalStartDMA(const extDevice_t *dev) LL_EX_DMA_EnableIT_TC(streamRegsRx); // Update streams - LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); - LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->dmaInitTx); + LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->dmaInitRx); /* Note from AN4031 * @@ -436,7 +436,7 @@ void spiInternalStartDMA(const extDevice_t *dev) LL_EX_DMA_EnableIT_TC(streamRegsTx); // Update streams - LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->dmaInitTx); /* Note from AN4031 * diff --git a/src/platform/STM32/bus_spi_stdperiph.c b/src/platform/STM32/bus_spi_stdperiph.c index 04a8e63af7..7b74aa6b0f 100644 --- a/src/platform/STM32/bus_spi_stdperiph.c +++ b/src/platform/STM32/bus_spi_stdperiph.c @@ -106,29 +106,29 @@ void spiInitDevice(SPIDevice device) void spiInternalResetDescriptors(busDevice_t *bus) { - DMA_InitTypeDef *initTx = bus->initTx; + DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; - DMA_StructInit(initTx); - initTx->DMA_Channel = bus->dmaTx->channel; - initTx->DMA_DIR = DMA_DIR_MemoryToPeripheral; - initTx->DMA_Mode = DMA_Mode_Normal; - initTx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; - initTx->DMA_Priority = DMA_Priority_Low; - initTx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; - initTx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - initTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_StructInit(dmaInitTx); + dmaInitTx->DMA_Channel = bus->dmaTx->channel; + dmaInitTx->DMA_DIR = DMA_DIR_MemoryToPeripheral; + dmaInitTx->DMA_Mode = DMA_Mode_Normal; + dmaInitTx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; + dmaInitTx->DMA_Priority = DMA_Priority_Low; + dmaInitTx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; + dmaInitTx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + dmaInitTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; if (bus->dmaRx) { - DMA_InitTypeDef *initRx = bus->initRx; + DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; - DMA_StructInit(initRx); - initRx->DMA_Channel = bus->dmaRx->channel; - initRx->DMA_DIR = DMA_DIR_PeripheralToMemory; - initRx->DMA_Mode = DMA_Mode_Normal; - initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; - initRx->DMA_Priority = DMA_Priority_Low; - initRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; - initRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_StructInit(dmaInitRx); + dmaInitRx->DMA_Channel = bus->dmaRx->channel; + dmaInitRx->DMA_DIR = DMA_DIR_PeripheralToMemory; + dmaInitRx->DMA_Mode = DMA_Mode_Normal; + dmaInitRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR; + dmaInitRx->DMA_Priority = DMA_Priority_Low; + dmaInitRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable; + dmaInitRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; } } @@ -182,36 +182,36 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) int len = segment->len; uint8_t *txData = segment->u.buffers.txData; - DMA_InitTypeDef *initTx = bus->initTx; + DMA_InitTypeDef *dmaInitTx = bus->dmaInitTx; if (txData) { - initTx->DMA_Memory0BaseAddr = (uint32_t)txData; - initTx->DMA_MemoryInc = DMA_MemoryInc_Enable; + dmaInitTx->DMA_Memory0BaseAddr = (uint32_t)txData; + dmaInitTx->DMA_MemoryInc = DMA_MemoryInc_Enable; } else { dummyTxByte = 0xff; - initTx->DMA_Memory0BaseAddr = (uint32_t)&dummyTxByte; - initTx->DMA_MemoryInc = DMA_MemoryInc_Disable; + dmaInitTx->DMA_Memory0BaseAddr = (uint32_t)&dummyTxByte; + dmaInitTx->DMA_MemoryInc = DMA_MemoryInc_Disable; } - initTx->DMA_BufferSize = len; + dmaInitTx->DMA_BufferSize = len; if (dev->bus->dmaRx) { uint8_t *rxData = segment->u.buffers.rxData; - DMA_InitTypeDef *initRx = bus->initRx; + DMA_InitTypeDef *dmaInitRx = bus->dmaInitRx; if (rxData) { - initRx->DMA_Memory0BaseAddr = (uint32_t)rxData; - initRx->DMA_MemoryInc = DMA_MemoryInc_Enable; + dmaInitRx->DMA_Memory0BaseAddr = (uint32_t)rxData; + dmaInitRx->DMA_MemoryInc = DMA_MemoryInc_Enable; } else { - initRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte; - initRx->DMA_MemoryInc = DMA_MemoryInc_Disable; + dmaInitRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte; + dmaInitRx->DMA_MemoryInc = DMA_MemoryInc_Disable; } // If possible use 16 bit memory writes to prevent atomic access issues on gyro data - if ((initRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1)) { - initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + if ((dmaInitRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1)) { + dmaInitRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; } else { - initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + dmaInitRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; } - initRx->DMA_BufferSize = len; + dmaInitRx->DMA_BufferSize = len; } } @@ -240,8 +240,8 @@ void spiInternalStartDMA(const extDevice_t *dev) DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE); // Update streams - DMA_Init(streamRegsTx, dev->bus->initTx); - DMA_Init(streamRegsRx, dev->bus->initRx); + DMA_Init(streamRegsTx, dev->bus->dmaInitTx); + DMA_Init(streamRegsRx, dev->bus->dmaInitRx); /* Note from AN4031 * @@ -269,7 +269,7 @@ void spiInternalStartDMA(const extDevice_t *dev) DMA_ITConfig(streamRegsTx, DMA_IT_TC, ENABLE); // Update stream - DMA_Init(streamRegsTx, dev->bus->initTx); + DMA_Init(streamRegsTx, dev->bus->dmaInitTx); /* Note from AN4031 * diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk index e8f7914941..8106ccf43d 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -6,7 +6,8 @@ MCU_COMMON_SRC += \ common/stm32/config_flash.c \ common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_hw.c \ - common/stm32/io_impl.c + common/stm32/io_impl.c \ + common/stm32/serial_uart_hw.c SIZE_OPTIMISED_SRC += \ common/stm32/config_flash.c \ diff --git a/src/platform/STM32/platform_mcu.h b/src/platform/STM32/platform_mcu.h index 1f69a3cfc9..2f7e24993c 100644 --- a/src/platform/STM32/platform_mcu.h +++ b/src/platform/STM32/platform_mcu.h @@ -360,6 +360,57 @@ extern uint8_t _dmaram_end__; #error Unknown MCU family #endif -#if !defined(STM32G4) && !defined(STM32H7) +#if defined(STM32F4) || defined(STM32F7) #define USE_TX_IRQ_HANDLER #endif + +#if defined(STM32H7) +#define UART_TX_BUFFER_ATTRIBUTE DMA_RAM /* D2 SRAM */ +#define UART_RX_BUFFER_ATTRIBUTE DMA_RAM /* D2 SRAM */ +#elif defined(STM32G4) +#define UART_TX_BUFFER_ATTRIBUTE DMA_RAM_W /* SRAM MPU NOT_BUFFERABLE */ +#define UART_RX_BUFFER_ATTRIBUTE DMA_RAM_R /* SRAM MPU NOT CACHABLE */ +#elif defined(STM32F7) +#define UART_TX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT /* DTCM RAM */ +#define UART_RX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT /* DTCM RAM */ +#elif defined(STM32F4) +#define UART_TX_BUFFER_ATTRIBUTE /* EMPTY */ +#define UART_RX_BUFFER_ATTRIBUTE /* EMPTY */ +#endif + +#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) + // pin AF mode is configured for each pin individually +#define UART_TRAIT_AF_PIN 1 +#elif defined(STM32F4) +// all pins on given uart use same AF +#define UART_TRAIT_AF_PORT 1 +#else +#error Unknown STM MCU when defining UART_TRAIT_x +#endif + +#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) +#define UART_TRAIT_PINSWAP 1 +#endif + +#if defined(STM32F4) +#define UARTHARDWARE_MAX_PINS 4 +#elif defined(STM32F7) +#define UARTHARDWARE_MAX_PINS 4 +#elif defined(STM32H7) +#define UARTHARDWARE_MAX_PINS 5 +#elif defined(STM32G4) +#define UARTHARDWARE_MAX_PINS 3 +#endif + +#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) +#define UART_REG_RXD(base) ((base)->RDR) +#define UART_REG_TXD(base) ((base)->TDR) +#elif defined(STM32F4) +#define UART_REG_RXD(base) ((base)->DR) +#define UART_REG_TXD(base) ((base)->DR) +#endif + + +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) +#define DMA_TRAIT_CHANNEL 1 +#endif diff --git a/src/platform/STM32/vcp_hal/usbd_cdc_interface.h b/src/platform/STM32/vcp_hal/usbd_cdc_interface.h index 4a4d81d0ca..2c8e7a34f1 100644 --- a/src/platform/STM32/vcp_hal/usbd_cdc_interface.h +++ b/src/platform/STM32/vcp_hal/usbd_cdc_interface.h @@ -83,6 +83,7 @@ typedef struct __attribute__ ((packed)) } LINE_CODING; extern USBD_CDC_ItfTypeDef USBD_CDC_fops; +extern USBD_HandleTypeDef USBD_Device; uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength); uint32_t CDC_Send_FreeBytes(void); diff --git a/src/main/drivers/serial_uart_hw.c b/src/platform/common/stm32/serial_uart_hw.c similarity index 65% rename from src/main/drivers/serial_uart_hw.c rename to src/platform/common/stm32/serial_uart_hw.c index 91e42c005f..74e32147cf 100644 --- a/src/main/drivers/serial_uart_hw.c +++ b/src/platform/common/stm32/serial_uart_hw.c @@ -28,7 +28,7 @@ #include "platform.h" -#if defined(USE_UART) && !defined(SIMULATOR_BUILD) +#if defined(USE_UART) #include "build/build_config.h" @@ -39,6 +39,10 @@ #include "drivers/serial_impl.h" #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" + +#include "pg/serial_uart.h" // TODO: split this function into mcu-specific UART files ? static void enableRxIrq(const uartHardware_t *hardware) @@ -207,4 +211,106 @@ void uartConfigureExternalPinInversion(uartPort_t *uartPort) #endif } +#ifdef USE_DMA +void uartConfigureDma(uartDevice_t *uartdev) +{ + uartPort_t *uartPort = &(uartdev->port); + const uartHardware_t *hardware = uartdev->hardware; + +#ifdef USE_DMA_SPEC + const serialPortIdentifier_e uartPortIdentifier = hardware->identifier; + const uartDeviceIdx_e uartDeviceIdx = uartDeviceIdxFromIdentifier(uartPortIdentifier); + if (uartDeviceIdx == UARTDEV_INVALID) { + return; + } + const int resourceIdx = serialResourceIndex(uartPortIdentifier); + const int ownerIndex = serialOwnerIndex(uartPortIdentifier); + const resourceOwner_e ownerTxRx = serialOwnerTxRx(uartPortIdentifier); // rx is always +1 + + const dmaChannelSpec_t *dmaChannelSpec; + const serialUartConfig_t *cfg = serialUartConfig(resourceIdx); + if (!cfg) { + return; + } + if (cfg->txDmaopt != DMA_OPT_UNUSED) { + dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_TX, uartDeviceIdx, cfg->txDmaopt); + if (dmaChannelSpec) { + uartPort->txDMAResource = dmaChannelSpec->ref; +#if DMA_TRAIT_CHANNEL + uartPort->txDMAChannel = dmaChannelSpec->channel; +#elif DMA_TRAIT_MUX + uartPort->txDMAMuxId = dmaChannelSpec->dmaMuxId; +#endif + } + } + + if (cfg->rxDmaopt != DMA_OPT_UNUSED) { + dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_RX, uartDeviceIdx, cfg->txDmaopt); + if (dmaChannelSpec) { + uartPort->rxDMAResource = dmaChannelSpec->ref; +#if DMA_TRAIT_CHANNEL + uartPort->rxDMAChannel = dmaChannelSpec->channel; +#elif DMA_TRAIT_MUX + uartPort->rxDMAMuxId = dmaChannelSpec->dmaMuxId; +#endif + } + } +#else /* USE_DMA_SPEC */ + // Non USE_DMA_SPEC does not support configurable ON/OFF of UART DMA + + if (hardware->rxDMAResource) { + uartPort->rxDMAResource = hardware->rxDMAResource; +#if DMA_TRAIT_CHANNEL + uartPort->rxDMAChannel = hardware->rxDMAChannel; +#elif DMA_TRAIT_MUX + uartPort->rxDMAMuxId = hardware->rxDMAMuxId; +#endif + } + + if (hardware->txDMAResource) { + uartPort->txDMAResource = hardware->txDMAResource; +#if DMA_TRAIT_CHANNEL + uartPort->txDMAChannel = hardware->txDMAChannel; +#elif DMA_TRAIT_MUX + uartPort->txDMAMuxId = hardware->txDMAMuxId; +#endif + } +#endif /* USE_DMA_SPEC */ + + if (uartPort->txDMAResource) { + const dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->txDMAResource); + if (dmaAllocate(identifier, ownerTxRx, ownerIndex)) { + dmaEnable(identifier); +#if DMA_TRAIT_MUX + dmaMuxEnable(identifier, uartPort->txDMAMuxId); +#endif + dmaSetHandler(identifier, uartDmaIrqHandler, hardware->txPriority, (uint32_t)uartdev); + uartPort->txDMAPeripheralBaseAddr = (uint32_t)&UART_REG_TXD(hardware->reg); + } + } + + if (uartPort->rxDMAResource) { + const dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->rxDMAResource); + if (dmaAllocate(identifier, ownerTxRx + 1, ownerIndex)) { + dmaEnable(identifier); +#if DMA_TRAIT_MUX + dmaMuxEnable(identifier, uartPort->rxDMAMuxId); +#endif + uartPort->rxDMAPeripheralBaseAddr = (uint32_t)&UART_REG_RXD(hardware->reg); + } + } +} +#endif + +void uartEnableTxInterrupt(uartPort_t *uartPort) +{ +#if defined(USE_HAL_DRIVER) + __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); +#elif defined(USE_ATBSP_DRIVER) + usart_interrupt_enable(uartPort->USARTx, USART_TDBE_INT, TRUE); +#else + USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE); +#endif +} + #endif /* USE_UART */