From 9c4a01e0b06f959415a2d5890a49c374b825dd26 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 21 Oct 2019 17:44:02 +1100
Subject: [PATCH] AP_Bootloader: speed up CAN fw load

---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  1 +
 Tools/AP_Bootloader/can.cpp           | 37 +++++++++++++++++----------
 Tools/AP_Bootloader/can.h             |  2 +-
 3 files changed, 25 insertions(+), 15 deletions(-)

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index 2635677f65..f2a26e7746 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -83,6 +83,7 @@ int main(void)
         timeout = 10000;
         can_set_node_id(m & 0xFF);
     }
+    can_check_update();
     if (!can_check_firmware()) {
         // bad firmware CRC, don't try and boot
         timeout = 0;
diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp
index b87586ad76..556243717b 100644
--- a/Tools/AP_Bootloader/can.cpp
+++ b/Tools/AP_Bootloader/can.cpp
@@ -243,19 +243,21 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
     if (transfer->payload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) {
         return;
     }
-    uint32_t offset = 0;
-    canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id);
-    offset += 8;
-    for (uint8_t i=0; i<transfer->payload_len-1; i++) {
-        canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]);
-        offset += 8;
-    }
-    fw_update.ofs = 0;
-    fw_update.last_ms = 0;
-    fw_update.sector = 0;
-    fw_update.sector_ofs = 0;
     if (fw_update.node_id == 0) {
-        fw_update.node_id = transfer->source_node_id;
+        uint32_t offset = 0;
+        canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id);
+        offset += 8;
+        for (uint8_t i=0; i<transfer->payload_len-1; i++) {
+            canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]);
+            offset += 8;
+        }
+        fw_update.ofs = 0;
+        fw_update.last_ms = 0;
+        fw_update.sector = 0;
+        fw_update.sector_ofs = 0;
+        if (fw_update.node_id == 0) {
+            fw_update.node_id = transfer->source_node_id;
+        }
     }
 
     uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE];
@@ -547,6 +549,10 @@ void can_set_node_id(uint8_t node_id)
  */
 bool can_check_firmware(void)
 {
+    if (fw_update.node_id != 0) {
+        // we're doing an update, don't boot this fw
+        return false;
+    }
     const uint8_t sig[8] = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 };
     const uint8_t *flash = (const uint8_t *)(FLASH_LOAD_ADDRESS + FLASH_BOOTLOADER_LOAD_KB*1024);
     const uint32_t flash_size = (BOARD_FLASH_SIZE - FLASH_BOOTLOADER_LOAD_KB)*1024;
@@ -579,10 +585,10 @@ bool can_check_firmware(void)
     return true;
 }
 
-void can_start()
+// check for a firmware update marker left by app
+void can_check_update(void)
 {
 #if HAL_RAM_RESERVE_START >= 256
-    // check for a firmware update marker left by app
     struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START;
     if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) {
         can_set_node_id(comms->my_node_id);
@@ -592,7 +598,10 @@ void can_start()
     // clear comms region
     memset(comms, 0, sizeof(struct app_bootloader_comms));
 #endif
+}
 
+void can_start()
+{
     // calculate optimal CAN timings given PCLK1 and baudrate
     CanardSTM32CANTimings timings {};
     canardSTM32ComputeCANTimings(STM32_PCLK1, baudrate, &timings);
diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h
index 2239a4e718..5ef6a59ad4 100644
--- a/Tools/AP_Bootloader/can.h
+++ b/Tools/AP_Bootloader/can.h
@@ -2,4 +2,4 @@ void can_start();
 void can_update();
 void can_set_node_id(uint8_t node_id);
 bool can_check_firmware(void);
-
+void can_check_update(void);