Skip to content

Commit

Permalink
AP_Bootloader: speed up DroneCAN fw update
Browse files Browse the repository at this point in the history 
pipeline file read requests to reduce impact of transport latency
  • Loading branch information
@tridge
tridge committed Aug 1, 2023
1 parent 4f6e52c commit 217f9d9
Showing 1 changed file with 142 additions and 64 deletions.
206 changes: 142 additions & 64 deletions Tools/AP_Bootloader/can.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include <AP_CheckFirmware/AP_CheckFirmware.h>

static CanardInstance canard;
static uint32_t canard_memory_pool[4096/4];
static uint32_t canard_memory_pool[8*1024/4];
#ifndef HAL_CAN_DEFAULT_NODE_ID
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID
#endif
Expand Down Expand Up @@ -66,14 +66,27 @@ static uavcan_protocol_NodeStatus node_status;
static uint32_t send_next_node_id_allocation_request_at_ms;
static uint8_t node_id_allocation_unique_id_offset;

// keep up to 4 transfers in progress
#ifndef FW_UPDATE_PIPELINE_LEN
#define FW_UPDATE_PIPELINE_LEN 4
#endif

static struct {
uint64_t ofs;
uint32_t last_ms;
uint32_t rtt_ms;
uint32_t ofs;
uint8_t node_id;
uint8_t transfer_id;
uint8_t path[sizeof(uavcan_protocol_file_Path::path.data)+1];
uint8_t sector;
uint32_t sector_ofs;
uint8_t transfer_id;
uint8_t idx;
struct {
uint8_t tx_id;
uint32_t sent_ms;
uint32_t offset;
bool have_reply;
uavcan_protocol_file_ReadResponse pkt;
} reads[FW_UPDATE_PIPELINE_LEN];
} fw_update;

/*
Expand Down Expand Up @@ -150,84 +163,145 @@ static void handle_get_node_info(CanardInstance* ins,
/*
send a read for a fw update
*/
static void send_fw_read(void)
static bool send_fw_read(uint8_t idx)
{
uint32_t now = AP_HAL::millis();
if (now - fw_update.last_ms < 750) {
// the server may still be responding
return;
}
fw_update.last_ms = now;
auto &r = fw_update.reads[idx];
r.tx_id = fw_update.transfer_id;
r.have_reply = false;

uavcan_protocol_file_ReadRequest pkt {};
pkt.offset = fw_update.ofs;
pkt.path.path.len = strlen((const char *)fw_update.path);
pkt.offset = r.offset;
memcpy(pkt.path.path.data, fw_update.path, pkt.path.path.len);

uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE];
uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer, true);

canardRequestOrRespond(&canard,
fw_update.node_id,
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
UAVCAN_PROTOCOL_FILE_READ_ID,
&fw_update.transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardRequest,
&buffer[0],
total_size);
if (canardRequestOrRespond(&canard,
fw_update.node_id,
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
UAVCAN_PROTOCOL_FILE_READ_ID,
&fw_update.transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardRequest,
&buffer[0],
total_size) > 0) {
// mark it as having been sent
r.sent_ms = AP_HAL::millis();
return true;
}
return false;
}

/*
send a read for a fw update
*/
static void send_fw_reads(void)
{
const uint32_t now = AP_HAL::millis();

for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
const uint8_t idx = (fw_update.idx+i) % FW_UPDATE_PIPELINE_LEN;
const auto &r = fw_update.reads[idx];
if (r.have_reply) {
continue;
}
if (r.sent_ms != 0 && now - r.sent_ms < 10+2*MAX(250,fw_update.rtt_ms)) {
// waiting on a response
continue;
}
if (!send_fw_read(idx)) {
break;
}
}
}

