From 139439c0e05511f9dd4de44399020e512e2a6097 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 21 Apr 2015 17:24:31 -1000 Subject: Document pass on main.c --- .../boards/px4cannode-v1/bootloader/uavcan/main.c | 1277 ++++++++++++-------- 1 file changed, 775 insertions(+), 502 deletions(-) (limited to 'src/drivers/boards/px4cannode-v1') diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c index d7859ea7a..27075a769 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + #include #include "board_config.h" @@ -21,36 +61,15 @@ #include "board_config.h" +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ -uint64_t get_short_unique_id(void); -void send_log_message(uint8_t node_id, uint8_t level, uint8_t stage, - uint8_t status); - -int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id); -int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, - uint32_t *node_id); -void poll_getnodeinfo(uint8_t node_id, uint32_t uptime, uint8_t status); - -int wait_for_beginfirmwareupdate(bl_timer_id tboot, - uint8_t * fw_path, - uint8_t * fw_path_length, - uint8_t * fw_source_node_id); - -void file_getinfo(size_t * fw_image_size, uint64_t * fw_image_crc, - const uint8_t * fw_path, uint8_t fw_path_length, - uint8_t fw_source_node_id); - -flash_error_t file_read_and_program(uint8_t fw_source_node_id, - uint8_t fw_path_length, - const uint8_t * fw_path, - size_t fw_image_size, - uint32_t * fw_word0); - -static uint8_t is_app_valid(uint32_t first_word); -void find_descriptor(void); -void application_run(size_t fw_image_size); +/**************************************************************************** + * Private Types + ****************************************************************************/ -volatile struct { +typedef volatile struct bootloader_t { can_speed_t bus_speed; volatile uint8_t node_id; volatile uint8_t status_code; @@ -62,18 +81,64 @@ volatile struct { bool app_bl_request; bool sent_node_info_response; -} bootloader; - +} bootloader_t; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +bootloader_t bootloader; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: uptime_process + * + * Description: + * Run as a timer callback off the system tick ISR. This function counts + * Seconds of up time. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ static void uptime_process(bl_timer_id id, void *context) { bootloader.uptime++; } -/* - * Once we have a noide id - * Handle GetNodeInfo replies regardless of application state - */ + +/**************************************************************************** + * Name: node_info_process + * + * Description: + * Run as a timer callback off the system tick ISR. + * Once we have a node id, this process is started and handles the + * GetNodeInfo replies. This function uses the fifoGetNodeInfo and + * MBGetNodeInfo of the CAN so it can coexist regardless of + * application state and what is it doing. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ static void node_info_process(bl_timer_id id, void *context) { @@ -118,410 +183,309 @@ static void node_info_process(bl_timer_id id, void *context) bootloader.sent_node_info_response = true; } - - } -/* - * Once we have a noide id - * Handle GetNodeInfo replies regardless of application state - */ +/**************************************************************************** + * Name: node_status_process + * + * Description: + * Run as a timer callback off the system tick ISR. + * Once we have a node id, this process is started and sends the + * NodeStatus messages. This function uses the MBNodeStatus + * of the CAN so it can coexist regardless of application state ant + * what is it doing. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ + static void node_status_process(bl_timer_id id, void *context) { uavcan_tx_nodestatus(bootloader.node_id, bootloader.uptime, bootloader.status_code); } -__EXPORT int main(int argc, char *argv[]) +/**************************************************************************** + * Name: send_log_message + * + * Description: + * This functions sends uavcan logmessage type data. See uavcan/protocol.h + * UAVCAN_LOGMESSAGE_xxx defines. + * + * Input Parameters: + * node_id - This node's node id + * level - Log Level of the logmessage DEBUG, INFO, WARN, ERROR + * stage - The Stage the application is at. see UAVCAN_LOGMESSAGE_STAGE_x + * status - The status of that stage. Start, Fail OK + * + * Returned Value: + * None + * + ****************************************************************************/ +static void send_log_message(uint8_t node_id, uint8_t level, uint8_t stage, + uint8_t status) { + uavcan_logmessage_t message; + uavcan_frame_id_t frame_id; + uint8_t payload[8]; + size_t frame_len; - uint64_t fw_image_crc; - size_t fw_image_size; - uint32_t fw_word0; - uint8_t fw_path[200]; - uint8_t fw_path_length; - uint8_t fw_source_node_id; - uint8_t error_log_stage; - flash_error_t status; - bootloader_app_shared_t common; - -#ifdef OPT_ENABLE_WD - /* ... */ -#endif + frame_id.transfer_id = 0; + frame_id.last_frame = 1u; + frame_id.frame_index = 0; + frame_id.source_node_id = node_id; + frame_id.transfer_type = MESSAGE_BROADCAST; + frame_id.data_type_id = UAVCAN_LOGMESSAGE_DTID; - memset((void *)&bootloader, 0, sizeof(bootloader)); + message.level = level; + message.message[0] = stage; + message.message[1] = status; + frame_len = uavcan_pack_logmessage(payload, &message); + can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBAll); +} - bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING; +/**************************************************************************** + * Name: find_descriptor + * + * Description: + * This functions looks through the application image in flash on 8 byte + * aligned boundaries to find the Application firmware descriptor. + * Once it is found the bootloader.fw_image_descriptor is set to point to + * it. + * + * + * Input Parameters: + * None + * + * Returned State: + * If found bootloader.fw_image_descriptor points to the app_descriptor_t + * of the application firmware image. + * If not found bootloader.fw_image_descriptor = NULL + * + ****************************************************************************/ + +static void find_descriptor(void) +{ + bootloader.fw_image_descriptor = NULL; + uint64_t *p = (uint64_t *) APPLICATION_LOAD_ADDRESS; + union + { + uint64_t ull; + char text[sizeof(uint64_t)]; + } sig = { + .text = {APP_DESCRIPTOR_SIGNATURE} + }; + do { + if (*p == sig.ull) { + bootloader.fw_image_descriptor = (volatile app_descriptor_t *) p; + } + } while(bootloader.fw_image_descriptor == NULL && ++p < APPLICATION_LAST_64BIT_ADDRRESS); +} - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT; +/**************************************************************************** + * Name: is_app_valid + * + * Description: + * This functions validates the applications image based on the validity of + * the Application firmware descriptor's crc and the value of the first word + * in the FLASH image. + * + * + * Input Parameters: + * first_word - the value read from the first word of the Application's + * in FLASH image. + * + * Returned Value: + * true if the application in flash is valid., false otherwise. + * + ****************************************************************************/ + +static bool is_app_valid(uint32_t first_word) +{ + uint64_t crc; + size_t i, length, crc_offset; + uint8_t byte; - bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS); + find_descriptor(); -#if defined(OPT_WAIT_FOR_GETNODEINFO) - bootloader.wait_for_getnodeinfo = 1; -#endif + if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu) + { + return false; + } -#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) - bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT; -#endif + length = bootloader.fw_image_descriptor->image_size; + crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) - + (size_t) bootloader.fw_image; + crc = CRC64_INITIAL; + for (i = 0u; i < 4u; i++) + { + crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u))); + } + for (i = 4u; i < length; i++) + { + if (crc_offset <= i && i < crc_offset + 8u) + { + /* Zero out the CRC field while computing the CRC */ + byte = 0u; + } + else + { + byte = ((volatile uint8_t *)bootloader.fw_image)[i]; + } + crc = crc64_add(crc, byte); + } + crc ^= CRC64_OUTPUT_XOR; - bootloader.app_valid = is_app_valid(bootloader.fw_image[0]); + return crc == bootloader.fw_image_descriptor->image_crc; +} - board_indicate(reset); +/**************************************************************************** + * Name: get_dynamic_node_id + * + * Description: + * This functions performs the client side of the uavcan specification + * for dynamic node allocation. + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * allocated_node_id - A pointer to the location that should receive the + * allocated node id. + * Returned Value: + * CAN_OK - Indicates the a node id has been allocated to this + * node. + * CAN_BOOT_TIMEOUT - Indicates that tboot expired before an allocation + * was done. + * + ****************************************************************************/ + +static int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) +{ + uavcan_hardwareversion_t hw_version; + uavcan_frame_id_t rx_frame_id; + uint32_t rx_message_id; + uint8_t rx_payload[8]; + size_t rx_len; + uint16_t rx_crc; - bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && - common.bus_speed && common.node_id; - bootloader_app_shared_invalidate(); + struct { + uint8_t node_id; + uint8_t transfer_id; + uint8_t allocated_node_id; + uint8_t frame_index; + } server; - bl_timer_cb_t p = null_cb; - p.cb = uptime_process; - timer_allocate(modeRepeating|modeStarted, 1000, &p); + *allocated_node_id = 0; - p.cb = node_info_process; - bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p); + uint8_t transfer_id = 0; - p.cb = node_status_process; - bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p); + size_t i; + size_t offset; + uint16_t expected_crc; + bool unique_id_matched = false; - bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0); + board_get_hardware_version(&hw_version); - if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) { - timer_start(tboot); - } + memset(&server,0 , sizeof(server)); + rx_crc = 0u; - if (bootloader.app_bl_request) { + /* + Rule A: on initialization, the client subscribes to + uavcan.protocol.dynamic_node_id.Allocation and starts a Request Timer with + interval of Trequestinterval = 1 second + */ - bootloader.bus_speed = common.bus_speed; - bootloader.node_id = (uint8_t) common.node_id; - can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); + bl_timer_id trequest = timer_allocate(modeTimeout|modeStarted, 1000, 0); - } else { + do { + /* + Rule B. On expiration of Request Timer: + 1) Request Timer restarts. + 2) The client broadcasts a first-stage Allocation request message, + where the fields are assigned following values: + node_id - preferred node ID, or zero if the + client doesn't have any preference + first_part_of_unique_id - true + unique_id - first 7 bytes of unique ID + */ + if (timer_expired(trequest)) { + uavcan_tx_allocation_message(*allocated_node_id, 16u, hw_version.unique_id, + 0u, transfer_id++); + timer_start(trequest); + } - can_speed_t speed; + if (can_rx(&rx_message_id, &rx_len, rx_payload, fifoAll) && + uavcan_parse_message_id(&rx_frame_id, rx_message_id, UAVCAN_DYNAMICNODEIDALLOCATION_DTID)) { - if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) { - goto boot; - } + /* + Rule C. On any Allocation message, even if other rules also match: + 1) Request Timer restarts. + */ - /* reset uptime */ - bootloader.uptime = 0; - common.bus_speed = can_speed2freq(speed); - bootloader.node_id = (uint8_t) common.node_id; + timer_start(trequest); - } - // Start Processes that requier Node Id - timer_start(tinfo); - timer_start(tstatus); + /* + Skip this message if it's anonymous (from another client), or if it's + from a different server to the one we're listening to at the moment + */ + if (!rx_frame_id.source_node_id || (server.node_id && + rx_frame_id.source_node_id != server.node_id)) { + continue; + } else if (!server.node_id || + rx_frame_id.transfer_id != server.transfer_id || + rx_frame_id.frame_index == 0) { + /* First (only?) frame of the transfer */ + if (rx_frame_id.last_frame) { + offset = 1u; + } else { + rx_crc = (uint16_t)(rx_payload[0] | (rx_payload[1] << 8u)); + server.allocated_node_id = rx_payload[2]; + offset = 3u; + } + server.node_id = rx_frame_id.source_node_id; + server.transfer_id = rx_frame_id.transfer_id; + unique_id_matched = 0u; + server.frame_index = 1u; + } else if (rx_frame_id.frame_index != server.frame_index) { + /* Abort if the frame index is wrong */ + server.node_id = 0u; + continue; + } else { + offset = 0u; + server.frame_index++; + } - while (!bootloader.sent_node_info_response) { - if (timer_expired(tboot)) - { - goto boot; - } - } + /* + Rule D. On an Allocation message WHERE (source node ID is + non-anonymous) AND (client's unique ID starts with the bytes available + in the field unique_id) AND (unique_id is less than 16 bytes long): + 1) The client broadcasts a second-stage Allocation request message, + where the fields are assigned following values: + node_id - same value as in the first-stage + first_part_of_unique_id - false + unique_id - at most 7 bytes of local unique ID + with an offset equal to number of + bytes in the received unique ID - if (bootloader.app_valid && - (bootloader.wait_for_getnodeinfo || - bootloader.app_bl_request)) { + Rule E. On an Allocation message WHERE (source node ID is + non-anonymous) AND (unique_id fully matches client's unique ID) AND + (node_id in the received message is not zero): + 1) Request Timer stops. + 2) The client initializes its node_id with the received value. + 3) The client terminates subscription to Allocation messages. + 4) Exit. + */ - timer_start(tboot); - } - - do - { - if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path, - &fw_path_length, - &fw_source_node_id)) - { - goto boot; - } - } - while (!fw_source_node_id); - - timer_stop(tboot); - board_indicate(fw_update_start); - - file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length, - fw_source_node_id); - - //todo:Check this - if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc) - { - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO; - goto failure; - } - - /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_INFO, - UAVCAN_LOGMESSAGE_STAGE_ERASE, - UAVCAN_LOGMESSAGE_RESULT_START); - - - /* Need to signal that the app is no longer valid if Node Info Request are done */ - - bootloader.app_valid = false; - - status = bl_flash_erase(APPLICATION_LOAD_ADDRESS); - if (status != FLASH_OK) - { - /* UAVCANBootloader_v0.3 #28.8: [Erase - * Failed]:INDICATE_FW_UPDATE_ERASE_FAIL */ - board_indicate(fw_update_erase_fail); - - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE; - goto failure; - } - - status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path, - fw_image_size, &fw_word0); - if (status != FLASH_OK) - { - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM; - goto failure; - } - - /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */ - if (!is_app_valid(fw_word0)) - { - bootloader.app_valid = 0u; - - /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */ - board_indicate(fw_update_invalid_crc); - - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE; - goto failure; - } - - /* UAVCANBootloader_v0.3 #46: [crc == fw_crc]:write word0 */ - status = bl_flash_write_word((uint32_t) bootloader.fw_image, (uint8_t *) & fw_word0); - if (status != FLASH_OK) - { - /* Not specifically listed in UAVCANBootloader_v0.3: - * 1023.LogMessage.uavcan */ - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE; - goto failure; - } - - /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */ - bootloader_app_shared_write(&common, BootLoader); - - /* Send a completion log message */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_INFO, - UAVCAN_LOGMESSAGE_STAGE_FINALIZE, - UAVCAN_LOGMESSAGE_RESULT_OK); - - /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */ - -boot: - /* UAVCANBootloader_v0.3 #50: jump_to_app */ - application_run(fw_image_size); - - /* We will fall thru if the Iamage is bad */ - -failure: - /* UAVCANBootloader_v0.3 #28.2: - * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */ - /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #38: [(retries == 0 && - * timeout)]:1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_ERROR, - error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL); - - /* UAVCANBootloader_v0.3 #28.3: - * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #28.10: [Erase - * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #39: [(retries == 0 && - * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL; - - /* UAVCANBootloader_v0.3 #28.1: - * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ - /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(), - * RestartWithDelay(20,000ms) */ - /* UAVCANBootloader_v0.3 #45: [crc - * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ - - bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0); - while(!timer_expired(tmr)) - { - ; - } - timer_free(tmr); - up_systemreset(); -} - -uint64_t get_short_unique_id(void) -{ - uavcan_hardwareversion_t hw_version; - uint64_t result; - size_t i; - - board_get_hardware_version(&hw_version); - - result = CRC64_INITIAL; - for (i = 0; i < 16u; i++) - { - result = crc64_add(result, hw_version.unique_id[i]); - } - - return (result ^ CRC64_OUTPUT_XOR) & 0x01FFFFFFFFFFFFFFL; -} - -int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, uint32_t *node_id) -{ - board_indicate(autobaud_start); - int rv = can_autobaud(speed, tboot); - if (rv != CAN_BOOT_TIMEOUT) { - can_init(*speed, CAN_Mode_Normal); - - board_indicate(autobaud_end); - board_indicate(allocation_start); - rv = get_dynamic_node_id(tboot, node_id); - if (rv != CAN_BOOT_TIMEOUT) { - board_indicate(allocation_end); - } - } - return rv; -} - -int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) -{ - uavcan_hardwareversion_t hw_version; - uavcan_frame_id_t rx_frame_id; - - uint32_t rx_message_id; - uint8_t rx_payload[8]; - size_t rx_len; - uint16_t rx_crc; - - struct { - uint8_t node_id; - uint8_t transfer_id; - uint8_t allocated_node_id; - uint8_t frame_index; - } server; - - *allocated_node_id = 0; - - uint8_t transfer_id = 0; - - size_t i; - size_t offset; - uint16_t expected_crc; - - bool unique_id_matched = false; - - - board_get_hardware_version(&hw_version); - - memset(&server,0 , sizeof(server)); - rx_crc = 0u; - - /* - Rule A: on initialization, the client subscribes to - uavcan.protocol.dynamic_node_id.Allocation and starts a Request Timer with - interval of Trequestinterval = 1 second - */ - - bl_timer_id trequest = timer_allocate(modeTimeout|modeStarted, 1000, 0); - - do { - /* - Rule B. On expiration of Request Timer: - 1) Request Timer restarts. - 2) The client broadcasts a first-stage Allocation request message, - where the fields are assigned following values: - node_id - preferred node ID, or zero if the - client doesn't have any preference - first_part_of_unique_id - true - unique_id - first 7 bytes of unique ID - */ - if (timer_expired(trequest)) { - uavcan_tx_allocation_message(*allocated_node_id, 16u, hw_version.unique_id, - 0u, transfer_id++); - timer_start(trequest); - } - - if (can_rx(&rx_message_id, &rx_len, rx_payload, fifoAll) && - uavcan_parse_message_id(&rx_frame_id, rx_message_id, UAVCAN_DYNAMICNODEIDALLOCATION_DTID)) { - - /* - Rule C. On any Allocation message, even if other rules also match: - 1) Request Timer restarts. - */ - - timer_start(trequest); - - /* - Skip this message if it's anonymous (from another client), or if it's - from a different server to the one we're listening to at the moment - */ - if (!rx_frame_id.source_node_id || (server.node_id && - rx_frame_id.source_node_id != server.node_id)) { - continue; - } else if (!server.node_id || - rx_frame_id.transfer_id != server.transfer_id || - rx_frame_id.frame_index == 0) { - /* First (only?) frame of the transfer */ - if (rx_frame_id.last_frame) { - offset = 1u; - } else { - rx_crc = (uint16_t)(rx_payload[0] | (rx_payload[1] << 8u)); - server.allocated_node_id = rx_payload[2]; - offset = 3u; - } - server.node_id = rx_frame_id.source_node_id; - server.transfer_id = rx_frame_id.transfer_id; - unique_id_matched = 0u; - server.frame_index = 1u; - } else if (rx_frame_id.frame_index != server.frame_index) { - /* Abort if the frame index is wrong */ - server.node_id = 0u; - continue; - } else { - offset = 0u; - server.frame_index++; - } - - /* - Rule D. On an Allocation message WHERE (source node ID is - non-anonymous) AND (client's unique ID starts with the bytes available - in the field unique_id) AND (unique_id is less than 16 bytes long): - 1) The client broadcasts a second-stage Allocation request message, - where the fields are assigned following values: - node_id - same value as in the first-stage - first_part_of_unique_id - false - unique_id - at most 7 bytes of local unique ID - with an offset equal to number of - bytes in the received unique ID - - Rule E. On an Allocation message WHERE (source node ID is - non-anonymous) AND (unique_id fully matches client's unique ID) AND - (node_id in the received message is not zero): - 1) Request Timer stops. - 2) The client initializes its node_id with the received value. - 3) The client terminates subscription to Allocation messages. - 4) Exit. - */ - - /* Count the number of unique ID bytes matched */ - for (i = offset; i < rx_len && unique_id_matched < 16u && - hw_version.unique_id[unique_id_matched] == rx_payload[i]; - unique_id_matched++, i++); + /* Count the number of unique ID bytes matched */ + for (i = offset; i < rx_len && unique_id_matched < 16u && + hw_version.unique_id[unique_id_matched] == rx_payload[i]; + unique_id_matched++, i++); if (i < rx_len) { /* Abort if we didn't match the whole unique ID */ @@ -550,11 +514,31 @@ int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) return *allocated_node_id == 0 ? CAN_BOOT_TIMEOUT : CAN_OK; } - -int wait_for_beginfirmwareupdate(bl_timer_id tboot, - uint8_t * fw_path, - uint8_t * fw_path_length, - uint8_t * fw_source_node_id) +/**************************************************************************** + * Name: wait_for_beginfirmwareupdate + * + * Description: + * This functions performs the client side of the uavcan specification + * for begin firmware update. + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * fw_path - A pointer to the location that should receive the + * path of the firmware file to read. + * fw_path_length - A pointer to return the path length in. + * fw_source_node_id - A pointer to return the node id of the node to + * read the file from. + * Returned Value: + * CAN_OK - Indicates the a beginfirmwareupdate was received and + * processed successful. + * CAN_BOOT_TIMEOUT - Indicates that tboot expired before a + * beginfirmwareupdate was received. + * + ****************************************************************************/ + +static int wait_for_beginfirmwareupdate(bl_timer_id tboot, uint8_t * fw_path, + uint8_t * fw_path_length, + uint8_t * fw_source_node_id) { uavcan_beginfirmwareupdate_request_t request; uavcan_frame_id_t frame_id; @@ -602,10 +586,32 @@ int wait_for_beginfirmwareupdate(bl_timer_id tboot, return status; } -void file_getinfo(size_t * fw_image_size, - uint64_t * fw_image_crc, - const uint8_t * fw_path, - uint8_t fw_path_length, uint8_t fw_source_node_id) +/**************************************************************************** + * Name: file_getinfo + * + * Description: + * This functions performs the client side of the uavcan specification + * for getinfo (for a file). + * + * Input Parameters: + * fw_image_size - A pointer to the location that should receive the + * firmware image size. + * fw_image_crc - A pointer to the location that should receive the + * firmware image crc. + * fw_path - A pointer to the path of the firmware file that's + * info is being requested. + * fw_path_length - The path length of the firmware file that's + * info is being requested. + * fw_source_node_id - The node id of the node to get the info from. + * + * Returned Value: + * None. + * + ****************************************************************************/ + +static void file_getinfo(size_t * fw_image_size, uint64_t * fw_image_crc, + const uint8_t * fw_path, uint8_t fw_path_length, + uint8_t fw_source_node_id) { uavcan_getinfo_request_t request; uavcan_getinfo_response_t response; @@ -658,7 +664,31 @@ void file_getinfo(size_t * fw_image_size, } } -flash_error_t file_read_and_program(uint8_t fw_source_node_id, +/**************************************************************************** + * Name: file_read_and_program + * + * Description: + * This functions performs the client side of the uavcan specification + * for file read and programs the flash. + * + * Input Parameters: + * fw_source_node_id - The node id of the node to read the file from. + * fw_path_length - The path length of the firmware file that is + * being read. + * fw_path - A pointer to the path of the firmware file that + * is being read. + * fw_image_size - The size the fw image file should be. + * fw_word0 - A pointer to the loactions to store the first + * ford of the fw image. + * Returned Value: + * FLASH_OK - Indicates that the correct amount of data has + * ben programed to the FLASH. + * processed successful. + * FLASH_ERROR - Indicates that an error occurred + * + ****************************************************************************/ + +static flash_error_t file_read_and_program(uint8_t fw_source_node_id, uint8_t fw_path_length, const uint8_t * fw_path, size_t fw_image_size, @@ -670,8 +700,11 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id, can_error_t can_status; flash_error_t flash_status; uint32_t next_read_deadline; - uint8_t transfer_id, retries, write_remainder[4], write_remainder_length, - *data; + uint8_t transfer_id; + uint8_t retries; + uint8_t write_remainder[4]; + uint8_t write_remainder_length; + uint8_t *data; /* Set up the read request */ for (i = 0; i < fw_path_length; i++) @@ -791,102 +824,35 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id, flash_status == FLASH_OK); /* - * Return success iff the last read succeeded, the last write succeeded, the + * Return success if the last read succeeded, the last write succeeded, the * correct number of bytes were written, and the length of the last response * was zero. */ if (can_status == CAN_OK && flash_status == FLASH_OK && bytes_written == fw_image_size && response.data_length == 0u) { return FLASH_OK; - } - else - { - return FLASH_ERROR; - } -} - -void send_log_message(uint8_t node_id, - uint8_t level, uint8_t stage, uint8_t status) -{ - uavcan_logmessage_t message; - uavcan_frame_id_t frame_id; - uint8_t payload[8]; - size_t frame_len; - - frame_id.transfer_id = 0; - frame_id.last_frame = 1u; - frame_id.frame_index = 0; - frame_id.source_node_id = node_id; - frame_id.transfer_type = MESSAGE_BROADCAST; - frame_id.data_type_id = UAVCAN_LOGMESSAGE_DTID; - - message.level = level; - message.message[0] = stage; - message.message[1] = status; - frame_len = uavcan_pack_logmessage(payload, &message); - can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBAll); -} - -static uint8_t is_app_valid(uint32_t first_word) -{ - uint64_t crc; - size_t i, length, crc_offset; - uint8_t byte; - - find_descriptor(); - - /* UAVCANBootloader_v0.3 #7: bool AppValid = (AppFlash[0] != 0xffffffff) && - * ComputeAppCRC(APP_LOADADDR, APP_INFO_OFFSET) */ - if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu) - { - return 0u; - } - - length = bootloader.fw_image_descriptor->image_size; - crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) - - (size_t) bootloader.fw_image; - - crc = CRC64_INITIAL; - for (i = 0u; i < 4u; i++) - { - crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u))); - } - for (i = 4u; i < length; i++) - { - if (crc_offset <= i && i < crc_offset + 8u) - { - /* Zero out the CRC field while computing the CRC */ - byte = 0u; - } - else - { - byte = ((volatile uint8_t *)bootloader.fw_image)[i]; - } - crc = crc64_add(crc, byte); - } - crc ^= CRC64_OUTPUT_XOR; - - return crc == bootloader.fw_image_descriptor->image_crc; -} - -void find_descriptor(void) -{ - bootloader.fw_image_descriptor = NULL; - uint64_t *p = (uint64_t *) APPLICATION_LOAD_ADDRESS; - union - { - uint64_t ull; - char text[sizeof(uint64_t)]; - } sig = { - .text = {APP_DESCRIPTOR_SIGNATURE} - }; - do { - if (*p == sig.ull) { - bootloader.fw_image_descriptor = (volatile app_descriptor_t *) p; - } - } while(bootloader.fw_image_descriptor == NULL && ++p < APPLICATION_LAST_64BIT_ADDRRESS); + } + else + { + return FLASH_ERROR; + } } +/**************************************************************************** + * Name: do_jump + * + * Description: + * This functions begins the execution of the application firmware. + * + * Input Parameters: + * stacktop - The value that should be loaded into the stack pointer. + * entrypoint - The address to jump to. + * + * Returned Value: + * Does not return. + * + ****************************************************************************/ + static void do_jump(uint32_t stacktop, uint32_t entrypoint) { asm volatile ("msr msp, %0 \n" @@ -895,7 +861,26 @@ static void do_jump(uint32_t stacktop, uint32_t entrypoint) for (;;); } -void application_run(size_t fw_image_size) +/**************************************************************************** + * Name: application_run + * + * Description: + * This functions will test the application image is valid by + * checking the value of the first word for != 0xffffffff and the + * second word for an address that lies inside od the application + * fw image in flash. + * + * Input Parameters: + * fw_image_size - The size the fw image is. + * + * Returned Value: + * If the Image is valid this code does not return, but runs the application + * via do_jump. + * If the image is invalid the function returns. + * + ****************************************************************************/ + +static void application_run(size_t fw_image_size) { /* * We refuse to program the first word of the app until the upload is marked @@ -932,3 +917,291 @@ void application_run(size_t fw_image_size) do_jump(bootloader.fw_image[0], bootloader.fw_image[1]); } } + +/**************************************************************************** + * Name: autobaud_and_get_dynamic_node_id + * + * Description: + * This helper functions wraps the auto baud and node id allocation + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * speed - A pointer to the location to receive the speed detected by + * the auto baud function. + * node_id - A pointer to the location to receive the allocated node_id + * from the dynamic node allocation. + * + * Returned Value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, + can_speed_t *speed, + uint32_t *node_id) +{ + + board_indicate(autobaud_start); + + int rv = can_autobaud(speed, tboot); + + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(autobaud_end); + board_indicate(allocation_start); + rv = get_dynamic_node_id(tboot, node_id); + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(allocation_end); + } + } + return rv; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: main + * + * Description: + * Called by the os_start code + * + * Input Parameters: + * Not used. + * + * Returned Value: + * Does not return. + * + ****************************************************************************/ + +__EXPORT int main(int argc, char *argv[]) +{ + + uint64_t fw_image_crc; + size_t fw_image_size = 0; + uint32_t fw_word0; + uint8_t fw_path[200]; + uint8_t fw_path_length; + uint8_t fw_source_node_id; + uint8_t error_log_stage; + flash_error_t status; + bootloader_app_shared_t common; + +#ifdef OPT_ENABLE_WD + /* ... */ +#endif + + memset((void *)&bootloader, 0, sizeof(bootloader)); + + bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING; + + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT; + + bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS); + +#if defined(OPT_WAIT_FOR_GETNODEINFO) + bootloader.wait_for_getnodeinfo = 1; +#endif + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT; +#endif + + + bootloader.app_valid = is_app_valid(bootloader.fw_image[0]); + + board_indicate(reset); + + + bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && + common.bus_speed && common.node_id; + bootloader_app_shared_invalidate(); + + bl_timer_cb_t p = null_cb; + p.cb = uptime_process; + timer_allocate(modeRepeating|modeStarted, 1000, &p); + + p.cb = node_info_process; + bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p); + + p.cb = node_status_process; + bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p); + + + + bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0); + + if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) { + timer_start(tboot); + } + + if (bootloader.app_bl_request) { + + bootloader.bus_speed = common.bus_speed; + bootloader.node_id = (uint8_t) common.node_id; + can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); + + } else { + + can_speed_t speed; + + if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) { + goto boot; + } + + /* reset uptime */ + bootloader.uptime = 0; + common.bus_speed = can_speed2freq(speed); + bootloader.node_id = (uint8_t) common.node_id; + + } + // Start Processes that requier Node Id + timer_start(tinfo); + timer_start(tstatus); + + while (!bootloader.sent_node_info_response) { + if (timer_expired(tboot)) + { + goto boot; + } + } + + if (bootloader.app_valid && + (bootloader.wait_for_getnodeinfo || + bootloader.app_bl_request)) { + + timer_start(tboot); + } + + do + { + if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path, + &fw_path_length, + &fw_source_node_id)) + { + goto boot; + } + } + while (!fw_source_node_id); + + timer_stop(tboot); + board_indicate(fw_update_start); + + file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length, + fw_source_node_id); + + //todo:Check this + if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc) + { + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO; + goto failure; + } + + /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_INFO, + UAVCAN_LOGMESSAGE_STAGE_ERASE, + UAVCAN_LOGMESSAGE_RESULT_START); + + + /* Need to signal that the app is no longer valid if Node Info Request are done */ + + bootloader.app_valid = false; + + status = bl_flash_erase(APPLICATION_LOAD_ADDRESS); + if (status != FLASH_OK) + { + /* UAVCANBootloader_v0.3 #28.8: [Erase + * Failed]:INDICATE_FW_UPDATE_ERASE_FAIL */ + board_indicate(fw_update_erase_fail); + + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE; + goto failure; + } + + status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path, + fw_image_size, &fw_word0); + if (status != FLASH_OK) + { + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM; + goto failure; + } + + /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */ + if (!is_app_valid(fw_word0)) + { + bootloader.app_valid = 0u; + + /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */ + board_indicate(fw_update_invalid_crc); + + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE; + goto failure; + } + + /* UAVCANBootloader_v0.3 #46: [crc == fw_crc]:write word0 */ + status = bl_flash_write_word((uint32_t) bootloader.fw_image, (uint8_t *) & fw_word0); + if (status != FLASH_OK) + { + /* Not specifically listed in UAVCANBootloader_v0.3: + * 1023.LogMessage.uavcan */ + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE; + goto failure; + } + + /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */ + bootloader_app_shared_write(&common, BootLoader); + + /* Send a completion log message */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_INFO, + UAVCAN_LOGMESSAGE_STAGE_FINALIZE, + UAVCAN_LOGMESSAGE_RESULT_OK); + + /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */ + +boot: + /* UAVCANBootloader_v0.3 #50: jump_to_app */ + application_run(fw_image_size); + + /* We will fall thru if the Iamage is bad */ + +failure: + /* UAVCANBootloader_v0.3 #28.2: + * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */ + /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #38: [(retries == 0 && + * timeout)]:1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_ERROR, + error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL); + + /* UAVCANBootloader_v0.3 #28.3: + * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #28.10: [Erase + * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #39: [(retries == 0 && + * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL; + + /* UAVCANBootloader_v0.3 #28.1: + * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ + /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(), + * RestartWithDelay(20,000ms) */ + /* UAVCANBootloader_v0.3 #45: [crc + * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ + + bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0); + while(!timer_expired(tmr)) + { + ; + } + timer_free(tmr); + up_systemreset(); +} -- cgit v1.2.3