wip

Author: jschall

bootloader/Makefile

@@ -24,14 +24,19 @@ uavcan_nodestatus_publisher \
 uavcan_allocatee \
 spi_device \
 driver_profiLED \
-uavcan_timesync
+uavcan_timesync \
+uavcan_debug
 
 MESSAGES_ENABLED = \
 uavcan.protocol.GetNodeInfo \
 uavcan.protocol.file.BeginFirmwareUpdate \
 uavcan.protocol.file.Read \
 uavcan.protocol.RestartNode \
-uavcan.equipment.indication.LightsCommand
+uavcan.equipment.indication.LightsCommand \
+com.hex.file.FileStreamStart \
+com.hex.file.FileStreamChunk
+
+DSDL_NAMESPACE_DIRS += dsdl/com
 
 LOAD_REGION = bl
 

bootloader/dsdl/com/hex/equipment/gnss/20210.BodyPosition.uavcan

@@ -0,0 +1 @@
+int18[3] body_pos_mm

bootloader/dsdl/com/hex/equipment/gnss/20211.MovingBaseFix.uavcan

@@ -0,0 +1,10 @@
+uavcan.Timestamp timestamp
+uint8[16] base_in_use_hwid
+void6
+uint2 CARRIER_SOLUTION_TYPE_NONE = 0
+uint2 CARRIER_SOLUTION_TYPE_FLOAT = 1
+uint2 CARRIER_SOLUTION_TYPE_FIXED = 2
+uint2 carrier_solution_type
+float32[<=3] pos_rel_body
+float32[3] pos_rel_ecef
+float16[<=6] pos_rel_ecef_covariance

bootloader/dsdl/com/hex/equipment/gnss/gnss.zip

@@ -0,0 +1,10 @@
+bool preserve_source
+bool overwrite_destination
+void6
+
+uavcan.protocol.file.Path source
+uavcan.protocol.file.Path destination
+
+---
+
+uavcan.protocol.file.Error error

bootloader/dsdl/com/hex/file/242.Modify.uavcan

@@ -0,0 +1,4 @@
+uavcan.protocol.file.Path path
+uint40 offset
+---
+uavcan.protocol.file.Error error

bootloader/dsdl/com/hex/file/243.FileStreamStart.uavcan

@@ -0,0 +1,6 @@
+uint8 source_node_id
+uavcan.protocol.file.Path source_path
+uavcan.protocol.file.Path dest_path
+---
+void7
+bool ack

bootloader/dsdl/com/hex/file/244.StartDownload.uavcan

@@ -0,0 +1,3 @@
+uavcan.protocol.file.Path path
+uint40 offset
+uint8[<=256] data

bootloader/dsdl/com/hex/file/42444.FileStreamChunk.uavcan

@@ -1,12 +1,4 @@
 source [find interface/stlink.cfg]
 source [find target/stm32h7x.cfg]
-reset_config srst_only separate connect_assert_srst
-$_TARGETNAME configure -rtos ChibiOS
-$_TARGETNAME configure -event gdb-attach {
-  halt
-}
-$_TARGETNAME configure -event gdb-attach {
-  reset init
-}
+$_CHIPNAME.cpu0 configure -rtos ChibiOS
 init
-reset halt

bootloader/openocd.cfg

@@ -15,6 +15,18 @@
 #include <uavcan.protocol.RestartNode.h>
 #include <uavcan.protocol.GetNodeInfo.h>
 
+#ifdef MODULE_UAVCAN_DEBUG_ENABLED
+#include <modules/uavcan_debug/uavcan_debug.h>
+#define BL_DEBUG(...) uavcan_send_debug_msg(UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG, "BL", __VA_ARGS__)
+#else
+#define BL_DEBUG(...) {}
+#endif
+
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+#include <com.hex.file.FileStreamStart.h>
+#include <com.hex.file.FileStreamChunk.h>
+#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+
 #if __GNUC__ != 6 || __GNUC_MINOR__ != 3 || __GNUC_PATCHLEVEL__ != 1
 #error Please use arm-none-eabi-gcc 6.3.1.
 #endif
