AP_Bootloader: speed up DroneCAN fw update

pipeline file read requests to reduce impact of transport latency

Tools/AP_Bootloader/can.cpp

@@ -47,18 +47,20 @@ static CANConfig cancfg = {
     CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
     0 // filled in below
 };
+// pipelining is not faster when using ChibiOS CAN driver
+#define FW_UPDATE_PIPELINE_LEN 1
 #else
 static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
 #endif
 
 #ifndef CAN_APP_VERSION_MAJOR
-#define CAN_APP_VERSION_MAJOR                                           1
+#define CAN_APP_VERSION_MAJOR                                           2
 #endif
 #ifndef CAN_APP_VERSION_MINOR
 #define CAN_APP_VERSION_MINOR                                           0
 #endif
 #ifndef CAN_APP_NODE_NAME
-#define CAN_APP_NODE_NAME                                               "org.ardupilot.ap_periph"
+#define CAN_APP_NODE_NAME "org.ardupilot." CHIBIOS_BOARD_NAME
 #endif
 
 static uint8_t node_id_allocation_transfer_id;
@@ -66,14 +68,30 @@ 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;
 
+static void processTx(void);
+
+// 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 rtt_ms;
-    uint32_t last_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];
+    uint16_t erased_to;
 } fw_update;
 
 /*
@@ -150,32 +168,83 @@ 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();
+    auto &r = fw_update.reads[idx];
-    if (now - fw_update.last_ms < 750) {
+    r.tx_id = fw_update.transfer_id;
-        // the server may still be responding
+    r.have_reply = false;
-        return;
-    }
-    fw_update.last_ms = now;
 
     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,
+    if (canardRequestOrRespond(&canard,
-                           fw_update.node_id,
+                               fw_update.node_id,
-                           UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
+                               UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
-                           UAVCAN_PROTOCOL_FILE_READ_ID,
+                               UAVCAN_PROTOCOL_FILE_READ_ID,
-                           &fw_update.transfer_id,
+                               &fw_update.transfer_id,
-                           CANARD_TRANSFER_PRIORITY_HIGH,
+                               CANARD_TRANSFER_PRIORITY_HIGH,
-                           CanardRequest,
+                               CanardRequest,
-                           &buffer[0],
+                               &buffer[0],
-                           total_size);
+                               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;
+        }
+    }
+}
+
+/*
+  erase up to at least the given sector number
+ */
+static void erase_to(uint16_t sector)
+{
+    if (sector < fw_update.erased_to) {
+        return;
+    }
+    flash_func_erase_sector(sector);
+    fw_update.erased_to = sector+1;
+
+    /*
+      pre-erase any non-erased pages up to end of flash. This puts all
+      the load of erasing at the start of flashing which is much
+      faster than flashing as we go on boards with small flash
+      sectors.  We stop at the first already erased page so we don't
+      end up wasting time erasing already erased pages when the
+      firmware is much smaller than the total flash size
+     */
+    while (flash_func_sector_size(fw_update.erased_to) != 0 &&
+           !flash_func_is_erased(fw_update.erased_to)) {
+        flash_func_erase_sector(fw_update.erased_to);
+        fw_update.erased_to++;
+    }
 }
 
 /*
@@ -183,51 +252,100 @@ static void send_fw_read(void)
  */
 static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer)
 {
-    const uint8_t tx_id = fw_update.transfer_id;
+    if (transfer->source_node_id != fw_update.node_id) {
-    if ((transfer->transfer_id+1)%256 != tx_id ||
-        transfer->source_node_id != fw_update.node_id) {
         return;
     }
-    uavcan_protocol_file_ReadResponse pkt;
+    /*
-    if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) {
+      match the response to a sent request
-        return;
+     */
-    }
+    uint8_t idx = 0;
-    const uint16_t len = pkt.data.len;
+    bool found = false;
-    const uint16_t len_words = (len+3)/4;
+    for (idx=0; idx<FW_UPDATE_PIPELINE_LEN; idx++) {
-    const uint8_t *buf = (uint8_t *)pkt.data.data;
+        const auto &r = fw_update.reads[idx];
-    uint32_t buf32[len_words] {};
+        if (r.tx_id == transfer->transfer_id) {
-    memcpy((uint8_t*)buf32, buf, len);
+            found = true;
-
+            break;
-    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);
-
-    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();
         }
     }
