diff --git a/platforms/nuttx/src/px4/nxp/imx9/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/nxp/imx9/px4io_serial/px4io_serial.cpp index 5d3403bef045..5fc5bedc63ea 100644 --- a/platforms/nuttx/src/px4/nxp/imx9/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/nxp/imx9/px4io_serial/px4io_serial.cpp @@ -77,6 +77,7 @@ ArchPX4IOSerial::~ArchPX4IOSerial() { /* Stop any ongoing DMA transfer */ + _waiting_for_dma = false; _stop_dma(); /* Detach our interrupt handler */ @@ -261,14 +262,15 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) ret = nxsem_tickwait_uninterruptible(&_recv_sem, MSEC2TICK(10)); - if (ret == -ETIMEDOUT) { + if (ret != OK) { + _waiting_for_dma = false; _stop_dma(); + nxsem_reset(&_recv_sem, 0); perf_count(_pc_timeouts); perf_cancel(_pc_txns); /* don't count this as a transaction */ - } - if (ret == OK) { + } else { /* Check packet CRC */ uint8_t crc = _current_packet->crc; @@ -348,7 +350,7 @@ ArchPX4IOSerial::_do_interrupt() * call the callback function. Stopping a non-running dma has no effect. */ - imx9_dmach_stop(_rx_dma); + _stop_dma(); } /* count uart errors */ @@ -361,10 +363,6 @@ ArchPX4IOSerial::_do_interrupt() void ArchPX4IOSerial::_stop_dma() { - /* Mark that we don't care about any DMA data any more */ - - _waiting_for_dma = false; - /* Stop the DMA channels */ imx9_dmach_stop(_tx_dma); @@ -375,8 +373,4 @@ ArchPX4IOSerial::_stop_dma() while (getreg32(PX4IO_SERIAL_BASE + IMX9_LPUART_STAT_OFFSET) & LPUART_STAT_RDRF) { getreg32(PX4IO_SERIAL_BASE + IMX9_LPUART_DATA_OFFSET); } - - /* Clear any error status flags */ - - modreg32(ERR_FLAGS_MASK, ERR_FLAGS_MASK, PX4IO_SERIAL_BASE + IMX9_LPUART_STAT_OFFSET); }