px4io firmware and px4io driver: Add CRC32 check for paranoid data integrity assurances.i2c_crc_master

2 files changed, 79 insertions, 42 deletions

diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
index c57ddf65b..a2a01c16b 100755
--- a/src/drivers/px4io/px4io_i2c.cpp
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -47,6 +47,7 @@
 #include <debug.h>
 #include <errno.h>
 #include <unistd.h>
+#include <crc32.h>
 
 #include <arch/board/board.h>
 #include <board_config.h>
@@ -121,8 +122,8 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count)
 		page,
 		offset
 	};
-
-	i2c_msg_s	msgv[2];
+	uint32_t 	crc = 0;
+	i2c_msg_s	msgv[3];
 
 	msgv[0].flags = 0;
 	msgv[0].buffer = addr;
@@ -132,7 +133,14 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count)
 	msgv[1].buffer = (uint8_t *)values;
 	msgv[1].length = 2 * count;
 
-	int ret = transfer(msgv, 2);
+	msgv[2].flags = I2C_M_NORESTART;
+	msgv[2].buffer = (uint8_t *)&crc;
+	msgv[2].length = sizeof(crc);
+
+	crc = crc32part(&addr[0], 2, crc);
+	crc = crc32part((const uint8_t*)data, 2 * count, crc);
+
+	int ret = transfer(msgv, 3);
 	if (ret == OK)
 		ret = count;
 	return ret;
@@ -150,7 +158,8 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count)
 		page,
 		offset
 	};
-	i2c_msg_s	msgv[2];
+	uint32_t crc;
+	i2c_msg_s	msgv[3];
 
 	msgv[0].flags = 0;
 	msgv[0].buffer = addr;
@@ -160,9 +169,23 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count)
 	msgv[1].buffer = (uint8_t *)values;
 	msgv[1].length = 2 * count;
 
-	int ret = transfer(msgv, 2);
-	if (ret == OK)
-		ret = count;
+	msgv[2].flags = I2C_M_READ | I2C_M_NORESTART;
+	msgv[2].buffer = (uint8_t *)&crc;
+	msgv[2].length = sizeof(crc);
+
+	int ret = transfer(msgv, 3);
+	if (ret == OK) {
+		uint32_t tcrc = 0;
+
+		tcrc = crc32part(&addr[0], 2, tcrc);
+		tcrc = crc32part((const uint8_t*)data, 2 * count, tcrc);
+
+		if (tcrc != crc) {
+			ret = EIO;
+		} else {
+			ret = count;
+		}
+	}
 	return ret;
 }
 
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 6d1d1fc2d..7b7e0dbd9 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -38,6 +38,8 @@
  */
 
 #include <stdint.h>
+#include <string.h>
+#include <crc32.h>
 
 #include <nuttx/arch.h>
 #include <arch/board/board.h>
@@ -77,14 +79,12 @@ static void		i2c_dump(void);
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
 
-static uint8_t		rx_buf[68];
+static uint8_t		xfer_buf[72];
 static unsigned		rx_len;
 
-static const uint8_t	junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
-
-static const uint8_t	*tx_buf = junk_buf;
-static unsigned		tx_len = sizeof(junk_buf);
+static unsigned		tx_len;
 unsigned		tx_count;
+volatile unsigned	i2c_crc_errors;
 
 static uint8_t		selected_page;
 static uint8_t		selected_offset;
@@ -247,7 +247,7 @@ i2c_rx_setup(void)
 	 * bailing out of a transaction while the master is still babbling at us.
 	 */
 	rx_len = 0;
-	stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
+	stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&xfer_buf[0], sizeof(xfer_buf),
 		DMA_CCR_CIRC |
 		DMA_CCR_MINC |
 		DMA_CCR_PSIZE_32BITS |
@@ -260,39 +260,55 @@ i2c_rx_setup(void)
 static void
 i2c_rx_complete(void)
 {
-	rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
+	rx_len = sizeof(xfer_buf) - stm32_dmaresidual(rx_dma);
 	stm32_dmastop(rx_dma);
 
-	if (rx_len >= 2) {
-		selected_page = rx_buf[0];
-		selected_offset = rx_buf[1];
+	// 2 bytes = address, count
+	if (rx_len == 2) {
+		uint16_t *regs;
+		unsigned reg_count;
+
+		/* initial CRC over the address/count */
+		uint32_t crc = crc32part(xfer_buf, rx_len, 0);
 
-		/* work out how many registers are being written */
-		unsigned count = (rx_len - 2) / 2;
-		if (count > 0) {
-			registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
+		/* work out which registers are being addressed */
+		int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
+		if (ret == 0) {
+			tx_len = reg_count * 2;
+			memcpy(xfer_buf, regs, tx_len);
 		} else {
-			/* no registers written, must be an address cycle */
-			uint16_t *regs;
-			unsigned reg_count;
-
-			/* work out which registers are being addressed */
-			int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
-			if (ret == 0) {
-				tx_buf = (uint8_t *)regs;
-				tx_len = reg_count * 2;
-			} else {
-				tx_buf = junk_buf;
-				tx_len = sizeof(junk_buf);
-			}
+			tx_len = 4;
+			memset(xfer_buf, 0xff, tx_len);
+		}
+
+		/* incremental CRC update with the data */
+		*(uint32_t *)&xfer_buf[tx_len] = crc32part(xfer_buf, tx_len, crc);
+		tx_len += 4;
+
+		/* disable interrupts while reconfiguring DMA for the selected registers */
+		irqstate_t flags = irqsave();
 
-			/* disable interrupts while reconfiguring DMA for the selected registers */
-			irqstate_t flags = irqsave();
+		stm32_dmastop(tx_dma);
+		i2c_tx_setup();
 
-			stm32_dmastop(tx_dma);
-			i2c_tx_setup();
+		irqrestore(flags);
+	}
+
+	// > 6 bytes = address, count, registers, crc32
+	if (rx_len > 6) {
+
+		// check CRC
+		if (crc32part(xfer_buf, rx_len - 4, 0) != *(uint32_t *)&xfer_buf[rx_len - 4]) {
+			i2c_crc_errors++;
+		} else {
+			selected_page = xfer_buf[0];
+			selected_offset = xfer_buf[1];
 
-			irqrestore(flags);
+			/* work out how many registers are being written */
+			unsigned count = (rx_len - 2) / 2;
+			if (count > 0) {
+				registers_set(selected_page, selected_offset, (const uint16_t *)&xfer_buf[2], count);
+			}
 		}
 	}
 
@@ -309,7 +325,7 @@ i2c_tx_setup(void)
 	 * to deal with bailing out of a transaction while the master is still 
 	 * babbling at us.
 	 */
-	stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
+	stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&xfer_buf[0], tx_len,
 		DMA_CCR_DIR |
 		DMA_CCR_CIRC |
 		DMA_CCR_MINC |
@@ -328,8 +344,6 @@ i2c_tx_complete(void)
 
 	/* for debug purposes, save the length of the last transmit as seen by the DMA */
 
-	/* leave tx_buf/tx_len alone, so that a retry will see the same data */
-
 	/* prepare for the next transaction */
 	i2c_tx_setup();
 }