From ec73a7072b720731867699c5dd977a37d1a4ecf9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 31 Oct 2018 11:24:51 +1100
Subject: [PATCH] AP_IOMCU: use more bandwidth efficient IO read

---
 libraries/AP_IOMCU/AP_IOMCU.cpp              | 10 ++++++++--
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 14 +++++++++++++-
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   |  2 +-
 3 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 15f40203ec..0b829f1951 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -360,14 +360,20 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     pkt.page = page;
     pkt.offset = offset;
     pkt.crc = 0;
+
+    uint8_t pkt_size = pkt.get_size();
+    if (config.protocol_version == IOMCU_PROTOCOL_VERSION) {
+        // save bandwidth on reads
+        pkt_size = 4;
+    }
     
     /*
       the protocol is a bit strange, as it unnecessarily sends the
       same size packet that it expects to receive. This means reading
       a large number of registers wastes a lot of serial bandwidth
      */
-    pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
-    if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) {
+    pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
+    if (uart.write((uint8_t *)&pkt, pkt_size) != pkt_size) {
         protocol_fail_count++;
         return false;
     }
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 790b602d8d..c66b74e444 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -156,6 +156,8 @@ void loop(void)
 
 void AP_IOMCU_FW::init()
 {
+    config.protocol_version = IOMCU_PROTOCOL_VERSION;
+
     thread_ctx = chThdGetSelfX();
 
     if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) {
@@ -261,8 +263,18 @@ void AP_IOMCU_FW::rcin_update()
 void AP_IOMCU_FW::process_io_packet()
 {
     uint8_t rx_crc = rx_io_packet.crc;
+    uint8_t calc_crc;
     rx_io_packet.crc = 0;
-    uint8_t calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, rx_io_packet.get_size());
+    uint8_t pkt_size = rx_io_packet.get_size();
+    if (rx_io_packet.code == CODE_READ) {
+        // allow for more bandwidth efficient read packets
+        calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, 4);
+        if (calc_crc != rx_crc) {
+            calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
+        }
+    } else {
+        calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
+    }
     if (rx_crc != calc_crc) {
         memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
         tx_io_packet.count = 0;
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index be12d22457..5fad322ce4 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -90,7 +90,7 @@ enum iopage {
 #define FORCE_SAFETY_MAGIC 22027
 
 struct PACKED page_config {
-    uint16_t protocol_version = IOMCU_PROTOCOL_VERSION;
+    uint16_t protocol_version;
 };
 
 struct PACKED page_reg_status {