/src/modules/px4iofirmware/serial.c
C | 358 lines | 205 code | 70 blank | 83 comment | 23 complexity | f074778f2cb87aa2b9b4113285479740 MD5 | raw file
- /****************************************************************************
- *
- * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
- /**
- * @file serial.c
- *
- * Serial communication for the PX4IO module.
- */
- #include <stdint.h>
- #include <unistd.h>
- #include <termios.h>
- #include <fcntl.h>
- #include <string.h>
- #include <nuttx/arch.h>
- #include <arch/board/board.h>
- /* XXX might be able to prune these */
- #include <chip.h>
- #include <up_internal.h>
- #include <up_arch.h>
- #include <stm32.h>
- #include <systemlib/perf_counter.h>
- //#define DEBUG
- #include "px4io.h"
- static perf_counter_t pc_txns;
- static perf_counter_t pc_errors;
- static perf_counter_t pc_ore;
- static perf_counter_t pc_fe;
- static perf_counter_t pc_ne;
- static perf_counter_t pc_idle;
- static perf_counter_t pc_badidle;
- static perf_counter_t pc_regerr;
- static perf_counter_t pc_crcerr;
- static void rx_handle_packet(void);
- static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
- static DMA_HANDLE tx_dma;
- static DMA_HANDLE rx_dma;
- static int serial_interrupt(int irq, void *context);
- static void dma_reset(void);
- static struct IOPacket dma_packet;
- /* serial register accessors */
- #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)
- #define rCR1 REG(STM32_USART_CR1_OFFSET)
- #define rCR2 REG(STM32_USART_CR2_OFFSET)
- #define rCR3 REG(STM32_USART_CR3_OFFSET)
- #define rGTPR REG(STM32_USART_GTPR_OFFSET)
- void
- interface_init(void)
- {
- pc_txns = perf_alloc(PC_ELAPSED, "txns");
- pc_errors = perf_alloc(PC_COUNT, "errors");
- pc_ore = perf_alloc(PC_COUNT, "overrun");
- pc_fe = perf_alloc(PC_COUNT, "framing");
- pc_ne = perf_alloc(PC_COUNT, "noise");
- pc_idle = perf_alloc(PC_COUNT, "idle");
- pc_badidle = perf_alloc(PC_COUNT, "badidle");
- pc_regerr = perf_alloc(PC_COUNT, "regerr");
- pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
- /* 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;
- /* clear status/errors */
- (void)rSR;
- (void)rDR;
- /* 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);
- /* connect our interrupt */
- irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
- up_enable_irq(PX4FMU_SERIAL_VECTOR);
- /* enable UART and error/idle interrupts */
- rCR3 = USART_CR3_EIE;
- rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
- #if 0 /* keep this for signal integrity testing */
- for (;;) {
- while (!(rSR & USART_SR_TXE))
- ;
- rDR = 0xfa;
- while (!(rSR & USART_SR_TXE))
- ;
- rDR = 0xa0;
- }
- #endif
- /* configure RX DMA and return to listening state */
- dma_reset();
- debug("serial init");
- }
- static void
- rx_handle_packet(void)
- {
- /* check packet CRC */
- uint8_t crc = dma_packet.crc;
- dma_packet.crc = 0;
- if (crc != crc_packet(&dma_packet)) {
- perf_count(pc_crcerr);
- /* send a CRC error reply */
- dma_packet.count_code = PKT_CODE_CORRUPT;
- dma_packet.page = 0xff;
- dma_packet.offset = 0xff;
- return;
- }
- if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
- /* it's a blind write - pass it on */
- if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
- perf_count(pc_regerr);
- dma_packet.count_code = PKT_CODE_ERROR;
- } else {
- dma_packet.count_code = PKT_CODE_SUCCESS;
- }
- return;
- }
- if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
- /* it's a read - get register pointer for reply */
- unsigned count;
- uint16_t *registers;
- if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
- perf_count(pc_regerr);
- dma_packet.count_code = PKT_CODE_ERROR;
- } else {
- /* constrain reply to requested size */
- if (count > PKT_MAX_REGS) {
- count = PKT_MAX_REGS;
- }
- if (count > PKT_COUNT(dma_packet)) {
- count = PKT_COUNT(dma_packet);
- }
- /* copy reply registers into DMA buffer */
- memcpy((void *)&dma_packet.regs[0], registers, count * 2);
- dma_packet.count_code = count | PKT_CODE_SUCCESS;
- }
- return;
- }
- /* send a bad-packet error reply */
- dma_packet.count_code = PKT_CODE_CORRUPT;
- dma_packet.page = 0xff;
- dma_packet.offset = 0xfe;
- }
- static void
- rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
- {
- /*
- * We are here because DMA completed, or UART reception stopped and
- * we think we have a packet in the buffer.
- */
- perf_begin(pc_txns);
- /* disable UART DMA */
- rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
- /* handle the received packet */
- rx_handle_packet();
- /* re-set DMA for reception first, so we are ready to receive before we start sending */
- dma_reset();
- /* send the reply to the just-processed request */
- dma_packet.crc = 0;
- dma_packet.crc = crc_packet(&dma_packet);
- stm32_dmasetup(
- tx_dma,
- (uint32_t)&rDR,
- (uint32_t)&dma_packet,
- PKT_SIZE(dma_packet),
- DMA_CCR_DIR |
- DMA_CCR_MINC |
- DMA_CCR_PSIZE_8BITS |
- DMA_CCR_MSIZE_8BITS);
- stm32_dmastart(tx_dma, NULL, NULL, false);
- rCR3 |= USART_CR3_DMAT;
- perf_end(pc_txns);
- }
- static int
- serial_interrupt(int irq, void *context)
- {
- static bool abort_on_idle = false;
- uint32_t sr = rSR; /* get UART status register */
- (void)rDR; /* required to clear any of the interrupt status that brought us here */
- if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
- USART_SR_NE | /* noise error - we have lost a byte due to noise */
- USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
- perf_count(pc_errors);
- if (sr & USART_SR_ORE) {
- perf_count(pc_ore);
- }
- if (sr & USART_SR_NE) {
- perf_count(pc_ne);
- }
- if (sr & USART_SR_FE) {
- perf_count(pc_fe);
- }
- /* send a line break - this will abort transmission/reception on the other end */
- rCR1 |= USART_CR1_SBK;
- /* when the line goes idle, abort rather than look at the packet */
- abort_on_idle = true;
- }
- if (sr & USART_SR_IDLE) {
- /*
- * If we saw an error, don't bother looking at the packet - it should have
- * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
- * ready for their retry.
- */
- if (abort_on_idle) {
- abort_on_idle = false;
- dma_reset();
- return 0;
- }
- /*
- * The sender has stopped sending - this is probably the end of a packet.
- * Check the received length against the length in the header to see if
- * we have something that looks like a packet.
- */
- unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
- if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
- /* it was too short - possibly truncated */
- perf_count(pc_badidle);
- dma_reset();
- return 0;
- }
- /*
- * Looks like we received a packet. Stop the DMA and go process the
- * packet.
- */
- perf_count(pc_idle);
- stm32_dmastop(rx_dma);
- rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
- }
- return 0;
- }
- static void
- dma_reset(void)
- {
- rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
- (void)rSR;
- (void)rDR;
- (void)rDR;
- /* kill any pending DMA */
- stm32_dmastop(tx_dma);
- stm32_dmastop(rx_dma);
- /* reset the RX side */
- 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 |
- DMA_CCR_PRIVERYHI);
- /* start receive DMA ready for the next packet */
- stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
- rCR3 |= USART_CR3_DMAR;
- }