@@ -40,13 +52,17 @@ static const struct shared_hw_info_s _hw_info = BOARD_CONFIG_HW_INFO_STRUCTURE;
 static struct {
     bool in_progress;
     uint32_t ofs;
+    uint32_t read_req_ofs;
     uint32_t app_start_ofs;
     uint8_t uavcan_idx;
     uint8_t read_transfer_id;
     uint8_t retries;
     uint8_t source_node_id;
     int32_t last_erased_page;
     char path[201];
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+    bool using_stream_mode;
+#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE
 } flash_state;
 
 static struct {
@@ -64,11 +80,18 @@ static struct worker_thread_listener_task_s restart_req_listener_task;
 static struct worker_thread_timer_task_s delayed_restart_task;
 static struct worker_thread_listener_task_s getnodeinfo_req_listener_task;
 
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+static struct worker_thread_listener_task_s filestreamchunk_listener_task;
+static void filestreamchunk_handler(size_t msg_size, const void* buf, void* ctx);
+
+// static struct worker_thread_listener_task_s filestreamstart_res_listener_task;
+// static void filestreamstart_res_handler(size_t msg_size, const void* buf, void* ctx);
+#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+
 static void file_beginfirmwareupdate_request_handler(size_t msg_size, const void* buf, void* ctx);
 static void begin_flash_from_path(uint8_t uavcan_idx, uint8_t source_node_id, const char* path);
 static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx);
-static void do_resend_read_request(void);
-static void do_send_read_request(void);
+static void do_send_read_request(bool retry);
 static uint32_t get_app_sec_size(void);
 static void start_boot(struct worker_thread_timer_task_s* task);
 static void update_app_info(void);
@@ -110,6 +133,14 @@ RUN_AFTER(UAVCAN_INIT) {
 
     struct pubsub_topic_s* getnodeinfo_req_topic = uavcan_get_message_topic(0, &uavcan_protocol_GetNodeInfo_req_descriptor);
     worker_thread_add_listener_task(&WT, &getnodeinfo_req_listener_task, getnodeinfo_req_topic, getnodeinfo_req_handler, NULL);
+
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+    struct pubsub_topic_s* filestreamchunk_topic = uavcan_get_message_topic(0, &com_hex_file_FileStreamChunk_descriptor);
+    worker_thread_add_listener_task(&WT, &filestreamchunk_listener_task, filestreamchunk_topic, filestreamchunk_handler, NULL);
+
+//     struct pubsub_topic_s* filestreamstart_res_topic = uavcan_get_message_topic(0, &com_hex_file_FileStreamStart_res_descriptor);
+//     worker_thread_add_listener_task(&WT, &filestreamstart_res_listener_task, filestreamstart_res_topic, filestreamstart_res_handler, NULL);
+#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE
 }
 
 static void getnodeinfo_req_handler(size_t msg_size, const void* buf, void* ctx) {
@@ -168,71 +199,141 @@ static void begin_flash_from_path(uint8_t uavcan_idx, uint8_t source_node_id, co
     cancel_boot_timer();
     memset(&flash_state, 0, sizeof(flash_state));
     flash_state.in_progress = true;
-    flash_state.ofs = 0; 
+    flash_state.ofs = 0;
+    flash_state.read_transfer_id = 255;
     flash_state.source_node_id = source_node_id;
     flash_state.uavcan_idx = uavcan_idx;
     strncpy(flash_state.path, path, 200);
-    worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, chTimeMS2I(500), false);
-    do_send_read_request();
+    worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, chTimeMS2I(2000), false);
+    do_send_read_request(false);
 
     corrupt_app();
     flash_state.last_erased_page = -1;
 }
 
