From be6ad7af3b65841d2b460e3064c166dc9167401f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 00:08:12 -0700 Subject: 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/modules/px4iofirmware/serial.c | 169 ++++++++++++++++++++++--------------- 1 file changed, 101 insertions(+), 68 deletions(-) (limited to 'src/modules/px4iofirmware/serial.c') diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f691969c4..3fedfeb2c 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); +} -- cgit v1.2.3