Always send and expect a reply for every message.

2 files changed, 39 insertions, 49 deletions

diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp
index 5d9eac076..1ce9e9c6d 100644
--- a/src/drivers/px4io/interface_serial.cpp
+++ b/src/drivers/px4io/interface_serial.cpp
@@ -167,8 +167,7 @@ private:
 	perf_counter_t		_perf_dmasetup;
 	perf_counter_t		_perf_timeouts;
 	perf_counter_t		_perf_errors;
-	perf_counter_t		_perf_wr;
-	perf_counter_t		_perf_rd;
+	perf_counter_t		_perf_txns;
 
 };
 
@@ -189,8 +188,7 @@ PX4IO_serial::PX4IO_serial() :
 	_perf_dmasetup(perf_alloc(PC_ELAPSED,	"dmasetup")),
 	_perf_timeouts(perf_alloc(PC_COUNT,	"timeouts")),
 	_perf_errors(perf_alloc(PC_COUNT,	"errors  ")),
-	_perf_wr(perf_alloc(PC_ELAPSED,		"writes  ")),
-	_perf_rd(perf_alloc(PC_ELAPSED,		"reads   "))
+	_perf_txns(perf_alloc(PC_ELAPSED,	"txns    "))
 {
 	/* allocate DMA */
 	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
@@ -305,10 +303,12 @@ PX4IO_serial::test(unsigned mode)
 					return -2;
 				if (count > 100) {
 					perf_print_counter(_perf_dmasetup);
-					perf_print_counter(_perf_wr);
+					perf_print_counter(_perf_txns);
+					perf_print_counter(_perf_timeouts);
+					perf_print_counter(_perf_errors);
 					count = 0;
 				}
-				usleep(100000);
+				usleep(10000);
 			}
 			return 0;
 		}
@@ -384,35 +384,29 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length)
 	(void)rDR;
 
 	/* start RX DMA */
-	if (expect_reply) {
-		perf_begin(_perf_rd);
-		perf_begin(_perf_dmasetup);
-
-		/* DMA setup time ~3µs */
-		_rx_dma_status = _dma_status_waiting;
-		_rx_length = 0;
-		stm32_dmasetup(
-			_rx_dma,
-			PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
-			reinterpret_cast<uint32_t>(&_dma_buffer),
-			sizeof(_dma_buffer),
-			DMA_SCR_DIR_P2M		|
-			DMA_SCR_MINC		|
-			DMA_SCR_PSIZE_8BITS	|
-			DMA_SCR_MSIZE_8BITS	|
-			DMA_SCR_PBURST_SINGLE	|
-			DMA_SCR_MBURST_SINGLE);
-		stm32_dmastart(_rx_dma, _dma_callback, this, false);
-		rCR3 |= USART_CR3_DMAR;
-
-		perf_end(_perf_dmasetup);
-	} else {
-		perf_begin(_perf_wr);
-	}
+	perf_begin(_perf_txns);
+	perf_begin(_perf_dmasetup);
+
+	/* DMA setup time ~3µs */
+	_rx_dma_status = _dma_status_waiting;
+	_rx_length = 0;
+	stm32_dmasetup(
+		_rx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		sizeof(_dma_buffer),
+		DMA_SCR_DIR_P2M		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_rx_dma, _dma_callback, this, false);
+	rCR3 |= USART_CR3_DMAR;
+
 
 	/* start TX DMA - no callback if we also expect a reply */
 	/* DMA setup time ~3µs */
-	perf_begin(_perf_dmasetup);
 	_tx_dma_status = _dma_status_waiting;
 	stm32_dmasetup(
 		_tx_dma,
@@ -476,7 +470,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length)
 	_rx_dma_status = _dma_status_inactive;
 
 	/* update counters */
-	perf_end(expect_reply ? _perf_rd : _perf_wr);
+	perf_end(_perf_txns);
 	if (ret != OK)
 		perf_count(_perf_errors);
 
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index 4eef99d9f..c109cb57f 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -192,7 +192,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	perf_count(pc_rx);
 
 	/* default to not sending a reply */
-	bool send_reply = false;
 	if (dma_packet.count & PKT_CTRL_WRITE) {
 
 		dma_packet.count &= ~PKT_CTRL_WRITE;
@@ -217,7 +216,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 			count = MAX_RW_REGS;
 
 		/* copy reply registers into DMA buffer */
-		send_reply = true;
 		memcpy((void *)&dma_packet.regs[0], registers, count);
 		dma_packet.count = count;
 	}
@@ -228,20 +226,18 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	dma_reset();
 
 	/* if we have a reply to send, start that now */
-	if (send_reply) {
-		stm32_dmasetup(
-			tx_dma,
-			(uint32_t)&rDR,
-			(uint32_t)&dma_packet,
-			sizeof(dma_packet),		/* XXX cut back to actual transmit size */
-			DMA_CCR_DIR		|
-			DMA_CCR_MINC		|
-			DMA_CCR_PSIZE_8BITS	|
-			DMA_CCR_MSIZE_8BITS);
-		sending = true;
-		stm32_dmastart(tx_dma, tx_dma_callback, NULL, false);
-		rCR3 |= USART_CR3_DMAT;
-	}
+	stm32_dmasetup(
+		tx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		sizeof(dma_packet),		/* XXX cut back to actual transmit size */
+		DMA_CCR_DIR		|
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+	sending = true;
+	stm32_dmastart(tx_dma, tx_dma_callback, NULL, false);
+	rCR3 |= USART_CR3_DMAT;
 }
 
 static int