From be6ad7af3b65841d2b460e3064c166dc9167401f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 00:08:12 -0700 Subject: [PATCH] Rework the FMU<->IO connection to use a simple fixed-size DMA packet; this should let us reduce overall latency and bump the bitrate up. Will still require some tuning. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 11 + src/drivers/boards/px4iov2/px4iov2_internal.h | 9 +- src/drivers/px4io/interface_serial.cpp | 343 ++++++++++-------- src/modules/px4iofirmware/serial.c | 169 +++++---- 4 files changed, 309 insertions(+), 223 deletions(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 78f6a2e65d..1698336b4b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -58,6 +58,17 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index 282ed75487..b8aa6d053e 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -59,8 +59,13 @@ /****************************************************************************** * Serial ******************************************************************************/ -#define SERIAL_BASE STM32_USART1_BASE -#define SERIAL_VECTOR STM32_IRQ_USART1 +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 /****************************************************************************** * GPIOS diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index d0af2912a3..9471cecddf 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -59,14 +59,28 @@ #include -#include +#include +#include /* XXX should really not be hardcoding v2 here */ #include "interface.h" +const unsigned max_rw_regs = 32; // by agreement w/IO + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[max_rw_regs]; +}; +#pragma pack(pop) + class PX4IO_serial : public PX4IO_interface { public: - PX4IO_serial(int port); + PX4IO_serial(); virtual ~PX4IO_serial(); virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -75,119 +89,133 @@ public: virtual bool ok(); private: - volatile uint32_t *_serial_base; - int _vector; + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory + + static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP; + DMA_HANDLE _tx_dma; - uint8_t *_tx_buf; - unsigned _tx_size; + static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP; + DMA_HANDLE _rx_dma; - const uint8_t *_rx_buf; - unsigned _rx_size; + /** set if we have started a transaction that expects a reply */ + bool _expect_reply; - hx_stream_t _stream; + /** saved DMA status (in case we care) */ + uint8_t _dma_status; + /** bus-ownership lock */ sem_t _bus_semaphore; - sem_t _completion_semaphore; - /** - * Send _tx_size bytes from the buffer, then - * if _rx_size is greater than zero wait for a packet - * to come back. - */ - int _wait_complete(); + /** client-waiting lock/signal */ + sem_t _completion_semaphore; /** - * Interrupt handler. + * Start the transaction with IO and wait for it to complete. + * + * @param expect_reply If true, expect a reply from IO. */ - static int _interrupt(int irq, void *context); - void _do_interrupt(); + int _wait_complete(bool expect_reply); /** - * Stream transmit callback + * DMA completion handler. */ - static void _tx(void *arg, uint8_t data); - void _do_tx(uint8_t data); + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_dma_callback(DMA_HANDLE handle, uint8_t status); /** - * Stream receive callback + * (re)configure the DMA */ - static void _rx(void *arg, const void *data, size_t length); - void _do_rx(const uint8_t *data, size_t length); + void _reset_dma(); /** * Serial register accessors. */ volatile uint32_t &_sreg(unsigned offset) { - return *(_serial_base + (offset / sizeof(uint32_t))); + return *(volatile uint32_t *)(PX4IO_SERIAL_BASE + offset); } - volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); } - volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); } - volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); } - volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); } - volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); } - volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); } - volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + uint32_t _SR() { return _sreg(STM32_USART_SR_OFFSET); } + void _SR(uint32_t val) { _sreg(STM32_USART_SR_OFFSET) = val; } + uint32_t _DR() { return _sreg(STM32_USART_DR_OFFSET); } + void _DR(uint32_t val) { _sreg(STM32_USART_DR_OFFSET) = val; } + uint32_t _BRR() { return _sreg(STM32_USART_BRR_OFFSET); } + void _BRR(uint32_t val) { _sreg(STM32_USART_BRR_OFFSET) = val; } + uint32_t _CR1() { return _sreg(STM32_USART_CR1_OFFSET); } + void _CR1(uint32_t val) { _sreg(STM32_USART_CR1_OFFSET) = val; } + uint32_t _CR2() { return _sreg(STM32_USART_CR2_OFFSET); } + void _CR2(uint32_t val) { _sreg(STM32_USART_CR2_OFFSET) = val; } + uint32_t _CR3() { return _sreg(STM32_USART_CR3_OFFSET); } + void _CR3(uint32_t val) { _sreg(STM32_USART_CR3_OFFSET) = val; } + uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } + }; -/* XXX hack to avoid expensive IRQ lookup */ -static PX4IO_serial *io_serial; +IOPacket PX4IO_serial::_dma_buffer; PX4IO_interface *io_serial_interface(int port) { - return new PX4IO_serial(port); + return new PX4IO_serial(); } -PX4IO_serial::PX4IO_serial(int port) : - _serial_base(0), - _vector(0), - _tx_buf(nullptr), - _tx_size(0), - _rx_size(0), - _stream(0) +PX4IO_serial::PX4IO_serial() : + _tx_dma(nullptr), + _rx_dma(nullptr), + _expect_reply(false) { - /* only allow one instance */ - if (io_serial != nullptr) + /* allocate DMA */ + _tx_dma = stm32_dmachannel(_serial_tx_dma); + _rx_dma = stm32_dmachannel(_serial_rx_dma); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return; - switch (port) { - case 5: - _serial_base = (volatile uint32_t *)STM32_UART5_BASE; - _vector = STM32_IRQ_UART5; - break; - default: - /* not a supported port */ - return; - } + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - /* XXX need to configure the port here */ + /* reset & configure the UART */ + _CR1(0); + _CR2(0); + _CR3(0); - /* need space for worst-case escapes + hx protocol overhead */ - /* XXX this is kinda gross, but hx transmits a byte at a time */ - _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - irq_attach(_vector, &_interrupt); + /* enable UART and DMA but no interrupts */ + _CR3(USART_CR3_DMAR | USART_CR3_DMAT); + _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE); - _stream = hx_stream_init(-1, _rx, this); + /* configure DMA */ + _reset_dma(); + /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); } PX4IO_serial::~PX4IO_serial() { + if (_tx_dma != nullptr) + stm32_dmafree(_tx_dma); + if (_rx_dma != nullptr) + stm32_dmafree(_rx_dma); - if (_tx_buf != nullptr) - delete[] _tx_buf; - - if (_vector) - irq_detach(_vector); - - if (io_serial == this) - io_serial = nullptr; - - if (_stream) - hx_stream_free(_stream); + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); @@ -196,13 +224,9 @@ PX4IO_serial::~PX4IO_serial() bool PX4IO_serial::ok() { - if (_serial_base == 0) - return false; - if (_vector == 0) - return false; - if (_tx_buf == nullptr) + if (_tx_dma == nullptr) return false; - if (!_stream) + if (_rx_dma == nullptr) return false; return true; @@ -211,22 +235,20 @@ PX4IO_serial::ok() int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > (HX_STREAM_MAX_FRAME - 2)) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - memcpy(&_tx_buf[2], (void *)values, count); - - _tx_size = count + 2; - _rx_size = 0; + _dma_buffer.count = num_values | PKT_CTRL_WRITE; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(false); sem_post(&_bus_semaphore); return result; @@ -235,31 +257,28 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > HX_STREAM_MAX_FRAME) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - _tx_buf[2] = num_values; - - _tx_size = 3; /* this tells IO that this is a read request */ - _rx_size = count; + _dma_buffer.count = num_values; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(true); if (result != OK) goto out; /* compare the received count with the expected count */ - if (_rx_size != count) { + if (_dma_buffer.count != num_values) { return -EIO; } else { + /* XXX implement check byte */ /* copy back the result */ - memcpy(values, &_rx_buf[0], count); + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); } out: sem_post(&_bus_semaphore); @@ -267,14 +286,18 @@ out: } int -PX4IO_serial::_wait_complete() +PX4IO_serial::_wait_complete(bool expect_reply) { - /* prepare the stream for transmission (also discards any received noise) */ - hx_stream_reset(_stream); - hx_stream_start(_stream, _tx_buf, _tx_size); - /* enable transmit-ready interrupt, which will start transmission */ - _CR1() |= USART_CR1_TXEIE; + /* save for callback use */ + _expect_reply = expect_reply; + + /* start RX DMA */ + if (expect_reply) + stm32_dmastart(_rx_dma, _dma_callback, this, false); + + /* start TX DMA - no callback if we also expect a reply */ + stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false); /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -285,68 +308,82 @@ PX4IO_serial::_wait_complete() abstime.tv_nsec -= 1000000000; } - /* wait for the transaction to complete */ - int ret = sem_timedwait(&_completion_semaphore, &abstime); + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); - return ret; -} + if (ret == OK) + break; -int -PX4IO_serial::_interrupt(int irq, void *context) -{ - /* ... because NuttX doesn't give us a handle per vector */ - io_serial->_do_interrupt(); - return 0; -} - -void -PX4IO_serial::_do_interrupt() -{ - uint32_t sr = _SR(); - - /* handle transmit completion */ - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - _CR1() &= ~USART_CR1_TXEIE; - - /* was this a tx-only operation? */ - if (_rx_size == 0) { - /* wake up waiting sender */ - sem_post(&_completion_semaphore); - } - } else { - _DR() = c; + if (ret == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _reset_dma(); + break; } } - if (sr & USART_SR_RXNE) { - uint8_t c = _DR(); - - hx_stream_rx(_stream, c); - } + return ret; } void -PX4IO_serial::_rx(void *arg, const void *data, size_t length) +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - PX4IO_serial *pserial = reinterpret_cast(arg); + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast(arg); - pserial->_do_rx((const uint8_t *)data, length); + ps->_do_dma_callback(handle, status); + } } void -PX4IO_serial::_do_rx(const uint8_t *data, size_t length) +PX4IO_serial::_do_dma_callback(DMA_HANDLE handle, uint8_t status) { - _rx_buf = data; - - if (length < _rx_size) - _rx_size = length; + /* on completion of a no-reply transmit, wake the sender */ + if (handle == _tx_dma) { + if ((status & DMA_STATUS_TCIF) && !_expect_reply) { + _dma_status = status; + sem_post(&_completion_semaphore); + } + /* XXX if we think we're going to see DMA errors, we should handle them here */ + } - /* notify waiting receiver */ - sem_post(&_completion_semaphore); + /* on completion of a reply, wake the waiter */ + if (handle == _rx_dma) { + if (status & DMA_STATUS_TCIF) { + /* XXX if we are worried about overrun/synch, check UART status here */ + _dma_status = status; + sem_post(&_completion_semaphore); + } + } } - - +void +PX4IO_serial::_reset_dma() +{ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); +} \ No newline at end of file diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f691969c4b..3fedfeb2c2 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,14 +56,29 @@ //#define DEBUG #include "px4io.h" -static hx_stream_t if_stream; static volatile bool sending = false; -static int serial_interrupt(int vector, void *context); -static void serial_callback(void *arg, const void *data, unsigned length); +static void dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +#define MAX_RW_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[MAX_RW_REGS]; +}; +#pragma pack(pop) + +static struct IOPacket dma_packet; /* serial register accessors */ -#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x)) +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) #define rSR REG(STM32_USART_SR_OFFSET) #define rDR REG(STM32_USART_DR_OFFSET) #define rBRR REG(STM32_USART_BRR_OFFSET) @@ -75,13 +90,51 @@ static void serial_callback(void *arg, const void *data, unsigned length); void interface_init(void) { - - /* XXX do serial port init here */ - - irq_attach(SERIAL_VECTOR, serial_interrupt); - if_stream = hx_stream_init(-1, serial_callback, NULL); - - /* XXX add stream stats counters? */ + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* enable UART and DMA */ + rCR3 = USART_CR3_DMAR | USART_CR3_DMAT; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE; + + /* configure DMA */ + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the first packet */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); debug("serial init"); } @@ -89,76 +142,56 @@ interface_init(void) void interface_tick() { - /* XXX nothing interesting to do here */ + /* XXX look for stuck/damaged DMA and reset? */ } -static int -serial_interrupt(int vector, void *context) +static void +dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - uint32_t sr = rSR; - - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(if_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - rCR1 &= ~USART_CR1_TXEIE; - sending = false; - } else { - rDR = c; - } + if (!(status & DMA_STATUS_TCIF)) { + /* XXX what do we do here? it's fatal! */ + return; } - if (sr & USART_SR_RXNE) { - uint8_t c = rDR; - - hx_stream_rx(if_stream, c); + /* if this is transmit-complete, make a note */ + if (handle == tx_dma) { + sending = false; + return; } - return 0; -} -static void -serial_callback(void *arg, const void *data, unsigned length) -{ - const uint8_t *message = (const uint8_t *)data; + /* we just received a request; sort out what to do */ + /* XXX implement check byte */ + /* XXX if we care about overruns, check the UART received-data-ready bit */ + bool send_reply = false; + if (dma_packet.count & PKT_CTRL_WRITE) { - /* malformed frame, ignore it */ - if (length < 2) - return; - - /* we got a new request while we were still sending the last reply - not supported */ - if (sending) - return; + /* it's a blind write - pass it on */ + registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count); + } else { - /* reads are page / offset / length */ - if (length == 3) { - uint16_t *registers; + /* it's a read - get register pointer for reply */ + int result; unsigned count; + uint16_t *registers; - /* get registers for response, send an empty reply on error */ - if (registers_get(message[0], message[1], ®isters, &count) < 0) + result = registers_get(dma_packet.page, dma_packet.offset, ®isters, &count); + if (result < 0) count = 0; - /* constrain count to requested size or message limit */ - if (count > message[2]) - count = message[2]; - if (count > HX_STREAM_MAX_FRAME) - count = HX_STREAM_MAX_FRAME; + /* constrain reply to packet size */ + if (count > MAX_RW_REGS) + count = MAX_RW_REGS; - /* start sending the reply */ - sending = true; - hx_stream_reset(if_stream); - hx_stream_start(if_stream, registers, count * 2 + 2); - - /* enable the TX-ready interrupt */ - rCR1 |= USART_CR1_TXEIE; - return; + /* copy reply registers into DMA buffer */ + send_reply = true; + memcpy((void *)&dma_packet.regs[0], registers, count); + dma_packet.count = count; + } - } else { + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); - /* it's a write operation, pass it to the register API */ - registers_set(message[0], - message[1], - (const uint16_t *)&message[2], - (length - 2) / 2); - } -} \ No newline at end of file + /* if we have a reply to send, start that now */ + if (send_reply) + stm32_dmastart(tx_dma, dma_callback, NULL, false); +}