Merged crccheck command

src/drivers/px4io/px4io.cpp

@@ -54,6 +54,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <math.h>
+#include <crc32.h>
 
 #include <arch/board/board.h>
 
@@ -96,6 +97,7 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 #define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 2)
+#define PX4IO_CHECK_CRC			_IOC(0xff00, 3)
 
 #define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
 #define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
@@ -1660,11 +1662,13 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
-	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC+1));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@@ -2157,6 +2161,19 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
+	case PX4IO_CHECK_CRC: {
+		/* check IO firmware CRC against passed value */		
+		uint32_t io_crc = 0;
+		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
+		if (ret != OK)
+			return ret;
+		if (io_crc != arg) {
+			debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
+			return -EINVAL;
+		}
+		break;
+	}
+
 	case PX4IO_INAIR_RESTART_ENABLE:
 
 		/* set/clear the 'in-air restart' bit */
@@ -2717,6 +2734,49 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "checkcrc")) {
+		/*
+		  check IO CRC against CRC of a file
+		 */
+		if (argc <= 2) {
+			printf("usage: px4io checkcrc filename\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		int fd = open(argv[2], O_RDONLY);
+		if (fd == -1) {
+			printf("open of %s failed - %d\n", argv[2], errno);
+			exit(1);			
+		}
+		const uint32_t app_size_max = 0xf000;
+		uint32_t fw_crc = 0;
+		uint32_t nbytes = 0;
+		while (true) {
+			uint8_t buf[16];
+			int n = read(fd, buf, sizeof(buf));
+			if (n <= 0) break;
+			fw_crc = crc32part(buf, n, fw_crc);
+			nbytes += n;
+		}
+		close(fd);
+		while (nbytes < app_size_max) {
+			uint8_t b = 0xff;
+			fw_crc = crc32part(&b, 1, fw_crc);			
+			nbytes++;
+		}
+
+		int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
+		if (ret != OK) {
+			printf("check CRC failed - %d\n", ret);
+			exit(1);			
+		}
+		printf("CRCs match\n");
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||

src/modules/px4iofirmware/protocol.h

@@ -189,6 +189,8 @@ enum {							/* DSM bind states */
 #define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
 #define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
 
+#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
 

src/modules/px4iofirmware/px4io.c

@@ -45,6 +45,7 @@
 #include <string.h>
 #include <poll.h>
 #include <signal.h>
+#include <crc32.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -124,6 +125,22 @@ heartbeat_blink(void)
 	LED_BLUE(heartbeat = !heartbeat);
 }
 
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+	// compute CRC of the current firmware
+	uint32_t sum = 0;
+	for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+		uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+		sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+	}
+	r_page_setup[PX4IO_P_SETUP_CRC]   = sum & 0xFFFF;
+	r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
 int
 user_start(int argc, char *argv[])
 {
@@ -136,6 +153,9 @@ user_start(int argc, char *argv[])
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* calculate our fw CRC so FMU can decide if we need to update */
+	calculate_fw_crc();
+
 	/*
 	 * Poll at 1ms intervals for received bytes that have not triggered
 	 * a DMA event.

src/modules/px4iofirmware/registers.c

@@ -157,6 +157,7 @@ volatile uint16_t	r_page_setup[] =
 #endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
+	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)