diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index c514043120..964627c636 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -114,7 +114,7 @@ public: ArchPX4IOSerial(); ArchPX4IOSerial(ArchPX4IOSerial &) = delete; ArchPX4IOSerial &operator = (const ArchPX4IOSerial &) = delete; - ~ArchPX4IOSerial(); + virtual ~ArchPX4IOSerial(); virtual int init(); virtual int ioctl(unsigned operation, unsigned &arg); @@ -159,7 +159,6 @@ private: /** * Performance counters. */ - perf_counter_t _pc_dmasetup; perf_counter_t _pc_dmaerrs; /** diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp index 91d4b0fe87..bb0c6a8d16 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp @@ -58,13 +58,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -99,7 +93,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -202,7 +195,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -241,7 +233,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -288,8 +279,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -309,6 +298,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -319,6 +310,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break; diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp index 20a952f421..fab13855c3 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp @@ -68,13 +68,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -109,7 +103,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -214,7 +207,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -256,7 +248,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -307,8 +298,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 &= ~USART_CR1_TE; //rCR1 |= USART_CR1_TE; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -328,8 +317,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - // stream transfer error, ensure TX DMA is also stopped before exiting early - stm32_dmastop(_tx_dma); + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -340,6 +329,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break; diff --git a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp index 451571f4cc..b6bd7e6414 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp @@ -106,13 +106,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -147,7 +141,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -252,7 +245,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -294,7 +286,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -354,8 +345,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 &= ~USART_CR1_TE; //rCR1 |= USART_CR1_TE; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -375,8 +364,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - // stream transfer error, ensure TX DMA is also stopped before exiting early - stm32_dmastop(_tx_dma); + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -387,6 +376,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break;