diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index f998b20d1c..63e6385bdc 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -44,7 +44,7 @@ int main(void) chThdSleepMilliseconds(1000); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); - + board_info.board_type = APJ_BOARD_ID; board_info.board_rev = 0; board_info.fw_size = (BOARD_FLASH_SIZE - FLASH_BOOTLOADER_LOAD_KB)*1024; diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index ec909d3d5b..0912652e65 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -130,16 +130,16 @@ volatile unsigned timer[NTIMERS]; static void sys_tick_handler(void *ctx) { chVTSetI(&systick_vt, MS2ST(1), sys_tick_handler, nullptr); - uint8_t i; - for (i = 0; i < NTIMERS; i++) - if (timer[i] > 0) { - timer[i]--; - } + uint8_t i; + for (i = 0; i < NTIMERS; i++) + if (timer[i] > 0) { + timer[i]--; + } - if ((led_state == LED_BLINK) && (timer[TIMER_LED] == 0)) { - led_toggle(LED_BOOTLOADER); - timer[TIMER_LED] = 50; - } + if ((led_state == LED_BLINK) && (timer[TIMER_LED] == 0)) { + led_toggle(LED_BOOTLOADER); + timer[TIMER_LED] = 50; + } } static void delay(unsigned msec) @@ -150,22 +150,22 @@ static void delay(unsigned msec) static void led_set(enum led_state state) { - led_state = state; + led_state = state; - switch (state) { - case LED_OFF: - led_off(LED_BOOTLOADER); - break; + switch (state) { + case LED_OFF: + led_off(LED_BOOTLOADER); + break; - case LED_ON: - led_on(LED_BOOTLOADER); - break; + case LED_ON: + led_on(LED_BOOTLOADER); + break; - case LED_BLINK: - /* restart the blink state machine ASAP */ - timer[TIMER_LED] = 0; - break; - } + case LED_BLINK: + /* restart the blink state machine ASAP */ + timer[TIMER_LED] = 0; + break; + } } static void @@ -179,10 +179,10 @@ do_jump(uint32_t stacktop, uint32_t entrypoint) SCB_DisableICache(); #endif - asm volatile( - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stacktop), "r"(entrypoint) :); + asm volatile( + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stacktop), "r"(entrypoint) :); } #define APP_START_ADDRESS (FLASH_LOAD_ADDRESS + FLASH_BOOTLOADER_LOAD_KB*1024U) @@ -190,31 +190,31 @@ do_jump(uint32_t stacktop, uint32_t entrypoint) void jump_to_app() { - const uint32_t *app_base = (const uint32_t *)(APP_START_ADDRESS); + const uint32_t *app_base = (const uint32_t *)(APP_START_ADDRESS); - /* - * We refuse to program the first word of the app until the upload is marked - * complete by the host. So if it's not 0xffffffff, we should try booting it. - */ - if (app_base[0] == 0xffffffff) { - return; - } + /* + * We refuse to program the first word of the app until the upload is marked + * complete by the host. So if it's not 0xffffffff, we should try booting it. + */ + if (app_base[0] == 0xffffffff) { + return; + } - /* - * The second word of the app is the entrypoint; it must point within the - * flash area (or we have a bad flash). - */ - if (app_base[1] < APP_START_ADDRESS) { - return; - } + /* + * The second word of the app is the entrypoint; it must point within the + * flash area (or we have a bad flash). + */ + if (app_base[1] < APP_START_ADDRESS) { + return; + } - if (app_base[1] >= (APP_START_ADDRESS + board_info.fw_size)) { - return; - } + if (app_base[1] >= (APP_START_ADDRESS + board_info.fw_size)) { + return; + } led_set(LED_OFF); - - /* kill the systick timer */ + + /* kill the systick timer */ chVTReset(&systick_vt); // stop USB driver @@ -223,57 +223,57 @@ jump_to_app() // disable all interrupt sources //port_disable(); - /* switch exception handlers to the application */ - *(volatile uint32_t *)SCB_VTOR = APP_START_ADDRESS; - - /* extract the stack and entrypoint from the app vector table and go */ - do_jump(app_base[0], app_base[1]); + /* switch exception handlers to the application */ + *(volatile uint32_t *)SCB_VTOR = APP_START_ADDRESS; + + /* extract the stack and entrypoint from the app vector table and go */ + do_jump(app_base[0], app_base[1]); } static void sync_response(void) { - uint8_t data[] = { - PROTO_INSYNC, // "in sync" - PROTO_OK // "OK" - }; + uint8_t data[] = { + PROTO_INSYNC, // "in sync" + PROTO_OK // "OK" + }; - cout(data, sizeof(data)); + cout(data, sizeof(data)); } #if defined(TARGET_HW_PX4_FMU_V4) static void bad_silicon_response(void) { - uint8_t data[] = { - PROTO_INSYNC, // "in sync" - PROTO_BAD_SILICON_REV // "issue with < Rev 3 silicon" - }; + uint8_t data[] = { + PROTO_INSYNC, // "in sync" + PROTO_BAD_SILICON_REV // "issue with < Rev 3 silicon" + }; - cout(data, sizeof(data)); + cout(data, sizeof(data)); } #endif static void invalid_response(void) { - uint8_t data[] = { - PROTO_INSYNC, // "in sync" - PROTO_INVALID // "invalid command" - }; + uint8_t data[] = { + PROTO_INSYNC, // "in sync" + PROTO_INVALID // "invalid command" + }; - cout(data, sizeof(data)); + cout(data, sizeof(data)); } static void failure_response(void) { - uint8_t data[] = { - PROTO_INSYNC, // "in sync" - PROTO_FAILED // "command failed" - }; + uint8_t data[] = { + PROTO_INSYNC, // "in sync" + PROTO_FAILED // "command failed" + }; - cout(data, sizeof(data)); + cout(data, sizeof(data)); } static volatile unsigned cin_count; @@ -287,500 +287,500 @@ static volatile unsigned cin_count; inline static bool wait_for_eoc(unsigned timeout) { - return cin(timeout) == PROTO_EOC; + return cin(timeout) == PROTO_EOC; } static void cout_word(uint32_t val) { - cout((uint8_t *)&val, 4); + cout((uint8_t *)&val, 4); } static int cin_word(uint32_t *wp, unsigned timeout) { - union { - uint32_t w; - uint8_t b[4]; - } u; + union { + uint32_t w; + uint8_t b[4]; + } u; - for (unsigned i = 0; i < 4; i++) { - int c = cin(timeout); + for (unsigned i = 0; i < 4; i++) { + int c = cin(timeout); - if (c < 0) { - return c; - } + if (c < 0) { + return c; + } - u.b[i] = c & 0xff; - } + u.b[i] = c & 0xff; + } - *wp = u.w; - return 0; + *wp = u.w; + return 0; } static uint32_t crc32(const uint8_t *src, unsigned len, unsigned state) { - static uint32_t crctab[256]; + static uint32_t crctab[256]; - /* check whether we have generated the CRC table yet */ - /* this is much smaller than a static table */ - if (crctab[1] == 0) { - for (unsigned i = 0; i < 256; i++) { - uint32_t c = i; + /* check whether we have generated the CRC table yet */ + /* this is much smaller than a static table */ + if (crctab[1] == 0) { + for (unsigned i = 0; i < 256; i++) { + uint32_t c = i; - for (unsigned j = 0; j < 8; j++) { - if (c & 1) { - c = 0xedb88320U ^ (c >> 1); + for (unsigned j = 0; j < 8; j++) { + if (c & 1) { + c = 0xedb88320U ^ (c >> 1); - } else { - c = c >> 1; - } - } + } else { + c = c >> 1; + } + } - crctab[i] = c; - } - } + crctab[i] = c; + } + } - for (unsigned i = 0; i < len; i++) { - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - } + for (unsigned i = 0; i < len; i++) { + state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + } - return state; + return state; } void bootloader(unsigned timeout) { - uint32_t address = board_info.fw_size; /* force erase before upload will work */ - uint32_t first_word = 0xffffffff; + uint32_t address = board_info.fw_size; /* force erase before upload will work */ + uint32_t first_word = 0xffffffff; chVTObjectInit(&systick_vt); chVTSet(&systick_vt, MS2ST(1), sys_tick_handler, nullptr); - - /* if we are working with a timeout, start it running */ - if (timeout) { - timer[TIMER_BL_WAIT] = timeout; - } - /* make the LED blink while we are idle */ - led_set(LED_BLINK); + /* if we are working with a timeout, start it running */ + if (timeout) { + timer[TIMER_BL_WAIT] = timeout; + } - while (true) { - volatile int c; - int arg; - static union { - uint8_t c[256]; - uint32_t w[64]; - } flash_buffer; + /* make the LED blink while we are idle */ + led_set(LED_BLINK); - // Wait for a command byte - led_off(LED_ACTIVITY); + while (true) { + volatile int c; + int arg; + static union { + uint8_t c[256]; + uint32_t w[64]; + } flash_buffer; - do { - /* if we have a timeout and the timer has expired, return now */ - if (timeout && !timer[TIMER_BL_WAIT]) { - return; - } + // Wait for a command byte + led_off(LED_ACTIVITY); - /* try to get a byte from the host */ - c = cin(0); + do { + /* if we have a timeout and the timer has expired, return now */ + if (timeout && !timer[TIMER_BL_WAIT]) { + return; + } - } while (c < 0); + /* try to get a byte from the host */ + c = cin(0); - led_on(LED_ACTIVITY); + } while (c < 0); - // handle the command byte - switch (c) { + led_on(LED_ACTIVITY); - // sync - // - // command: GET_SYNC/EOC - // reply: INSYNC/OK - // - case PROTO_GET_SYNC: + // handle the command byte + switch (c) { - /* expect EOC */ - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // sync + // + // command: GET_SYNC/EOC + // reply: INSYNC/OK + // + case PROTO_GET_SYNC: - break; + /* expect EOC */ + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - // get device info - // - // command: GET_DEVICE//EOC - // BL_REV reply: /INSYNC/EOC - // BOARD_ID reply: /INSYNC/EOC - // BOARD_REV reply: /INSYNC/EOC - // FW_SIZE reply: /INSYNC/EOC - // VEC_AREA reply /INSYNC/EOC - // bad arg reply: INSYNC/INVALID - // - case PROTO_GET_DEVICE: - /* expect arg then EOC */ - arg = cin(1000); + break; - if (arg < 0) { - goto cmd_bad; - } + // get device info + // + // command: GET_DEVICE//EOC + // BL_REV reply: /INSYNC/EOC + // BOARD_ID reply: /INSYNC/EOC + // BOARD_REV reply: /INSYNC/EOC + // FW_SIZE reply: /INSYNC/EOC + // VEC_AREA reply /INSYNC/EOC + // bad arg reply: INSYNC/INVALID + // + case PROTO_GET_DEVICE: + /* expect arg then EOC */ + arg = cin(1000); - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + if (arg < 0) { + goto cmd_bad; + } - switch (arg) { - case PROTO_DEVICE_BL_REV: { + if (!wait_for_eoc(2)) { + goto cmd_bad; + } + + switch (arg) { + case PROTO_DEVICE_BL_REV: { uint32_t bl_proto_rev = BL_PROTOCOL_VERSION; - cout((uint8_t *)&bl_proto_rev, sizeof(bl_proto_rev)); + cout((uint8_t *)&bl_proto_rev, sizeof(bl_proto_rev)); break; } - case PROTO_DEVICE_BOARD_ID: - cout((uint8_t *)&board_info.board_type, sizeof(board_info.board_type)); - break; + case PROTO_DEVICE_BOARD_ID: + cout((uint8_t *)&board_info.board_type, sizeof(board_info.board_type)); + break; - case PROTO_DEVICE_BOARD_REV: - cout((uint8_t *)&board_info.board_rev, sizeof(board_info.board_rev)); - break; + case PROTO_DEVICE_BOARD_REV: + cout((uint8_t *)&board_info.board_rev, sizeof(board_info.board_rev)); + break; - case PROTO_DEVICE_FW_SIZE: - cout((uint8_t *)&board_info.fw_size, sizeof(board_info.fw_size)); - break; + case PROTO_DEVICE_FW_SIZE: + cout((uint8_t *)&board_info.fw_size, sizeof(board_info.fw_size)); + break; - case PROTO_DEVICE_VEC_AREA: - for (unsigned p = 7; p <= 10; p++) { - uint32_t bytes = flash_func_read_word(p * 4); + case PROTO_DEVICE_VEC_AREA: + for (unsigned p = 7; p <= 10; p++) { + uint32_t bytes = flash_func_read_word(p * 4); - cout((uint8_t *)&bytes, sizeof(bytes)); - } + cout((uint8_t *)&bytes, sizeof(bytes)); + } - break; + break; - default: - goto cmd_bad; - } - - break; - - // erase and prepare for programming - // - // command: ERASE/EOC - // success reply: INSYNC/OK - // erase failure: INSYNC/FAILURE - // - case PROTO_CHIP_ERASE: - - /* expect EOC */ - if (!wait_for_eoc(2)) { - goto cmd_bad; - } - - // clear the bootloader LED while erasing - it stops blinking at random - // and that's confusing - led_set(LED_OFF); - - // erase all sectors - for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { - flash_func_erase_sector(i); - } - - // enable the LED while verifying the erase - led_set(LED_ON); - - // verify the erase - for (address = 0; address < board_info.fw_size; address += 4) { - if (flash_func_read_word(address) != 0xffffffff) { - goto cmd_fail; - } + default: + goto cmd_bad; } - address = 0; + break; - // resume blinking - led_set(LED_BLINK); - break; + // erase and prepare for programming + // + // command: ERASE/EOC + // success reply: INSYNC/OK + // erase failure: INSYNC/FAILURE + // + case PROTO_CHIP_ERASE: - // program bytes at current address - // - // command: PROG_MULTI///EOC - // success reply: INSYNC/OK - // invalid reply: INSYNC/INVALID - // readback failure: INSYNC/FAILURE - // - case PROTO_PROG_MULTI: // program bytes - // expect count + /* expect EOC */ + if (!wait_for_eoc(2)) { + goto cmd_bad; + } + + // clear the bootloader LED while erasing - it stops blinking at random + // and that's confusing led_set(LED_OFF); - - arg = cin(50); - if (arg < 0) { - goto cmd_bad; - } + // erase all sectors + for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { + flash_func_erase_sector(i); + } - // sanity-check arguments - if (arg % 4) { - goto cmd_bad; - } + // enable the LED while verifying the erase + led_set(LED_ON); - if ((address + arg) > board_info.fw_size) { - goto cmd_bad; - } + // verify the erase + for (address = 0; address < board_info.fw_size; address += 4) { + if (flash_func_read_word(address) != 0xffffffff) { + goto cmd_fail; + } + } - if (arg > sizeof(flash_buffer.c)) { - goto cmd_bad; - } + address = 0; - for (int i = 0; i < arg; i++) { - c = cin(1000); + // resume blinking + led_set(LED_BLINK); + break; - if (c < 0) { - goto cmd_bad; - } + // program bytes at current address + // + // command: PROG_MULTI///EOC + // success reply: INSYNC/OK + // invalid reply: INSYNC/INVALID + // readback failure: INSYNC/FAILURE + // + case PROTO_PROG_MULTI: // program bytes + // expect count + led_set(LED_OFF); - flash_buffer.c[i] = c; - } + arg = cin(50); - if (!wait_for_eoc(200)) { - goto cmd_bad; - } + if (arg < 0) { + goto cmd_bad; + } - if (address == 0) { + // sanity-check arguments + if (arg % 4) { + goto cmd_bad; + } + + if ((address + arg) > board_info.fw_size) { + goto cmd_bad; + } + + if (arg > sizeof(flash_buffer.c)) { + goto cmd_bad; + } + + for (int i = 0; i < arg; i++) { + c = cin(1000); + + if (c < 0) { + goto cmd_bad; + } + + flash_buffer.c[i] = c; + } + + if (!wait_for_eoc(200)) { + goto cmd_bad; + } + + if (address == 0) { // save the first word and don't program it until everything else is done first_word = flash_buffer.w[0]; // replace first word with bits we can overwrite later flash_buffer.w[0] = 0xffffffff; - } + } - arg /= 4; + arg /= 4; - for (int i = 0; i < arg; i++) { - // program the word + for (int i = 0; i < arg; i++) { + // program the word flash_func_write_word(address, flash_buffer.w[i]); - - // do immediate read-back verify - if (flash_func_read_word(address) != flash_buffer.w[i]) { - goto cmd_fail; - } - address += 4; - } + // do immediate read-back verify + if (flash_func_read_word(address) != flash_buffer.w[i]) { + goto cmd_fail; + } - break; + address += 4; + } - // fetch CRC of the entire flash area - // - // command: GET_CRC/EOC - // reply: /INSYNC/OK - // - case PROTO_GET_CRC: { - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + break; - // compute CRC of the programmed area - uint32_t sum = 0; + // fetch CRC of the entire flash area + // + // command: GET_CRC/EOC + // reply: /INSYNC/OK + // + case PROTO_GET_CRC: { + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - for (unsigned p = 0; p < board_info.fw_size; p += 4) { - uint32_t bytes; + // compute CRC of the programmed area + uint32_t sum = 0; - if ((p == 0) && (first_word != 0xffffffff)) { - bytes = first_word; + for (unsigned p = 0; p < board_info.fw_size; p += 4) { + uint32_t bytes; - } else { - bytes = flash_func_read_word(p); - } + if ((p == 0) && (first_word != 0xffffffff)) { + bytes = first_word; - sum = crc32((uint8_t *)&bytes, sizeof(bytes), sum); - } + } else { + bytes = flash_func_read_word(p); + } - cout_word(sum); - break; + sum = crc32((uint8_t *)&bytes, sizeof(bytes), sum); + } + + cout_word(sum); + break; } - // read a word from the OTP - // - // command: GET_OTP//EOC - // reply: /INSYNC/OK - case PROTO_GET_OTP: - // expect argument - { - uint32_t index = 0; + // read a word from the OTP + // + // command: GET_OTP//EOC + // reply: /INSYNC/OK + case PROTO_GET_OTP: + // expect argument + { + uint32_t index = 0; - if (cin_word(&index, 100)) { - goto cmd_bad; - } + if (cin_word(&index, 100)) { + goto cmd_bad; + } - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - cout_word(flash_func_read_otp(index)); - } - break; + cout_word(flash_func_read_otp(index)); + } + break; - // read the SN from the UDID - // - // command: GET_SN//EOC - // reply: /INSYNC/OK - case PROTO_GET_SN: - // expect argument - { - uint32_t index = 0; + // read the SN from the UDID + // + // command: GET_SN//EOC + // reply: /INSYNC/OK + case PROTO_GET_SN: + // expect argument + { + uint32_t index = 0; - if (cin_word(&index, 100)) { - goto cmd_bad; - } + if (cin_word(&index, 100)) { + goto cmd_bad; + } - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - cout_word(flash_func_read_sn(index)); - } - break; + cout_word(flash_func_read_sn(index)); + } + break; - // read the chip ID code - // - // command: GET_CHIP/EOC - // reply: /INSYNC/OK - case PROTO_GET_CHIP: { - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // read the chip ID code + // + // command: GET_CHIP/EOC + // reply: /INSYNC/OK + case PROTO_GET_CHIP: { + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - cout_word(get_mcu_id()); - } - break; + cout_word(get_mcu_id()); + } + break; - // read the chip description - // - // command: GET_CHIP_DES/EOC - // reply: /INSYNC/OK - case PROTO_GET_CHIP_DES: { - uint8_t buffer[MAX_DES_LENGTH]; - unsigned len = MAX_DES_LENGTH; + // read the chip description + // + // command: GET_CHIP_DES/EOC + // reply: /INSYNC/OK + case PROTO_GET_CHIP_DES: { + uint8_t buffer[MAX_DES_LENGTH]; + unsigned len = MAX_DES_LENGTH; - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - len = get_mcu_desc(len, buffer); - cout_word(len); - cout(buffer, len); - } - break; + len = get_mcu_desc(len, buffer); + cout_word(len); + cout(buffer, len); + } + break; #ifdef BOOT_DELAY_ADDRESS - case PROTO_SET_DELAY: { - /* - Allow for the bootloader to setup a - boot delay signature which tells the - board to delay for at least a - specified number of seconds on boot. - */ - int v = cin(100); + case PROTO_SET_DELAY: { + /* + Allow for the bootloader to setup a + boot delay signature which tells the + board to delay for at least a + specified number of seconds on boot. + */ + int v = cin(100); - if (v < 0) { - goto cmd_bad; - } + if (v < 0) { + goto cmd_bad; + } - uint8_t boot_delay = v & 0xFF; + uint8_t boot_delay = v & 0xFF; - if (boot_delay > BOOT_DELAY_MAX) { - goto cmd_bad; - } + if (boot_delay > BOOT_DELAY_MAX) { + goto cmd_bad; + } - // expect EOC - if (!wait_for_eoc(2)) { - goto cmd_bad; - } + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } - uint32_t sig1 = flash_func_read_word(BOOT_DELAY_ADDRESS); - uint32_t sig2 = flash_func_read_word(BOOT_DELAY_ADDRESS + 4); + uint32_t sig1 = flash_func_read_word(BOOT_DELAY_ADDRESS); + uint32_t sig2 = flash_func_read_word(BOOT_DELAY_ADDRESS + 4); - if (sig1 != BOOT_DELAY_SIGNATURE1 || - sig2 != BOOT_DELAY_SIGNATURE2) { - goto cmd_bad; - } + if (sig1 != BOOT_DELAY_SIGNATURE1 || + sig2 != BOOT_DELAY_SIGNATURE2) { + goto cmd_bad; + } - uint32_t value = (BOOT_DELAY_SIGNATURE1 & 0xFFFFFF00) | boot_delay; - flash_func_write_word(BOOT_DELAY_ADDRESS, value); + uint32_t value = (BOOT_DELAY_SIGNATURE1 & 0xFFFFFF00) | boot_delay; + flash_func_write_word(BOOT_DELAY_ADDRESS, value); - if (flash_func_read_word(BOOT_DELAY_ADDRESS) != value) { - goto cmd_fail; - } - } - break; + if (flash_func_read_word(BOOT_DELAY_ADDRESS) != value) { + goto cmd_fail; + } + } + break; #endif - // finalise programming and boot the system - // - // command: BOOT/EOC - // reply: INSYNC/OK - // - case PROTO_BOOT: + // finalise programming and boot the system + // + // command: BOOT/EOC + // reply: INSYNC/OK + // + case PROTO_BOOT: - // expect EOC - if (!wait_for_eoc(1000)) { - goto cmd_bad; - } + // expect EOC + if (!wait_for_eoc(1000)) { + goto cmd_bad; + } - // program the deferred first word - if (first_word != 0xffffffff) { - flash_func_write_word(0, first_word); + // program the deferred first word + if (first_word != 0xffffffff) { + flash_func_write_word(0, first_word); - if (flash_func_read_word(0) != first_word) { - goto cmd_fail; - } + if (flash_func_read_word(0) != first_word) { + goto cmd_fail; + } - // revert in case the flash was bad... - first_word = 0xffffffff; - } + // revert in case the flash was bad... + first_word = 0xffffffff; + } - // send a sync and wait for it to be collected - sync_response(); - delay(100); + // send a sync and wait for it to be collected + sync_response(); + delay(100); - // quiesce and jump to the app - return; + // quiesce and jump to the app + return; - case PROTO_DEBUG: - // XXX reserved for ad-hoc debugging as required - break; + case PROTO_DEBUG: + // XXX reserved for ad-hoc debugging as required + break; - default: - continue; - } + default: + continue; + } - // we got a command worth syncing, so kill the timeout because - // we are probably talking to the uploader - timeout = 0; + // we got a command worth syncing, so kill the timeout because + // we are probably talking to the uploader + timeout = 0; - // send the sync response for this command - sync_response(); - continue; + // send the sync response for this command + sync_response(); + continue; cmd_bad: - // send an 'invalid' response but don't kill the timeout - could be garbage - invalid_response(); - continue; + // send an 'invalid' response but don't kill the timeout - could be garbage + invalid_response(); + continue; cmd_fail: - // send a 'command failed' response but don't kill the timeout - could be garbage - failure_response(); - continue; + // send a 'command failed' response but don't kill the timeout - could be garbage + failure_response(); + continue; #if defined(TARGET_HW_PX4_FMU_V4) bad_silicon: - // send the bad silicon response but don't kill the timeout - could be garbage - bad_silicon_response(); - continue; + // send the bad silicon response but don't kill the timeout - could be garbage + bad_silicon_response(); + continue; #endif - } + } } diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index a33f1d5fca..a9556055bd 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -8,7 +8,10 @@ #include "hwdef.h" #include #include +#include #include "support.h" +#include "mcu_f4.h" +#include "mcu_f7.h" int16_t cin(unsigned timeout_ms) { @@ -72,28 +75,74 @@ uint32_t flash_func_sector_size(uint32_t sector) void flash_func_erase_sector(uint32_t sector) { - stm32_flash_erasepage(flash_base_page+sector); + stm32_flash_erasepage(flash_base_page+sector); } +// read one-time programmable memory uint32_t flash_func_read_otp(uint32_t idx) { - return 0; + if (idx & 3) { + return 0; + } + + if (idx > OTP_SIZE) { + return 0; + } + + return *(uint32_t *)(idx + OTP_BASE); } +// read chip serial number uint32_t flash_func_read_sn(uint32_t idx) { - return 0; + return *(uint32_t *)(UDID_START + idx); } uint32_t get_mcu_id(void) { - return 0; + return *(uint32_t *)DBGMCU_BASE; } -uint32_t get_mcu_desc(uint32_t len, uint8_t *buf) +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +uint32_t get_mcu_desc(uint32_t max, uint8_t *revstr) { - buf[0] = 'A'; - return 1; + uint32_t idcode = (*(uint32_t *)DBGMCU_BASE); + int32_t mcuid = idcode & DEVID_MASK; + uint16_t revid = ((idcode & REVID_MASK) >> 16); + + mcu_des_t des = mcu_descriptions[STM32_UNKNOWN]; + + for (int i = 0; i < ARRAY_SIZE_SIMPLE(mcu_descriptions); i++) { + if (mcuid == mcu_descriptions[i].mcuid) { + des = mcu_descriptions[i]; + break; + } + } + + for (int i = 0; i < ARRAY_SIZE_SIMPLE(silicon_revs); i++) { + if (silicon_revs[i].revid == revid) { + des.rev = silicon_revs[i].rev; + } + } + + uint8_t *endp = &revstr[max - 1]; + uint8_t *strp = revstr; + + while (strp < endp && *des.desc) { + *strp++ = *des.desc++; + } + + if (strp < endp) { + *strp++ = ','; + } + + if (strp < endp) { + *strp++ = des.rev; + } + + return strp - revstr; } void led_on(unsigned led) @@ -173,7 +222,7 @@ void *memcpy(void *dest, const void *src, size_t n) { uint8_t *tdest = (uint8_t *)dest; uint8_t *tsrc = (uint8_t *)src; - for(int i=0; i