diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 6951f1ba93..782834ac65 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -12,6 +12,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL &hal; @@ -95,9 +97,14 @@ enum ioevents { #define PAGE_REG_SETUP_PWM_RATE_MASK 2 #define PAGE_REG_SETUP_DEFAULTRATE 3 #define PAGE_REG_SETUP_ALTRATE 4 +#define PAGE_REG_SETUP_REBOOT_BL 10 +#define PAGE_REG_SETUP_CRC 11 #define PAGE_REG_SETUP_SBUS_RATE 19 #define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21 +// magic value for rebooting to bootloader +#define REBOOT_BL_MAGIC 14662 + #define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12 #define PAGE_REG_SETUP_FORCE_SAFETY_ON 14 #define FORCE_SAFETY_MAGIC 22027 @@ -111,6 +118,18 @@ AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) : */ void AP_IOMCU::init(void) { + //upload_fw(fw_name); + + // uart runs at 1.5MBit + uart.begin(1500*1000, 256, 256); + uart.set_blocking_writes(false); + uart.set_unbuffered_writes(true); + + // check IO firmware CRC + hal.scheduler->delay(2000); + + check_crc(); + thread_ctx = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(1024), "IOMCU", @@ -146,12 +165,11 @@ void AP_IOMCU::event_failed(uint8_t event) void AP_IOMCU::thread_main(void) { thread_ctx = chThdGetSelfX(); - - // uart runs at 1.5MBit + uart.begin(1500*1000, 256, 256); uart.set_blocking_writes(false); uart.set_unbuffered_writes(true); - + trigger_event(IOEVENT_INIT); while (true) { @@ -411,7 +429,7 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons // wait for the expected number of reply bytes or timeout if (!uart.wait_timeout(4, 10)) { - hal.console->printf("no reply for %u/%u/%u\n", page, offset, count); + //hal.console->printf("no reply for %u/%u/%u\n", page, offset, count); return false; } @@ -613,4 +631,42 @@ void AP_IOMCU::update_safety_options(void) } } +/* + check ROMFS firmware against CRC on IOMCU, and if incorrect then upload new firmware + */ +bool AP_IOMCU::check_crc(void) +{ + // flash size minus 4k bootloader + const uint32_t flash_size = 0x10000 - 0x1000; + uint32_t fw_size; + + fw = AP_ROMFS::find_file(fw_name, fw_size); + if (!fw) { + hal.console->printf("failed to find %s\n", fw_name); + return false; + } + uint32_t crc = crc_crc32(0, fw, fw_size); + + // pad to max size + while (fw_size < flash_size) { + uint8_t b = 0xff; + crc = crc_crc32(crc, &b, 1); + fw_size++; + } + + uint32_t io_crc = 0; + if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc) && + io_crc == crc) { + hal.console->printf("IOMCU: CRC ok\n"); + return true; + } + + const uint16_t magic = REBOOT_BL_MAGIC; + write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic); + hal.scheduler->delay(100); + upload_fw(fw_name); + + return false; +} + #endif // HAL_WITH_IO_MCU diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index f85e2cc786..ff3a782964 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -194,6 +194,58 @@ private: uint32_t last_servo_out_us; bool corked; + + // firmware upload + const char *fw_name = "io_firmware.bin"; + const uint8_t *fw; + + bool upload_fw(const char *filename); + bool recv_byte_with_timeout(uint8_t *c, uint32_t timeout_ms); + bool recv_bytes(uint8_t *p, uint32_t count); + void drain(void); + bool send(uint8_t c); + bool send(const uint8_t *p, uint32_t count); + bool get_sync(uint32_t timeout = 40); + bool sync(); + bool get_info(uint8_t param, uint32_t &val); + bool erase(); + bool program(uint32_t fw_size); + bool verify_rev2(uint32_t fw_size); + bool verify_rev3(uint32_t fw_size_local); + bool reboot(); + + bool check_crc(void); + + enum { + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_INVALID = 0x13, + PROTO_BAD_SILICON_REV = 0x14, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_GET_CRC = 0x29, + PROTO_GET_OTP = 0x2a, + PROTO_GET_SN = 0x2b, + PROTO_GET_CHIP = 0x2c, + PROTO_SET_DELAY = 0x2d, + PROTO_GET_CHIP_DES = 0x2e, + PROTO_REBOOT = 0x30, + + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 5, /**< supported bootloader protocol */ + INFO_BOARD_ID = 2, /**< board type */ + INFO_BOARD_REV = 3, /**< board revision */ + INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ + + PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */ + }; }; #endif // HAL_WITH_IO_MCU diff --git a/libraries/AP_IOMCU/fw_uploader.cpp b/libraries/AP_IOMCU/fw_uploader.cpp new file mode 100644 index 0000000000..b1dcc8a951 --- /dev/null +++ b/libraries/AP_IOMCU/fw_uploader.cpp @@ -0,0 +1,454 @@ +/* + 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 . + */ +/* + uploader for IOMCU partly based on px4io_uploader.cpp from px4 + */ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * 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. + * + ****************************************************************************/ + +#include + +#if HAL_WITH_IO_MCU + +#include "AP_IOMCU.h" +#include +#include + +#define debug(fmt, args ...) do { hal.console->printf("IOMCU: " fmt "\n", ## args); } while(0) + +extern const AP_HAL::HAL &hal; + +/* + upload a firmware to the IOMCU + */ +bool AP_IOMCU::upload_fw(const char *filename) +{ + uint32_t fw_size; + fw = AP_ROMFS::find_file(filename, fw_size); + + // set baudrate for bootloader + uart.begin(115200, 256, 256); + + bool ret = false; + + /* look for the bootloader for 150 ms */ + for (uint8_t i = 0; i < 15; i++) { + ret = sync(); + if (ret) { + break; + } + hal.scheduler->delay(10); + } + + if (!ret) { + debug("IO update failed sync"); + return false; + } + + uint32_t bl_rev; + ret = get_info(INFO_BL_REV, bl_rev); + + if (!ret) { + debug("Err: failed to contact bootloader"); + return false; + } + if (bl_rev > BL_REV) { + debug("Err: unsupported bootloader revision %u", unsigned(bl_rev)); + return false; + } + debug("found bootloader revision: %u", unsigned(bl_rev)); + + ret = erase(); + if (!ret) { + debug("erase failed"); + return false; + } + + ret = program(fw_size); + if (!ret) { + debug("program failed"); + return false; + } + + if (bl_rev <= 2) { + ret = verify_rev2(fw_size); + } else { + ret = verify_rev3(fw_size); + } + + if (!ret) { + debug("verify failed"); + return false; + } + + ret = reboot(); + + if (!ret) { + debug("reboot failed"); + return false; + } + + debug("update complete"); + + // sleep for enough time for the IO chip to boot + hal.scheduler->delay(100); + + return true; +} + +/* + receive a byte from the IO bootloader + */ +bool AP_IOMCU::recv_byte_with_timeout(uint8_t *c, uint32_t timeout_ms) +{ + uint32_t start = AP_HAL::millis(); + do { + int16_t v = uart.read(); + if (v >= 0) { + *c = uint8_t(v); + return true; + } + hal.scheduler->delay_microseconds(50); + } while (AP_HAL::millis() - start < timeout_ms); + + return false; +} + +/* + receive multiple bytes from the bootloader + */ +bool AP_IOMCU::recv_bytes(uint8_t *p, uint32_t count) +{ + bool ret = true; + + while (count--) { + ret = recv_byte_with_timeout(p++, 5000); + if (!ret) { + break; + } + } + + return ret; +} + +/* + discard any pending bytes + */ +void AP_IOMCU::drain(void) +{ + uint8_t c; + bool ret; + + do { + ret = recv_byte_with_timeout(&c, 40); + } while (ret); +} + +/* + send a byte to the bootloader + */ +bool AP_IOMCU::send(uint8_t c) +{ + if (uart.write(c) != 1) { + return false; + } + return true; +} + +/* + send a buffer to the bootloader + */ +bool AP_IOMCU::send(const uint8_t *p, uint32_t count) +{ + bool ret = true; + + while (count--) { + ret = send(*p++); + if (!ret) { + break; + } + } + + return ret; +} + +/* + wait for bootloader protocol sync + */ +bool AP_IOMCU::get_sync(uint32_t timeout_ms) +{ + uint8_t c[2]; + bool ret; + + ret = recv_byte_with_timeout(c, timeout_ms); + if (!ret) { + return false; + } + + ret = recv_byte_with_timeout(c + 1, timeout_ms); + if (!ret) { + return ret; + } + + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { + debug("bad sync 0x%02x,0x%02x", c[0], c[1]); + return false; + } + + return true; +} + +/* + drain then get sync with bootloader + */ +bool AP_IOMCU::sync() +{ + drain(); + + /* complete any pending program operation */ + for (uint32_t i = 0; i < (PROG_MULTI_MAX + 6); i++) { + send(0); + } + + send(PROTO_GET_SYNC); + send(PROTO_EOC); + return get_sync(); +} + +/* + get bootloader version + */ +bool AP_IOMCU::get_info(uint8_t param, uint32_t &val) +{ + bool ret; + + send(PROTO_GET_DEVICE); + send(param); + send(PROTO_EOC); + + ret = recv_bytes((uint8_t *)&val, sizeof(val)); + if (!ret) { + return ret; + } + + return get_sync(); +} + +/* + erase IO firmware + */ +bool AP_IOMCU::erase() +{ + debug("erase..."); + send(PROTO_CHIP_ERASE); + send(PROTO_EOC); + return get_sync(10000); +} + +/* + send new firmware to bootloader + */ +bool AP_IOMCU::program(uint32_t fw_size) +{ + bool ret = false; + uint32_t sent = 0; + + if (fw_size & 3) { + return false; + } + + debug("programming %u bytes...", (unsigned)fw_size); + + while (sent < fw_size) { + /* get more bytes to program */ + uint32_t n = fw_size - sent; + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; + } + + send(PROTO_PROG_MULTI); + send(n); + send(&fw[sent], n); + send(PROTO_EOC); + + ret = get_sync(1000); + if (!ret) { + debug("Failed at %u", (unsigned)sent); + return false; + } + + sent += n; + } + debug("upload OK"); + return true; +} + +/* + verify firmware for a rev2 bootloader + */ +bool AP_IOMCU::verify_rev2(uint32_t fw_size) +{ + ssize_t count; + bool ret; + size_t sent = 0; + + debug("verify..."); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + if (!ret) { + return ret; + } + + while (sent < fw_size) { + /* get more bytes to verify */ + uint32_t n = fw_size - sent; + if (n > 4) { + n = 4; + } + + send(PROTO_READ_MULTI); + send(n); + send(PROTO_EOC); + + + for (uint8_t i = 0; i 4) { + n = 4; + } + + /* calculate crc32 sum */ + sum = crc_crc32(sum, &fw[bytes_read], n); + + bytes_read += n; + } + + /* fill the rest with 0xff */ + while (bytes_read < fw_size_remote) { + sum = crc_crc32(sum, &fill_blank, 1); + bytes_read++; + } + + /* request CRC from IO */ + send(PROTO_GET_CRC); + send(PROTO_EOC); + + ret = recv_bytes((uint8_t *)(&crc), sizeof(crc)); + if (!ret) { + debug("did not receive CRC checksum"); + return ret; + } + + /* compare the CRC sum from the IO with the one calculated */ + if (sum != crc) { + debug("CRC wrong: received: 0x%x, expected: 0x%x", (unsigned)crc, (unsigned)sum); + return false; + } + return true; +} + +/* + reboot IO MCU + */ +bool AP_IOMCU::reboot() +{ + send(PROTO_REBOOT); + hal.scheduler->delay(200); + send(PROTO_EOC); + hal.scheduler->delay(200); + return true; +} + +#endif // HAL_WITH_IO_MCU