-static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx) {
+static void process_chunk(size_t chunk_size, const void* chunk) {
+    if (flash_state.ofs + chunk_size > get_app_sec_size()) {
+        do_fail_update();
+        BL_DEBUG("fail: file too large");
+        return;
+    }
+
+    if (chunk_size == 0) {
+        on_update_complete();
+        return;
+    }
+
+    int32_t curr_page = get_app_page_from_ofs(flash_state.ofs + chunk_size);
+    if (curr_page > flash_state.last_erased_page) {
+        for (int32_t i=flash_state.last_erased_page+1; i<=curr_page; i++) {
+            erase_app_page(i);
+        }
+    }
+
+    if (chunk_size < 256) {
+        struct flash_write_buf_s buf = {((chunk_size/FLASH_WORD_SIZE) + 1) * FLASH_WORD_SIZE, chunk};
+        flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf);
+        on_update_complete();
+    } else {
+        struct flash_write_buf_s buf = {chunk_size, chunk};
+        flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf);
+        flash_state.ofs += chunk_size;
+        do_send_read_request(false);
+    }
+}
+
+// TODO factor common code out of read_response_handler and filestreamchunk_handler
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+static void filestreamchunk_handler(size_t msg_size, const void* buf, void* ctx) {
     (void)msg_size;
     (void)ctx;
+
     if (flash_state.in_progress) {
         const struct uavcan_deserialized_message_s* msg_wrapper = buf;
-        const struct uavcan_protocol_file_Read_res_s *res = (const struct uavcan_protocol_file_Read_res_s*)msg_wrapper->msg;
+        const struct com_hex_file_FileStreamChunk_s *msg = (const struct com_hex_file_FileStreamChunk_s*)msg_wrapper->msg;
 
-        if (msg_wrapper->transfer_id != flash_state.read_transfer_id) {
+        if (msg->path.path_len != strnlen(flash_state.path, 200) || memcmp(flash_state.path, msg->path.path, msg->path.path_len) != 0) {
+            // Not our stream
             return;
         }
 
-        if (res->error.value != 0 || flash_state.ofs + res->data_len > get_app_sec_size()) {
-            do_fail_update();
+        flash_state.using_stream_mode = true;
+        worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(2000));
+
+        if (msg->offset > flash_state.ofs) {
+            // We need to ask for the stream to go back to our offset
+            struct com_hex_file_FileStreamStart_req_s req;
+            req.path.path_len = strnlen(flash_state.path, 200);
+            req.offset = flash_state.ofs;
+            memcpy(req.path.path, flash_state.path, req.path.path_len);
+            uavcan_request(flash_state.uavcan_idx, &com_hex_file_FileStreamStart_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &req);
             return;
         }
 
-        if (res->data_len == 0) {
-            on_update_complete();
+        if (msg->offset != flash_state.ofs) {
             return;
         }
 
-        int32_t curr_page = get_app_page_from_ofs(flash_state.ofs + res->data_len);
-        if (curr_page > flash_state.last_erased_page) {
-            for (int32_t i=flash_state.last_erased_page+1; i<=curr_page; i++) {
-                erase_app_page(i);
-            }
+        process_chunk(msg->data_len, msg->data);
+    }
+}
+#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+
+static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx) {
+    (void)msg_size;
+    (void)ctx;
+    if (flash_state.in_progress) {
+        const struct uavcan_deserialized_message_s* msg_wrapper = buf;
+        const struct uavcan_protocol_file_Read_res_s *res = (const struct uavcan_protocol_file_Read_res_s*)msg_wrapper->msg;
+
+        if (msg_wrapper->transfer_id != flash_state.read_transfer_id || flash_state.ofs != flash_state.read_req_ofs) {
+            return;
         }
 
-        if (res->data_len < 256) {
-            struct flash_write_buf_s buf = {((res->data_len/FLASH_WORD_SIZE) + 1) * FLASH_WORD_SIZE, (void*)res->data};
-            flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf);
-            on_update_complete();
-        } else {
-            struct flash_write_buf_s buf = {res->data_len, (void*)res->data};
-            flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf);
-            flash_state.ofs += res->data_len;
-            do_send_read_request();
+        if (res->error.value != 0) {
+            do_fail_update();
+            BL_DEBUG("fail: file read error");
+            return;
         }
+
+        process_chunk(res->data_len, res->data);
     }
 }
 
-static void do_resend_read_request(void) {
+static void do_send_read_request(bool retry) {
+
+#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE
+    if (!flash_state.using_stream_mode) {
+        struct uavcan_protocol_file_Read_req_s read_req;
+        flash_state.read_req_ofs = read_req.offset = flash_state.ofs;
+        strncpy((char*)read_req.path.path,flash_state.path,sizeof(read_req.path));
+        read_req.path.path_len = strnlen(flash_state.path,sizeof(read_req.path));
+        flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &read_req);
+    }
+
+    if ((retry && flash_state.using_stream_mode) || (flash_state.ofs < 2048 && !flash_state.using_stream_mode)) {
+        // attempt to start stream mode with the first few requests
+        struct com_hex_file_FileStreamStart_req_s req;
+        req.offset = flash_state.ofs;
+        req.path.path_len = strnlen(flash_state.path, 200);
+        memcpy(req.path.path, flash_state.path, req.path.path_len);
+        uavcan_request(flash_state.uavcan_idx, &com_hex_file_FileStreamStart_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &req);
+    }
+#else
     struct uavcan_protocol_file_Read_req_s read_req;
     read_req.offset =  flash_state.ofs;
     strncpy((char*)read_req.path.path,flash_state.path,sizeof(read_req.path));
     read_req.path.path_len = strnlen(flash_state.path,sizeof(read_req.path));
-    flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_HIGH, flash_state.source_node_id, &read_req);
-    worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(500));
-    flash_state.retries++;
-}
+    flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &read_req);
+#endif
 
