diff options
Diffstat (limited to 'src/drivers/boards/px4cannode-v1')
19 files changed, 3386 insertions, 0 deletions
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c new file mode 100644 index 000000000..6170f2d33 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c @@ -0,0 +1,335 @@ +#include <nuttx/config.h> +#include "app_config.h" + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> +#include <limits.h> + +#include "chip.h" +#include "stm32.h" +#include <chip/stm32_can.h> +#include "nvic.h" + +#include "bl_macros.h" +#include "driver.h" +#include "crc.h" +#include "timer.h" + +#include <arch/board/board.h> + +#define INAK_TIMEOUT 65535 + +#define CAN_1MBAUD_SJW 0 +#define CAN_1MBAUD_BS1 6u +#define CAN_1MBAUD_BS2 0 +#define CAN_1MBAUD_PRESCALER 4u + +#define CAN_500KBAUD_SJW 0 +#define CAN_500KBAUD_BS1 6u +#define CAN_500KBAUD_BS2 0 +#define CAN_500KBAUD_PRESCALER 8u + +#define CAN_250KBAUD_SJW 0 +#define CAN_250KBAUD_BS1 6u +#define CAN_250KBAUD_BS2 0 +#define CAN_250KBAUD_PRESCALER 16u + +#define CAN_125KBAUD_SJW 0 +#define CAN_125KBAUD_BS1 6u +#define CAN_125KBAUD_BS2 0 +#define CAN_125KBAUD_PRESCALER 32u + +#define CAN_BTR_LBK_SHIFT 30 + +#define WAIT_TX_READY_MS ((TIMER_HRT_CYCLES_PER_MS-(TIMER_HRT_CYCLES_PER_MS/4)) + +/* Number of CPU cycles for a single bit time at the supported speeds */ + +#define CAN_1MBAUD_BIT_CYCLES (1*(TIMER_HRT_CYCLES_PER_US)) +#define CAN_500KBAUD_BIT_CYCLES (2*(TIMER_HRT_CYCLES_PER_US)) +#define CAN_250KBAUD_BIT_CYCLES (4*(TIMER_HRT_CYCLES_PER_US)) +#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US)) + +/* All the mesured samples were off byt 28 counts this implys that + * the CAN_MSR_RX is sampled on a differnt clock then the CPU */ + +#define CAN_BAUD_ADJUST 28 + +#define CAN_BAUD_TIME_IN_MS 200 +#define CAN_BAUD_SAMPLES_NEEDED 32 +#define CAN_BAUD_SAMPLES_DISCARDED 8 + +int can_init(can_speed_t speed, uint8_t mode) +{ + int speedndx = speed -1; + /* + * TODO: use full-word writes to reduce the number of loads/stores. + * + * Also consider filter use -- maybe set filters for all the message types we + * want. */ + + + const uint32_t bitrates[] = { + (CAN_125KBAUD_SJW << 24) | + (CAN_125KBAUD_BS1 << 16) | + (CAN_125KBAUD_BS2 << 20) | (CAN_125KBAUD_PRESCALER - 1), + + (CAN_250KBAUD_SJW << 24) | + (CAN_250KBAUD_BS1 << 16) | + (CAN_250KBAUD_BS2 << 20) | (CAN_250KBAUD_PRESCALER - 1), + + (CAN_500KBAUD_SJW << 24) | + (CAN_500KBAUD_BS1 << 16) | + (CAN_500KBAUD_BS2 << 20) | (CAN_500KBAUD_PRESCALER - 1), + + (CAN_1MBAUD_SJW << 24) | + (CAN_1MBAUD_BS1 << 16) | + (CAN_1MBAUD_BS2 << 20) | (CAN_1MBAUD_PRESCALER - 1) + }; + /* Remove Unknow Offset */ + if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) { + return -EINVAL; + } + uint32_t timeout; + /* + * Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP + * knock down Sleep and raise CAN_MCR_INRQ + */ + + putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR); + + /* Wait until initialization mode is acknowledged */ + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) + { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) + { + /* We are in initialization mode */ + + break; + } + } + + if (timeout < 1) + { + /* + * Initialization failed, not much we can do now other than try a normal + * startup. */ + return -ETIME; + } + + + putreg32(bitrates[speedndx]| mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR); + + putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF, STM32_CAN1_MCR); + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) + { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) + { + /* We are in initialization mode */ + + break; + } + } + if (timeout < 1) + { + return -ETIME; + } + + /* + * CAN filter initialization -- accept everything on RX FIFO 0, and only + * GetNodeInfo requests on RX FIFO 1. */ + putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); + putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ + putreg32(3, STM32_CAN1_FS1R); /* Enable 32-bit mode for filters 0 and 1 */ + + /* Filter 0 masks -- data type ID 551 only */ + putreg32(UAVCAN_GETNODEINFO_DTID << 22, STM32_CAN1_FIR(0, 1)); + /* Top 10 bits of ID */ + putreg32(0xFFC00000, STM32_CAN1_FIR(0, 2)); + + /* Filter 1 masks -- everything is don't-care */ + putreg32(0, STM32_CAN1_FIR(1, 1)); + putreg32(0, STM32_CAN1_FIR(1, 2)); + + putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */ + putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the + * rest of filters */ + putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */ + + putreg32(0, STM32_CAN1_FMR); /* Leave init Mode */ + + return OK; +} + +void can_tx(uint32_t message_id, size_t length, + const uint8_t * message, uint8_t mailbox) +{ + uint32_t data[2]; + + memcpy(data, message, sizeof(data)); + + /* + * Just block while waiting for the mailbox. Give it an extra 0.75 ms per + * frame to avoid an issue Ben was seeing with packets going missing on a USBtin. */ + + uint32_t mask = CAN_TSR_TME0 << mailbox; + time_hrt_cycles_t begin = timer_hrt_read(); + + while (((getreg32(STM32_CAN1_TSR) & mask) == 0) || + timer_hrt_elapsed(begin, timer_hrt_read()) < WAIT_TX_READY_MS)); + + putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox)); + putreg32(data[0], STM32_CAN1_TDLR(mailbox)); + putreg32(data[1], STM32_CAN1_TDHR(mailbox)); + putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ, + STM32_CAN1_TIR(mailbox)); +} + + +uint8_t can_rx(uint32_t * out_message_id, size_t * out_length, + uint8_t * out_message, uint8_t fifo) +{ + uint32_t data[2]; + uint8_t rv = 0; + const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R }; + + if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK) + { + + rv = 1; + /* If so, process it */ + + *out_message_id = + getreg32(STM32_CAN1_RIR(fifo) & CAN_RIR_EXID_MASK) >> + CAN_RIR_EXID_SHIFT; + *out_length = + getreg32(STM32_CAN1_RDTR(fifo) & CAN_RDTR_DLC_MASK) >> + CAN_RDTR_DLC_SHIFT; + data[0] = getreg32(STM32_CAN1_RDLR(fifo)); + data[1] = getreg32(STM32_CAN1_RDHR(fifo)); + + putreg32(CAN_RFR_RFOM, fifos[fifo & 1]); + + memcpy(out_message, data, sizeof(data)); + } + + return rv; +} + +static inline uint32_t read_msr_rx(void) +{ + return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX; +} + +static uint32_t read_msr(time_hrt_cycles_t *now) +{ + __asm__ __volatile__ ("\tcpsid i\n"); + *now = timer_hrt_read(); + uint32_t msr = read_msr_rx(); + __asm__ __volatile__ ("\tcpsie i\n"); + return msr; +} + +static int read_bits_times(time_hrt_cycles_t *times, size_t max) +{ + uint32_t samplecnt = 0; + bl_timer_id ab_timer= timer_allocate(modeTimeout|modeStarted, CAN_BAUD_TIME_IN_MS, 0); + time_ref_t ab_ref = timer_ref(ab_timer); + uint32_t msr; + uint32_t last_msr = read_msr(times); + + while(samplecnt < max && !timer_ref_expired(ab_ref)) + { + do { + msr = read_msr(×[samplecnt]); + } while(!(msr ^ last_msr) && !timer_ref_expired(ab_ref)); + last_msr = msr; + samplecnt++; + } + timer_free(ab_timer); + return samplecnt; +} + +int can_autobaud(can_speed_t *can_speed, bl_timer_id tboot) +{ + + *can_speed = CAN_UNKNOWN; + + volatile int attempt = 0; + /* Threshold are at 1.5 Bit times */ + + + /* + * We are here because there was a reset or the app invoked + * the bootloader with no bit rate set. + */ + + time_hrt_cycles_t bit_time; + time_hrt_cycles_t min_cycles; + int sample; + can_speed_t speed = CAN_125KBAUD; + + time_hrt_cycles_t samples[128]; + + while(1) { + + + while(1) { + + min_cycles = ULONG_MAX; + int samplecnt = read_bits_times(samples, arraySize(samples)); + + if (timer_expired(tboot)) { + return CAN_BOOT_TIMEOUT; + } + + if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) & + CAN_RFR_FMP_MASK) { + *can_speed = speed; + return CAN_OK; + } + + if (samplecnt < CAN_BAUD_SAMPLES_NEEDED ) { + continue; + } + + for (sample = 0; sample < samplecnt; sample += 2) { + bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample+1]); + if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) { + min_cycles = bit_time; + } + } + break; + } + + uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES/4; + samples[1] = min_cycles; + speed = CAN_125KBAUD; + while(min_cycles < bit34 && speed < CAN_1MBAUD) { + speed++; + bit34 /= 2; + } + + attempt++; + can_init(speed, CAN_Mode_Silent); + + + } /* while(1) */ + + return CAN_OK; +} + +int can_speed2freq(can_speed_t speed) +{ + return 1000000 >> (CAN_1MBAUD-speed); +} + +can_speed_t can_freq2speed(int freq) +{ + return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); +} + diff --git a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h new file mode 100644 index 000000000..138e14ba3 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h @@ -0,0 +1,47 @@ +#pragma once + +#include "timer.h" + +typedef enum +{ + CAN_UNKNOWN = 0, + CAN_125KBAUD = 1, + CAN_250KBAUD = 2, + CAN_500KBAUD = 3, + CAN_1MBAUD = 4 +} can_speed_t; + +typedef enum +{ + CAN_Mode_Normal = 0, // Bits 30 and 31 00 + CAN_Mode_LoopBack = 1, // Bit 30: Loop Back Mode (Debug) + CAN_Mode_Silent = 2, // Bit 31: Silent Mode (Debug) + CAN_Mode_Silent_LoopBack = 3 // Bits 30 and 31 11 +} can_mode_t; + +typedef enum +{ + fifoAll = 0, + MBAll = 0, + /* + Receive from FIFO 1 -- filters are configured to push the messages there, + and this is called from SysTick so needs to avoid the same FIFOs/mailboxes + as the rest of the application. + */ + + fifoGetNodeInfo = 1, + MBGetNodeInfo = 1, + MBNodeStatus = 1, + + +} can_fifo_mailbox_t; + +void can_tx(uint32_t message_id, size_t length, + const uint8_t * message, uint8_t mailbox); +uint8_t can_rx(uint32_t * out_message_id, size_t * out_length, + uint8_t * out_message, uint8_t fifo); +int can_init(can_speed_t speed, can_mode_t mode); +int can_autobaud(can_speed_t *can_speed, bl_timer_id tboot); + +int can_speed2freq(can_speed_t); +can_speed_t can_freq2speed(int freq); diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/app_config.h b/src/drivers/boards/px4cannode-v1/bootloader/common/app_config.h new file mode 100644 index 000000000..3f40d6b56 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/app_config.h @@ -0,0 +1,36 @@ +#pragma once + +#include "protocol.h" + +#define OPT_TBOOT_MS 2000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 200 +#define OPT_BL_NUMBER_TIMERS 6 + +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_CPU_FREQ_HZ 72000000u + +#define OPT_ENABLE_WD +#define OPT_MIN_APP_PROGRESS 0u +#define OPT_RESTART_TIMEOUT_MS 20000u + +#define OPT_APPLICATION_IMAGE_OFFSET 8192u +#define OPT_APPLICATION_IMAGE_LENGTH 49152u + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_NUMBER_PAGES STM32_FLASH_NPAGES +#define FLASH_PAGE_SIZE STM32_FLASH_PAGESIZE +#define FLASH_SIZE (FLASH_NUMBER_PAGES*FLASH_PAGE_SIZE) + +#define UAVCAN_SERVICE_RETRIES 3u +#define UAVCAN_SERVICE_TIMEOUT_MS 1000 +#define UAVCAN_NODESTATUS_INTERVAL_MS 500 + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/bl_macros.h b/src/drivers/boards/px4cannode-v1/bootloader/common/bl_macros.h new file mode 100644 index 000000000..648dc890a --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/bl_macros.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane <david_s5@nscdg.com> + * + * 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. + * + ****************************************************************************/ + +/** + * @file bl_macros.h + * + * A set of useful macros for enhanced runtime and compile time + * error detection and warning suppression. + * + * Define NO_BLOAT to reduce bloat from file name inclusion. + * + * The arraySize() will compute the size of an array regardless + * it's type + * + * INVALID_CASE(c) should be used is case statements to ferret out + * unintended behavior + * + * UNUSED(var) will suppress compile time warnings of unused + * variables + * + * CCASSERT(predicate) Will generate a compile time error it the + * predicate is false + */ +#include <assert.h> + +#ifndef _BL_MACROS_H +#define _BL_MACROS_H + + +#if !defined(arraySize) +#define arraySize(a) (sizeof((a))/sizeof((a[0]))) +#endif + +#if !defined(NO_BLOAT) +#if defined(__BASE_FILE__) +#define _FILE_NAME_ __BASE_FILE__ +#else +#define _FILE_NAME_ __FILE__ +#endif +#else +#define _FILE_NAME_ "" +#endif + +#if !defined(INVALID_CASE) +#define INVALID_CASE(c) printf("Invalid Case %d, %s:%d",(c),__BASE_FILE__,__LINE__) /* todo use PANIC */ +#endif + +#if !defined(UNUSED) +#define UNUSED(var) (void)(var) +#endif + +#if !defined(CAT) +#if !defined(_CAT) +#define _CAT(a, b) a ## b +#endif +#define CAT(a, b) _CAT(a, b) +#endif + +#if !defined(CCASSERT) +#if defined(static_assert) +# define CCASSERT(predicate) static_assert(predicate) +# else +# define CCASSERT(predicate) _x_CCASSERT_LINE(predicate, __LINE__) +# if !defined(_x_CCASSERT_LINE) +# define _x_CCASSERT_LINE(predicate, line) typedef char CAT(constraint_violated_on_line_,line)[2*((predicate)!=0)-1] __attribute__ ((unused)) ; +# endif +# endif +#endif + + +#endif /* _BL_MACROS_H */ diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c new file mode 100644 index 000000000..b8d649ea6 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c @@ -0,0 +1,113 @@ +#include <nuttx/config.h> + +#include <stdint.h> +#include <string.h> + +#include "chip.h" +#include "stm32.h" + +#include <errno.h> +#include "boot_app_shared.h" +#include "crc.h" + +#define BOOTLOADER_COMMON_APP_SIGNATURE 0xB0A04150u +#define BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE 0xB0A0424Cu + +/* CAN_FiRx where (i=0..27|13, x=1, 2) + * STM32_CAN1_FIR(i,x) + * Using i = 2 does not requier there block + * to be enabled nor FINIT in CAN_FMR to be set. + * todo:Validate this claim on F2, F3 + */ +#define crc_HiLOC STM32_CAN1_FIR(2,1) +#define crc_LoLOC STM32_CAN1_FIR(2,2) +#define signature_LOC STM32_CAN1_FIR(3,1) +#define bus_speed_LOC STM32_CAN1_FIR(3,2) +#define node_id_LOC STM32_CAN1_FIR(4,1) +#define CRC_H 1 +#define CRC_L 0 + +inline static void read(bootloader_app_shared_t * pcommon) +{ + pcommon->signature = getreg32(signature_LOC); + pcommon->bus_speed = getreg32(bus_speed_LOC); + pcommon->node_id = getreg32(node_id_LOC); + pcommon->crc.ul[CRC_L] = getreg32(crc_LoLOC); + pcommon->crc.ul[CRC_H] = getreg32(crc_HiLOC); + +} + +inline static void write(bootloader_app_shared_t * pcommon) +{ + putreg32(pcommon->signature, signature_LOC); + putreg32(pcommon->bus_speed, bus_speed_LOC); + putreg32(pcommon->node_id, node_id_LOC); + putreg32(pcommon->crc.ul[CRC_L], crc_LoLOC); + putreg32(pcommon->crc.ul[CRC_H], crc_HiLOC); + +} + +static uint64_t calulate_signature(bootloader_app_shared_t * pcommon) +{ + size_t size = sizeof(bootloader_app_shared_t) - sizeof(pcommon->crc); + uint64_t crc = CRC64_INITIAL; + uint8_t *protected = (uint8_t *) & pcommon->signature; + + while (size--) + { + crc = crc64_add(crc, *protected++); + } + crc ^= CRC64_OUTPUT_XOR; + return crc; +} + +void bootloader_app_shared_init(bootloader_app_shared_t * pcommon, eRole_t role) +{ + memset(pcommon, 0, sizeof(bootloader_app_shared_t)); + if (role != Invalid) + { + pcommon->signature = + (role == + App ? BOOTLOADER_COMMON_APP_SIGNATURE : + BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); + } + +} + +int bootloader_app_shared_read(bootloader_app_shared_t * pcommon, + eRole_t role) +{ + int rv = -EBADR; + bootloader_app_shared_t working; + + read(&working); + + if ((role == App ? working.signature == BOOTLOADER_COMMON_APP_SIGNATURE + : working.signature == BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE) + && (working.crc.ull == calulate_signature(&working))) + { + *pcommon = working; + rv = OK; + } + return rv; +} + +void bootloader_app_shared_write(bootloader_app_shared_t * common, + eRole_t role) +{ + bootloader_app_shared_t working = *common; + working.signature = + (role == + App ? BOOTLOADER_COMMON_APP_SIGNATURE : + BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); + working.crc.ull = calulate_signature(&working); + write(&working); + +} + +void bootloader_app_shared_invalidate(void) +{ + bootloader_app_shared_t working; + bootloader_app_shared_init(&working, Invalid); + write(&working); +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h new file mode 100644 index 000000000..b13ba1131 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h @@ -0,0 +1,88 @@ + +/* + * + */ +#pragma once + +/* +Application firmware descriptor. + +This is located by the linker script somewhere aftert the vector table. +(within the first several kilobytes of the application); +This struct must be aligned on an 8-byte boundary. + +The bootloader scans through the application FLASH image until it +finds the signature. + +The image_crc is calculated as follows: + 1) All fields of this struct must be initalized with the correct information about + the firmware image bin file (Here after refered to as image) + 2) image_crc set to 0; + 3) The CRC 64 is caculated over the image from offset 0 up to and including the + last byte of the image file. + 4) The caculated CRC 64 is stored in image_crc + 5) The new image file is then written to a file a ".img" extention. +*/ +typedef struct + { + uint64_t signature; + uint64_t image_crc; + uint32_t image_size; + uint32_t vcs_commit; + uint8_t major_version; + uint8_t minor_version; + uint8_t reserved[6]; + } +__attribute__ ((packed)) app_descriptor_t; + +// APDesc00 +#define APP_DESCRIPTOR_SIGNATURE_ID 'A','P','D','e','s','c' +#define APP_DESCRIPTOR_SIGNATURE_REV '0','0' +#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV + +/* +Bootloader/app common structure. + +The data in this struct is passed in SRAM or the the CAN filter registers +from bootloader to application and application to bootloader. + +Do not assume any mapping or location for the passing of this data +that is done in the read and write routines and is abstracted on purpose. + +The application must write BOOTLOADER_COMMON_APP_SIGNATURE to the signature +field when passing data to the bootloader; when the bootloader passes data +to the app, it must write BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE to the +signature field. + +The CRC is calculated over the structure from signature to the last byte. +The resuluting values are then copied to the CAN filter registers by +bootloader_app_shared_read and bootloader_app_shared_write. + +*/ +typedef struct + { + union + { + uint64_t ull; + uint32_t ul[2]; + } crc; + uint32_t signature; + uint32_t bus_speed; + uint32_t node_id; + } __attribute__ ((packed)) bootloader_app_shared_t; + +typedef enum eRole + { + Invalid, + App, + BootLoader + } eRole_t; + + +void bootloader_app_shared_init(bootloader_app_shared_t * common, + eRole_t role); +int bootloader_app_shared_read(bootloader_app_shared_t * common, + eRole_t role); +void bootloader_app_shared_write(bootloader_app_shared_t * common, + eRole_t role); +void bootloader_app_shared_invalidate(void); diff --git a/src/drivers/boards/px4cannode-v1/bootloader/module.mk b/src/drivers/boards/px4cannode-v1/bootloader/module.mk new file mode 100644 index 000000000..f826bbb31 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/module.mk @@ -0,0 +1,14 @@ +# +# Board-specific Bootloader code for the PX4CANNODE +# + +SRCS = src/boot.c \ + src/timer.c \ + src/flash.c \ + src/crc.c \ + can/driver.c \ + uavcan/protocol.c \ + uavcan/main.c \ + common/boot_app_shared.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h b/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h new file mode 100644 index 000000000..3d2a802eb --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h @@ -0,0 +1,283 @@ +/************************************************************************************ + * configs/olimexino-stm32/src/olimexino-stm32.h + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * David_s5 <david_s5@nscdg.com> + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H +#define __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> +#include <stdint.h> + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +#if STM32_NSPI < 1 +# undef CONFIG_STM32_SPI1 +# undef CONFIG_STM32_SPI2 +#elif STM32_NSPI < 2 +# undef CONFIG_STM32_SPI2 +#endif + +/* LEDs ***************************************************************************** + * + * GPIO Function MPU Board + * Pin # Name + * -- ----- -------------------------------- ---------------------------- + * + * PA[05] PA5/SPI1_SCK/ADC5 21 D13(SCK1/LED1) + * PA[01] PA1/USART2_RTS/ADC1/TIM2_CH2 15 D3(LED2) + */ + +#define GPIO_LED1 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTA | GPIO_PIN5 | GPIO_OUTPUT_CLEAR) +#define GPIO_LED_GREEN GPIO_LED1 +#define GPIO_LED2 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTA | GPIO_PIN1 | GPIO_OUTPUT_CLEAR) +#define GPIO_LED_YELLOW GPIO_LED2 + +/* BUTTON *************************************************************************** + * + * GPIO Function MPU Board + * Pin # Name + * -- ----- -------------------------------- ---------------------------- + * + * PC[09] PC9/TIM3_CH4 40 BOOT0 + * + */ + +#define BUTTON_BOOT0n (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN9 | \ + GPIO_EXTI) +#define IRQBUTTON BUTTON_BOOT0_BIT + +/* USBs ***************************************************************************** + * + * GPIO Function MPU Board + * Pin # Name + * -- ----- -------------------------------- ---------------------------- + * + * PC[11] PC11/USART3_RX 52 USB_P + * PC[12] PC12/USART3_CK 53 DISC + * + */ + +#define GPIO_USB_VBUS (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN11) +#define GPIO_USB_PULLUPn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTC | GPIO_PIN12 | GPIO_OUTPUT_SET) + +/* SPI *************************************************************************** + * + * GPIO Function MPU Board + * Pin # Name + * -- ----- -------------------------------- ---------------------------- + * + * PC[09] PA4/SPI1_NSS/USART2_CK/ADC4 20 D10(#SS1) + * PD[02] PD2/TIM3_ETR 54 D25(MMC_CS) + */ + +#define GPIO_SPI1_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTC | GPIO_PIN9 | GPIO_OUTPUT_SET) +#define USER_CSn GPIO_SPI1_SSn + +#define GPIO_SPI2_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTD | GPIO_PIN2 | GPIO_OUTPUT_SET) +#define MMCSD_CSn GPIO_SPI2_SSn + +/* CAN *************************************************************************** + * + * GPIO Function MPU Board + * Pin # Name + * -- ----- -------------------------------- ---------------------------- + * + * PB[08] PB8/TIM4_CH3/I2C1_SCL/CANRX 61 D14(CANRX) + * PB[09] PB9/TIM4_CH4/I2C1_SDA/CANTX 62 D24(CANTX) + * PC[13] PC13/ANTI_TAMP 2 D21(CAN_CTRL) + */ + +#define GPIO_CAN_CTRL (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \ + GPIO_PORTC | GPIO_PIN13 | GPIO_OUTPUT_CLEAR) + +#define GPIO_GETNODEINFO_JUMPER (BUTTON_BOOT0n&~(GPIO_EXTI)) + + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins. + * + ************************************************************************************/ + +#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \ + defined(CONFIG_STM32_SPI3) +void weak_function stm32_spiinitialize(void); +#endif + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins. + * + ************************************************************************************/ + +void stm32_usbinitialize(void); + +/************************************************************************************ + * Name: stm32_usb_set_pwr_callback() + * + * Description: + * Called to setup set a call back for USB power state changes. + * + * Inputs: + * pwr_changed_handler: An interrupt handler that will be called on VBUS power + * state changes. + * + ************************************************************************************/ + +void stm32_usb_set_pwr_callback(xcpt_t pwr_changed_handler); + +/**************************************************************************** + * Name: stm32_led_initialize + * + * Description: + * This functions is called very early in initialization to perform board- + * specific initialization of LED-related resources. This includes such + * things as, for example, configure GPIO pins to drive the LEDs and also + * putting the LEDs in their correct initial state. + * + * NOTE: In most architectures, LED initialization() is called from + * board-specific initialization and should, therefore, have the name + * <arch>_led_intialize(). But there are a few architectures where the + * LED initialization function is still called from common chip + * architecture logic. This interface is not, however, a common board + * interface in any event and the name board_led_initialization is + * deprecated. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_ARCH_LEDS +void stm32_led_initialize(void); +#endif + +/************************************************************************************ + * Name: stm32_can_initialize + * + * Description: + * Called at application startup time to initialize the CAN functionality. + * + ************************************************************************************/ + +#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2)) +int stm32_can_initialize(void); +#endif + +/**************************************************************************** + * Name: usbmsc_archinitialize + * + * Description: + * Called from the application system/usbmc or the boards_nsh if the + * application is not included. + * Perform architecture specific initialization. This function must + * configure the block device to export via USB. This function must be + * provided by architecture-specific logic in order to use this add-on. + * + ****************************************************************************/ + +#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_USBMSC) +int usbmsc_archinitialize(void); +#endif + +/**************************************************************************** + * Name: composite_archinitialize + * + * Description: + * Called from the application system/composite or the boards_nsh if the + * application is not included. + * Perform architecture specific initialization. This function must + * configure the block device to export via USB. This function must be + * provided by architecture-specific logic in order to use this add-on. + * + ****************************************************************************/ + +#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_COMPOSITE) +extern int composite_archinitialize(void); +#endif + +uint8_t board_get_product_name(uint8_t * product_name); +void board_get_hardware_version(uavcan_hardwareversion_t * hw_version); +void board_initialize(void); +void board_indicate_reset(void); +void board_indicate_autobaud_start(void); +void board_indicate_autobaud_end(void); +void board_indicate_allocation_start(void); +void board_indicate_allocation_end(void); +void board_indicate_fw_update_start(void); +void board_indicate_fw_update_erase_fail(void); +void board_indicate_fw_update_invalid_response(void); +void board_indicate_fw_update_timeout(void); +void board_indicate_fw_update_invalid_crc(void); +void board_indicate_jump_to_app(void); +void stm32_boarddeinitialize(void); +uint8_t board_get_wait_for_getnodeinfo_flag(void); + + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H */ diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c new file mode 100644 index 000000000..efa8f83eb --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c @@ -0,0 +1,199 @@ +/************************************************************************************ + * configs/olimexino-stm32/src/bl_boot.c + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * David Sidrane <david_s5@nscdg.com> + * + * 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 NuttX 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 <nuttx/config.h> +#include "app_config.h" + +#include <debug.h> +#include <arch/board/board.h> + +#include <nuttx/board.h> + + +#include "board_config.h" + + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN_CTRL); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +void stm32_boarddeinitialize(void) +{ + + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); +} + +#include <nuttx/config.h> +#include "app_config.h" + +#include <stdint.h> + +#include "stm32.h" + + + +uint8_t board_get_product_name(uint8_t * product_name) +{ + product_name[0] = 'h'; + product_name[1] = 'i'; + product_name[2] = '!'; + return 3u; +} + +void board_get_hardware_version(uavcan_hardwareversion_t * hw_version) +{ + uint32_t i; + volatile uint8_t *stm32f_uid = (volatile uint8_t *)STM32_UNIQUE_DEVICE_ID; + + hw_version->major = 1u; + hw_version->minor = 0u; + + for (i = 0u; i < 12u; i++) + { + hw_version->unique_id[i] = stm32f_uid[i]; + } + for (; i < 16u; i++) + { + hw_version->unique_id[i] = 0u; + } + + for (i = 0u; i < 255u; i++) + { + hw_version->certificate_of_authenticity[i] = 0; + } + + hw_version->certificate_of_authenticity_length = 0u; +} + +void board_indicate_reset(void) +{ + +} + +void board_indicate_autobaud_start(void) +{ + +} + +void board_indicate_autobaud_end(void) +{ + +} + +void board_indicate_allocation_start(void) +{ + +} + +void board_indicate_allocation_end(void) +{ + +} + +void board_indicate_fw_update_start(void) +{ + +} + +void board_indicate_fw_update_erase_fail(void) +{ + +} + +void board_indicate_fw_update_invalid_response(void) +{ + +} + +void board_indicate_fw_update_timeout(void) +{ + +} + +void board_indicate_fw_update_invalid_crc(void) +{ + +} + +void board_indicate_jump_to_app(void) +{ + +} + +uint8_t board_get_wait_for_getnodeinfo_flag(void) +{ + return 1u; +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c new file mode 100644 index 000000000..cee1dae3f --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c @@ -0,0 +1,68 @@ +#include <stdint.h> +#include <stdlib.h> +#include "crc.h" + +/* +CRC-16-CCITT +Initial value: 0xFFFF +Poly: 0x1021 +Reverse: no +Output xor: 0 +*/ +uint16_t crc16_add(uint16_t crc, uint8_t value) +{ + uint32_t i; + const uint16_t poly = 0x1021u; + crc ^= (uint16_t) ((uint16_t) value << 8u); + for (i = 0; i < 8; i++) + { + if (crc & (1u << 15u)) + { + crc = (uint16_t) ((crc << 1u) ^ poly); + } + else + { + crc = (uint16_t) (crc << 1u); + } + } + return crc; +} + +uint16_t crc16_signature(uint16_t initial, size_t length, + const uint8_t *bytes) +{ + size_t i; + for (i = 0u; i < length; i++) { + initial = crc16_add(initial, bytes[i]); + } + return initial ^ CRC16_OUTPUT_XOR; +} + + +/* +CRC-64-WE +Description: http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 +Initial value: 0xFFFFFFFFFFFFFFFF +Poly: 0x42F0E1EBA9EA3693 +Reverse: no +Output xor: 0xFFFFFFFFFFFFFFFF +Check: 0x62EC59E3F1A4F00A +*/ +uint64_t crc64_add(uint64_t crc, uint8_t value) +{ + uint32_t i; + const uint64_t poly = 0x42F0E1EBA9EA3693ull; + crc ^= (uint64_t) value << 56u; + for (i = 0; i < 8; i++) + { + if (crc & (1ull << 63u)) + { + crc = (uint64_t) (crc << 1u) ^ poly; + } + else + { + crc = (uint64_t) (crc << 1u); + } + } + return crc; +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h new file mode 100644 index 000000000..8eaf39c04 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h @@ -0,0 +1,11 @@ +#pragma once + +#define CRC16_INITIAL 0xFFFFu +#define CRC16_OUTPUT_XOR 0x0000u +uint16_t crc16_add(uint16_t crc, uint8_t value); +uint16_t crc16_signature(uint16_t initial, size_t length, + const uint8_t *bytes); + +#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull +#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull +uint64_t crc64_add(uint64_t crc, uint8_t value); diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c new file mode 100644 index 000000000..cdf177ac6 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c @@ -0,0 +1,68 @@ +#include <nuttx/config.h> +#include "app_config.h" + +#include <stdint.h> +#include <stdlib.h> + +#include <nuttx/progmem.h> + +#include "chip.h" +#include "stm32.h" + +#include "flash.h" + + +flash_error_t bl_flash_erase(void) +{ + /* + * FIXME (?): this may take a long time, and while flash is being erased it + * might not be possible to execute interrupts, send NodeStatus messages etc. + * We can pass a per page callback or yeild */ + + flash_error_t status = FLASH_ERROR_AFU; + + ssize_t bllastpage = up_progmem_getpage(APPLICATION_LOAD_ADDRESS - 1); + + if (bllastpage >= 0) + { + + status = FLASH_ERROR_SUICIDE; + ssize_t appfirstpage = up_progmem_getpage(APPLICATION_LOAD_ADDRESS); + + if (appfirstpage > bllastpage) + { + + size_t pagecnt = up_progmem_npages() - (bllastpage + 1); + + /* Erase the whole application flash region */ + status = FLASH_OK; + + while (status == FLASH_OK && pagecnt--) + { + + ssize_t ps = up_progmem_erasepage(appfirstpage); + if (ps <= 0) + { + status = FLASH_ERASE_ERROR; + } + } + } + } + return status; +} + +flash_error_t bl_flash_write_word(uint32_t flash_address, const uint8_t data[4]) +{ + + flash_error_t status = FLASH_ERROR; + if (flash_address >= APPLICATION_LOAD_ADDRESS && + (flash_address + sizeof(data)) <= (uint32_t) APPLICATION_LAST_32BIT_ADDRRESS) + { + if (sizeof(data) == + up_progmem_write((size_t) flash_address, (void *)data, sizeof(data))) + { + status = FLASH_OK; + } + } + return status; +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h new file mode 100644 index 000000000..2be47848b --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h @@ -0,0 +1,16 @@ +#pragma once + +typedef enum + { + FLASH_OK = 0, + FLASH_ERROR, + FLASH_ERASE_ERROR, + FLASH_ERASE_VERIFY_ERROR, + FLASH_ERROR_SUICIDE, + FLASH_ERROR_AFU, + + } flash_error_t; + +flash_error_t bl_flash_erase(void); +flash_error_t bl_flash_write_word(uint32_t flash_address, const uint8_t * word); +uint64_t flash_crc(uint32_t flash_address, size_t length, uint64_t initial_crc); diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/ostubs.c b/src/drivers/boards/px4cannode-v1/bootloader/src/ostubs.c new file mode 100644 index 000000000..0ce747101 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/ostubs.c @@ -0,0 +1,121 @@ + +/************************************************************************************ + * configs/olimexino-stm32/src/stm32_nss.c + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * David Sidrane <david_s5@nscdg.com> + * + * 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 NuttX 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 <nuttx/config.h> + +#include <nuttx/init.h> +#include <nuttx/arch.h> + +#include <stdbool.h> +#include <stdio.h> +#include <syslog.h> +#include <errno.h> + +#include <nuttx/board.h> + +#include "up_internal.h" + +#include "timer.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +FAR void *malloc(size_t size); +void exit(int status); +void sched_ufree(FAR void *address); +int __wrap_up_svcall(int irq, FAR void *context); +void os_start(void); + + +__EXPORT void os_start(void) +{ + + timer_init(); + + /* Initialize the interrupt subsystem */ + + up_irqinitialize(); + + +#if !defined(CONFIG_SUPPRESS_INTERRUPTS) && !defined(CONFIG_SUPPRESS_TIMER_INTS) && \ + !defined(CONFIG_SYSTEMTICK_EXTCLK) + up_timer_initialize(); +#endif + + while (1) + { + main(0, 0); + } +} + +FAR void *malloc(size_t size) +{ + return NULL; +} + +void exit(int status) +{ + while (1); +} + +void sched_ufree(FAR void *address) +{ + +} + +int __wrap_up_svcall(int irq, FAR void *context) +{ + return 0; +} +volatile dq_queue_t g_readytorun; diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c new file mode 100644 index 000000000..ebb8fecbc --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c @@ -0,0 +1,183 @@ + +#include <nuttx/config.h> + +#include "app_config.h" + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> + +#include <nuttx/arch.h> +#include <arch/irq.h> + +#include "bl_macros.h" +#include "timer.h" +#include "nvic.h" + +#include <arch/board/board.h> + + + +typedef enum { + OneShot = modeOneShot, + Repeating = modeRepeating, + Timeout = modeTimeout, + + modeMsk = 0x3 , + running = modeStarted, + inuse = 0x80, + +} bl_timer_ctl_t; + +typedef struct { + bl_timer_cb_t usr; + time_ms_t count; + time_ms_t reload; + bl_timer_ctl_t ctl; +} bl_timer_t; + +bl_timer_t timers[OPT_BL_NUMBER_TIMERS]; + +bl_timer_cb_t null_cb = + { + 0, + 0 + }; + + +static time_ms_t sys_tic; + +time_ms_t timer_tic(void) +{ + return sys_tic; +} + +void sched_process_timer(void) +{ + PROBE(1,true); + PROBE(1,false); + + sys_tic++; + time_ms_t ms_elapsed = (CONFIG_USEC_PER_TICK/1000); + bl_timer_id t; + for( t = arraySize(timers)-1; (int8_t) t >= 0; t--) { + if ((timers[t].ctl & (inuse|running)) == (inuse|running)) { + if (timers[t].count != 0 && timers[t].count > ms_elapsed) { + timers[t].count -= ms_elapsed; + } else { + timers[t].count = 0; + + switch(timers[t].ctl & ~(inuse|running)) { + + case Timeout: + break; + case OneShot: { + bl_timer_cb_t user = timers[t].usr; + memset(&timers[t], 0, sizeof(timers[t])); + if (user.cb) { + user.cb(t, user.context); + } + } + break; + case Repeating: + timers[t].count = timers[t].reload; + if (timers[t].usr.cb) { + timers[t].usr.cb(t, timers[t].usr.context); + } + break; + default: + break; + } + } + } + } +} + +bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, bl_timer_cb_t *fc) +{ + bl_timer_id t; + irqstate_t s = irqsave(); + + for(t = arraySize(timers)-1; (int8_t)t >= 0; t--) { + + if ((timers[t].ctl & inuse) == 0 ) { + + timers[t].reload = msfromnow; + timers[t].count = msfromnow; + timers[t].usr = fc ? *fc : null_cb; + timers[t].ctl = (mode & (modeMsk|running)) | (inuse); + break; + } + } + + irqrestore(s); + return t; +} + + +void timer_free(bl_timer_id id) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers)); + irqstate_t s = irqsave(); + memset(&timers[id], 0, sizeof(timers[id])); + irqrestore(s); +} + +void timer_start(bl_timer_id id) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].count = timers[id].reload; + timers[id].ctl |= running; + irqrestore(s); + +} +void timer_stop(bl_timer_id id) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].ctl &= ~running; + irqrestore(s); + +} + +int timer_expired(bl_timer_id id) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + int rv = ((timers[id].ctl & running) && timers[id].count == 0); + irqrestore(s); + return rv; +} + +time_ref_t timer_ref(bl_timer_id id) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + return (time_ref_t) &timers[id].count; +} + +void timer_restart(bl_timer_id id, time_ms_t ms) +{ + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].count = timers[id].reload = ms; + timers[id].ctl |= running; + irqrestore(s); +} + +__attribute__ ((visibility ("default"))) +void timer_init(void) +{ +/* PROBE_INIT(7); + PROBE(1,true); + PROBE(2,true); + PROBE(3,true); + PROBE(1,false); + PROBE(2,false); + PROBE(3,false); +// *((uint32_t *)0x40011010) = 0x100; // PROBE(3,true); +// *((uint32_t *)0x40011014) = 0x100; // PROBE(3,false); +*/ + memset(timers,0,sizeof(timers)); +} + diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h new file mode 100644 index 000000000..530a60859 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h @@ -0,0 +1,81 @@ +#pragma once + + +#include "stm32.h" +#include "nvic.h" + +typedef enum { + //! Specifies a one-shot timer. After notification timer is discarded. + modeOneShot = 1, + //! Specifies a repeating timer. + modeRepeating = 2, + //! Specifies a persisten start / stop timer. + modeTimeout = 3, + + modeStarted = 0x40 + + +} bl_timer_modes_t; + +typedef uint8_t bl_timer_id; +typedef uint32_t time_ms_t; +typedef volatile time_ms_t* time_ref_t; + +typedef uint32_t time_hrt_cycles_t; + +typedef void (*bl_timer_ontimeout)(bl_timer_id id, void *context); + +typedef struct { + void *context; + bl_timer_ontimeout cb; + +} bl_timer_cb_t; + +extern bl_timer_cb_t null_cb; + + +void timer_init(void); +bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, bl_timer_cb_t *fc); +void timer_free(bl_timer_id id); +void timer_start(bl_timer_id id); +void timer_restart(bl_timer_id id, time_ms_t ms); +void timer_stop(bl_timer_id id); +int timer_expired(bl_timer_id id); +time_ms_t timer_tic(void); + +time_ref_t timer_ref(bl_timer_id id); +static inline int timer_ref_expired(time_ref_t ref) +{ + return *ref == 0; +} + + +#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000) +#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000) + +static inline time_hrt_cycles_t timer_hrt_read(void) +{ + return getreg32(NVIC_SYSTICK_CURRENT); +} + +static inline time_hrt_cycles_t timer_hrt_max(void) +{ + return getreg32(NVIC_SYSTICK_RELOAD) + 1; +} + +static inline time_hrt_cycles_t timer_hrt_elapsed(time_hrt_cycles_t begin, time_hrt_cycles_t end) +{ + /* It is a down count from NVIC_SYSTICK_RELOAD */ + + time_hrt_cycles_t elapsed = begin - end; + time_hrt_cycles_t reload = timer_hrt_max(); + + /* Did it wrap */ + if (elapsed > reload) { + elapsed += reload; + } + + return elapsed; +} + + diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c new file mode 100644 index 000000000..a990b389c --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c @@ -0,0 +1,934 @@ +#include <nuttx/config.h> +#include "app_config.h" + +#include <nuttx/arch.h> + +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> + +#include "chip.h" +#include "stm32.h" +#include "nvic.h" + +#include "flash.h" +#include "timer.h" +#include "driver.h" +#include "protocol.h" +#include "crc.h" +#include "boot_app_shared.h" + +#include "board_config.h" + + +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); + +volatile struct { + can_speed_t bus_speed; + volatile uint8_t node_id; + volatile uint8_t status_code; + volatile bool app_valid; + volatile uint32_t uptime; + volatile app_descriptor_t *fw_image_descriptor; + volatile uint32_t *fw_image; + bool wait_for_getnodeinfo; + bool app_bl_request; + bool sent_node_info_response; + +} bootloader; + + +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 + */ + +static void node_info_process(bl_timer_id id, void *context) +{ + + uavcan_getnodeinfo_response_t response; + uavcan_nodestatus_t node_status; + uavcan_frame_id_t frame_id; + size_t frame_len; + uint32_t rx_message_id; + uint8_t frame_payload[8]; + + node_status.uptime_sec = bootloader.uptime; + node_status.status_code = bootloader.status_code; + node_status.vendor_specific_status_code = 0u; + uavcan_pack_nodestatus(response.nodestatus, &node_status); + + board_get_hardware_version(&response.hardware_version); + response.name_length = board_get_product_name(response.name); + memset(&response.software_version,0, sizeof(response.software_version)); + + if (bootloader.app_valid) { + response.software_version.major = + bootloader.fw_image_descriptor->major_version; + response.software_version.minor = + bootloader.fw_image_descriptor->minor_version; + response.software_version.optional_field_mask = 3u; + bootloader.fw_image_descriptor->minor_version; + response.software_version.vcs_commit = + bootloader.fw_image_descriptor->vcs_commit; + response.software_version.image_crc = + bootloader.fw_image_descriptor->image_crc; + } + + rx_message_id = 0u; + if (can_rx(&rx_message_id, &frame_len, frame_payload, fifoGetNodeInfo) && + uavcan_parse_message_id(&frame_id, rx_message_id, UAVCAN_GETNODEINFO_DTID) && + frame_payload[0] == bootloader.node_id && + frame_id.last_frame) { + uavcan_tx_getnodeinfo_response(bootloader.node_id, &response, + frame_id.source_node_id, + frame_id.transfer_id); + + bootloader.sent_node_info_response = true; + } + + +} + +/* + * Once we have a noide id + * Handle GetNodeInfo replies regardless of application state + */ +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[]) +{ + + 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 + + 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(); + 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++); + + if (i < rx_len) { + /* Abort if we didn't match the whole unique ID */ + server.node_id = 0u; + } else if (rx_frame_id.last_frame && unique_id_matched < 16u) { + /* Case D */ + uavcan_tx_allocation_message(*allocated_node_id, 16u, hw_version.unique_id, + unique_id_matched, transfer_id++); + } else if (rx_frame_id.last_frame) { + /* Validate CRC */ + expected_crc = UAVCAN_ALLOCATION_CRC; + expected_crc = crc16_add(expected_crc, server.allocated_node_id); + expected_crc = crc16_signature(expected_crc, 16u, + hw_version.unique_id); + + /* Case E */ + if (rx_crc == expected_crc) { + *allocated_node_id = server.allocated_node_id >> 1u; + break; + } + } + } + } while (!timer_expired(tboot)); + + timer_free(trequest); + 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) +{ + uavcan_beginfirmwareupdate_request_t request; + uavcan_frame_id_t frame_id; + size_t i; + uint8_t frame_payload[8]; + can_error_t status; + + status = CAN_ERROR; + fw_path[0] = 0; + *fw_source_node_id = 0; + + while (status != CAN_OK) + { + if (timer_expired(tboot)) { + return CAN_BOOT_TIMEOUT; + } + status = uavcan_rx_beginfirmwareupdate_request(bootloader.node_id, + &request, &frame_id); + } + + if (status == CAN_OK) + { + /* UAVCANBootloader_v0.3 #22.2: Resp580.BeginFirmwareUpdate.uavcan */ + + /* Send an ERROR_OK response */ + frame_payload[0] = frame_id.source_node_id; + frame_payload[1] = 0u; + + frame_id.last_frame = 1u; + frame_id.frame_index = 0u; + frame_id.source_node_id = bootloader.node_id; + frame_id.transfer_type = SERVICE_RESPONSE; + + can_tx(uavcan_make_message_id(&frame_id), 2u, frame_payload, MBAll); + + /* UAVCANBootloader_v0.3 #22.3: fwPath = image_file_remote_path */ + for (i = 0u; i < request.path_length; i++) + { + fw_path[i] = request.path[i]; + } + *fw_path_length = request.path_length; + /* UAVCANBootloader_v0.3 #22.4: fwSourceNodeID = source_node_id */ + *fw_source_node_id = request.source_node_id; + } + 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) +{ + uavcan_getinfo_request_t request; + uavcan_getinfo_response_t response; + uint8_t transfer_id, retries, i; + can_error_t status; + + for (i = 0; i < fw_path_length; i++) + { + request.path[i] = fw_path[i]; + } + request.path_length = fw_path_length; + + /* UAVCANBootloader_v0.3 #25: SetRetryAndTimeout(3,1000MS) */ + retries = UAVCAN_SERVICE_RETRIES; + transfer_id = 0; + + *fw_image_size = 0; + *fw_image_crc = 0; + + while (retries) + { + /* UAVCANBootloader_v0.3 #26: 585.GetInfo.uavcan(path) */ + uavcan_tx_getinfo_request(bootloader.node_id, &request, + fw_source_node_id, transfer_id); + + /* UAVCANBootloader_v0.3 #28: + * 585.GetInfo.uavcan(fw_path,fw_crc,fw_size...), */ + status = + uavcan_rx_getinfo_response(bootloader.node_id, &response, + fw_source_node_id, transfer_id, + UAVCAN_SERVICE_TIMEOUT_MS); + + transfer_id++; + + /* UAVCANBootloader_v0.3 #27: validateFileInfo(file_info, &errorcode) */ + if (status == CAN_OK && response.error == UAVCAN_FILE_ERROR_OK && + (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE) && + (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE) && + response.size > 0u && response.size < OPT_APPLICATION_IMAGE_LENGTH) + { + /* UAVCANBootloader_v0.3 #28.4: save(file_info) */ + *fw_image_size = response.size; + *fw_image_crc = response.crc64; + break; + } + else + { + retries--; + } + } +} + +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) +{ + uavcan_read_request_t request; + uavcan_read_response_t response; + size_t bytes_written, i, write_length; + 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; + + /* Set up the read request */ + for (i = 0; i < fw_path_length; i++) + { + request.path[i] = fw_path[i]; + } + request.path_length = fw_path_length; + request.offset = 0u; + + bytes_written = 0u; + write_remainder_length = 0u; + transfer_id = 0u; + next_read_deadline = 0u; + + do + { + /* + * Rate limiting on read requests: - 2/sec on a 125 Kbaud bus - 4/sec on + * a 250 Kbaud bus - 8/sec on a 500 Kbaud bus - 16/sec on a 1 Mbaud bus */ + while (bootloader.uptime < next_read_deadline); + next_read_deadline = bootloader.uptime + 6u; + + /* UAVCANBootloader_v0.3 #28.11: SetRetryAndTimeout(3,1000MS) */ + retries = UAVCAN_SERVICE_RETRIES; + can_status = CAN_ERROR; + while (retries && can_status != CAN_OK) + { + /* UAVCANBootloader_v0.3 #28.12: 588.Read.uavcan(0, path) */ + /* UAVCANBootloader_v0.3 #33: 588.Read.uavcan(N,path) */ + request.offset = bytes_written + write_remainder_length; + uavcan_tx_read_request(bootloader.node_id, &request, + fw_source_node_id, transfer_id); + + /* UAVCANBootloader_v0.3 #28.12.1: 588.Read.uavcan(0,path,data) */ + /* UAVCANBootloader_v0.3 #33.1: 583.Read.uavcan(0,path,data) */ + can_status = uavcan_rx_read_response(bootloader.node_id, + &response, fw_source_node_id, + transfer_id, + UAVCAN_SERVICE_TIMEOUT_MS); + + transfer_id++; + + /* UAVCANBootloader_v0.3 #34: ValidateReadResp(resp) */ + if (can_status != CAN_OK || response.error != UAVCAN_FILE_ERROR_OK) + { + can_status = CAN_ERROR; + + /* UAVCANBootloader_v0.3 #35: [(retries != 0 && timeout) || + * !ValidReadReadResponse]:INDICATE_FW_UPDATE_INVALID_RESPONSE */ + board_indicate_fw_update_invalid_response(); + + /* UAVCANBootloader_v0.3 #36: [(retries != 0 && timeout) || + * !ValidReadReadResponse]:1023.LogMessage.uavcan */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_ERROR, + UAVCAN_LOGMESSAGE_STAGE_PROGRAM, + UAVCAN_LOGMESSAGE_RESULT_FAIL); + retries--; + } + } + + if (can_status != CAN_OK) + { + /* UAVCANBootloader_v0.3 #37: [(retries == 0 && + * timeout]:INDICATE_FW_UPDATE_TIMEOUT */ + board_indicate_fw_update_timeout(); + break; + } + + data = response.data; + write_length = response.data_length; + + /* + * If this is the last read (zero length) and there are bytes left to + * write, pad the firmware image out with zeros to ensure a full-word + * write. */ + if (write_length == 0u && write_remainder_length > 0u) + { + write_length = 4u - write_remainder_length; + data[0] = data[1] = data[2] = 0u; + } + + /* + * If the length of a previous read was not a multiple of 4, we'll have a + * few bytes left over which need to be combined with the next write as + * all writes must be word-aligned and a whole number of words long. */ + flash_status = FLASH_OK; + while (write_length && flash_status == FLASH_OK) + { + write_remainder[write_remainder_length++] = *(data++); + write_length--; + + if (write_remainder_length == 4u) + { + if (bytes_written == 0u) + { + /* UAVCANBootloader_v0.3 #30: SaveWord0 */ + ((uint8_t *) fw_word0)[0] = write_remainder[0]; + ((uint8_t *) fw_word0)[1] = write_remainder[1]; + ((uint8_t *) fw_word0)[2] = write_remainder[2]; + ((uint8_t *) fw_word0)[3] = write_remainder[3]; + } + else + { + flash_status = + bl_flash_write_word((uint32_t) + (&bootloader.fw_image[bytes_written >> 2u]), + write_remainder); + } + + bytes_written += 4u; + write_remainder_length = 0u; + } + } + } + while (bytes_written <= fw_image_size && response.data_length != 0u && + flash_status == FLASH_OK); + + /* + * Return success iff 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); +} + +static void do_jump(uint32_t stacktop, uint32_t entrypoint) +{ + asm volatile ("msr msp, %0 \n" + "bx %1 \n"::"r" (stacktop), "r"(entrypoint):); + // just to keep noreturn happy + for (;;); +} + +void application_run(size_t fw_image_size) +{ + /* + * 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. + + * The second word of the app is the entrypoint; it must point within the + * flash area (or we have a bad flash). + */ + if (bootloader.fw_image[0] != 0xffffffff + && bootloader.fw_image[1] > APPLICATION_LOAD_ADDRESS + && bootloader.fw_image[1] < (APPLICATION_LOAD_ADDRESS + fw_image_size)) + { + + (void)irqsave(); + + stm32_boarddeinitialize(); + + /* kill the systick interrupt */ + up_disable_irq(STM32_IRQ_SYSTICK); + putreg32((NVIC_SYSTICK_CTRL_CLKSOURCE | NVIC_SYSTICK_CTRL_TICKINT), + NVIC_SYSTICK_CTRL); + + /* and set a specific LED pattern */ + board_indicate_jump_to_app(); + + /* the interface */ + + /* switch exception handlers to the application */ + __asm volatile ("dsb"); + __asm volatile ("isb"); + putreg32(APPLICATION_LOAD_ADDRESS, NVIC_VECTAB); + __asm volatile ("dsb"); + /* extract the stack and entrypoint from the app vector table and go */ + do_jump(bootloader.fw_image[0], bootloader.fw_image[1]); + } +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c new file mode 100644 index 000000000..fdd078113 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c @@ -0,0 +1,458 @@ +#include <nuttx/config.h> +#include "app_config.h" + +#include <stdint.h> +#include <stdlib.h> + +#include "chip.h" +#include "stm32.h" + +#include "timer.h" +#include "protocol.h" +#include "driver.h" +#include "crc.h" + + +#define CAN_REQUEST_TIMEOUT 1000 + +static void uavcan_tx_multiframe_( + uavcan_frame_id_t *frame_id, + uint8_t dest_node_id, + size_t message_length, + const uint8_t *message, + uint16_t initial_crc, + uint8_t mailbox +); +static can_error_t uavcan_rx_multiframe_( + uint8_t node_id, + uavcan_frame_id_t *frame_id, + size_t *message_length, + uint8_t *message, + uint16_t initial_crc, + uint32_t timeout_cycles +); + + +size_t uavcan_pack_nodestatus( + uint8_t *data, + const uavcan_nodestatus_t *payload +) { + /* No need to clear the top 4 bits of uptime_sec */ + data[0] = ((uint8_t*)&payload->uptime_sec)[0]; + data[1] = ((uint8_t*)&payload->uptime_sec)[1]; + data[2] = ((uint8_t*)&payload->uptime_sec)[2]; + data[3] = ((uint8_t*)&payload->uptime_sec)[3] | payload->status_code; + data[4] = ((uint8_t*)&payload->vendor_specific_status_code)[0]; + data[5] = ((uint8_t*)&payload->vendor_specific_status_code)[1]; + return 6u; +} + + +size_t uavcan_pack_logmessage( + uint8_t *data, + const uavcan_logmessage_t *payload +) { + data[0] = (uint8_t)(((payload->level & 0x7u) << 5u) | 4u); + data[1] = UAVCAN_LOGMESSAGE_SOURCE_0; + data[2] = UAVCAN_LOGMESSAGE_SOURCE_1; + data[3] = UAVCAN_LOGMESSAGE_SOURCE_2; + data[4] = UAVCAN_LOGMESSAGE_SOURCE_3; + data[5] = payload->message[0]; + data[6] = payload->message[1]; + return 7u; +} + + +uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id) { + return (frame_id->transfer_id & 0x7u) | + ((frame_id->last_frame ? 1u : 0u) << 3u) | + ((frame_id->frame_index & 0x3Fu) << 4u) | + ((frame_id->source_node_id & 0x7Fu) << 10u) | + ((frame_id->transfer_type & 0x3u) << 17u) | + ((frame_id->data_type_id & 0x3FFu) << 19u); +} + + +int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, + uint16_t expected_id) +{ + frame_id->transfer_id = message_id & 0x7u; + frame_id->last_frame = (message_id & 0x8u) ? 1u : 0u; + frame_id->frame_index = (message_id >> 4u) & 0x3Fu; + frame_id->source_node_id = (message_id >> 10u) & 0x7Fu; + frame_id->transfer_type = (message_id >> 17u) & 0x3u; + frame_id->data_type_id = (message_id >> 19u) & 0x3FFu; + return expected_id == frame_id->data_type_id; +} + + +void uavcan_tx_nodestatus( + uint8_t node_id, + uint32_t uptime_sec, + uint8_t status_code +) { + uavcan_nodestatus_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_NODESTATUS_DTID; + + message.uptime_sec = uptime_sec; + message.status_code = status_code; + message.vendor_specific_status_code = 0u; + frame_len = uavcan_pack_nodestatus(payload, &message); + + can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBNodeStatus); +} + + +void uavcan_tx_allocation_message( + uint8_t requested_node_id, + size_t unique_id_length, + const uint8_t *unique_id, + uint8_t unique_id_offset, + uint8_t transfer_id +) { + uint8_t payload[8]; + uavcan_frame_id_t frame_id; + size_t i, max_offset, checksum; + + max_offset = unique_id_offset + 7u; + if (max_offset > unique_id_length) { + max_offset = unique_id_length; + } + + payload[0] = (uint8_t)((requested_node_id << 1u) | + (unique_id_offset ? 0u : 1u)); + for (checksum = 0u, i = 0u; i < max_offset - unique_id_offset; i++) { + payload[i + 1u] = unique_id[unique_id_offset + i]; + checksum += unique_id[unique_id_offset + i]; + } + + frame_id.transfer_id = transfer_id; + frame_id.last_frame = 1u; + frame_id.frame_index = (uint8_t)checksum; + frame_id.source_node_id = 0u; + frame_id.transfer_type = MESSAGE_BROADCAST; + frame_id.data_type_id = UAVCAN_DYNAMICNODEIDALLOCATION_DTID; + + can_tx(uavcan_make_message_id(&frame_id), i + 1u, + payload, MBAll); +} + + +void uavcan_tx_getnodeinfo_response( + uint8_t node_id, + uavcan_getnodeinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id +) { + /* + This sends via mailbox 1 because it's called from SysTick. It may also + clobber response->name because it moves the name to the end of the COA. + */ + uint32_t i; + uavcan_frame_id_t frame_id; + size_t fixed_length, contiguous_length, packet_length; + + fixed_length = 6u + sizeof(uavcan_softwareversion_t) + 2u + 16u + 1u; + contiguous_length = fixed_length + + response->hardware_version.certificate_of_authenticity_length; + packet_length = contiguous_length + response->name_length; + + /* Move name so it's contiguous with the start of the packet */ + for (i = 0u; i < response->name_length; i++) { + ((uint8_t*)response)[contiguous_length + i] = response->name[i]; + } + + /* Set up the message ID */ + frame_id.transfer_id = transfer_id; + frame_id.source_node_id = node_id; + frame_id.transfer_type = SERVICE_RESPONSE; + frame_id.data_type_id = UAVCAN_GETNODEINFO_DTID; + + uavcan_tx_multiframe_(&frame_id, dest_node_id, packet_length, + (const uint8_t*)response, UAVCAN_GETNODEINFO_CRC, + 1u); +} + +can_error_t uavcan_rx_beginfirmwareupdate_request( + uint8_t node_id, + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *frame_id +) { + size_t length; + can_error_t status; + + length = sizeof(uavcan_beginfirmwareupdate_request_t) - 1; + frame_id->transfer_id = 0xFFu; + frame_id->source_node_id = 0xFFu; + frame_id->transfer_type = SERVICE_REQUEST; + frame_id->data_type_id = UAVCAN_BEGINFIRMWAREUPDATE_DTID; + + status = uavcan_rx_multiframe_(node_id, frame_id, &length, + (uint8_t*)request, + UAVCAN_BEGINFIRMWAREUPDATE_CRC, CAN_REQUEST_TIMEOUT); + + if (status == CAN_OK && length >= 1u) { + request->path_length = (uint8_t)(length - 1u); + return CAN_OK; + } else { + return CAN_ERROR; + } +} + + +void uavcan_tx_read_request( + uint8_t node_id, + const uavcan_read_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id +) { + uavcan_frame_id_t frame_id; + + /* Set up the message ID */ + frame_id.transfer_id = transfer_id; + frame_id.source_node_id = node_id; + frame_id.transfer_type = SERVICE_REQUEST; + frame_id.data_type_id = UAVCAN_READ_DTID; + + uavcan_tx_multiframe_(&frame_id, dest_node_id, request->path_length + 4u, + (const uint8_t*)request, UAVCAN_READ_CRC, MBAll); +} + + +can_error_t uavcan_rx_read_response( + uint8_t node_id, + uavcan_read_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ticks +) { + uavcan_frame_id_t frame_id; + size_t length; + can_error_t status; + + length = sizeof(uavcan_read_response_t) - 1; + frame_id.transfer_id = transfer_id; + frame_id.source_node_id = dest_node_id; + frame_id.transfer_type = SERVICE_RESPONSE; + frame_id.data_type_id = UAVCAN_READ_DTID; + + status = uavcan_rx_multiframe_(node_id, &frame_id, &length, + (uint8_t*)response, UAVCAN_READ_CRC, + timeout_ticks); + + if (status == CAN_OK && length >= 2u) { + response->data_length = (uint8_t)(length - 2u); + return CAN_OK; + } else { + return CAN_ERROR; + } +} + + +void uavcan_tx_getinfo_request( + uint8_t node_id, + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id +) { + uavcan_frame_id_t frame_id; + + /* Set up the message ID */ + frame_id.transfer_id = transfer_id; + frame_id.source_node_id = node_id; + frame_id.transfer_type = SERVICE_REQUEST; + frame_id.data_type_id = UAVCAN_GETINFO_DTID; + + uavcan_tx_multiframe_(&frame_id, dest_node_id, request->path_length, + (const uint8_t*)request, UAVCAN_GETINFO_CRC, MBAll); +} + + +can_error_t uavcan_rx_getinfo_response( + uint8_t node_id, + uavcan_getinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ticks +) { + uavcan_frame_id_t frame_id; + size_t length; + can_error_t status; + + length = sizeof(uavcan_getinfo_response_t); + frame_id.transfer_id = transfer_id; + frame_id.source_node_id = dest_node_id; + frame_id.transfer_type = SERVICE_RESPONSE; + frame_id.data_type_id = UAVCAN_GETINFO_DTID; + + status = uavcan_rx_multiframe_(node_id, &frame_id, &length, + (uint8_t*)response, UAVCAN_GETINFO_CRC, + timeout_ticks); + + if (status == CAN_OK && length == sizeof(uavcan_getinfo_response_t)) { + return CAN_OK; + } else { + return CAN_ERROR; + } +} + + +static void uavcan_tx_multiframe_( + uavcan_frame_id_t *frame_id, + uint8_t dest_node_id, + size_t message_length, + const uint8_t *message, + uint16_t initial_crc, + uint8_t mailbox +) { + uint32_t i, m; + uint16_t frame_crc; + uint8_t payload[8]; + + /* Calculate the message CRC */ + frame_crc = crc16_signature(initial_crc, message_length, message); + + /* Ensure message ID has the frame details zeroed */ + frame_id->last_frame = 0u; + frame_id->frame_index = 0u; + + /* + Send the message -- only prepend CRC if the message will not fit within a + single frame + */ + payload[0] = dest_node_id; + m = 1u; + if (message_length > 7u) { + payload[m++] = (uint8_t)frame_crc; + payload[m++] = (uint8_t)(frame_crc >> 8u); + } + for (i = 0u; i < message_length; i++) { + payload[m++] = message[i]; + if (i == message_length - 1u) { + break; + } else if (m == 8u) { + can_tx(uavcan_make_message_id(frame_id), 8u, payload, mailbox); + frame_id->frame_index++; + payload[0] = dest_node_id; + m = 1u; + } + } + + /* Send the last (only?) frame */ + frame_id->last_frame = 1u; + can_tx(uavcan_make_message_id(frame_id), m, payload, mailbox); +} + + +static can_error_t uavcan_rx_multiframe_(uint8_t node_id, + uavcan_frame_id_t *frame_id, + size_t *message_length, + uint8_t *message, + uint16_t initial_crc, + uint32_t timeout_ticks) +{ + uavcan_frame_id_t rx_id; + size_t rx_length; + size_t m; + size_t i; + uint32_t rx_message_id; + uint16_t calculated_crc; + uint16_t message_crc; + uint8_t payload[8]; + uint8_t got_frame; + uint8_t num_frames; + + bl_timer_id timer = timer_allocate(modeTimeout|modeStarted, timeout_ticks, 0); + + num_frames = 0u; + rx_id.last_frame = 0u; + message_crc = 0u; + i = 0; + + do { + rx_message_id = 0u; + rx_length = 0u; + got_frame = can_rx(&rx_message_id, &rx_length, payload, MBAll); + if (!got_frame) { + continue; + } + + uavcan_parse_message_id(&rx_id, rx_message_id, frame_id->transfer_type); + + /* + Skip this frame if the source node ID is wrong, or if the data type or + transfer type do not match what we're expecting + */ + if ((frame_id->source_node_id != 0xFFu && + rx_id.source_node_id != frame_id->source_node_id) || + rx_id.transfer_type != frame_id->transfer_type || + rx_id.data_type_id != frame_id->data_type_id || + payload[0] != node_id) { + continue; + } + + /* + Get the CRC if this is the first frame of a multi-frame transfer. + + Also increase the timeout to UAVCAN_SERVICE_TIMEOUT_TICKS. + */ + if (rx_id.frame_index == 0u) { + num_frames = 0u; + frame_id->transfer_id = rx_id.transfer_id; + if (frame_id->source_node_id == 0xFFu) { + frame_id->source_node_id = rx_id.source_node_id; + } + } + + /* Skip if the transfer ID is wrong */ + if ((frame_id->transfer_id ^ rx_id.transfer_id) & 0x7u) { + continue; + } + + /* + Get the CRC and increase the service timeout if this is the first + frame + */ + if (num_frames == 0u && !rx_id.last_frame) { + timer_restart(timer, UAVCAN_SERVICE_TIMEOUT_MS); + message_crc = (uint16_t)(payload[1] | (payload[2] << 8u)); + m = 3u; + } else { + m = 1u; + } + + /* Copy message bytes to the response */ + for (; m < rx_length && i < *message_length; m++, i++) { + message[i] = payload[m]; + } + num_frames++; + + if (rx_id.last_frame) { + break; + } + } while (!timer_expired(timer)); + timer_free(timer); + if (!rx_id.last_frame) { + return CAN_ERROR; + } else { + /* Validate CRC */ + calculated_crc = crc16_signature(initial_crc, i, message); + + *message_length = i; + + if (num_frames == 1u || message_crc == calculated_crc) { + return CAN_OK; + } else { + return CAN_ERROR; + } + } +} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h new file mode 100644 index 000000000..933a6d6e7 --- /dev/null +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h @@ -0,0 +1,229 @@ +#pragma once + +#include <stdint.h> +#include <stdlib.h> + +typedef enum + { + CAN_OK = 0, + CAN_BOOT_TIMEOUT, + CAN_ERROR + } can_error_t; + + /* UAVCAN message formats */ + typedef enum { + SERVICE_RESPONSE = 0, + SERVICE_REQUEST = 1, + MESSAGE_BROADCAST = 2, + MESSAGE_UNICAST = 3 + } uavcan_transfertype_t; + + typedef struct { + uint8_t transfer_id; + uint8_t last_frame; + uint8_t frame_index; + uint8_t source_node_id; + uavcan_transfertype_t transfer_type; + uint16_t data_type_id; + } __attribute__((packed)) uavcan_frame_id_t; + + typedef struct { + uint32_t uptime_sec; + uint8_t status_code; + uint16_t vendor_specific_status_code; + } __attribute__((packed)) uavcan_nodestatus_t; + + #define UAVCAN_NODESTATUS_DTID 550u + #define UAVCAN_NODESTATUS_PUBLICATION_CYCLES (72000000u / 2u) + #define UAVCAN_NODESTATUS_STATUS_OK 0u + #define UAVCAN_NODESTATUS_STATUS_INITIALIZING 1u + #define UAVCAN_NODESTATUS_STATUS_WARNING 2u + #define UAVCAN_NODESTATUS_STATUS_CRITICAL 3u + + + typedef struct { + uint8_t major; + uint8_t minor; + uint8_t optional_field_mask; + uint32_t vcs_commit; + uint64_t image_crc; + } __attribute__((packed)) uavcan_softwareversion_t; + + typedef struct { + uint8_t major; + uint8_t minor; + uint8_t unique_id[16]; + uint8_t certificate_of_authenticity_length; + uint8_t certificate_of_authenticity[255]; + } __attribute__((packed)) uavcan_hardwareversion_t; + + typedef struct { + uint8_t nodestatus[6]; + + uavcan_softwareversion_t software_version; + uavcan_hardwareversion_t hardware_version; + + uint8_t name[80]; + uint8_t name_length; + } __attribute__((packed)) uavcan_getnodeinfo_response_t; + + #define UAVCAN_GETNODEINFO_DTID 551u + #define UAVCAN_GETNODEINFO_CRC 0x14BBu + + + typedef struct { + uint8_t node_id; /* bottom bit is the first part flag */ + uint8_t unique_id[16]; + } __attribute__((packed)) uavcan_allocation_t; + + #define UAVCAN_DYNAMICNODEIDALLOCATION_DTID 559u + + + typedef struct { + uint8_t level; + uint8_t message[2]; + } __attribute__((packed)) uavcan_logmessage_t; + + #define UAVCAN_LOGMESSAGE_DTID 1023u + #define UAVCAN_LOGMESSAGE_LEVEL_DEBUG 0u + #define UAVCAN_LOGMESSAGE_LEVEL_INFO 1u + #define UAVCAN_LOGMESSAGE_LEVEL_WARNING 2u + #define UAVCAN_LOGMESSAGE_LEVEL_ERROR 3u + #define UAVCAN_LOGMESSAGE_SOURCE_0 'B' + #define UAVCAN_LOGMESSAGE_SOURCE_1 'O' + #define UAVCAN_LOGMESSAGE_SOURCE_2 'O' + #define UAVCAN_LOGMESSAGE_SOURCE_3 'T' + + #define UAVCAN_LOGMESSAGE_STAGE_INIT 'I' + #define UAVCAN_LOGMESSAGE_STAGE_GET_INFO 'G' + #define UAVCAN_LOGMESSAGE_STAGE_ERASE 'E' + #define UAVCAN_LOGMESSAGE_STAGE_READ 'R' + #define UAVCAN_LOGMESSAGE_STAGE_PROGRAM 'P' + #define UAVCAN_LOGMESSAGE_STAGE_VALIDATE 'V' + #define UAVCAN_LOGMESSAGE_STAGE_FINALIZE 'F' + #define UAVCAN_LOGMESSAGE_RESULT_START 's' + #define UAVCAN_LOGMESSAGE_RESULT_FAIL 'f' + #define UAVCAN_LOGMESSAGE_RESULT_OK 'o' + + + typedef struct { + uint8_t source_node_id; + uint8_t path[200]; + uint8_t path_length; + } __attribute__((packed)) uavcan_beginfirmwareupdate_request_t; + + typedef struct { + uint8_t error; + } __attribute__((packed)) uavcan_beginfirmwareupdate_response_t; + + #define UAVCAN_BEGINFIRMWAREUPDATE_DTID 580u + #define UAVCAN_BEGINFIRMWAREUPDATE_CRC 0x729Eu + #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_OK 0u + #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_INVALID_MODE 1u + #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_IN_PROGRESS 2u + #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_UNKNOWN 255u + + + typedef struct { + uint8_t path[200]; + uint8_t path_length; + } __attribute__((packed)) uavcan_getinfo_request_t; + + typedef struct { + uint64_t crc64; + uint32_t size; + uint16_t error; + uint8_t entry_type; + } __attribute__((packed)) uavcan_getinfo_response_t; + + #define UAVCAN_GETINFO_DTID 585u + #define UAVCAN_GETINFO_CRC 0x37A0u + #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE 0x01u + #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_DIRECTORY 0x02u + #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_SYMLINK 0x04u + #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE 0x08u + #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_WRITEABLE 0x10u + + + typedef struct { + uint32_t offset; + uint8_t path[200]; + uint8_t path_length; + } __attribute__((packed)) uavcan_read_request_t; + + typedef struct { + uint16_t error; + uint8_t data[250]; + uint8_t data_length; + } __attribute__((packed)) uavcan_read_response_t; + + #define UAVCAN_READ_DTID 588u + #define UAVCAN_READ_CRC 0x2921u + + #define UAVCAN_FILE_ERROR_OK 0u + /* Left the others out for now because we don't really care why it failed */ + + + #define UAVCAN_ALLOCATION_CRC 0x7BAAu + + + size_t uavcan_pack_nodestatus( + uint8_t *data, + const uavcan_nodestatus_t *payload + ); + size_t uavcan_pack_logmessage( + uint8_t *data, + const uavcan_logmessage_t *payload + ); + uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id); + int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, + uint16_t expected_id); + void uavcan_tx_nodestatus( + uint8_t node_id, + uint32_t uptime_sec, + uint8_t status_code + ); + void uavcan_tx_allocation_message( + uint8_t requested_node_id, + size_t unique_id_length, + const uint8_t *unique_id, + uint8_t unique_id_offset, + uint8_t transfer_id + ); + void uavcan_tx_getnodeinfo_response( + uint8_t node_id, + uavcan_getnodeinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id + ); + can_error_t uavcan_rx_beginfirmwareupdate_request( + uint8_t node_id, + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *out_frame_id + ); + void uavcan_tx_read_request( + uint8_t node_id, + const uavcan_read_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id + ); + can_error_t uavcan_rx_read_response( + uint8_t node_id, + uavcan_read_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ticks + ); + void uavcan_tx_getinfo_request( + uint8_t node_id, + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id + ); + can_error_t uavcan_rx_getinfo_response( + uint8_t node_id, + uavcan_getinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ticks + ); |