+    if (!found) {
+        // not a current transfer
+        return;
+    }
+    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);
+
+        if (fw_update.ofs == 0) {
+            flash_set_keep_unlocked(true);
+        }
+
+        const uint32_t sector_size = flash_func_sector_size(fw_update.sector);
+        if (sector_size == 0) {
+            // firmware is too big
+            fw_update.node_id = 0;
+            flash_write_flush();
+            flash_set_keep_unlocked(false);
+            node_status.vendor_specific_status_code = uint8_t(check_fw_result_t::FAIL_REASON_BAD_LENGTH_APP);
+            break;
+        }
+        if (fw_update.sector_ofs == 0) {
+            erase_to(fw_update.sector);
+        }
+        if (fw_update.sector_ofs+len > sector_size) {
+            erase_to(fw_update.sector+1);
+        }
+        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();
+            flash_set_keep_unlocked(false);
+            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);
+        processTx();
+
+        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;
 }
 
 /*
@@ -243,14 +361,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;
         }
@@ -270,8 +387,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)
@@ -606,6 +721,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
@@ -693,8 +811,8 @@ void can_start()
 
 void can_update()
 {
-    // do one loop of CAN support. If we are doing a few update then
+    // do one loop of CAN support. If we are doing a firmware update
-    // loop until it is finished
+    // then loop until it is finished
     do {
         processTx();
         processRx();
@@ -706,8 +824,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);
 }
 

Tools/AP_Bootloader/support.cpp

@@ -143,6 +143,11 @@ uint32_t flash_func_sector_size(uint32_t sector)
     return stm32_flash_getpagesize(flash_base_page+sector);
 }
 
+bool flash_func_is_erased(uint32_t sector)
+{
+    return stm32_flash_ispageerased(flash_base_page+sector);
+}
+
 bool flash_func_erase_sector(uint32_t sector, bool force_erase)
 {
 #if AP_BOOTLOADER_ALWAYS_ERASE
@@ -348,6 +353,17 @@ void uprintf(const char *fmt, ...)
 #endif
 }
 
+static void thread_sleep_ms(uint32_t ms)
+{
+    while (ms > 0) {
+        // don't sleep more than 65 at a time, to cope with 16 bit
+        // timer
+        const uint32_t dt = ms > 65? 65: ms;
+        chThdSleepMilliseconds(dt);
+        ms -= dt;
+    }
+}
+
 // generate a pulse sequence forever, for debugging
 void led_pulses(uint8_t npulses)
 {
@@ -355,11 +371,11 @@ void led_pulses(uint8_t npulses)
     while (true) {
         for (uint8_t i=0; i<npulses; i++) {
             led_on(LED_BOOTLOADER);
-            chThdSleepMilliseconds(200);
+            thread_sleep_ms(200);
             led_off(LED_BOOTLOADER);
-            chThdSleepMilliseconds(200);
+            thread_sleep_ms(200);
         }
-        chThdSleepMilliseconds(2000);
+        thread_sleep_ms(2000);
     }
 }
 
@@ -431,7 +447,7 @@ void init_uarts(void)
     sduStart(&SDU1, &serusbcfg1);
     
     usbDisconnectBus(serusbcfg1.usbp);
-    chThdSleepMilliseconds(1000);
+    thread_sleep_ms(1000);
     usbStart(serusbcfg1.usbp, &usbcfg);
     usbConnectBus(serusbcfg1.usbp);
 #endif

Tools/AP_Bootloader/support.h

@@ -25,6 +25,7 @@ uint32_t flash_func_read_word(uint32_t offset);
 bool flash_func_write_word(uint32_t offset, uint32_t v);
 bool flash_func_write_words(uint32_t offset, uint32_t *v, uint8_t n);
 uint32_t flash_func_sector_size(uint32_t sector);
+bool flash_func_is_erased(uint32_t sector);
 bool flash_func_erase_sector(uint32_t sector, bool force_erase = false);
 uint32_t flash_func_read_otp(uint32_t idx);
 uint32_t flash_func_read_sn(uint32_t idx);