|
|
@ -198,14 +198,19 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (rx_bounce_buf == nullptr) { |
|
|
|
if (rx_bounce_buf[0] == nullptr && sdef.dma_rx) { |
|
|
|
rx_bounce_buf = (uint8_t *)hal.util->malloc_type(RX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); |
|
|
|
rx_bounce_buf[0] = (uint8_t *)hal.util->malloc_type(RX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); |
|
|
|
} |
|
|
|
} |
|
|
|
if (tx_bounce_buf == nullptr) { |
|
|
|
if (rx_bounce_buf[1] == nullptr && sdef.dma_rx) { |
|
|
|
|
|
|
|
rx_bounce_buf[1] = (uint8_t *)hal.util->malloc_type(RX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
if (tx_bounce_buf == nullptr && sdef.dma_tx) { |
|
|
|
tx_bounce_buf = (uint8_t *)hal.util->malloc_type(TX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); |
|
|
|
tx_bounce_buf = (uint8_t *)hal.util->malloc_type(TX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); |
|
|
|
chVTObjectInit(&tx_timeout); |
|
|
|
chVTObjectInit(&tx_timeout); |
|
|
|
tx_bounce_buf_ready = true; |
|
|
|
tx_bounce_buf_ready = true; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
rx_dma_enabled = rx_bounce_buf[0] != nullptr && rx_bounce_buf[1] != nullptr; |
|
|
|
|
|
|
|
tx_dma_enabled = tx_bounce_buf != nullptr; |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
/*
|
|
|
@ -254,9 +259,9 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) |
|
|
|
if (_baudrate != 0) { |
|
|
|
if (_baudrate != 0) { |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
bool was_initialised = _device_initialised; |
|
|
|
bool was_initialised = _device_initialised; |
|
|
|
//setup Rx DMA
|
|
|
|
// setup Rx DMA
|
|
|
|
if(!_device_initialised) { |
|
|
|
if (!_device_initialised) { |
|
|
|
if (sdef.dma_rx) { |
|
|
|
if (rx_dma_enabled) { |
|
|
|
osalDbgAssert(rxdma == nullptr, "double DMA allocation"); |
|
|
|
osalDbgAssert(rxdma == nullptr, "double DMA allocation"); |
|
|
|
chSysLock(); |
|
|
|
chSysLock(); |
|
|
|
rxdma = dmaStreamAllocI(sdef.dma_rx_stream_id, |
|
|
|
rxdma = dmaStreamAllocI(sdef.dma_rx_stream_id, |
|
|
@ -274,7 +279,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) |
|
|
|
dmaSetRequestSource(rxdma, sdef.dma_rx_channel_id); |
|
|
|
dmaSetRequestSource(rxdma, sdef.dma_rx_channel_id); |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
} |
|
|
|
} |
|
|
|
if (sdef.dma_tx) { |
|
|
|
if (tx_dma_enabled) { |
|
|
|
// we only allow for sharing of the TX DMA channel, not the RX
|
|
|
|
// we only allow for sharing of the TX DMA channel, not the RX
|
|
|
|
// DMA channel, as the RX side is active all the time, so
|
|
|
|
// DMA channel, as the RX side is active all the time, so
|
|
|
|
// cannot be shared
|
|
|
|
// cannot be shared
|
|
|
@ -294,15 +299,12 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) |
|
|
|
sercfg.cr3 = _cr3_options; |
|
|
|
sercfg.cr3 = _cr3_options; |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (!sdef.dma_tx && !sdef.dma_rx) { |
|
|
|
if (rx_dma_enabled) { |
|
|
|
} else { |
|
|
|
sercfg.cr1 |= USART_CR1_IDLEIE; |
|
|
|
if (sdef.dma_rx) { |
|
|
|
sercfg.cr3 |= USART_CR3_DMAR; |
|
|
|
sercfg.cr1 |= USART_CR1_IDLEIE; |
|
|
|
} |
|
|
|
sercfg.cr3 |= USART_CR3_DMAR; |
|
|
|
if (tx_dma_enabled) { |
|
|
|
} |
|
|
|
sercfg.cr3 |= USART_CR3_DMAT; |
|
|
|
if (sdef.dma_tx) { |
|
|
|
|
|
|
|
sercfg.cr3 |= USART_CR3_DMAT; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
sercfg.irq_cb = rx_irq_cb; |
|
|
|
sercfg.irq_cb = rx_irq_cb; |
|
|
|
#endif // HAL_UART_NODMA
|
|
|
|
#endif // HAL_UART_NODMA
|
|
|
@ -312,7 +314,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) |
|
|
|
sdStart((SerialDriver*)sdef.serial, &sercfg); |
|
|
|
sdStart((SerialDriver*)sdef.serial, &sercfg); |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if(sdef.dma_rx) { |
|
|
|
if (rx_dma_enabled) { |
|
|
|
//Configure serial driver to skip handling RX packets
|
|
|
|
//Configure serial driver to skip handling RX packets
|
|
|
|
//because we will handle them via DMA
|
|
|
|
//because we will handle them via DMA
|
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
@ -374,7 +376,8 @@ void UARTDriver::dma_rx_enable(void) |
|
|
|
uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE; |
|
|
|
uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE; |
|
|
|
dmamode |= STM32_DMA_CR_CHSEL(sdef.dma_rx_channel_id); |
|
|
|
dmamode |= STM32_DMA_CR_CHSEL(sdef.dma_rx_channel_id); |
|
|
|
dmamode |= STM32_DMA_CR_PL(0); |
|
|
|
dmamode |= STM32_DMA_CR_PL(0); |
|
|
|
dmaStreamSetMemory0(rxdma, rx_bounce_buf); |
|
|
|
rx_bounce_idx ^= 1; |
|
|
|
|
|
|
|
dmaStreamSetMemory0(rxdma, rx_bounce_buf[rx_bounce_idx]); |
|
|
|
dmaStreamSetTransactionSize(rxdma, RX_BOUNCE_BUFSIZE); |
|
|
|
dmaStreamSetTransactionSize(rxdma, RX_BOUNCE_BUFSIZE); |
|
|
|
dmaStreamSetMode(rxdma, dmamode | STM32_DMA_CR_DIR_P2M | |
|
|
|
dmaStreamSetMode(rxdma, dmamode | STM32_DMA_CR_DIR_P2M | |
|
|
|
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); |
|
|
|
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); |
|
|
@ -418,7 +421,7 @@ void UARTDriver::rx_irq_cb(void* self) |
|
|
|
{ |
|
|
|
{ |
|
|
|
#if HAL_USE_SERIAL == TRUE |
|
|
|
#if HAL_USE_SERIAL == TRUE |
|
|
|
UARTDriver* uart_drv = (UARTDriver*)self; |
|
|
|
UARTDriver* uart_drv = (UARTDriver*)self; |
|
|
|
if (!uart_drv->sdef.dma_rx) { |
|
|
|
if (!uart_drv->rx_dma_enabled) { |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
dmaStreamDisable(uart_drv->rxdma); |
|
|
|
dmaStreamDisable(uart_drv->rxdma); |
|
|
@ -441,16 +444,30 @@ void UARTDriver::rx_irq_cb(void* self) |
|
|
|
} |
|
|
|
} |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
|
|
handle a RX DMA full interrupt |
|
|
|
|
|
|
|
*/ |
|
|
|
void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) |
|
|
|
void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) |
|
|
|
{ |
|
|
|
{ |
|
|
|
#if HAL_USE_SERIAL == TRUE |
|
|
|
#if HAL_USE_SERIAL == TRUE |
|
|
|
UARTDriver* uart_drv = (UARTDriver*)self; |
|
|
|
UARTDriver* uart_drv = (UARTDriver*)self; |
|
|
|
if (!uart_drv->sdef.dma_rx) { |
|
|
|
if (!uart_drv->rx_dma_enabled) { |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
stm32_cacheBufferInvalidate(uart_drv->rx_bounce_buf, RX_BOUNCE_BUFSIZE); |
|
|
|
uint16_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(uart_drv->rxdma); |
|
|
|
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(uart_drv->rxdma); |
|
|
|
const uint8_t bounce_idx = uart_drv->rx_bounce_idx; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// restart the DMA transfers immediately. This switches to the
|
|
|
|
|
|
|
|
// other bounce buffer. We restart the DMA before we copy the data
|
|
|
|
|
|
|
|
// out to minimise the time with DMA disabled, which allows us to
|
|
|
|
|
|
|
|
// handle much higher receiver baudrates
|
|
|
|
|
|
|
|
dmaStreamDisable(uart_drv->rxdma); |
|
|
|
|
|
|
|
uart_drv->dma_rx_enable(); |
|
|
|
|
|
|
|
|
|
|
|
if (len > 0) { |
|
|
|
if (len > 0) { |
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
|
|
we have data to copy out |
|
|
|
|
|
|
|
*/ |
|
|
|
if (uart_drv->half_duplex) { |
|
|
|
if (uart_drv->half_duplex) { |
|
|
|
uint32_t now = AP_HAL::micros(); |
|
|
|
uint32_t now = AP_HAL::micros(); |
|
|
|
if (now - uart_drv->hd_write_us < uart_drv->hd_read_delay_us) { |
|
|
|
if (now - uart_drv->hd_write_us < uart_drv->hd_read_delay_us) { |
|
|
@ -458,15 +475,11 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
stm32_cacheBufferInvalidate(uart_drv->rx_bounce_buf, len); |
|
|
|
stm32_cacheBufferInvalidate(uart_drv->rx_bounce_buf[bounce_idx], len); |
|
|
|
uart_drv->_readbuf.write(uart_drv->rx_bounce_buf, len); |
|
|
|
uart_drv->_readbuf.write(uart_drv->rx_bounce_buf[bounce_idx], len); |
|
|
|
|
|
|
|
|
|
|
|
uart_drv->receive_timestamp_update(); |
|
|
|
uart_drv->receive_timestamp_update(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
//restart the DMA transfers
|
|
|
|
|
|
|
|
dmaStreamDisable(uart_drv->rxdma); |
|
|
|
|
|
|
|
uart_drv->dma_rx_enable(); |
|
|
|
|
|
|
|
if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { |
|
|
|
if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { |
|
|
|
chSysLockFromISR(); |
|
|
|
chSysLockFromISR(); |
|
|
|
chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); |
|
|
|
chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); |
|
|
@ -857,7 +870,7 @@ void UARTDriver::write_pending_bytes(void) |
|
|
|
uint32_t n; |
|
|
|
uint32_t n; |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (sdef.dma_tx) { |
|
|
|
if (tx_dma_enabled) { |
|
|
|
check_dma_tx_completion(); |
|
|
|
check_dma_tx_completion(); |
|
|
|
} |
|
|
|
} |
|
|
|
#endif |
|
|
|
#endif |
|
|
@ -869,7 +882,7 @@ void UARTDriver::write_pending_bytes(void) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (sdef.dma_tx) { |
|
|
|
if (tx_dma_enabled) { |
|
|
|
write_pending_bytes_DMA(n); |
|
|
|
write_pending_bytes_DMA(n); |
|
|
|
} else |
|
|
|
} else |
|
|
|
#endif |
|
|
|
#endif |
|
|
@ -883,7 +896,7 @@ void UARTDriver::write_pending_bytes(void) |
|
|
|
_first_write_started_us = AP_HAL::micros(); |
|
|
|
_first_write_started_us = AP_HAL::micros(); |
|
|
|
} |
|
|
|
} |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (sdef.dma_tx) { |
|
|
|
if (tx_dma_enabled) { |
|
|
|
// when we are using DMA we have a reliable indication that a write
|
|
|
|
// when we are using DMA we have a reliable indication that a write
|
|
|
|
// has completed from the DMA completion interrupt
|
|
|
|
// has completed from the DMA completion interrupt
|
|
|
|
if (_last_write_completed_us != 0) { |
|
|
|
if (_last_write_completed_us != 0) { |
|
|
@ -936,7 +949,7 @@ void UARTDriver::_timer_tick(void) |
|
|
|
if (!_initialised) return; |
|
|
|
if (!_initialised) return; |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (sdef.dma_rx && rxdma) { |
|
|
|
if (rx_dma_enabled && rxdma) { |
|
|
|
chSysLock(); |
|
|
|
chSysLock(); |
|
|
|
//Check if DMA is enabled
|
|
|
|
//Check if DMA is enabled
|
|
|
|
//if not, it might be because the DMA interrupt was silenced
|
|
|
|
//if not, it might be because the DMA interrupt was silenced
|
|
|
@ -949,8 +962,8 @@ void UARTDriver::_timer_tick(void) |
|
|
|
if (!enabled) { |
|
|
|
if (!enabled) { |
|
|
|
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(rxdma); |
|
|
|
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(rxdma); |
|
|
|
if (len != 0) { |
|
|
|
if (len != 0) { |
|
|
|
stm32_cacheBufferInvalidate(rx_bounce_buf, len); |
|
|
|
stm32_cacheBufferInvalidate(rx_bounce_buf[rx_bounce_idx], len); |
|
|
|
_readbuf.write(rx_bounce_buf, len); |
|
|
|
_readbuf.write(rx_bounce_buf[rx_bounce_idx], len); |
|
|
|
|
|
|
|
|
|
|
|
receive_timestamp_update(); |
|
|
|
receive_timestamp_update(); |
|
|
|
if (_rts_is_active) { |
|
|
|
if (_rts_is_active) { |
|
|
@ -982,7 +995,7 @@ void UARTDriver::_timer_tick(void) |
|
|
|
_in_timer = true; |
|
|
|
_in_timer = true; |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if (!sdef.dma_rx) |
|
|
|
if (!rx_dma_enabled) |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
{ |
|
|
|
{ |
|
|
|
// try to fill the read buffer
|
|
|
|
// try to fill the read buffer
|
|
|
@ -1132,7 +1145,7 @@ bool UARTDriver::set_unbuffered_writes(bool on) |
|
|
|
#ifdef HAL_UART_NODMA |
|
|
|
#ifdef HAL_UART_NODMA |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
#else |
|
|
|
#else |
|
|
|
if (on && !sdef.dma_tx) { |
|
|
|
if (on && !tx_dma_enabled) { |
|
|
|
// we can't implement low latemcy writes safely without TX DMA
|
|
|
|
// we can't implement low latemcy writes safely without TX DMA
|
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
@ -1196,9 +1209,9 @@ void UARTDriver::configure_parity(uint8_t v) |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if(sdef.dma_rx) { |
|
|
|
if (rx_dma_enabled) { |
|
|
|
//Configure serial driver to skip handling RX packets
|
|
|
|
// Configure serial driver to skip handling RX packets
|
|
|
|
//because we will handle them via DMA
|
|
|
|
// because we will handle them via DMA
|
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
|
} |
|
|
|
} |
|
|
|
#endif |
|
|
|
#endif |
|
|
@ -1229,7 +1242,7 @@ void UARTDriver::set_stop_bits(int n) |
|
|
|
|
|
|
|
|
|
|
|
sdStart((SerialDriver*)sdef.serial, &sercfg); |
|
|
|
sdStart((SerialDriver*)sdef.serial, &sercfg); |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
#ifndef HAL_UART_NODMA |
|
|
|
if(sdef.dma_rx) { |
|
|
|
if (rx_dma_enabled) { |
|
|
|
//Configure serial driver to skip handling RX packets
|
|
|
|
//Configure serial driver to skip handling RX packets
|
|
|
|
//because we will handle them via DMA
|
|
|
|
//because we will handle them via DMA
|
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
|
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; |
|
|
|