From 432c2d1758197ff071da2a774ab557bdcb004e00 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 5 Feb 2022 12:45:08 +1100
Subject: [PATCH] AP_Bootloader: support firmware update from px4 firmware

allows droncan_gui_tool to update px4 firmware with ArduPilot
bootloader
---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  6 +++-
 Tools/AP_Bootloader/can.cpp           | 49 +++++++++++++++++++++++++--
 Tools/AP_Bootloader/can.h             |  2 +-
 3 files changed, 52 insertions(+), 5 deletions(-)

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index 7f0960c71d..2a65c43c4a 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -94,7 +94,11 @@ int main(void)
         timeout = 10000;
         can_set_node_id(m & 0xFF);
     }
-    can_check_update();
+    if (can_check_update()) {
+        // trying to update firmware, stay in bootloader
+        try_boot = false;
+        timeout = 0;
+    }
     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 756ff91163..d51ec769bc 100644
--- a/Tools/AP_Bootloader/can.cpp
+++ b/Tools/AP_Bootloader/can.cpp
@@ -678,18 +678,61 @@ bool can_check_firmware(void)
 }
 
 // check for a firmware update marker left by app
-void can_check_update(void)
+bool can_check_update(void)
 {
+    bool ret = false;
 #if HAL_RAM_RESERVE_START >= 256
     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);
         fw_update.node_id = comms->server_node_id;
         memcpy(fw_update.path, comms->path, UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1);
+        ret = true;
+        // clear comms region
+        memset(comms, 0, sizeof(struct app_bootloader_comms));
     }
-    // clear comms region
-    memset(comms, 0, sizeof(struct app_bootloader_comms));
 #endif
+#if defined(CAN1_BASE) && defined(RCC_APB1ENR_CAN1EN)
+    // check for px4 fw update. px4 uses the filter registers in CAN1
+    // to communicate with the bootloader. This only works on CAN1
+    if (!ret && stm32_was_software_reset()) {
+        uint32_t *fir = (uint32_t *)(CAN1_BASE + 0x240);
+        struct PACKED app_shared {
+            union {
+                uint64_t ull;
+                uint32_t ul[2];
+                uint8_t  valid;
+            } crc;
+            uint32_t signature;
+            uint32_t bus_speed;
+            uint32_t node_id;
+        } *app = (struct app_shared *)&fir[4];
+        /* we need to enable the CAN peripheral in order to look at
+           the FIR registers.
+        */
+        RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+        static const uint32_t app_signature = 0xb0a04150;
+        if (app->signature == app_signature &&
+            app->node_id > 0 && app->node_id < 128) {
+            // crc is in reversed word order in FIR registers
+            uint32_t sig[3];
+            memcpy((uint8_t *)&sig[0], (const uint8_t *)&app->signature, sizeof(sig));
+            const uint64_t crc = crc_crc64(sig, 3);
+            const uint32_t *crc32 = (const uint32_t *)&crc;
+            if (crc32[0] == app->crc.ul[1] &&
+                crc32[1] == app->crc.ul[0]) {
+                // reset signature so we don't get in a boot loop
+                app->signature = 0;
+                // setup node ID
+                can_set_node_id(app->node_id);
+                // and baudrate
+                baudrate = app->bus_speed;
+                ret = true;
+            }
+        }
+    }
+#endif
+    return ret;
 }
 
 void can_start()
diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h
index 5ef6a59ad4..22a8a24c9f 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);
+bool can_check_update(void);