From 57ac86edd916ded485f5c0ee962897f044e63fe6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 21 Dec 2023 15:06:36 +0000 Subject: [PATCH] AP_IOMCU: fix occasional startup internal errors with mixing allow DIRECT_PWM pages to be smaller than max channels correct some over-eager register clearing in the global interrupt handler (NFC) only sent TX events when using shared DMA (NFC) zero out rx packet code and size to prevent errors with spurious callbacks add a comment and check for offsets that are codes --- libraries/AP_IOMCU/AP_IOMCU.cpp | 5 +- libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 59 +++++++++++++------- libraries/AP_IOMCU/iofirmware/ioprotocol.h | 1 + 3 files changed, 44 insertions(+), 21 deletions(-) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index b77f4f94c9..4768ec6e2f 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -691,7 +691,10 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 */ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs) { - while (count > PKT_MAX_REGS) { + // The use of offset is very, very evil - it can either be a command within the page + // or a genuine offset, offsets within PAGE_SETUP are assumed to be commands, otherwise to be an + // actual offset + while (page != PAGE_SETUP && count > PKT_MAX_REGS) { if (!write_registers(page, offset, PKT_MAX_REGS, regs)) { return false; } diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 55e67dd997..2b1e65d145 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -51,6 +51,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); */ #define IOMCU_ENABLE_RESET_TEST 0 +//#define IOMCU_LOOP_TIMING_DEBUG // enable timing GPIO pings #ifdef IOMCU_LOOP_TIMING_DEBUG #undef TOGGLE_PIN_DEBUG @@ -103,9 +104,6 @@ static void dma_rx_end_cb(hal_uart_driver *uart) chSysLockFromISR(); uart->usart->CR3 &= ~USART_CR3_DMAR; - (void)uart->usart->SR; // sequence to clear IDLE status - (void)uart->usart->DR; - (void)uart->usart->DR; dmaStreamDisable(uart->dmarx); iomcu.process_io_packet(); @@ -139,8 +137,11 @@ static void dma_tx_end_cb(hal_uart_driver *uart) TOGGLE_PIN_DEBUG(108); TOGGLE_PIN_DEBUG(108); #endif - +#if AP_HAL_SHARED_DMA_ENABLED + chSysLockFromISR(); chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END); + chSysUnlockFromISR(); +#endif } /* replacement for ChibiOS uart_lld_serve_interrupt() */ @@ -154,41 +155,44 @@ static void idle_rx_handler(hal_uart_driver *uart) USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE | USART_SR_PE)) { /* framing error - start/stop bit lost or line break */ + + (void)uart->usart->DR; /* SR reset step 2 - clear ORE | FE.*/ + /* send a line break - this will abort transmission/reception on the other end */ chSysLockFromISR(); - uart->usart->SR = ~USART_SR_LBD; uart->usart->CR1 = cr1 | USART_CR1_SBK; + iomcu.reg_status.num_errors++; iomcu.reg_status.err_uart++; + /* disable RX DMA */ uart->usart->CR3 &= ~USART_CR3_DMAR; - (void)uart->usart->SR; // clears ORE | FE - (void)uart->usart->DR; - (void)uart->usart->DR; setup_rx_dma(uart); chSysUnlockFromISR(); - return; } if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) { - chSysLockFromISR(); - /* TC interrupt cleared and disabled.*/ uart->usart->SR &= ~USART_SR_TC; uart->usart->CR1 = cr1 & ~USART_CR1_TCIE; - +#ifdef HAL_GPIO_LINE_GPIO105 + TOGGLE_PIN_DEBUG(105); + TOGGLE_PIN_DEBUG(105); +#endif /* End of transmission, a callback is generated.*/ - _uart_tx2_isr_code(uart); - - chSysUnlockFromISR(); + dma_tx_end_cb(uart); } - if (sr & USART_SR_IDLE) { + if ((sr & USART_SR_IDLE) && (cr1 & USART_CR1_IDLEIE)) { + (void)uart->usart->DR; /* SR reset step 2 - clear IDLE.*/ + /* the DMA size is the maximum packet size, but smaller packets are perfectly possible leading to an IDLE ISR. The data still must be processed. */ + + /* End of receive, a callback is generated.*/ dma_rx_end_cb(uart); } } @@ -248,9 +252,9 @@ static UARTConfig uart_cfg = { dma_tx_end_cb, dma_rx_end_cb, nullptr, - nullptr, - idle_rx_handler, - nullptr, + nullptr, // error + idle_rx_handler, // global irq + nullptr, // idle 1500000, //1.5MBit USART_CR1_IDLEIE, 0, @@ -660,6 +664,12 @@ void AP_IOMCU_FW::process_io_packet() { iomcu.reg_status.total_pkts++; + if (rx_io_packet.code == CODE_NOOP) { + iomcu.reg_status.num_errors++; + iomcu.reg_status.err_bad_opcode++; + return; + } + uint8_t rx_crc = rx_io_packet.crc; uint8_t calc_crc; rx_io_packet.crc = 0; @@ -714,9 +724,18 @@ void AP_IOMCU_FW::process_io_packet() default: { iomcu.reg_status.num_errors++; iomcu.reg_status.err_bad_opcode++; + rx_io_packet.code = CODE_NOOP; + rx_io_packet.count = 0; + return; } break; } + + // prevent a spurious DMA callback from doing anything bad + rx_io_packet.code = CODE_NOOP; + rx_io_packet.count = 0; + + return; } /* @@ -942,7 +961,7 @@ bool AP_IOMCU_FW::handle_code_write() // no input when override is active break; } - if (rx_io_packet.count != sizeof(reg_direct_pwm.pwm)/2) { + if (rx_io_packet.count > sizeof(reg_direct_pwm.pwm)/2) { return false; } /* copy channel data */ diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 01a29b15ff..9e13fafc48 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -37,6 +37,7 @@ enum iocode { // read types CODE_READ = 0, CODE_WRITE = 1, + CODE_NOOP = 2, // reply codes CODE_SUCCESS = 0,