mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
205 lines
7.8 KiB
C
205 lines
7.8 KiB
C
/*
|
|
* This file is part of Betaflight.
|
|
*
|
|
* Betaflight is 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.
|
|
*
|
|
* Betaflight is distributed in the hope that it 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/>.
|
|
*/
|
|
|
|
/*
|
|
* Authors:
|
|
* jflyper - Refactoring, cleanup and made pin-configurable
|
|
* Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
|
|
* Hamasaki/Timecop - Initial baseflight code
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#ifdef USE_UART
|
|
|
|
#include "build/build_config.h"
|
|
#include "build/atomic.h"
|
|
|
|
#include "common/utils.h"
|
|
#include "drivers/inverter.h"
|
|
#include "drivers/nvic.h"
|
|
#include "drivers/rcc.h"
|
|
|
|
#include "drivers/serial.h"
|
|
#include "drivers/serial_uart.h"
|
|
#include "drivers/serial_uart_impl.h"
|
|
|
|
// XXX uartReconfigure does not handle resource management properly.
|
|
|
|
void uartReconfigure(uartPort_t *uartPort)
|
|
{
|
|
DAL_UART_DeInit(&uartPort->Handle);
|
|
uartPort->Handle.Init.BaudRate = uartPort->port.baudRate;
|
|
// according to the stm32 documentation wordlen has to be 9 for parity bits
|
|
// this does not seem to matter for rx but will give bad data on tx!
|
|
uartPort->Handle.Init.WordLength = (uartPort->port.options & SERIAL_PARITY_EVEN) ? UART_WORDLENGTH_9B : UART_WORDLENGTH_8B;
|
|
uartPort->Handle.Init.StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_STOPBITS_2 : USART_STOPBITS_1;
|
|
uartPort->Handle.Init.Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_PARITY_EVEN : USART_PARITY_NONE;
|
|
uartPort->Handle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
|
uartPort->Handle.Init.OverSampling = UART_OVERSAMPLING_16;
|
|
uartPort->Handle.Init.Mode = 0;
|
|
|
|
if (uartPort->port.mode & MODE_RX)
|
|
uartPort->Handle.Init.Mode |= UART_MODE_RX;
|
|
if (uartPort->port.mode & MODE_TX)
|
|
uartPort->Handle.Init.Mode |= UART_MODE_TX;
|
|
|
|
// config external pin inverter (no internal pin inversion available)
|
|
uartConfigureExternalPinInversion(uartPort);
|
|
|
|
#ifdef TARGET_USART_CONFIG
|
|
// TODO: move declaration into header file
|
|
void usartTargetConfigure(uartPort_t *);
|
|
usartTargetConfigure(uartPort);
|
|
#endif
|
|
|
|
if (uartPort->port.options & SERIAL_BIDIR) {
|
|
DAL_HalfDuplex_Init(&uartPort->Handle);
|
|
} else {
|
|
DAL_UART_Init(&uartPort->Handle);
|
|
}
|
|
|
|
// Receive DMA or IRQ
|
|
if (uartPort->port.mode & MODE_RX) {
|
|
#ifdef USE_DMA
|
|
if (uartPort->rxDMAResource) {
|
|
uartPort->rxDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->rxDMAResource;
|
|
uartPort->txDMAHandle.Init.Channel = uartPort->rxDMAChannel;
|
|
uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
|
uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
|
|
uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
|
|
uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
|
uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
|
uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR;
|
|
#if defined(APM32F4)
|
|
uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
|
uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
|
|
uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
|
|
uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
|
|
#endif
|
|
uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
|
|
|
|
DAL_DMA_DeInit(&uartPort->rxDMAHandle);
|
|
DAL_DMA_Init(&uartPort->rxDMAHandle);
|
|
/* Associate the initialized DMA handle to the UART handle */
|
|
__DAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle);
|
|
|
|
DAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize);
|
|
|
|
uartPort->rxDMAPos = __DAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle);
|
|
} else
|
|
#endif
|
|
{
|
|
/* Enable the UART Parity Error Interrupt */
|
|
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_PEIEN);
|
|
|
|
/* Enable the UART Error Interrupt: (Frame error, noise error, overrun error) */
|
|
SET_BIT(uartPort->USARTx->CTRL3, USART_CTRL3_ERRIEN);
|
|
|
|
/* Enable the UART Data Register not empty Interrupt */
|
|
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_RXBNEIEN);
|
|
|
|
/* Enable Idle Line detection */
|
|
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_IDLEIEN);
|
|
}
|
|
}
|
|
|
|
// Transmit DMA or IRQ
|
|
if (uartPort->port.mode & MODE_TX) {
|
|
#ifdef USE_DMA
|
|
if (uartPort->txDMAResource) {
|
|
uartPort->txDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->txDMAResource;
|
|
uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel;
|
|
uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH;
|
|
uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
|
|
uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
|
|
uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
|
uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
|
uartPort->txDMAHandle.Init.Mode = DMA_NORMAL;
|
|
uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
|
uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
|
|
uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
|
|
uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
|
|
uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
|
|
|
|
DAL_DMA_DeInit(&uartPort->txDMAHandle);
|
|
DAL_StatusTypeDef status = DAL_DMA_Init(&uartPort->txDMAHandle);
|
|
if (status != DAL_OK) {
|
|
while (1);
|
|
}
|
|
/* Associate the initialized DMA handle to the UART handle */
|
|
__DAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
|
|
|
|
__DAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
|
|
} else
|
|
#endif
|
|
{
|
|
|
|
/* Enable the UART Transmit Data Register Empty Interrupt */
|
|
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXBEIEN);
|
|
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXCIEN);
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef USE_DMA
|
|
void uartTryStartTxDMA(uartPort_t *s)
|
|
{
|
|
// uartTryStartTxDMA must be protected, since it is called from
|
|
// uartWrite and handleUsartTxDma (an ISR).
|
|
|
|
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART_TXDMA) {
|
|
if (IS_DMA_ENABLED(s->txDMAResource)) {
|
|
// DMA is already in progress
|
|
return;
|
|
}
|
|
|
|
DAL_UART_StateTypeDef state = DAL_UART_GetState(&s->Handle);
|
|
if ((state & DAL_UART_STATE_BUSY_TX) == DAL_UART_STATE_BUSY_TX) {
|
|
// UART is still transmitting
|
|
return;
|
|
}
|
|
|
|
if (s->port.txBufferHead == s->port.txBufferTail) {
|
|
// No more data to transmit
|
|
s->txDMAEmpty = true;
|
|
return;
|
|
}
|
|
|
|
unsigned chunk;
|
|
uint32_t fromwhere = s->port.txBufferTail;
|
|
|
|
if (s->port.txBufferHead > s->port.txBufferTail) {
|
|
chunk = s->port.txBufferHead - s->port.txBufferTail;
|
|
s->port.txBufferTail = s->port.txBufferHead;
|
|
} else {
|
|
chunk = s->port.txBufferSize - s->port.txBufferTail;
|
|
s->port.txBufferTail = 0;
|
|
}
|
|
s->txDMAEmpty = false;
|
|
DAL_UART_Transmit_DMA(&s->Handle, (uint8_t*)s->port.txBuffer + fromwhere, chunk);
|
|
}
|
|
}
|
|
#endif
|
|
#endif // USE_UART
|