/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* CAN bootloader support */ #include #include #if HAL_USE_CAN == TRUE || HAL_NUM_CAN_IFACES #include #include #include #include "support.h" #include #include "can.h" #include "bl_protocol.h" #include #include "app_comms.h" #include #include #include #include static CanardInstance canard; static uint32_t canard_memory_pool[4096/4]; #ifndef HAL_CAN_DEFAULT_NODE_ID #define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID #endif static uint8_t initial_node_id = HAL_CAN_DEFAULT_NODE_ID; // can config for 1MBit static uint32_t baudrate = 1000000U; #if HAL_USE_CAN 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 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." CHIBIOS_BOARD_NAME #endif static uint8_t node_id_allocation_transfer_id; 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 #if CH_CFG_USE_MUTEXES == TRUE static HAL_Semaphore can_mutex; #endif static struct { uint32_t rtt_ms; uint32_t ofs; uint8_t node_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; /* get cpu unique ID */ static void readUniqueID(uint8_t* out_uid) { uint8_t len = sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data); memset(out_uid, 0, len); memcpy(out_uid, (const void *)UDID_START, MIN(len,12)); } /* simple 16 bit random number generator */ static uint16_t get_randomu16(void) { static uint32_t m_z = 1234; static uint32_t m_w = 76542; m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); return ((m_z << 16) + m_w) & 0xFFFF; } /** * Returns a pseudo random integer in a given range */ static uint32_t get_random_range(uint16_t range) { return get_randomu16() % range; } /* handle a GET_NODE_INFO request */ static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer) { uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {}; uavcan_protocol_GetNodeInfoResponse pkt {}; node_status.uptime_sec = AP_HAL::millis() / 1000U; pkt.status = node_status; pkt.software_version.major = CAN_APP_VERSION_MAJOR; pkt.software_version.minor = CAN_APP_VERSION_MINOR; readUniqueID(pkt.hardware_version.unique_id); // use hw major/minor for APJ_BOARD_ID so we know what fw is // compatible with this hardware pkt.hardware_version.major = APJ_BOARD_ID >> 8; pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF; char name[strlen(CAN_APP_NODE_NAME)+1]; strcpy(name, CAN_APP_NODE_NAME); pkt.name.len = strlen(CAN_APP_NODE_NAME); memcpy(pkt.name.data, name, pkt.name.len); uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, true); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], total_size); } /* send a read for a fw update */ static bool send_fw_read(uint8_t idx) { auto &r = fw_update.reads[idx]; r.tx_id = fw_update.transfer_id; r.have_reply = false; uavcan_protocol_file_ReadRequest pkt {}; 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); 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; isource_node_id != fw_update.node_id) { return; } /* match the response to a sent request */ uint8_t idx = 0; bool found = false; for (idx=0; idxtransfer_id) { found = true; break; } } 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); } /* handle a begin firmware update request. We start pulling in the file data */ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) { if (fw_update.node_id == 0) { uavcan_protocol_file_BeginFirmwareUpdateRequest pkt; if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &pkt)) { return; } 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; isource_node_id; } } uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, true); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], total_size); } static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) { // Rule C - updating the randomized time interval send_next_node_id_allocation_request_at_ms = AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { node_id_allocation_unique_id_offset = 0; return; } struct uavcan_protocol_dynamic_node_id_Allocation msg; if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { return; } // Obtaining the local unique ID uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; readUniqueID(my_unique_id); // Matching the received UID against the local one if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { node_id_allocation_unique_id_offset = 0; return; // No match, return } if (msg.unique_id.len < sizeof(msg.unique_id.data)) { // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. node_id_allocation_unique_id_offset = msg.unique_id.len; send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; } else { // Allocation complete - copying the allocated node ID from the message canardSetLocalNodeID(ins, msg.node_id); } } /** * This callback is invoked by the library when a new message or request or response is received. */ static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise. */ if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { if (transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { handle_allocation_response(ins, transfer); } return; } switch (transfer->data_type_id) { case UAVCAN_PROTOCOL_GETNODEINFO_ID: handle_get_node_info(ins, transfer); break; case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: handle_begin_firmware_update(ins, transfer); break; case UAVCAN_PROTOCOL_FILE_READ_ID: handle_file_read_response(ins, transfer); break; case UAVCAN_PROTOCOL_RESTARTNODE_ID: NVIC_SystemReset(); break; } } /** * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received * by the local node. * If the callback returns true, the library will receive the transfer. * If the callback returns false, the library will ignore the transfer. * All transfers that are addressed to other nodes are always ignored. */ static bool shouldAcceptTransfer(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { (void)source_node_id; if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { /* * If we're in the process of allocation of dynamic node ID, accept only relevant transfers. */ if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID)) { *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; return true; } return false; } switch (data_type_id) { case UAVCAN_PROTOCOL_GETNODEINFO_ID: *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE; return true; case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; return true; case UAVCAN_PROTOCOL_RESTARTNODE_ID: *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; return true; case UAVCAN_PROTOCOL_FILE_READ_ID: *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; return true; default: break; } return false; } #if HAL_USE_CAN static void processTx(void) { static uint8_t fail_count; for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { CANTxFrame txmsg {}; txmsg.DLC = txf->data_len; memcpy(txmsg.data8, txf->data, 8); txmsg.EID = txf->id & CANARD_CAN_EXT_ID_MASK; txmsg.IDE = 1; txmsg.RTR = 0; if (canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE) == MSG_OK) { canardPopTxQueue(&canard); fail_count = 0; } else { // just exit and try again later. If we fail 8 times in a row // then start discarding to prevent the pool filling up if (fail_count < 8) { fail_count++; } else { canardPopTxQueue(&canard); } return; } } } static void processRx(void) { CANRxFrame rxmsg {}; while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) { CanardCANFrame rx_frame {}; #ifdef HAL_GPIO_PIN_LED_BOOTLOADER palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER); #endif const uint64_t timestamp = AP_HAL::micros64(); memcpy(rx_frame.data, rxmsg.data8, 8); rx_frame.data_len = rxmsg.DLC; if(rxmsg.IDE) { rx_frame.id = CANARD_CAN_FRAME_EFF | rxmsg.EID; } else { rx_frame.id = rxmsg.SID; } canardHandleRxFrame(&canard, &rx_frame, timestamp); } } #else // Use HAL CAN interface static void processTx(void) { static uint8_t fail_count; for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { AP_HAL::CANFrame txmsg {}; txmsg.dlc = txf->data_len; memcpy(txmsg.data, txf->data, 8); txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); // push message with 1s timeout bool send_ok = false; for (uint8_t i=0; i 0); } if (send_ok) { canardPopTxQueue(&canard); fail_count = 0; } else { // just exit and try again later. If we fail 8 times in a row // then start discarding to prevent the pool filling up if (fail_count < 8) { fail_count++; } else { canardPopTxQueue(&canard); } return; } } } static void processRx(void) { AP_HAL::CANFrame rxmsg; while (true) { bool got_pkt = false; for (uint8_t i=0; i MaxLenOfUniqueIDInRequest) { uid_size = MaxLenOfUniqueIDInRequest; } memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); // Broadcasting the request canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, node_id_allocation_transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &allocation_request[0], (uint16_t) (uid_size + 1)); // Preparing for timeout; if response is received, this value will be updated from the callback. node_id_allocation_unique_id_offset = 0; } static void send_node_status(void) { uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; node_status.uptime_sec = AP_HAL::millis() / 1000U; uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, true); static uint8_t transfer_id; // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)! canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len); } /** * This function is called at 1 Hz rate from the main loop. */ static void process1HzTasks(uint64_t timestamp_usec) { canardCleanupStaleTransfers(&canard, timestamp_usec); if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { node_status.mode = fw_update.node_id?UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE; send_node_status(); } } void can_set_node_id(uint8_t node_id) { initial_node_id = node_id; } // check for a firmware update marker left by app bool can_check_update(void) { bool ret = false; #if HAL_RAM_RESERVE_START >= 256 struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; 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; ipath, sizeof(uavcan_protocol_file_Path::path.data)+1); memset(comms, 0, sizeof(*comms)); ret = true; } #endif #if defined(CAN1_BASE) && defined(RCC_APB1ENR_CAN1EN) // check for px4 fw update. px4 uses the filter registers in CAN1 // to communicate with the bootloader. This only works on CAN1 if (!ret && stm32_was_software_reset()) { uint32_t *fir = (uint32_t *)(CAN1_BASE + 0x240); struct PACKED app_shared { union { uint64_t ull; uint32_t ul[2]; uint8_t valid; } crc; uint32_t signature; uint32_t bus_speed; uint32_t node_id; } *app = (struct app_shared *)&fir[4]; /* we need to enable the CAN peripheral in order to look at the FIR registers. */ RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; static const uint32_t app_signature = 0xb0a04150; if (app->signature == app_signature && app->node_id > 0 && app->node_id < 128) { // crc is in reversed word order in FIR registers uint32_t sig[3]; memcpy((uint8_t *)&sig[0], (const uint8_t *)&app->signature, sizeof(sig)); const uint64_t crc = crc_crc64(sig, 3); const uint32_t *crc32 = (const uint32_t *)&crc; if (crc32[0] == app->crc.ul[1] && crc32[1] == app->crc.ul[0]) { // reset signature so we don't get in a boot loop app->signature = 0; // setup node ID can_set_node_id(app->node_id); // and baudrate baudrate = app->bus_speed; ret = true; } } } #endif return ret; } void can_start() { node_status.vendor_specific_status_code = uint8_t(check_good_firmware()); node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE; #if HAL_USE_CAN // calculate optimal CAN timings given PCLK1 and baudrate CanardSTM32CANTimings timings {}; canardSTM32ComputeCANTimings(STM32_PCLK1, baudrate, &timings); cancfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS2(timings.bit_segment_2-1) | CAN_BTR_TS1(timings.bit_segment_1-1) | CAN_BTR_BRP(timings.bit_rate_prescaler-1); canStart(&CAND1, &cancfg); #else for (uint8_t i=0; i= 1000) { last_1Hz_ms = now; process1HzTasks(AP_HAL::micros64()); } if (fw_update.node_id != 0) { 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); } // printf to CAN LogMessage for debugging void can_printf(const char *fmt, ...) { // only on H7 for now, where we have plenty of flash #if defined(STM32H7) uavcan_protocol_debug_LogMessage pkt {}; uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {}; va_list ap; va_start(ap, fmt); uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap); va_end(ap); pkt.text.len = MIN(n, sizeof(pkt.text.data)); uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, true); static uint8_t logmsg_transfer_id; canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, logmsg_transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len); #endif // defined(STM32H7) } #endif // HAL_USE_CAN