mirror of https://github.com/ArduPilot/ardupilot
AP_Bootloader: added app descriptor checking for CAN fw
This commit is contained in:
parent
0a4576728e
commit
92bd1a485a
|
@ -83,6 +83,15 @@ int main(void)
|
||||||
timeout = 10000;
|
timeout = 10000;
|
||||||
can_set_node_id(m & 0xFF);
|
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
|
#endif
|
||||||
|
|
||||||
// if we fail to boot properly we want to pause in bootloader to give
|
// if we fail to boot properly we want to pause in bootloader to give
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#if HAL_USE_CAN == TRUE
|
#if HAL_USE_CAN == TRUE
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_Math/crc.h>
|
||||||
#include <canard.h>
|
#include <canard.h>
|
||||||
#include "support.h"
|
#include "support.h"
|
||||||
#include <uavcan/protocol/dynamic_node_id/Allocation.h>
|
#include <uavcan/protocol/dynamic_node_id/Allocation.h>
|
||||||
|
@ -32,6 +33,7 @@
|
||||||
#include "bl_protocol.h"
|
#include "bl_protocol.h"
|
||||||
#include <drivers/stm32/canard_stm32.h>
|
#include <drivers/stm32/canard_stm32.h>
|
||||||
#include "app_comms.h"
|
#include "app_comms.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
||||||
static CanardInstance canard;
|
static CanardInstance canard;
|
||||||
|
@ -221,7 +223,9 @@ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* tra
|
||||||
fw_update.node_id = 0;
|
fw_update.node_id = 0;
|
||||||
// now flash the first word
|
// now flash the first word
|
||||||
flash_func_write_word(0, app_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
|
// 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;
|
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()
|
void can_start()
|
||||||
{
|
{
|
||||||
#if HAL_RAM_RESERVE_START >= 256
|
#if HAL_RAM_RESERVE_START >= 256
|
||||||
|
|
|
@ -1,3 +1,5 @@
|
||||||
void can_start();
|
void can_start();
|
||||||
void can_update();
|
void can_update();
|
||||||
void can_set_node_id(uint8_t node_id);
|
void can_set_node_id(uint8_t node_id);
|
||||||
|
bool can_check_firmware(void);
|
||||||
|
|
||||||
|
|
|
@ -10,12 +10,19 @@ def build(bld):
|
||||||
target='libcanard')
|
target='libcanard')
|
||||||
|
|
||||||
bld.ap_program(
|
bld.ap_program(
|
||||||
use=['ap','libcanard'],
|
use=['ap','libcanard','AP_Bootloader_libs'],
|
||||||
program_groups='bootloader',
|
program_groups='bootloader',
|
||||||
includes=[bld.env.SRCROOT + '/modules/libcanard',
|
includes=[bld.env.SRCROOT + '/modules/libcanard',
|
||||||
bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated']
|
bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated']
|
||||||
)
|
)
|
||||||
|
|
||||||
|
bld.ap_stlib(
|
||||||
|
name= 'AP_Bootloader_libs',
|
||||||
|
ap_vehicle='AP_Bootloader',
|
||||||
|
ap_libraries= [
|
||||||
|
'AP_Math'
|
||||||
|
])
|
||||||
|
|
||||||
bld(
|
bld(
|
||||||
# build libcanard headers
|
# build libcanard headers
|
||||||
source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"),
|
source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"),
|
||||||
|
|
Loading…
Reference in New Issue