-static void do_send_read_request(void) {
-    do_resend_read_request();
-    flash_state.retries = 0;
+    worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(1000));
+
+    if (retry) {
+        flash_state.retries++;
+    } else {
+        flash_state.retries = 0;
+    }
 }
 
 static uint32_t get_app_sec_size(void) {
@@ -414,9 +515,10 @@ static void delayed_restart_func(struct worker_thread_timer_task_s* task) {
 static void read_request_response_timeout(struct worker_thread_timer_task_s* task) {
     (void)task;
     if (flash_state.in_progress) {
-        do_resend_read_request();
-        if (flash_state.retries > 10) { // retry for 5 seconds
+        do_send_read_request(true);
+        if (flash_state.retries > 100) { // retry for 5 seconds
             do_fail_update();
+            BL_DEBUG("fail: out of retries");
         }
     }
 }

bootloader/out.bin

@@ -672,16 +672,20 @@
 /**
  * @brief   ISR enter hook.
  */
+#ifndef CH_CFG_IRQ_PROLOGUE_HOOK
 #define CH_CFG_IRQ_PROLOGUE_HOOK() {                                        \
   /* IRQ prologue code here.*/                                              \
 }
+#endif
 
 /**
  * @brief   ISR exit hook.
  */
+#ifndef CH_CFG_IRQ_EPILOGUE_HOOK
 #define CH_CFG_IRQ_EPILOGUE_HOOK() {                                        \
   /* IRQ epilogue code here.*/                                              \
 }
+#endif
 
 /**
  * @brief   Idle thread enter hook.

bootloader/src/bootloader.c

@@ -209,7 +209,7 @@ CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes
 CPPWARN = -Wall -Wextra -Wundef
 
 # List all user C define here, like -D_DEBUG=1
-UDEFS += -DGIT_HASH=0x$(shell git rev-parse --short=8 HEAD) $(MODULES_ENABLED_DEFS) -DCORTEX_ENABLE_WFI_IDLE=TRUE
+UDEFS += -DGIT_HASH=0x$(shell git rev-parse --short=8 HEAD) $(MODULES_ENABLED_DEFS) -DCORTEX_ENABLE_WFI_IDLE=FALSE
 
 # Define ASM defines here
 UADEFS =

include/chconf.h

@@ -567,9 +567,9 @@ static void can_expire_handler(struct worker_thread_timer_task_s* task) {
     for (uint8_t i=0; i < instance->num_tx_mailboxes; i++) {
         chSysLock();
         if (instance->tx_mailbox[i].state == CAN_TX_MAILBOX_PENDING && can_tx_frame_expired_X(instance->tx_mailbox[i].frame)) {
-            if (instance->driver_iface->abort_tx_mailbox_I(instance->driver_ctx, i)) {
-                instance->tx_mailbox[i].state = CAN_TX_MAILBOX_ABORTING;
-            }
+            // NOTE: order matters below - abort_tx_mailbox_I may call functions that read the mailbox state
+            instance->tx_mailbox[i].state = CAN_TX_MAILBOX_ABORTING;
+            instance->driver_iface->abort_tx_mailbox_I(instance->driver_ctx, i);
         }
         chSysUnlock();
     }

mk/build.mk

@@ -4,7 +4,7 @@
 typedef void (*driver_start_t)(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate);
 typedef void (*driver_stop_t)(void* ctx);
 
-typedef bool (*driver_mailbox_abort_t)(void* ctx, uint8_t mb_idx);
+typedef void (*driver_mailbox_abort_t)(void* ctx, uint8_t mb_idx);
 typedef bool (*driver_load_tx_mailbox_t)(void* ctx, uint8_t mb_idx, struct can_frame_s* frame);
 typedef bool (*driver_pop_rx_frame_t)(void* ctx, uint8_t mb_idx, struct can_frame_s* frame);
 typedef bool (*driver_rx_frame_available_t)(void* ctx, uint8_t mb_idx);