perf: Process CRSF telemetry by frame (#4038)

Co-authored-by: 3djc <lesitewebdejc@hotmail.com>
Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
This commit is contained in:
Raphael Coeffic
2023-10-19 13:31:30 +10:00
committed by GitHub
co-authored by 3djc Peter Feerick
parent eaabb2ce97
commit c93695eeaa
31 changed files with 418 additions and 169 deletions
@@ -22,6 +22,8 @@
#include "stm32_serial_driver.h"
#include <string.h>
#define MIN(a, b) ((a) < (b) ? (a) : (b))
// Serial buffer state
struct stm32_buffer_state {
volatile uint32_t ridx;
@@ -141,7 +143,7 @@ static inline void _usart_isr_handler(_STM32_USART n)
#endif
#if defined (USART2)
DEFINE_USART_IRQ(USART2);
DEFINE_USART_IRQ(USART2);
#endif
#if defined (USART3)
@@ -416,6 +418,82 @@ static int stm32_serial_get_last_byte(void* ctx, uint32_t idx, uint8_t* data)
return 1;
}
static int stm32_serial_get_buffered_bytes(void* ctx)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return -1;
auto sp = st->sp;
const auto& rx_buf = sp->rx_buffer;
auto buf_len = rx_buf.length;
if (!buf_len) return -1;
uint32_t widx;
auto usart = sp->usart;
const auto& buf_st = st->rx_buf;
if (LL_USART_IsEnabledDMAReq_RX(usart->USARTx)) {
auto dma = usart->rxDMA;
auto stream = usart->rxDMA_Stream;
widx = buf_len - LL_DMA_GetDataLength(dma, stream);
} else {
widx = buf_st.widx;
}
return (widx - buf_st.ridx) & (buf_len - 1);
}
static inline void _copy_buffer_chunk(const stm32_serial_buffer& rx_buf,
stm32_buffer_state& buf_st,
uint8_t* buf, uint32_t len)
{
memcpy(buf, rx_buf.buffer + buf_st.ridx, len);
buf_st.ridx = (buf_st.ridx + len) & (rx_buf.length - 1);
}
static int stm32_serial_copy_rx_buffer(void* ctx, uint8_t* buf, uint32_t len)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return -1;
auto sp = st->sp;
const auto& rx_buf = sp->rx_buffer;
auto buf_len = rx_buf.length;
if (!buf_len) return -1;
uint32_t widx;
auto usart = sp->usart;
auto& buf_st = st->rx_buf;
if (LL_USART_IsEnabledDMAReq_RX(usart->USARTx)) {
auto dma = usart->rxDMA;
auto stream = usart->rxDMA_Stream;
widx = buf_len - LL_DMA_GetDataLength(dma, stream);
} else {
widx = buf_st.widx;
}
if (buf_st.ridx == widx) return 0;
int res = 0;
if (buf_st.ridx > widx) {
auto cp_len = MIN(len, buf_len - buf_st.ridx);
_copy_buffer_chunk(rx_buf, buf_st, buf, cp_len);
buf += cp_len;
len -= cp_len;
res += cp_len;
}
if (buf_st.ridx < widx) {
auto cp_len = MIN(len, widx - buf_st.ridx);
_copy_buffer_chunk(rx_buf, buf_st, buf, cp_len);
len -= cp_len;
res += cp_len;
}
return res;
}
static void stm32_serial_clear_rx_buffer(void* ctx)
{
auto st = (stm32_serial_state*)ctx;
@@ -464,12 +542,13 @@ static void stm32_serial_hw_option(void* ctx, uint32_t option)
stm32_usart_set_hw_option(usart, option);
}
static void stm32_serial_set_idle_cb(void* ctx, void (*on_idle)())
static void stm32_serial_set_idle_cb(void* ctx, void (*on_idle)(void*), void* param)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return;
st->callbacks.on_idle = on_idle;
st->callbacks.on_idle_ctx = param;
uint32_t enabled = (on_idle != nullptr);
stm32_usart_set_idle_irq(st->sp->usart, enabled);
@@ -485,6 +564,8 @@ const etx_serial_driver_t STM32SerialDriver = {
.enableRx = stm32_enable_rx,
.getByte = stm32_serial_get_byte,
.getLastByte = stm32_serial_get_last_byte,
.getBufferedBytes = stm32_serial_get_buffered_bytes,
.copyRxBuffer = stm32_serial_copy_rx_buffer,
.clearRxBuffer = stm32_serial_clear_rx_buffer,
.getBaudrate = stm32_serial_get_baudrate,
.setBaudrate = stm32_serial_set_baudrate,