From 92bd1a485afa4afad89f4faab84e7b2a8d002df4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 21 Oct 2019 17:11:14 +1100
Subject: [PATCH] AP_Bootloader: added app descriptor checking for CAN fw

---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  9 ++++++
 Tools/AP_Bootloader/can.cpp           | 43 ++++++++++++++++++++++++++-
 Tools/AP_Bootloader/can.h             |  2 ++
 Tools/AP_Bootloader/wscript           |  9 +++++-
 4 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index ec45da6e03..2635677f65 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -83,6 +83,15 @@ int main(void)
         timeout = 10000;
         can_set_node_id(m & 0xFF);
     }
+    if (!can_check_firmware()) {
+        // bad firmware CRC, don't try and boot
+        timeout = 0;
+        try_boot = false;
+    } else if (timeout != 0) {
+        // fast boot for good firmware
+        try_boot = true;
+        timeout = 1000;
+    }
 #endif
     
     // if we fail to boot properly we want to pause in bootloader to give
diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp
index 2274d43a28..b87586ad76 100644
--- a/Tools/AP_Bootloader/can.cpp
+++ b/Tools/AP_Bootloader/can.cpp
@@ -19,6 +19,7 @@
 
 #if HAL_USE_CAN == TRUE
 #include <AP_Math/AP_Math.h>
+#include <AP_Math/crc.h>
 #include <canard.h>
 #include "support.h"
 #include <uavcan/protocol/dynamic_node_id/Allocation.h>
@@ -32,6 +33,7 @@
 #include "bl_protocol.h"
 #include <drivers/stm32/canard_stm32.h>
 #include "app_comms.h"
+#include <stdio.h>
 
 
 static CanardInstance canard;
@@ -221,7 +223,9 @@ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* tra
         fw_update.node_id = 0;
         // now flash the first word
         flash_func_write_word(0, app_first_word);
-        jump_to_app();
+        if (can_check_firmware()) {
+            jump_to_app();
+        }
     }
 
     // show offset number we are flashing in kbyte as crude progress indicator
@@ -538,6 +542,43 @@ void can_set_node_id(uint8_t node_id)
     initial_node_id = node_id;
 }
 
+/*
+  check firmware CRC to see if it matches
+ */
+bool can_check_firmware(void)
+{
+    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;
+    const uint8_t desc_len = 16;
+    const uint8_t *ptr = (const uint8_t *)memmem(flash, flash_size-(desc_len+sizeof(sig)), sig, sizeof(sig));
+    if (ptr == nullptr) {
+        // no application signature
+        printf("No app sig\n");
+        return false;
+    }
+    ptr += sizeof(sig);
+    uint32_t desc[4];
+    memcpy(desc, ptr, sizeof(desc));
+
+    // check length
+    if (desc[2] > flash_size) {
+        printf("Bad fw length %u\n", desc[2]);
+        return false;
+    }
+
+    uint32_t len1 = ptr - flash;
+    uint32_t len2 = desc[2] - (len1 + sizeof(desc));
+    uint32_t crc1 = crc_crc32(0, flash, len1);
+    uint32_t crc2 = crc_crc32(0, ptr+sizeof(desc), len2);
+    if (crc1 != desc[0] || crc2 != desc[1]) {
+        printf("Bad app CRC 0x%08x:0x%08x 0x%08x:0x%08x\n", desc[0], desc[1], crc1, crc2);
+        return false;
+    }
+    printf("Good firmware\n");
+    return true;
+}
+
 void can_start()
 {
 #if HAL_RAM_RESERVE_START >= 256
diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h
index e65b5ca869..2239a4e718 100644
--- a/Tools/AP_Bootloader/can.h
+++ b/Tools/AP_Bootloader/can.h
@@ -1,3 +1,5 @@
 void can_start();
 void can_update();
 void can_set_node_id(uint8_t node_id);
+bool can_check_firmware(void);
+
diff --git a/Tools/AP_Bootloader/wscript b/Tools/AP_Bootloader/wscript
index 3b41e9a1aa..90ad61a3d8 100644
--- a/Tools/AP_Bootloader/wscript
+++ b/Tools/AP_Bootloader/wscript
@@ -10,12 +10,19 @@ def build(bld):
               target='libcanard')
 
     bld.ap_program(
-        use=['ap','libcanard'],
+        use=['ap','libcanard','AP_Bootloader_libs'],
         program_groups='bootloader',
         includes=[bld.env.SRCROOT + '/modules/libcanard',
                   bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated']
         )
 
+    bld.ap_stlib(
+        name= 'AP_Bootloader_libs',
+        ap_vehicle='AP_Bootloader',
+        ap_libraries= [
+        'AP_Math'
+        ])
+    
     bld(
         # build libcanard headers
         source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"),