/*
handle response to file read for fw update
*/
static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer)
{
const uint8_t tx_id = fw_update.transfer_id;
if ((transfer->transfer_id+1)%256 != tx_id ||
transfer->source_node_id != fw_update.node_id) {
if (transfer->source_node_id != fw_update.node_id) {
return;
}
uavcan_protocol_file_ReadResponse pkt;
if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) {
return;
/*
match the response to a sent request
*/
uint8_t idx = 0;
bool found = false;
for (idx=0; idx<FW_UPDATE_PIPELINE_LEN; idx++) {
const auto &r = fw_update.reads[idx];
if (r.tx_id == transfer->transfer_id) {
found = true;
break;
}
}
const uint16_t len = pkt.data.len;
const uint16_t len_words = (len+3)/4;
const uint8_t *buf = (uint8_t *)pkt.data.data;
uint32_t buf32[len_words] {};
memcpy((uint8_t*)buf32, buf, len);

const uint32_t sector_size = flash_func_sector_size(fw_update.sector);
if (fw_update.sector_ofs == 0) {
flash_func_erase_sector(fw_update.sector);
if (!found) {
// not a current transfer
return;
}
if (fw_update.sector_ofs+len > sector_size) {
flash_func_erase_sector(fw_update.sector+1);
if (uavcan_protocol_file_ReadResponse_decode(transfer, &fw_update.reads[idx].pkt)) {
return;
}
fw_update.reads[idx].have_reply = true;
uint32_t rtt = MIN(3000,MAX(AP_HAL::millis() - fw_update.reads[idx].sent_ms, 25));
fw_update.rtt_ms = uint32_t(0.9 * fw_update.rtt_ms + 0.1 * rtt);

while (fw_update.reads[fw_update.idx].have_reply) {
auto &r = fw_update.reads[fw_update.idx];
if (r.offset != fw_update.ofs) {
// bad sequence
r.have_reply = false;
r.sent_ms = 0;
break;
}
const auto &pkt = r.pkt;
const uint16_t len = pkt.data.len;
const uint16_t len_words = (len+3U)/4U;
const uint8_t *buf = (uint8_t *)pkt.data.data;
uint32_t buf32[len_words] {};
memcpy((uint8_t*)buf32, buf, len);

const uint32_t sector_size = flash_func_sector_size(fw_update.sector);
if (fw_update.sector_ofs == 0) {
flash_func_erase_sector(fw_update.sector);
}
if (fw_update.sector_ofs+len > sector_size) {
flash_func_erase_sector(fw_update.sector+1);
}

flash_write_buffer(fw_update.ofs, buf32, len_words);
if (!flash_write_buffer(fw_update.ofs, buf32, len_words)) {
continue;
}

fw_update.ofs += len;
fw_update.sector_ofs += len;
if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) {
fw_update.sector++;
fw_update.sector_ofs -= sector_size;
}
if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) {
fw_update.node_id = 0;
flash_write_flush();
const auto ok = check_good_firmware();
node_status.vendor_specific_status_code = uint8_t(ok);
if (ok == check_fw_result_t::CHECK_FW_OK) {
jump_to_app();
fw_update.ofs += len;
fw_update.sector_ofs += len;
if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) {
fw_update.sector++;
fw_update.sector_ofs -= sector_size;
}

if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) {
fw_update.node_id = 0;
flash_write_flush();
const auto ok = check_good_firmware();
node_status.vendor_specific_status_code = uint8_t(ok);
if (ok == check_fw_result_t::CHECK_FW_OK) {
jump_to_app();
}
return;
}

r.have_reply = false;
r.sent_ms = 0;
r.offset += FW_UPDATE_PIPELINE_LEN*sizeof(uavcan_protocol_file_ReadResponse::data.data);
send_fw_read(fw_update.idx);

fw_update.idx = (fw_update.idx + 1) % FW_UPDATE_PIPELINE_LEN;
}

// show offset number we are flashing in kbyte as crude progress indicator
node_status.vendor_specific_status_code = 1 + (fw_update.ofs / 1024U);

fw_update.last_ms = 0;
}

/*
Expand All @@ -243,14 +317,13 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
if (pkt.image_file_remote_path.path.len > sizeof(fw_update.path)-1) {
return;
}
memset(&fw_update, 0, sizeof(fw_update));
for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data);
}
memcpy(fw_update.path, pkt.image_file_remote_path.path.data, pkt.image_file_remote_path.path.len);
fw_update.path[pkt.image_file_remote_path.path.len] = 0;

fw_update.node_id = pkt.source_node_id;
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;
}
Expand All @@ -270,8 +343,6 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
CanardResponse,
&buffer[0],
total_size);

send_fw_read();
}

static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
Expand Down Expand Up @@ -606,6 +677,9 @@ bool can_check_update(void)
if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) {
can_set_node_id(comms->my_node_id);
fw_update.node_id = comms->server_node_id;
for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data);
}
memcpy(fw_update.path, comms->path, sizeof(uavcan_protocol_file_Path::path.data)+1);
ret = true;
// clear comms region
Expand Down Expand Up @@ -693,8 +767,8 @@ void can_start()

void can_update()
{
// do one loop of CAN support. If we are doing a few update then
// loop until it is finished
// do one loop of CAN support. If we are doing a firmware update
// then loop until it is finished
do {
processTx();
processRx();
Expand All @@ -706,8 +780,12 @@ void can_update()
process1HzTasks(AP_HAL::micros64());
}
if (fw_update.node_id != 0) {
send_fw_read();
send_fw_reads();
}
#if CH_CFG_ST_FREQUENCY >= 1000000
// give a bit of time for background processing
chThdSleepMicroseconds(200);
#endif
} while (fw_update.node_id != 0);
}

Expand Down

0 comments on commit 217f9d9

Please sign in to comment.