diff --git a/platforms/CMakeLists.txt b/platforms/CMakeLists.txt index cca36ff6c781..a2a39344fc75 100644 --- a/platforms/CMakeLists.txt +++ b/platforms/CMakeLists.txt @@ -32,5 +32,3 @@ ############################################################################ add_subdirectory(common) - - diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index d7f453833353..5391b0a75632 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -33,7 +33,7 @@ set(SRCS) -if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2") +if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") list(APPEND SRCS px4_log.cpp ) diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 9230453a2dfd..6f26fabc4bdf 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -60,6 +60,17 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") bootloader_lib drivers_board ) +elseif("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + set(SCRIPT_PREFIX ${PX4_BOARD_LABEL}_) + add_subdirectory(src/canbootloader) + list(APPEND nuttx_libs + canbootloader + drivers_board + ) + + target_link_libraries(px4 PRIVATE + -Wl,-wrap,sched_process_timer -Wl,-wrap,sem_post -Wl,-wrap,sem_wait + ) else() if(NOT "${PX4_BOARD_LINKER_PREFIX}" STREQUAL "") set(SCRIPT_PREFIX ${PX4_BOARD_LINKER_PREFIX}-) diff --git a/platforms/nuttx/src/canbootloader/CMakeLists.txt b/platforms/nuttx/src/canbootloader/CMakeLists.txt new file mode 100644 index 000000000000..4752d5021423 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(canbootloader + arch/stm32/board_identity.c + arch/stm32/drivers/can/driver.c + common/boot_app_shared.c + fs/flash.c + protocol/uavcan.c + sched/timer.c + uavcan/main.c + util/crc.c + util/random.c +) + +include_directories(include) +target_include_directories(canbootloader INTERFACE include) +target_compile_options(canbootloader PRIVATE -Wno-error) diff --git a/platforms/nuttx/src/canbootloader/arch/stm32/board_identity.c b/platforms/nuttx/src/canbootloader/arch/stm32/board_identity.c new file mode 100644 index 000000000000..a976b3a76adc --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm32/board_identity.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_identity.c + * Implementation of STM32 based Board identity API + */ + +#include +#include +#include + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +int board_get_mfguid(mfguid_t mfgid) +{ + uint32_t *chip_uuid = (uint32_t *) STM32_SYSMEM_UID; + uint32_t *rv = (uint32_t *) &mfgid[0]; + + for (int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + *rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + } + + return PX4_CPU_MFGUID_BYTE_LENGTH; +} diff --git a/platforms/nuttx/src/canbootloader/arch/stm32/drivers/can/driver.c b/platforms/nuttx/src/canbootloader/arch/stm32/drivers/can/driver.c new file mode 100644 index 000000000000..55d7bcf77e54 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm32/drivers/can/driver.c @@ -0,0 +1,557 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "boot_config.h" + +#include +#include +#include +#include +#include + +#include "chip.h" +#include "stm32.h" +#include +#include "nvic.h" + +#include "board.h" +#include "px4_macros.h" +#include "can.h" +#include "crc.h" +#include "timer.h" + +#include + + +#define INAK_TIMEOUT 65535 + +#define CAN_TX_TIMEOUT_MS (200 /(1000/(1000000/CONFIG_USEC_PER_TICK))) + +#define SJW_POS 24 +#define BS1_POS 16 +#define BS2_POS 20 + +#define CAN_TSR_RQCP_SHFTS 8 + +#define FILTER_ID 1 +#define FILTER_MASK 2 + +#if STM32_PCLK1_FREQUENCY == 45000000 || STM32_PCLK1_FREQUENCY == 36000000 +/* Sample 88.9 % */ +# define QUANTA 9 +# define BS1_VALUE 6 +# define BS2_VALUE 0 +#elif STM32_PCLK1_FREQUENCY == 42000000 +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#else +# warning Undefined QUANTA bsed on Clock Rate +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#endif + +#define CAN_1MBAUD_SJW 0 +#define CAN_1MBAUD_BS1 BS1_VALUE +#define CAN_1MBAUD_BS2 BS2_VALUE +#define CAN_1MBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/1000000/QUANTA) + +#define CAN_500KBAUD_SJW 0 +#define CAN_500KBAUD_BS1 BS1_VALUE +#define CAN_500KBAUD_BS2 BS2_VALUE +#define CAN_500KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/500000/QUANTA) + +#define CAN_250KBAUD_SJW 0 +#define CAN_250KBAUD_BS1 BS1_VALUE +#define CAN_250KBAUD_BS2 BS2_VALUE +#define CAN_250KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/250000/QUANTA) + +#define CAN_125KBAUD_SJW 0 +#define CAN_125KBAUD_BS1 BS1_VALUE +#define CAN_125KBAUD_BS2 BS2_VALUE +#define CAN_125KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/125000/QUANTA) + +#define CAN_BTR_LBK_SHIFT 30 + +// Number of CPU cycles for a single bit time at the supported speeds +#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US)) + +#define CAN_BAUD_TIME_IN_MS 200 +#define CAN_BAUD_SAMPLES_NEEDED 32 +#define CAN_BAUD_SAMPLES_DISCARDED 8 + +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; +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a can_speed_t to a bit rate in Hz + * + * Input Parameters: + * can_speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + * Returned value: + * Bit rate in Hz + * + ****************************************************************************/ +int can_speed2freq(can_speed_t speed) + +{ + return 1000000 >> (CAN_1MBAUD - speed); +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a frequency in Hz to a can_speed_t in the range + * CAN_125KBAUD to CAN_1MBAUD. + * + * Input Parameters: + * freq - Bit rate in Hz + * + * Returned value: + * A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + ****************************************************************************/ +can_speed_t can_freq2speed(int freq) +{ + return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); +} + +/**************************************************************************** + * Name: can_tx + * + * Description: + * This function is called to transmit a CAN frame using the supplied + * mailbox. It will busy wait on the mailbox if not available. + * + * Input Parameters: + * message_id - The CAN message's EXID field + * length - The number of bytes of data - the DLC field + * message - A pointer to 8 bytes of data to be sent (all 8 bytes will be + * loaded into the CAN transmitter but only length bytes will + * be sent. + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * The CAN_OK of the data sent or CAN_ERROR if a time out occurred + * + ****************************************************************************/ +uint8_t 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. + */ + + uint32_t mask = CAN_TSR_TME0 << mailbox; + + /* Reset the indication of timer expired */ + + timer_hrt_clear_wrap(); + uint32_t cnt = CAN_TX_TIMEOUT_MS; + + while ((getreg32(STM32_CAN1_TSR) & mask) == 0) { + if (timer_hrt_wrap()) { + timer_hrt_clear_wrap(); + + if (--cnt == 0) { + return CAN_ERROR; + } + } + } + + /* + * To allow detection of completion - Set the LEC to + * 'No error' state off all 1s + */ + + putreg32(CAN_ESR_LEC_MASK, STM32_CAN1_ESR); + + 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)); + return CAN_OK; +} + +/**************************************************************************** + * Name: can_rx + * + * Description: + * This function is called to receive a CAN frame from a supplied fifo. + * It does not block if there is not available, but returns 0 + * + * Input Parameters: + * message_id - A pointer to return the CAN message's EXID field + * length - A pointer to return the number of bytes of data - the DLC field + * message - A pointer to return 8 bytes of data to be sent (all 8 bytes will + * be written from the CAN receiver but only length bytes will be sent. + * fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo. + * + * Returned value: + * The length of the data read or 0 if the fifo was empty + * + ****************************************************************************/ +uint8_t can_rx(uint32_t *message_id, size_t *length, uint8_t *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 */ + + *message_id = (getreg32(STM32_CAN1_RIR(fifo)) & CAN_RIR_EXID_MASK) >> + CAN_RIR_EXID_SHIFT; + *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(message, data, sizeof(data)); + } + + return rv; +} + +/**************************************************************************** + * Name: can_autobaud + * + * Description: + * This function will attempt to detect the bit rate in use on the CAN + * interface until the timeout provided expires or the successful detection + * occurs. + * + * It will initialize the CAN block for a given bit rate + * to test that a message can be received. The CAN interface is left + * operating at the detected bit rate and in CAN_Mode_Normal mode. + * + * Input Parameters: + * can_speed - A pointer to return detected can_speed_t from CAN_UNKNOWN to + * CAN_1MBAUD + * timeout - The timer id of a timer to use as the maximum time to wait for + * successful bit rate detection. This timer may be not running + * in which case the auto baud code will try indefinitely to + * detect the bit rate. + * + * Returned value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) +{ + *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(timeout)) { + return CAN_BOOT_TIMEOUT; + } + + if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) & + CAN_RFR_FMP_MASK) { + *can_speed = speed; + can_init(speed, CAN_Mode_Normal); + 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; +} + +/**************************************************************************** + * Name: can_init + * + * Description: + * This function is used to initialize the CAN block for a given bit rate and + * mode. + * + * Input Parameters: + * speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * mode - One of the can_mode_t of Normal, LoopBack and Silent or + * combination thereof. + * + * Returned value: + * OK - on Success or a negate errno value + * + ****************************************************************************/ +int can_init(can_speed_t speed, can_mode_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 << SJW_POS) | + (CAN_125KBAUD_BS1 << BS1_POS) | + (CAN_125KBAUD_BS2 << BS2_POS) | (CAN_125KBAUD_PRESCALER - 1), + + (CAN_250KBAUD_SJW << SJW_POS) | + (CAN_250KBAUD_BS1 << BS1_POS) | + (CAN_250KBAUD_BS2 << BS2_POS) | (CAN_250KBAUD_PRESCALER - 1), + + (CAN_500KBAUD_SJW << SJW_POS) | + (CAN_500KBAUD_BS1 << BS1_POS) | + (CAN_500KBAUD_BS2 << BS2_POS) | (CAN_500KBAUD_PRESCALER - 1), + + (CAN_1MBAUD_SJW << SJW_POS) | + (CAN_1MBAUD_BS1 << BS1_POS) | + (CAN_1MBAUD_BS2 << BS2_POS) | (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 | CAN_MCR_TXFP, 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. */ + + /* Set FINIT - Initialization mode for the filters- */ + putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); + + putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ + + putreg32(3, STM32_CAN1_FS1R); /* Single 32-bit scale configuration for filters 0 and 1 */ + + /* Filter 0 masks -- DTIDGetNodeInfo requests only */ + + uavcan_protocol_t protocol; + + protocol.id.u32 = 0; + protocol.ser.type_id = DTIDReqGetNodeInfo; + protocol.ser.service_not_message = true; + protocol.ser.request_not_response = true; + + /* Set the Filter ID */ + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_ID)); + + /* Set the Filter Mask */ + protocol.ser.type_id = BIT_MASK(LengthUavCanServiceTypeID); + + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_MASK)); + + /* Filter 1 masks -- everything is don't-care */ + putreg32(0, STM32_CAN1_FIR(1, FILTER_ID)); + putreg32(0, STM32_CAN1_FIR(1, FILTER_MASK)); + + 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 */ + + /* Clear FINIT - Active mode for the filters- */ + + putreg32(0, STM32_CAN1_FMR); + + return OK; +} + +/**************************************************************************** + * Name: can_cancel_on_error + * + * Description: + * This function will test for transition completion or any error. + * If the is a error the the transmit will be aborted. + * + * Input Parameters: + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * CAN_OK - on Success or a CAN_ERROR if the cancellation was needed + * + ****************************************************************************/ +void can_cancel_on_error(uint8_t mailbox) +{ + uint32_t rvalue; + + /* Wait for completion the all 1's wat set in the tx code*/ + while (CAN_ESR_LEC_MASK == ((rvalue = (getreg32(STM32_CAN1_ESR) & CAN_ESR_LEC_MASK)))); + + /* Any errors */ + if (rvalue) { + + putreg32(0, STM32_CAN1_ESR); + + /* Abort the the TX in case of collision wuth NART false */ + + putreg32(CAN_TSR_ABRQ0 << (mailbox * CAN_TSR_RQCP_SHFTS), STM32_CAN1_TSR); + } +} diff --git a/platforms/nuttx/src/canbootloader/common/boot_app_shared.c b/platforms/nuttx/src/canbootloader/common/boot_app_shared.c new file mode 100644 index 000000000000..582f7c5e6fba --- /dev/null +++ b/platforms/nuttx/src/canbootloader/common/boot_app_shared.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include + +#include "chip.h" +#include "stm32.h" + +#include +#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 *pshared) +{ + pshared->signature = getreg32(signature_LOC); + pshared->bus_speed = getreg32(bus_speed_LOC); + pshared->node_id = getreg32(node_id_LOC); + pshared->crc.ul[CRC_L] = getreg32(crc_LoLOC); + pshared->crc.ul[CRC_H] = getreg32(crc_HiLOC); +} + +inline static void write(bootloader_app_shared_t *pshared) +{ + putreg32(pshared->signature, signature_LOC); + putreg32(pshared->bus_speed, bus_speed_LOC); + putreg32(pshared->node_id, node_id_LOC); + putreg32(pshared->crc.ul[CRC_L], crc_LoLOC); + putreg32(pshared->crc.ul[CRC_H], crc_HiLOC); +} + +static uint64_t calulate_signature(bootloader_app_shared_t *pshared) +{ + uint64_t crc; + crc = crc64_add_word(CRC64_INITIAL, pshared->signature); + crc = crc64_add_word(crc, pshared->bus_speed); + crc = crc64_add_word(crc, pshared->node_id); + crc ^= CRC64_OUTPUT_XOR; + return crc; +} + +static void bootloader_app_shared_init(bootloader_app_shared_t *pshared, eRole_t role) +{ + memset(pshared, 0, sizeof(bootloader_app_shared_t)); + + if (role != Invalid) { + pshared->signature = + (role == + App ? BOOTLOADER_COMMON_APP_SIGNATURE : + BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); + } +} + +/**************************************************************************** + * Name: bootloader_app_shared_read + * + * Description: + * Based on the role requested, this function will conditionally populate + * a bootloader_app_shared_t structure from the physical locations used + * to transfer the shared data to/from an application (internal data) . + * + * The functions will only populate the structure and return a status + * indicating success, if the internal data has the correct signature as + * requested by the Role AND has a valid crc. + * + * Input Parameters: + * shared - A pointer to a bootloader_app_shared_t return the data in if + * the internal data is valid for the requested Role + * role - An eRole_t of App or BootLoader to validate the internal data + * against. For a Bootloader this would be the value of App to + * read the application passed data. + * + * Returned value: + * OK - Indicates that the internal data has been copied to callers + * bootloader_app_shared_t structure. + * + * -EBADR - The Role or crc of the internal data was not valid. The copy + * did not occur. + * + ****************************************************************************/ +__EXPORT int bootloader_app_shared_read(bootloader_app_shared_t *shared, 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))) { + *shared = working; + rv = OK; + } + + return rv; +} + +/**************************************************************************** + * Name: bootloader_app_shared_write + * + * Description: + * Based on the role, this function will commit the data passed + * into the physical locations used to transfer the shared data to/from + * an application (internal data) . + * + * The functions will populate the signature and crc the data + * based on the provided Role. + * + * Input Parameters: + * shared - A pointer to a bootloader_app_shared_t data to commit to + * the internal data for passing to/from an application. + * role - An eRole_t of App or BootLoader to use in the internal data + * to be passed to/from an application. For a Bootloader this + * would be the value of Bootloader to write to the passed data. + * to the application via the internal data. + * + * Returned value: + * None. + * + ****************************************************************************/ +__EXPORT void bootloader_app_shared_write(bootloader_app_shared_t *shared, eRole_t role) +{ + bootloader_app_shared_t working = *shared; + working.signature = (role == App ? BOOTLOADER_COMMON_APP_SIGNATURE : BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); + working.crc.ull = calulate_signature(&working); + write(&working); +} + +/**************************************************************************** + * Name: bootloader_app_shared_invalidate + * + * Description: + * Invalidates the data passed the physical locations used to transfer + * the shared data to/from an application (internal data) . + * + * The functions will invalidate the signature and crc and shoulf be used + * to prevent deja vu. + * + * Input Parameters: + * None. + * + * Returned value: + * None. + * + ****************************************************************************/ +__EXPORT void bootloader_app_shared_invalidate(void) +{ + bootloader_app_shared_t working; + bootloader_app_shared_init(&working, Invalid); + write(&working); +} diff --git a/platforms/nuttx/src/canbootloader/fs/flash.c b/platforms/nuttx/src/canbootloader/fs/flash.c new file mode 100644 index 000000000000..2e4aaf51b372 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/fs/flash.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "boot_config.h" + +#include +#include + +#include + +#include "chip.h" +#include "stm32.h" + +#include "flash.h" +#include "blsched.h" + +/**************************************************************************** + * Name: bl_flash_erase + * + * Description: + * This function erases the flash starting at address and ending at + * address + nbytes. + * + * Input Parameters: + * address - A word-aligned address within the first page of flash to erase + * nbytes - The number of bytes to erase, rounding up to the next page. + * + * + * + * Returned value: + * On success FLASH_OK On Error one of the flash_error_t + * + ****************************************************************************/ + +flash_error_t bl_flash_erase(size_t address, size_t nbytes) +{ + /* + * 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(address - 1); + + if (bllastpage < 0) { + return FLASH_ERROR_AFU; + } + + ssize_t appstartpage = up_progmem_getpage(address); + ssize_t appendpage = up_progmem_getpage(address + nbytes - 4); + + if (appendpage < 0 || appstartpage < 0) { + return FLASH_ERROR_AFU; + } + + status = FLASH_ERROR_SUICIDE; + + if (bllastpage >= 0 && appstartpage > bllastpage) { + + /* Erase the whole application flash region */ + + status = FLASH_OK; + + while (status == FLASH_OK && appstartpage <= appendpage) { + bl_sched_yield(); + ssize_t ps = up_progmem_eraseblock(appstartpage++); + + if (ps <= 0) { + status = FLASH_ERASE_ERROR; + } + } + } + + return status; +} + +/**************************************************************************** + * Name: bl_flash_write + * + * Description: + * This function writes the flash starting at the given address + * + * Input Parameters: + * flash_address - The address of the flash to write + * must be word aligned + * data - A pointer to a buffer count bytes to be written + * to the flash. + * count - Number of bytes to write + * + * Returned value: + * On success FLASH_OK On Error one of the flash_error_t + * + ****************************************************************************/ + +flash_error_t bl_flash_write(uint32_t flash_address, uint8_t *data, ssize_t count) +{ + + flash_error_t status = FLASH_ERROR; + + if (flash_address >= APPLICATION_LOAD_ADDRESS && + (flash_address + count) <= (uint32_t) APPLICATION_LAST_8BIT_ADDRRESS) { + if (count == + up_progmem_write((size_t) flash_address, (void *)data, count)) { + status = FLASH_OK; + } + } + + return status; +} diff --git a/platforms/nuttx/src/canbootloader/include/bitminip.h b/platforms/nuttx/src/canbootloader/include/bitminip.h new file mode 100644 index 000000000000..56c48cae22a2 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/bitminip.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file board.h + * + * bootloader board interface + * This file contains the common interfaces that all boards + * have to supply + */ + +#pragma once + +#define BIT(pos) (1ull<<(pos)) +#define BIT_MASK(length) (BIT(length)-1) + +#define BITFEILD_MASK(lsb_pos, length) ( BIT_MASK(length) << (lsb_pos)) +#define BITFEILD_ISOLATE(x, lsb_pos, length) ((x) & (BITFEILD_MASK((lsb_pos), (length)))) +#define BITFEILD_EXCLUDE(x, lsb_pos, length) ((x) & ~(BITFEILD_MASK((lsb_pos), (length)))) +#define BITFEILD_GET(y, lsb_pos, length) (((y)>>(lsb_pos)) & BIT_MASK(length)) +#define BITFEILD_SET(y, x, lsb_pos, length) ( y= ((y) & ~BF_MASK(lsb_pos, length)) | BITFEILD_ISOLATE(x, lsb_pos, length)) diff --git a/platforms/nuttx/src/canbootloader/include/blsched.h b/platforms/nuttx/src/canbootloader/include/blsched.h new file mode 100644 index 000000000000..b5460456bc80 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/blsched.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * Name: sched_yield() + * + * Description: + * This function should be called in situation were the cpu may be + * busy for a long time without interrupts or the ability to run code + * to insure that the timer based process will be run. + * + * + * Input Parameters: + * None + * + * Returned value: + * None + * + ****************************************************************************/ +#if defined(OPT_USE_YIELD) +void bl_sched_yield(void); +#else +# define bl_sched_yield() +#endif diff --git a/platforms/nuttx/src/canbootloader/include/board.h b/platforms/nuttx/src/canbootloader/include/board.h new file mode 100644 index 000000000000..20fbf481794a --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/board.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file board.h + * + * bootloader board interface + * This file contains the common interfaces that all boards + * have to supply + */ +#pragma once + +#include "uavcan.h" +#include +#include + +typedef enum { + off, + reset, + autobaud_start, + autobaud_end, + allocation_start, + allocation_end, + fw_update_start, + fw_update_erase_fail, + fw_update_invalid_response, + fw_update_timeout, + fw_update_invalid_crc, + jump_to_app, +} uiindication_t; + +#ifndef __ASSEMBLY__ + +/************************************************************************************ + * Name: stm32_boarddeinitialize + * + * Description: + * This function is called by the bootloader code priore to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void stm32_boarddeinitialize(void); + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrive the product name. The retuned alue is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The imum number of chatater that can be written + * + * Returned Value: + * The length of charaacters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen); + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version); + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ + +void board_indicate(uiindication_t indication); + + +#endif /* __ASSEMBLY__ */ diff --git a/platforms/nuttx/src/canbootloader/include/board_common.h b/platforms/nuttx/src/canbootloader/include/board_common.h new file mode 100644 index 000000000000..7b2db815650b --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/board_common.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_common.h + * + * Provide the the common board interfaces + */ + +#pragma once + +/* Defined the types used for board UUID and MFG UID + * + * A type suitable for holding the byte format of the UUID + * + * The original PX4 stm32 (legacy) based implementation **displayed** the + * UUID as: ABCD EFGH IJKL + * Where: + * A was bit 31 and D was bit 0 + * E was bit 63 and H was bit 32 + * I was bit 95 and L was bit 64 + * + * Since the string was used by some manufactures to identify the units + * it must be preserved. + * + * For new targets moving forward we will use + * IJKL EFGH ABCD + */ + +/* A type suitable for defining the 8 bit format of the MFG UID + * This is always returned as MSD @ index 0 -LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + */ +typedef uint8_t mfguid_t[PX4_CPU_MFGUID_BYTE_LENGTH]; + +/************************************************************************************ + * Name: board_get_mfguid + * + * Description: + * All boards either provide a way to retrieve a manafactuers Uniqe ID or + * define BOARD_OVERRIDE_MFGUID. + * The MFGUID is returned as an array of bytes in + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + ************************************************************************************/ + +int board_get_mfguid(mfguid_t mfgid); diff --git a/platforms/nuttx/src/canbootloader/include/boot_app_shared.h b/platforms/nuttx/src/canbootloader/include/boot_app_shared.h new file mode 100644 index 000000000000..71f424895fc0 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/boot_app_shared.h @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +__BEGIN_DECLS + +/* Define the signature for the Application descriptor as 'APDesc' and a + * revision number of 00 used in app_descriptor_t + */ + +#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 + +/* N.B. the .ld file must emit this sections */ +# define boot_app_shared_section __attribute__((section(".app_descriptor"))) + +/* eRole defines the role of the bootloader_app_shared_t structure */ + +typedef enum eRole { + Invalid, + App, + BootLoader +} eRole_t; + +/**************************************************************************** + * + * Bootloader and Application shared structure. + * + * The data in this structure 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 by design. + * + * For reference, the following is performed based on eRole in API calls + * defined below: + * + * 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 application, 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 resulting values are then copied to the CAN filter + * registers by bootloader_app_shared_read and + * bootloader_app_shared_write. + * +****************************************************************************/ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" +typedef begin_packed_struct struct bootloader_app_shared_t { + union { + uint64_t ull; + uint32_t ul[2]; + uint8_t valid; + } crc; + uint32_t signature; + uint32_t bus_speed; + uint32_t node_id; +} end_packed_struct bootloader_app_shared_t; +#pragma GCC diagnostic pop + +/**************************************************************************** + * + * Application firmware descriptor. + * + * This structure located by the linker script somewhere after the vector table. + * (within the first several kilobytes of the beginning address of the + * application); + * + * This structure must be aligned on an 8-byte boundary. + * + * The bootloader will scan through the application FLASH image until it + * finds the signature. + * + * The image_crc is calculated as follows: + * 1) All fields of this structure must be initialized with the correct + * information about the firmware image bin file + * (Here after refereed to as image) + * 2) image_crc set to 0; + * 3) The CRC 64 is calculated over the image from offset 0 up to and including the + * last byte of the image file. + * 4) The calculated CRC 64 is stored in image_crc + * 5) The new image file is then written to a file a ".img" extension. + * +****************************************************************************/ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" +#pragma GCC diagnostic ignored "-Wpacked" +typedef begin_packed_struct struct app_descriptor_t { + uint8_t signature[sizeof(uint64_t)]; + uint64_t image_crc; + uint32_t image_size; + uint32_t vcs_commit; + uint8_t major_version; + uint8_t minor_version; + uint8_t reserved[6]; +} end_packed_struct app_descriptor_t; +#pragma GCC diagnostic pop + +/**************************************************************************** + * Name: bootloader_app_shared_read + * + * Description: + * Based on the role requested, this function will conditionally populate + * a bootloader_app_shared_t structure from the physical locations used + * to transfer the shared data to/from an application (internal data) . + * + * The functions will only populate the structure and return a status + * indicating success, if the internal data has the correct signature as + * requested by the Role AND has a valid crc. + * + * Input Parameters: + * shared - A pointer to a bootloader_app_shared_t return the data in if + * the internal data is valid for the requested Role + * role - An eRole_t of App or BootLoader to validate the internal data + * against. For a Bootloader this would be the value of App to + * read the application passed data. + * + * Returned value: + * OK - Indicates that the internal data has been copied to callers + * bootloader_app_shared_t structure. + * + * -EBADR - The Role or crc of the internal data was not valid. The copy + * did not occur. + * + ****************************************************************************/ + +int bootloader_app_shared_read(bootloader_app_shared_t *shared, eRole_t role); + +/**************************************************************************** + * Name: bootloader_app_shared_write + * + * Description: + * Based on the role, this function will commit the data passed + * into the physical locations used to transfer the shared data to/from + * an application (internal data) . + * + * The functions will populate the signature and crc the data + * based on the provided Role. + * + * Input Parameters: + * shared - A pointer to a bootloader_app_shared_t data to commit to + * the internal data for passing to/from an application. + * role - An eRole_t of App or BootLoader to use in the internal data + * to be passed to/from an application. For a Bootloader this + * would be the value of Bootloader to write to the passed data. + * to the application via the internal data. + * + * Returned value: + * None. + * + ****************************************************************************/ + +void bootloader_app_shared_write(bootloader_app_shared_t *shared, eRole_t role); + +/**************************************************************************** + * Name: bootloader_app_shared_invalidate + * + * Description: + * Invalidates the data passed the physical locations used to transfer + * the shared data to/from an application (internal data) . + * + * The functions will invalidate the signature and crc and should be used + * to prevent deja vu. + * + * Input Parameters: + * None. + * + * Returned value: + * None. + * + ****************************************************************************/ + +void bootloader_app_shared_invalidate(void); + +__END_DECLS diff --git a/platforms/nuttx/src/canbootloader/include/can.h b/platforms/nuttx/src/canbootloader/include/can.h new file mode 100644 index 000000000000..cded836c7177 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/can.h @@ -0,0 +1,218 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "timer.h" + +typedef enum { + CanPayloadLength = 8, +} can_const_t; + +typedef enum { + CAN_OK = 0, + CAN_BOOT_TIMEOUT, + CAN_ERROR +} can_error_t; + +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; + + +/* + * Receive from FIFO 1 -- filters are configured to push the messages there, + * and there are send/receive functions called off the SysTick ISR so + * we partition the usage of the CAN hardware to avoid the same FIFOs/mailboxes + * as the rest of the application uses. + */ +typedef enum { + + Fifo0 = 0, + MailBox0 = 0, + Fifo1 = 1, + MailBox1 = 1, + Fifo2 = 2, + MailBox2 = 2, + FifoNone = 3, + MailBoxNone = 3, + +} can_fifo_mailbox_t; + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a can_speed_t to a bit rate in Hz + * + * Input Parameters: + * can_speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + * Returned value: + * Bit rate in Hz + * + ****************************************************************************/ +int can_speed2freq(can_speed_t speed); + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a frequency in Hz to a can_speed_t in the range + * CAN_125KBAUD to CAN_1MBAUD. + * + * Input Parameters: + * freq - Bit rate in Hz + * + * Returned value: + * A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + ****************************************************************************/ +can_speed_t can_freq2speed(int freq); + +/**************************************************************************** + * Name: can_tx + * + * Description: + * This function is called to transmit a CAN frame using the supplied + * mailbox. It will busy wait on the mailbox if not available. + * + * Input Parameters: + * message_id - The CAN message's EXID field + * length - The number of bytes of data - the DLC field + * message - A pointer to 8 bytes of data to be sent (all 8 bytes will be + * loaded into the CAN transmitter but only length bytes will + * be sent. + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * The CAN_OK of the data sent or CAN_ERROR if a time out occurred + * + ****************************************************************************/ +uint8_t can_tx(uint32_t message_id, size_t length, const uint8_t *message, uint8_t mailbox); + +/**************************************************************************** + * Name: can_rx + * + * Description: + * This function is called to receive a CAN frame from a supplied fifo. + * It does not block if there is not available, but returns 0 + * + * Input Parameters: + * message_id - A pointer to return the CAN message's EXID field + * length - A pointer to return the number of bytes of data - the DLC field + * message - A pointer to return 8 bytes of data to be sent (all 8 bytes will + * be written from the CAN receiver but only length bytes will be sent. + * fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo. + * + * Returned value: + * The length of the data read or 0 if the fifo was empty + * + ****************************************************************************/ +uint8_t can_rx(uint32_t *message_id, size_t *length, uint8_t *message, uint8_t fifo); + +/**************************************************************************** + * Name: can_init + * + * Description: + * This function is used to initialize the CAN block for a given bit rate and + * mode. + * + * Input Parameters: + * speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * mode - One of the can_mode_t of Normal, LoopBack and Silent or + * combination thereof. + * + * Returned value: + * OK - on Success or a negate errno value + * + ****************************************************************************/ +int can_init(can_speed_t speed, can_mode_t mode); + +/**************************************************************************** + * Name: can_autobaud + * + * Description: + * This function will attempt to detect the bit rate in use on the CAN + * interface until the timeout provided expires or the successful detection + * occurs. + * + * It will initialize the CAN block for a given bit rate + * to test that a message can be received. The CAN interface is left + * operating at the detected bit rate and in CAN_Mode_Normal mode. + * + * Input Parameters: + * can_speed - A pointer to return detected can_speed_t from CAN_UNKNOWN to + * CAN_1MBAUD + * timeout - The timer id of a timer to use as the maximum time to wait for + * successful bit rate detection. This timer may be not running + * in which case the auto baud code will try indefinitely to + * detect the bit rate. + * + * Returned value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout); + +/**************************************************************************** + * Name: can_cancel_on_error + * + * Description: + * This function will test for transition completion or any error. + * If the is a error the the transmit will be aborted. + * + * Input Parameters: + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * None + * + ****************************************************************************/ +void can_cancel_on_error(uint8_t mailbox); diff --git a/platforms/nuttx/src/canbootloader/include/crc.h b/platforms/nuttx/src/canbootloader/include/crc.h new file mode 100644 index 000000000000..8313a83abbdc --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/crc.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#define CRC16_INITIAL 0xFFFFu +#define CRC16_OUTPUT_XOR 0x0000u +#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull +#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull + +/**************************************************************************** + * Name: crc16_add + * + * Description: + * Use to calculates a CRC-16-CCITT using the polynomial of + * 0x1021 by adding a value successive values. + * + * Input Parameters: + * crc - The running total of the crc 16 + * value - The value to add + * + * Returned Value: + * The current crc16 with the value processed. + * + ****************************************************************************/ +uint16_t crc16_add(uint16_t crc, uint8_t value); + +/**************************************************************************** + * Name: crc16_signature + * + * Description: + * Calculates a CRC-16-CCITT using the crc16_add + * function + * + * Input Parameters: + * initial - The Initial value to uses as the crc's starting point + * length - The number of bytes to add to the crc + * bytes - A pointer to any array of length bytes + * + * Returned Value: + * The crc16 of the array of bytes + * + ****************************************************************************/ +uint16_t crc16_signature(uint16_t initial, size_t length, const uint8_t *bytes); + +/**************************************************************************** + * Name: crc64_add_word + * + * Description: + * Calculates a CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693 + * See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 + * Check: 0x62EC59E3F1A4F00A + * + * Input Parameters: + * crc - The running total of the crc 64 + * value - The value to add + * + * Returned Value: + * The current crc64 with the value processed. + * + ****************************************************************************/ +uint64_t crc64_add_word(uint64_t crc, uint32_t value); diff --git a/platforms/nuttx/src/canbootloader/include/flash.h b/platforms/nuttx/src/canbootloader/include/flash.h new file mode 100644 index 000000000000..2da881539ffc --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/flash.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#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; + +/**************************************************************************** + * Name: bl_flash_erase + * + * Description: + * This function erases the flash starting at address and ending at + * address + nbytes. + * + * Input Parameters: + * address - A word-aligned address within the first page of flash to erase + * nbytes - The number of bytes to erase, rounding up to the next page. + * + * + * + * Returned value: + * On success FLASH_OK On Error one of the flash_error_t + * + ****************************************************************************/ +flash_error_t bl_flash_erase(size_t address, size_t nbytes); + +/**************************************************************************** + * Name: bl_flash_write + * + * Description: + * This function writes the flash starting at the given address + * + * Input Parameters: + * flash_address - The address of the flash to write + * must be word aligned + * data - A pointer to a buffer count bytes to be written + * to the flash. + * count - Number of bytes to write + * + * Returned value: + * On success FLASH_OK On Error one of the flash_error_t + * + ****************************************************************************/ +flash_error_t bl_flash_write(uint32_t flash_address, uint8_t *data, ssize_t count); diff --git a/platforms/nuttx/src/canbootloader/include/px4_config.h b/platforms/nuttx/src/canbootloader/include/px4_config.h new file mode 100644 index 000000000000..2493cf9218b2 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/px4_config.h @@ -0,0 +1,51 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_config.h + Configuration flags used in code. + */ + +#pragma once + +#if defined(__PX4_NUTTX) + +#include +#include +#include "px4_micro_hal.h" +#include + +#elif defined (__PX4_POSIX) +# include +#endif diff --git a/platforms/nuttx/src/canbootloader/include/px4_macros.h b/platforms/nuttx/src/canbootloader/include/px4_macros.h new file mode 100644 index 000000000000..3d570e2d252a --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/px4_macros.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_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 + +#ifndef _PX4_MACROS_H +#define _PX4_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(FREEZE_STR) +# define FREEZE_STR(s) #s +#endif + +#if !defined(STRINGIFY) +#define STRINGIFY(s) FREEZE_STR(s) +#endif + +#if !defined(CCASSERT) +#if defined(static_assert) +# define CCASSERT(predicate) static_assert(predicate, STRINGIFY(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 + +#if !defined(DO_PRAGMA) +# define DO_PRAGMA(x) _Pragma (#x) +#endif + +#if !defined(TODO) +# define TODO(x) DO_PRAGMA(message ("TODO - " #x)) +#endif + +#endif /* _PX4_MACROS_H */ diff --git a/platforms/nuttx/src/canbootloader/include/px4_micro_hal.h b/platforms/nuttx/src/canbootloader/include/px4_micro_hal.h new file mode 100644 index 000000000000..a2046b0de2bb --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/px4_micro_hal.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once +/* + * This file is a shim to bridge to nuttx_v3 + */ + +__BEGIN_DECLS + +# define PX4_CPU_UUID_BYTE_LENGTH 12 +# define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +# define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* Separator nnn:nnn:nnnn 2 char per byte term */ +# define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) +# define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) + + +#include +__END_DECLS diff --git a/platforms/nuttx/src/canbootloader/include/random.h b/platforms/nuttx/src/canbootloader/include/random.h new file mode 100644 index 000000000000..2b155b97ecff --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/random.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#define CRC16_INITIAL 0xFFFFu +#define CRC16_OUTPUT_XOR 0x0000u +#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull +#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull + +/**************************************************************************** + * Name: util_srand + * + * Description: + * This function seeds the random number generator + * + * + * Input Parameters: + * seed - The seed + * + * Returned value: + * None + * + ****************************************************************************/ +void util_srand(uint16_t seed); + +/**************************************************************************** + * Name: util_random + * + * Description: + * This function returns a random number between min and max + * + * + * Input Parameters: + * min - The minimum value the return value can be. + * max - The maximum value the return value can be. + * + * Returned value: + * A random number + * + ****************************************************************************/ +uint16_t util_random(uint16_t min, uint16_t max); diff --git a/platforms/nuttx/src/canbootloader/include/timer.h b/platforms/nuttx/src/canbootloader/include/timer.h new file mode 100644 index 000000000000..9bc5d2846c1f --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/timer.h @@ -0,0 +1,419 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + +/* + * We support two classes of timer interfaces. The first one is for structured + * timers that have an API for life cycle management and use. (timer_xx) + * The Second type of methods are for interfacing to a high resolution + * counter with fast access and are provided via an in line API (timer_hrt) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "stm32.h" +#include "nvic.h" + +#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000) +#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000) + +/* Types for timer access */ +typedef uint8_t bl_timer_id; /* A timer handle */ +typedef uint32_t time_ms_t; /* A timer value */ +typedef volatile time_ms_t *time_ref_t; /* A pointer to the internal + counter in the structure of a timer + used to do a time out test value */ + +typedef uint32_t time_hrt_cycles_t; /* A timer value type of the hrt */ + + +/* + * Timers + * + * There are 3 modes of operation for the timers. + * All modes support a call back on expiration. + * + */ +typedef enum { + /* Specifies a one-shot timer. After notification timer is discarded. */ + modeOneShot = 1, + /* Specifies a repeating timer. */ + modeRepeating = 2, + /* Specifies a persistent start / stop timer. */ + modeTimeout = 3, + /* Or'ed in to start the timer when allocated */ + modeStarted = 0x40 +} bl_timer_modes_t; + + +/* The call back function signature type */ + +typedef void (*bl_timer_ontimeout)(bl_timer_id id, void *context); + +/* + * A helper type for timer allocation to setup a callback + * There is a null_cb object (see below) that can be used to + * a bl_timer_cb_t. + * + * Typical usage is: + * + * void my_process(bl_timer_id id, void *context) { + * ... + * }; + * + * bl_timer_cb_t mycallback = null_cb; + * mycallback.cb = my_process; + * bl_timer_id mytimer = timer_allocate(modeRepeating|modeStarted, 100, &mycallback); + */ + +typedef struct { + void *context; + bl_timer_ontimeout cb; + +} bl_timer_cb_t; + + +extern const bl_timer_cb_t null_cb; + +/**************************************************************************** + * Name: timer_init + * + * Description: + * Called early in os_start to initialize the data associated with + * the timers + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +void timer_init(void); + +/**************************************************************************** + * Name: timer_allocate + * + * Description: + * Is used to allocate a timer. Allocation does not involve memory + * allocation as the data for the timer are compile time generated. + * See OPT_BL_NUMBER_TIMERS + * + * There is an inherent priority to the timers in that the first timer + * allocated is the first timer run per tick. + * + * There are 3 modes of operation for the timers. All modes support an + * optional call back on expiration. + * + * modeOneShot - Specifies a one-shot timer. After notification timer + * is resource is freed. + * modeRepeating - Specifies a repeating timer that will reload and + * call an optional. + * modeTimeout - Specifies a persistent start / stop timer. + * + * modeStarted - Or'ed in to start the timer when allocated + * + * + * Input Parameters: + * mode - One of bl_timer_modes_t with the Optional modeStarted + * msfromnow - The reload and initial value for the timer in Ms. + * fc - A pointer or NULL (0). If it is non null it can be any + * of the following: + * + * a) A bl_timer_cb_t populated on the users stack or + * in the data segment. The values are copied into the + * internal data structure of the timer and therefore do + * not have to persist after the call to timer_allocate + * + * b) The address of null_cb. This is identical to passing + * null for the value of fc. + * + * Returned Value: + * On success a value from 0 - OPT_BL_NUMBER_TIMERS-1 that is + * the bl_timer_id for subsequent timer operations + * -1 on failure. This indicates there are no free timers. + * + ****************************************************************************/ +bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, bl_timer_cb_t *fc); + +/**************************************************************************** + * Name: timer_free + * + * Description: + * Is used to free a timer. Freeing a timer does not involve memory + * deallocation as the data for the timer are compile time generated. + * See OPT_BL_NUMBER_TIMERS + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_free(bl_timer_id id); + +/**************************************************************************** + * Name: timer_start + * + * Description: + * Is used to Start a timer. The reload value is copied to the counter. + * And the running bit it set. There is no problem in Starting a running + * timer. But it will restart the timeout. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_start(bl_timer_id id); + +/**************************************************************************** + * Name: timer_restart + * + * Description: + * Is used to re start a timer with a new reload count. The reload value + * is copied to the counter and the running bit it set. There is no + * problem in restarting a running timer. + * + * Input Parameters: + * id - Returned from timer_allocate; + * ms - Is a time_ms_t and the new reload value to use. + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_restart(bl_timer_id id, time_ms_t ms); + +/**************************************************************************** + * Name: timer_stop + * + * Description: + * Is used to stop a timer. It is Ok to stop a stopped timer. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_stop(bl_timer_id id); + +/**************************************************************************** + * Name: timer_expired + * + * Description: + * Test if a timer that was configured as a modeTimeout timer is expired. + * To be expired the time has to be running and have a count of 0. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * No Zero if the timer is expired otherwise zero. + * + ****************************************************************************/ +int timer_expired(bl_timer_id id); + +/**************************************************************************** + * Name: timer_ref + * + * Description: + * Returns an time_ref_t that is a reference (pointer) to the internal counter + * of the timer selected by id. It should only be used with calls to + * timer_ref_expired. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * An internal reference that should be treated as opaque by the caller and + * should only be used with calls to timer_ref_expired. + * There is no reference counting on the reference and therefore does not + * require any operation to free it. + * + ****************************************************************************/ +time_ref_t timer_ref(bl_timer_id id); + +/**************************************************************************** + * Name: timer_ref_expired + * + * Description: + * Test if a timer, that was configured as a modeTimeout timer is expired + * based on the reference provided. + * + * Input Parameters: + * ref - Returned timer_ref; + * + * Returned Value: + * Non Zero if the timer is expired otherwise zero. + * + ****************************************************************************/ +static inline int timer_ref_expired(time_ref_t ref) +{ + return *ref == 0; +} + +/**************************************************************************** + * Name: timer_tic + * + * Description: + * Returns the system tic counter that counts in units of + * (CONFIG_USEC_PER_TICK/1000). By default 10 Ms. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +time_ms_t timer_tic(void); + +/**************************************************************************** + * Name: timer_hrt_read + * + * Description: + * Returns the hardware SysTic counter that counts in units of + * STM32_HCLK_FREQUENCY. This file defines TIMER_HRT_CYCLES_PER_US + * and TIMER_HRT_CYCLES_PER_MS that should be used to define times. + * + * Input Parameters: + * None + * + * Returned Value: + * The current value of the HW counter in the type of time_hrt_cycles_t. + * + ****************************************************************************/ +static inline time_hrt_cycles_t timer_hrt_read(void) +{ + return getreg32(NVIC_SYSTICK_CURRENT); +} + +/**************************************************************************** + * Name: timer_hrt_clear_wrap + * + * Description: + * Clears the wrap flag by reading the timer + * + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +static inline void timer_hrt_clear_wrap(void) +{ + (void)timer_hrt_read(); +} +/**************************************************************************** + * Name: timer_hrt_wrap + * + * Description: + * Returns true if SysTic counted to 0 since last time it was + * read. + * + * Input Parameters: + * None + * + * Returned Value: + * Returns true if timer counted to 0 since last time this was read. + * + ****************************************************************************/ +static inline bool timer_hrt_wrap(void) +{ + uint32_t rv = getreg32(NVIC_SYSTICK_CTRL); + return ((rv & NVIC_SYSTICK_CTRL_COUNTFLAG) ? true : false); +} + +/**************************************************************************** + * Name: timer_hrt_max + * + * Description: + * Returns the hardware SysTic reload value +1 + * + * Input Parameters: + * None + * + * Returned Value: + * The current SysTic reload of the HW counter in the type of + * time_hrt_cycles_t. + * + ****************************************************************************/ +static inline time_hrt_cycles_t timer_hrt_max(void) +{ + return getreg32(NVIC_SYSTICK_RELOAD) + 1; +} + +/**************************************************************************** + * Name: timer_hrt_elapsed + * + * Description: + * Returns the difference between 2 time values, taking into account + * the way the timer wrap. + * + * Input Parameters: + * begin - Beginning timer count. + * end - Ending timer count. + * + * Returned Value: + * The difference from begin to end + * + ****************************************************************************/ +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/platforms/nuttx/src/canbootloader/include/uavcan.h b/platforms/nuttx/src/canbootloader/include/uavcan.h new file mode 100644 index 000000000000..d5562c24c8eb --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/uavcan.h @@ -0,0 +1,841 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +/* + * Uavcan protocol CAN ID formats and Tail byte component definitions + * + * Most all the constants below are auto generated by the compiler in the + * section above. + + * For all UAVCAN_BIT_DEFINE({item name}, lsb_pos, length) entries + * it defines the following enumeration constants: + * + * Mask{item name} - The mask that is positioned at the lsb and + * length long + * BitPos{item name} - The bit position of the lsb + * Length{item name} - The number of bits in the field + * + * + * For the UAVCAN_DSDL_TYPE_DEF(name, dtid, service, signature, packsize, + * mailbox, fifo, inbound, outbound) + * it defines: + * + * uavcan dtid definitions are in the form nnnn.type_name.uavcan + * + * DSDL{type_name} - An internal zero based ID for the transfer + * format E.G DSDLNodeInfo would return the internal + * constant index derived from the line that + * UAVCAN_DSDL_TYPE_DEF(GetNodeInfo... is defined on + * in uavcan_dsdl_defs.h (as of this writing it would + * be the value 1) + * + * DTID{type_name} - The uavcan Number nnn.name E.G DTIDNodeInfo + * + * ServiceNotMessage{type_name} - True for DTID that are uavcan Services Type + * Defined Transfer Type + * + * SignatureCRC16{type_name} - Required for multi-frame Transfer Type + * + * PackedSize{type_name} - The packed size of the Transfer Type + * + * MailBox{type_name} - When sending this Transfer Type, use this + * mailbox + * + * Fifo{type_name} - When receiving this Transfer Type, use this + * fifo. - Filers need to be configured + * + * Fifo{type_name} - The uavcan Number nnn.name E.G DTIDNodeInfo + * + * InTailInit{type_name} - The value of the tail byte to expect at + * the end of a transfer + * + * OutTailInit{type_name} - The value of the tail byte to to use when + * sending this transfer + * + * For all UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) + * it defines the following enumeration constants: + * + * Mask{item name} - The mask that is positioned at the lsb and + * length long + * BitPos{item name} - The bit position of the lsb + * Length{item name} - The number of bits in the field + * PayloadOffset{} - The offset in the payload + * PayloadLength{} - The number of bytes + * + */ + + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "bitminip.h" +#include "can.h" +#include "px4_macros.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define sizeof_member(t, m) sizeof(((t *)0)->m) +#define UAVCAN_STRLEN(x) sizeof((x))-1 +#define uavcan_exclude(x, name) BITFEILD_EXCLUDE((x), BitPos##name,Length##name) +#define uavcan_make_uint16(d0, d1) (uint16_t)(((d1) << 8u) | (d0)) + +#define uavcan_dsdl_field(op, data_typ_name, field_name) (op##data_typ_name##field_name) +#define uavcan_bit_pos(data_typ_name, field_name) uavcan_dsdl_field(BitPos, data_typ_name, field_name) +#define uavcan_bit_mask(data_typ_name, field_name) uavcan_dsdl_field(Mask, data_typ_name, field_name) +#define uavcan_bit_count(data_typ_name, field_name) uavcan_dsdl_field(Length, data_typ_name, field_name) + +#define uavcan_byte_offset(data_typ_name, field_name) uavcan_dsdl_field(PayloadOffset, data_typ_name, field_name) +#define uavcan_byte_count(data_typ_name, field_name) uavcan_dsdl_field(PayloadLength, data_typ_name, field_name) + +#define uavcan_pack(d, data_typ_name, field_name) \ + (((d) << uavcan_bit_pos(data_typ_name, field_name)) & uavcan_bit_mask(data_typ_name, field_name)) +#define uavcan_ppack(d, data_typ_name, field_name) uavcan_pack(d->field_name, data_typ_name, field_name) +#define uavcan_rpack(d, data_typ_name, field_name) uavcan_pack(d.field_name, data_typ_name, field_name) + +#define uavcan_unpack(d, data_typ_name, field_name) \ + (((d) & uavcan_bit_mask(data_typ_name, field_name)) >> uavcan_bit_pos(data_typ_name, field_name)) +#define uavcan_punpack(d, data_typ_name, field_name) uavcan_unpack(d->field_name, data_typ_name, field_name) +#define uavcan_runpack(d, data_typ_name, field_name) uavcan_unpack(d.field_name, data_typ_name, field_name) + +#define uavcan_protocol_mask(field_name) (Mask##field_name) + +/**************************************************************************** + * Auto Generated Public Type Definitions + ****************************************************************************/ +/* CAN ID fields */ +#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) Mask##field_name = BITFEILD_MASK((lsb_pos), (length)), +typedef enum uavcan_can_id_mask_t { +#include "uavcan_can_id_defs.h" +} uavcan_can_id_mask_t; +#undef UAVCAN_BIT_DEFINE + +#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) BitPos##field_name = (lsb_pos), +typedef enum uavcan_can_id_pos_t { +#include "uavcan_can_id_defs.h" +} uavcan_can_id_pos_t; +#undef UAVCAN_BIT_DEFINE + +#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) Length##field_name = (length), +typedef enum uavcan_can_id_length_t { +#include "uavcan_can_id_defs.h" +} uavcan_can_id_length_t; +#undef UAVCAN_BIT_DEFINE + +/**************************************************************************** + * Public Type Definitions + ****************************************************************************/ +typedef enum uavcan_direction_t { + InBound = true, + OutBound = false, + + MessageIn = InBound, + MessageOut = OutBound, +} uavcan_direction_t; + +/* + * Uavcan protocol CAN ID formats and Tail byte component definitions + * + * Most all the constants below are auto generated by the compiler in the + * section above. + * It defines Mask{item name}, BitPos{item name} and Length{item name} + * for all UAVCAN_BIT_DEFINE({item name}, lsb_pos, length) entries + * + * [CAN ID[4]][data[0-7][tail[1]]] + * + */ + +/* General UAVCAN Constants */ + +typedef enum uavcan_general_t { + UavcanPriorityMax = 0, + UavcanPriorityMin = 31, + +} uavcan_general_t; + + +/* CAN ID definitions for native data manipulation */ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" +#pragma GCC diagnostic ignored "-Wpacked" +typedef begin_packed_struct struct can_id_t { + union { + uint32_t u32; + uint8_t b[sizeof(uint32_t)]; + }; +} end_packed_struct can_id_t; + +/* UAVCAN CAN ID Usage: Message definition */ + +typedef struct uavcan_message_t { +uint32_t source_node_id : LengthUavCanMessageSourceNodeID; +uint32_t service_not_message : LengthUavCanMessageServiceNotMessage; +uint32_t type_id : LengthUavCanMessageTypeID; +uint32_t priority : LengthUavCanMessagePriority; +} uavcan_message_t; + +/* UAVCAN CAN ID Usage: Anonymous Message Constants */ + +typedef enum uavcan_anon_const_t { + UavcanAnonymousNodeID = 0, + +} uavcan_anon_const_t; + + +/* UAVCAN CAN ID Usage: Anonymous Message definition */ + +typedef struct uavcan_anonymous_message_t { +uint32_t source_node_id : LengthUavCanAnonMessageSourceNodeID; +uint32_t service_not_message : LengthUavCanAnonMessageServiceNotMessage; +uint32_t type_id : LengthUavCanAnonMessageTypeID; +uint32_t discriminator : LengthUavCanAnonMessageDiscriminator; +uint32_t priority : LengthUavCanAnonMessagePriority; +} uavcan_anonymous_message_t; + +/* UAVCAN CAN ID Usage: Service Constants */ + +typedef enum uavcan_service_const_t { + UavcanServiceRetries = 3, + UavcanServiceTimeOutMs = 1000, + +} uavcan_service_const_t; + +/* UAVCAN CAN ID Usage: Service definition */ + +typedef struct uavcan_service_t { +uint32_t source_node_id : LengthUavCanServiceSourceNodeID; +uint32_t service_not_message : LengthUavCanServiceServiceNotMessage; +uint32_t dest_node_id : LengthUavCanServiceDestinationNodeID; +uint32_t request_not_response : LengthUavCanServiceRequestNotResponse; +uint32_t type_id : LengthUavCanServiceTypeID; +uint32_t priority : LengthUavCanServicePriority; +} uavcan_service_t; + +/* UAVCAN Tail Byte definitions for native data manipulation */ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpacked" +typedef begin_packed_struct struct can_tail_t { + union { + uint8_t u8; + }; +} end_packed_struct can_tail_t; +#pragma GCC diagnostic pop + +/* UAVCAN Tail Byte definitions */ + +typedef struct uavcan_tail_t { +uint8_t transfer_id : LengthUavCanTransferID; +uint8_t toggle : LengthUavCanToggle; +uint8_t eot : LengthUavCanEndOfTransfer; +uint8_t sot : LengthUavCanStartOfTransfer; +} uavcan_tail_t; + +/* UAVCAN Tail Byte Initialization constants */ + +typedef enum uavcan_tail_init_t { + SingleFrameTailInit = (MaskUavCanStartOfTransfer | MaskUavCanEndOfTransfer), + MultiFrameTailInit = (MaskUavCanStartOfTransfer), + BadTailState = (MaskUavCanStartOfTransfer | MaskUavCanToggle), + MaxUserPayloadLength = CanPayloadLength - sizeof(uavcan_tail_t), +} uavcan_tail_init_t; + +/* + * Assert that assumptions in code are true + * The code assumes it can manipulate a ALL sub protocol objects + * using MaskUavCanMessageServiceNotMessage, MaskUavCanMessagePriority + * and MaskUavCanMessageSourceNodeID + */ + +CCASSERT(MaskUavCanServicePriority == MaskUavCanAnonMessagePriority); +CCASSERT(MaskUavCanServicePriority == MaskUavCanMessagePriority); +CCASSERT(MaskUavCanServiceSourceNodeID == MaskUavCanAnonMessageSourceNodeID); +CCASSERT(MaskUavCanServiceSourceNodeID == MaskUavCanMessageSourceNodeID); +CCASSERT(MaskUavCanMessageServiceNotMessage == MaskUavCanAnonMessageServiceNotMessage); +CCASSERT(MaskUavCanMessageServiceNotMessage == MaskUavCanMessageServiceNotMessage); + +/**************************************************************************** + * Auto Generated Public Type Definitions + ****************************************************************************/ + +/* UAVCAN DSDL Type Definitions*/ +#define NA 0 +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) + +#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + UAVCAN_DSDL_TYPE_DEF(Msg##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound)) + +#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + UAVCAN_DSDL_TYPE_DEF(Req##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound)) + +#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + UAVCAN_DSDL_TYPE_DEF(Rsp##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound)) + +#define END_COMPONENTS SizeDSDLComponents, +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + DSDL##name, +typedef enum uavcan_dsdl_t { +#include "uavcan_dsdl_defs.h" + SizeDSDL, +} uavcan_dsdl_t; +#undef UAVCAN_DSDL_TYPE_DEF +#undef END_COMPONENTS +#define END_COMPONENTS + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + DTID##name = (dtid), +typedef enum uavcan_dsdl_dtid_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_dtid_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + SignatureCRC16##name = (signature), +typedef enum uavcan_dsdl_sig_crc16_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_sig_crc16_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + PackedSize##name = (packsize), +typedef enum uavcan_dsdl_packedsize_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_packedsize_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + MailBox##name = (mailbox), +typedef enum uavcan_dsdl_mb_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_mb_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + Fifo##name = (fifo), +typedef enum uavcan_dsdl_fifo_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_fifo_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + InTailInit##name = (inbound), +typedef enum uavcan_dsdl_inbound_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_inbound_t; +#undef UAVCAN_DSDL_TYPE_DEF + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + OutTailInit##name = (outbound), +typedef enum uavcan_dsdl_outbound_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_outbound_t; +#undef UAVCAN_DSDL_TYPE_DEF +#undef UAVCAN_DSDL_SRSP_DEF +#undef UAVCAN_DSDL_SREQ_DEF +#undef UAVCAN_DSDL_MESG_DEF +#undef UAVCAN_DSDL_BIT_DEF + +/* UAVCAN DSDL Fields of Type Definitions definitions*/ + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) +#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) +#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) +#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \ + Mask##data_typ_name##field_name = BITFEILD_MASK((lsb_pos), (length)), +typedef enum uavcan_dsdl_mask_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_mask_t; +#undef UAVCAN_DSDL_BIT_DEF + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \ + BitPos##data_typ_name##field_name = (lsb_pos), +typedef enum uavcan_dsdl_pos_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_pos_t; +#undef UAVCAN_DSDL_BIT_DEF + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \ + Length##data_typ_name##field_name = (length), +typedef enum uavcan_dsdl_length_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_length_t; +#undef UAVCAN_DSDL_BIT_DEF + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \ + PayloadOffset##data_typ_name##field_name = (payload_offset), +typedef enum uavcan_dsdl_payload_offset_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_payload_offset_t; +#undef UAVCAN_DSDL_BIT_DEF + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \ + PayloadLength##data_typ_name##field_name = (payload_length), +typedef enum uavcan_dsdl_payload_length_t { +#include "uavcan_dsdl_defs.h" +} uavcan_dsdl_payload_length_t; +#undef UAVCAN_DSDL_TYPE_DEF +#undef UAVCAN_DSDL_SRSP_DEF +#undef UAVCAN_DSDL_SREQ_DEF +#undef UAVCAN_DSDL_MESG_DEF +#undef UAVCAN_DSDL_BIT_DEF +#undef END_COMPONENTS +#undef NA + +/**************************************************************************** + * Public Type Definitions + ****************************************************************************/ + +/* Uavcan function return values */ + +typedef enum uavcan_error_t { + UavcanOk = 0, + UavcanBootTimeout = 1, + UavcanError = 3 +} uavcan_error_t; + + +/* + * Uavcan protocol CAN ID formats and Tail byte + * + * [CAN ID[4]][data[0-7][tail[1]]] + * + */ + +typedef begin_packed_struct struct uavcan_protocol_t { + union { + can_id_t id; + uavcan_message_t msg; + uavcan_anonymous_message_t ana; + uavcan_service_t ser; + }; + union { + can_tail_t tail_init; + uavcan_tail_t tail; + }; +} end_packed_struct uavcan_protocol_t; + + +/* + * Uavcan protocol DSDL Type Definitions + */ + +/**************************************** + * Uavcan NodeStatus + ****************************************/ + +typedef enum uavcan_NodeStatusConsts_t { + MAX_BROADCASTING_PERIOD_MS = 1000, + MIN_BROADCASTING_PERIOD_MS = 2, + OFFLINE_TIMEOUT_MS = 2000, + + HEALTH_OK = 0, + HEALTH_WARNING = 1, + HEALTH_ERROR = 2, + HEALTH_CRITICAL = 3, + + MODE_OPERATIONAL = 0, + MODE_INITIALIZATION = 1, + MODE_MAINTENANCE = 2, + MODE_SOFTWARE_UPDATE = 3, + + MODE_OFFLINE = 7, +} uavcan_NodeStatusConsts_t; + +typedef begin_packed_struct struct uavcan_NodeStatus_t { + uint32_t uptime_sec; + union { + uint8_t u8; + struct { +uint8_t sub_mode: LengthNodeStatussub_mode; +uint8_t mode : LengthNodeStatusmode; +uint8_t health : LengthNodeStatushealth; + }; + }; + uint16_t vendor_specific_status_code; +} end_packed_struct uavcan_NodeStatus_t; + +/**************************************** + * Uavcan GetNodeInfo composition + ****************************************/ + +/* SoftwareVersion */ +typedef enum uavcan_SoftwareVersionConsts_t { + OPTIONAL_FIELD_FLAG_VCS_COMMIT = 1, + OPTIONAL_FIELD_FLAG_IMAGE_CRC = 2, +} uavcan_SoftwareVersionConsts_t; + +typedef begin_packed_struct struct uavcan_SoftwareVersion_t { + uint8_t major; + uint8_t minor; + uint8_t optional_field_flags; + uint32_t vcs_commit; + uint64_t image_crc; +} end_packed_struct uavcan_SoftwareVersion_t; + +CCASSERT(PackedSizeSoftwareVersion == sizeof(uavcan_SoftwareVersion_t)); + +/* HardwareVersion */ + +typedef begin_packed_struct struct uavcan_HardwareVersion_t { + uint8_t major; + uint8_t minor; + uint8_t unique_id[PayloadLengthHardwareVersionunique_id]; + uint8_t certificate_of_authenticity_length; + uint8_t certificate_of_authenticity[PayloadLengthHardwareVersioncertificate_of_authenticity]; +} end_packed_struct uavcan_HardwareVersion_t; + +typedef enum uavcan_HardwareVersionConsts_t { + FixedSizeHardwareVersion = sizeof_member(uavcan_HardwareVersion_t, major) + \ + sizeof_member(uavcan_HardwareVersion_t, minor) + \ + sizeof_member(uavcan_HardwareVersion_t, unique_id) + \ + sizeof_member(uavcan_HardwareVersion_t, certificate_of_authenticity_length), +} uavcan_HardwareVersionConsts_t; + +typedef begin_packed_struct struct uavcan_GetNodeInfo_request_t { + uint8_t empty[CanPayloadLength]; +} end_packed_struct uavcan_GetNodeInfo_request_t; + +/* GetNodeInfo Response */ + +typedef begin_packed_struct struct uavcan_GetNodeInfo_response_t { + + uavcan_NodeStatus_t nodes_status;; + uavcan_SoftwareVersion_t software_version; + uavcan_HardwareVersion_t hardware_version; + + uint8_t name[PayloadLengthGetNodeInfoname]; + uint8_t name_length; +} end_packed_struct uavcan_GetNodeInfo_response_t; + +typedef enum uavcan_GetNodeInfoConsts_t { + FixedSizeGetNodeInfo = PackedSizeMsgNodeStatus + PackedSizeSoftwareVersion + FixedSizeHardwareVersion, + +} uavcan_GetNodeInfoConsts_t; + +/**************************************** + * Uavcan LogMessage + ****************************************/ + +typedef enum uavcan_LogMessageConsts_t { + LOGMESSAGE_LEVELDEBUG = 0, + LOGMESSAGE_LEVELINFO = 1, + LOGMESSAGE_LEVELWARNING = 2, + LOGMESSAGE_LEVELERROR = 3, +} uavcan_LogMessageConsts_t; + +typedef begin_packed_struct struct uavcan_LogMessage_t { + uint8_t level; + uint8_t source[uavcan_byte_count(LogMessage, source)]; + uint8_t text[uavcan_byte_count(LogMessage, text)]; +} end_packed_struct uavcan_LogMessage_t; + +CCASSERT(sizeof(uavcan_LogMessage_t) == PackedSizeMsgLogMessage); + +/**************************************** + * Uavcan Allocation + ****************************************/ + +typedef enum uavcan_AllocationConsts_t { + MAX_REQUEST_PERIOD_MS = 1400, + MIN_REQUEST_PERIOD_MS = 600, + MAX_FOLLOWUP_DELAY_MS = 400, + MIN_FOLLOWUP_DELAY_MS = 0, + FOLLOWUP_TIMEOUT_MS = 500, + MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 6, + ANY_NODE_ID = 0, + PriorityAllocation = UavcanPriorityMin - 1, +} uavcan_AllocationConsts_t; + +typedef begin_packed_struct struct uavcan_Allocation_t { + uint8_t node_id; /* bottom bit is the first part flag */ + uint8_t unique_id[PayloadLengthAllocationunique_id]; +} end_packed_struct uavcan_Allocation_t; + + +/**************************************** + * Uavcan Path + ****************************************/ + + +typedef begin_packed_struct struct uavcan_Path_t { + uint8_t u8[PayloadLengthPathpath]; +} uavcan_Path_t; + +typedef enum uavcan_PathConst_t { + SEPARATOR = '/', +} end_packed_struct uavcan_PathConst_t; + +/**************************************** + * Uavcan GetInfo Composition + ****************************************/ + +typedef enum uavcan_ErrorConst_t { + FILE_ERROR_OK = 0, + FILE_ERROR_UNKNOWN_ERROR = 32767, + FILE_ERROR_NOT_FOUND = 2, + FILE_ERROR_IO_ERROR = 5, + FILE_ERROR_ACCESS_DENIED = 13, + FILE_ERROR_IS_DIRECTORY = 21, + FILE_ERROR_INVALID_VALUE = 22, + FILE_ERROR_FILE_TOO_LARGE = 27, + FILE_ERROR_OUT_OF_SPACE = 28, + FILE_ERROR_NOT_IMPLEMENTED = 38, +} uavcan_ErrorConst_t; + +typedef begin_packed_struct struct uavcan_Error_t { + uint16_t value; +} end_packed_struct uavcan_Error_t; + +typedef enum uavcan_EntryTypeConst_t { + ENTRY_TYPE_FLAG_FILE = 1, + ENTRY_TYPE_FLAG_DIRECTORY = 2, + ENTRY_TYPE_FLAG_SYMLINK = 4, + ENTRY_TYPE_FLAG_READABLE = 8, + ENTRY_TYPE_FLAG_WRITEABLE = 16, +} uavcan_EntryTypeConst_t; + +typedef begin_packed_struct struct uavcan_EntryType_t { + uint8_t flags; +} end_packed_struct uavcan_EntryType_t; + + +/**************************************** + * Uavcan BeginFirmwareUpdate + ****************************************/ + +typedef enum uavcan_BeginFirmwareUpdateConst_t { + ERROR_OK = 0, + ERROR_INVALID_MODE = 1, + ERROR_IN_PROGRESS = 2, + ERROR_UNKNOWN = 255, +} uavcan_BeginFirmwareUpdateConst_t; + +typedef begin_packed_struct struct uavcan_BeginFirmwareUpdate_request { + uint8_t source_node_id; + uavcan_Path_t image_file_remote_path; +} end_packed_struct uavcan_BeginFirmwareUpdate_request; + +typedef begin_packed_struct struct uavcan_BeginFirmwareUpdate_response { + uint8_t error; +} end_packed_struct uavcan_BeginFirmwareUpdate_response; + + + +/**************************************** + * Uavcan GetInfo + ****************************************/ + +typedef begin_packed_struct struct uavcan_GetInfo_request_t { + uavcan_Path_t path; +} uavcan_GetInfo_request_t; +typedef enum uavcan_GetInfo_requestConst_t { + FixedSizeGetInfoRequest = 0, + +} end_packed_struct uavcan_GetInfo_requestConst_t; + +typedef begin_packed_struct struct uavcan_GetInfo_response_t { + uint32_t size; + uint8_t msbsize; + uavcan_Error_t error; + uavcan_EntryType_t entry_type; +} end_packed_struct uavcan_GetInfo_response_t; + + +/**************************************** + * Uavcan Read Composition + ****************************************/ + +typedef begin_packed_struct struct uavcan_Read_request_t { + uint32_t offset; + uint8_t msboffset; + uavcan_Path_t path; +} end_packed_struct uavcan_Read_request_t; + + +typedef enum uavcan_ReadRequestConsts_t { + FixedSizeReadRequest = sizeof_member(uavcan_Read_request_t, offset) + \ + sizeof_member(uavcan_Read_request_t, msboffset), +} uavcan_ReadRequestConsts_t; + + +typedef begin_packed_struct struct uavcan_Read_response_t { + uavcan_Error_t error; + uint8_t data[PayloadLengthReaddata]; +} end_packed_struct uavcan_Read_response_t; + +/**************************************************************************** + * Global Variables + ****************************************************************************/ +extern uint8_t g_this_node_id; +extern uint8_t g_server_node_id; +extern uint8_t g_uavcan_priority; +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ +/**************************************************************************** + * Name: uavcan_pack_GetNodeInfo_response + * + * Description: + * This function packs the data of a uavcan_NodeStatus_t into + * a uavcan_GetNodeInfo_response_t structure as array of bytes. + * Then it packs the uavcan_GetNodeInfo_response_t + * + * Input Parameters: + * response The uavcan_GetNodeInfo_response_t to be packed + * node_status - The uavcan_NodeStatus_t that is part of the composition + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ + +size_t uavcan_pack_GetNodeInfo_response(uavcan_GetNodeInfo_response_t + *response); + +/**************************************************************************** + * Name: uavcan_tx_dsdl + * + * Description: + * This helper function sends a uavcan service protocol, it + * assumes the protocol object has the destination node id set. + * + * Input Parameters: + * dsdl - An Uavcan DSDL Identifier (Auto Generated) + * protocol - A pointer to a uavcan_protocol_t to configure the send, + * the transfer with the dest_node_id set to that of the + * node we are making the request of. + * transfer - A pointer to the packed data of the transfer to be sent. + * length - The number of bytes of data + * +* Returned value: + * The UavcanOk of the data sent. Anything else indicates if a timeout + * occurred. + * + ****************************************************************************/ + +uavcan_error_t uavcan_tx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol, + const uint8_t *transfer, size_t transfer_length); + +/**************************************************************************** + * Name: uavcan_rx_dsdl + * + * Description: + * This function receives a uavcan Service response protocol transfer + * + * Input Parameters: + * dsdl - An Uavcan DSDL Identifier (Auto Generated) + * protocol - A pointer to a uavcan_protocol_t to configure the receive, + * based the dsdl for the DTID Service. + * If the request must come from a specific server + * then protocol->ser.source_node_id, should be set + * to that node id; + * + * in_out_transfer_length - The number of bytes of data to receive and the + * number received. + * timeout_ms - The amount of time in mS to wait for the initial transfer + * + * Returned value: + * None + * + ****************************************************************************/ +uavcan_error_t uavcan_rx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol, + uint8_t *transfer, size_t *in_out_transfer_length, + uint32_t timeout_ms); + +/**************************************************************************** + * Name: uavcan_tx_log_message + * + * Description: + * This functions sends uavcan LogMessage type data. The Source will be + * taken from the application defined debug_log_source + * + * Input Parameters: + * level - Log Level of the LogMessage Constants DEBUG, INFO, WARN, ERROR + * stage - The Stage the application is at. see Aplication defined + * LOGMESSAGE_STAGE_x + * status - The status of that stage. See Application defined + * LOGMESSAGE_RESULT_x + * + * Returned Value: + * None + * + ****************************************************************************/ +/* The application must define this */ +extern const uint8_t debug_log_source[uavcan_byte_count(LogMessage, source)]; + +void uavcan_tx_log_message(uavcan_LogMessageConsts_t level, uint8_t stage, + uint8_t status); + +/**************************************************************************** + * Name: uavcan_tx_allocation_message + * + * Description: + * This function sends a uavcan allocation message. + * + * Input Parameters: + * requested_node_id - This node's preferred node id 0 for no preference. + * unique_id_length - This node's length of it's unique identifier. + * unique_id - A pointer to the bytes that represent unique + * identifier. + * unique_id_offset - The offset equal 0 or the number of bytes in the + * the last received message that matched the unique ID + * field. + * random - The value to use as the discriminator of the + * anonymous message + * + * Returned value: + * None + * + ****************************************************************************/ + +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, + uint16_t random); diff --git a/platforms/nuttx/src/canbootloader/include/uavcan_can_id_defs.h b/platforms/nuttx/src/canbootloader/include/uavcan_can_id_defs.h new file mode 100644 index 000000000000..8a3cff8551ec --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/uavcan_can_id_defs.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* This file not a typical h file, is defines the UAVCAN dsdl + * usage and may be included several times in header or source + * file + */ + +/* UAVCAN_BIT_DEFINE( field_name, lsb_pos, length) */ + +/* Message Frame Format */ +UAVCAN_BIT_DEFINE(UavCanMessagePriority, 24, 5) +UAVCAN_BIT_DEFINE(UavCanMessageTypeID, 8, 16) +UAVCAN_BIT_DEFINE(UavCanMessageServiceNotMessage, 7, 1) +UAVCAN_BIT_DEFINE(UavCanMessageSourceNodeID, 0, 7) +/* Anonymous message Frame Format */ +UAVCAN_BIT_DEFINE(UavCanAnonMessagePriority, 24, 5) +UAVCAN_BIT_DEFINE(UavCanAnonMessageDiscriminator, 10, 14) +UAVCAN_BIT_DEFINE(UavCanAnonMessageTypeID, 8, 2) +UAVCAN_BIT_DEFINE(UavCanAnonMessageServiceNotMessage, 7, 1) +UAVCAN_BIT_DEFINE(UavCanAnonMessageSourceNodeID, 0, 7) +/* Service Frame Format */ +UAVCAN_BIT_DEFINE(UavCanServicePriority, 24, 5) +UAVCAN_BIT_DEFINE(UavCanServiceTypeID, 16, 8) +UAVCAN_BIT_DEFINE(UavCanServiceRequestNotResponse, 15, 1) +UAVCAN_BIT_DEFINE(UavCanServiceDestinationNodeID, 8, 7) +UAVCAN_BIT_DEFINE(UavCanServiceServiceNotMessage, 7, 1) +UAVCAN_BIT_DEFINE(UavCanServiceSourceNodeID, 0, 7) +/* Tail Format */ +UAVCAN_BIT_DEFINE(UavCanStartOfTransfer, 7, 1) +UAVCAN_BIT_DEFINE(UavCanEndOfTransfer, 6, 1) +UAVCAN_BIT_DEFINE(UavCanToggle, 5, 1) +UAVCAN_BIT_DEFINE(UavCanTransferID, 0, 5) diff --git a/platforms/nuttx/src/canbootloader/include/uavcan_dsdl_defs.h b/platforms/nuttx/src/canbootloader/include/uavcan_dsdl_defs.h new file mode 100644 index 000000000000..1687cac4f8a0 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/uavcan_dsdl_defs.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* This file not a typical h file, is defines the UAVCAN dsdl + * usage and may be included several times in header or source + * file + */ + +/* Components */ + +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(CRC, lsb, 0, 8, 0, 1) +UAVCAN_DSDL_BIT_DEF(CRC, msb, 0, 8, 1, 1) +UAVCAN_DSDL_BIT_DEF(CRC, data, 0, 8, 2, 4) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_TYPE_DEF(Path, 0, 0, 200, NA, NA, + NA, NA) +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(Path, path, 0, 8, 0, + 200) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_TYPE_DEF(SoftwareVersion, 0, 0, 15, NA, NA, + NA, NA) +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(SoftwareVersion, major, 0, 8, 0, + 1) +UAVCAN_DSDL_BIT_DEF(SoftwareVersion, minor, 0, 8, 1, + 1) +UAVCAN_DSDL_BIT_DEF(SoftwareVersion, optional_field_flags, 0, 8, 2, + 1) +UAVCAN_DSDL_BIT_DEF(SoftwareVersion, vcs_commit, 0, 32, 3, + 4) +UAVCAN_DSDL_BIT_DEF(SoftwareVersion, image_crc, 0, NA, 7, + 8) // NA becuase bit mask is 64 + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_TYPE_DEF(HardwareVersion, 0, 0, NA, NA, NA, + NA, NA) +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(HardwareVersion, unique_id, 0, 8, 0, + 16) +UAVCAN_DSDL_BIT_DEF(HardwareVersion, certificate_of_authenticity, 0, 8, 0, + 255) +END_COMPONENTS + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_MESG_DEF(Allocation, 1, 0xf258, 0, MailBox0, Fifo0, + MultiFrameTailInit, SingleFrameTailInit) /* Message */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(Allocation, node_id, 1, 7, 0, + 1) +UAVCAN_DSDL_BIT_DEF(Allocation, first_part_of_unique_id, 0, 1, 0, + 1) +UAVCAN_DSDL_BIT_DEF(Allocation, unique_id, 0, 8, 1, + 16) + + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_MESG_DEF(NodeStatus, 341, 0xbe5f, 7, MailBox1, FifoNone, + SingleFrameTailInit, SingleFrameTailInit) /* Message */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(NodeStatus, uptime_sec, 0, 32, 0, + 4) +UAVCAN_DSDL_BIT_DEF(NodeStatus, health, 6, 2, 5, + 1) +UAVCAN_DSDL_BIT_DEF(NodeStatus, mode, 3, 3, 5, + 1) +UAVCAN_DSDL_BIT_DEF(NodeStatus, sub_mode, 0, 3, 5, + 1) +UAVCAN_DSDL_BIT_DEF(NodeStatus, vendor_specific_status_code, 0, 16, 6, + 2) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_SREQ_DEF(GetNodeInfo, 1, 0xd9a7, 0, MailBox1, Fifo1, + SingleFrameTailInit, MultiFrameTailInit) /* Request */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) Empty */ + +UAVCAN_DSDL_SRSP_DEF(GetNodeInfo, 1, 0xd9a7, 0, MailBox1, Fifo1, + SingleFrameTailInit, MultiFrameTailInit) /* Response */ +UAVCAN_DSDL_BIT_DEF(GetNodeInfo, status, 0, 8, 0, + PackedSizeMsgNodeStatus) +UAVCAN_DSDL_BIT_DEF(GetNodeInfo, software_version, 0, 8, NA, + NA) +UAVCAN_DSDL_BIT_DEF(GetNodeInfo, hardware_version, 0, 8, NA, + NA) +UAVCAN_DSDL_BIT_DEF(GetNodeInfo, name, 0, 8, NA, + 80) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_SREQ_DEF(BeginFirmwareUpdate, 40, 0x729e, 0, MailBox0, Fifo0, + MultiFrameTailInit, SingleFrameTailInit) /* Request */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, source_node_id, 0, 8, 0, + 1) +UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, image_file_remote_path, 0, 8, 1, + PayloadLengthPathpath) + +UAVCAN_DSDL_SRSP_DEF(BeginFirmwareUpdate, 40, 0x729e, 0, MailBox0, Fifo0, + MultiFrameTailInit, SingleFrameTailInit) /* Response */ +UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, error, 0, 8, 0, + 1) +UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, optional_error_message, 0, 8, 1, + 128) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_SREQ_DEF(GetInfo, 45, 0x14b9, 0, MailBox0, Fifo0, + SingleFrameTailInit, MultiFrameTailInit) /* Request */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(GetInfo, path, 0, 8, 0, + PayloadLengthPathpath) + +UAVCAN_DSDL_SRSP_DEF(GetInfo, 45, 0x14b9, 0, MailBox0, Fifo0, + SingleFrameTailInit, MultiFrameTailInit) /* Response */ +UAVCAN_DSDL_BIT_DEF(GetInfo, size, 0, 32, 0, + 4) /* Response */ +UAVCAN_DSDL_BIT_DEF(GetInfo, sizemsb, 0, 8, 4, + 1) +UAVCAN_DSDL_BIT_DEF(GetInfo, error, 0, 16, 5, + 2) +UAVCAN_DSDL_BIT_DEF(GetInfo, entry_type, 0, 8, 7, + 1) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_SREQ_DEF(Read, 48, 0x2f12, 0, MailBox0, Fifo0, + SingleFrameTailInit, MultiFrameTailInit) /* Request */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(Read, offset, 0, 32, 0, + 4) /* Request */ +UAVCAN_DSDL_BIT_DEF(Read, msboffset, 0, 8, 4, + 1) +UAVCAN_DSDL_BIT_DEF(Read, path, 0, 8, 5, + PayloadLengthPathpath) + +UAVCAN_DSDL_SRSP_DEF(Read, 48, 0x2f12, 0, MailBox0, Fifo0, + SingleFrameTailInit, MultiFrameTailInit) // Responce +UAVCAN_DSDL_BIT_DEF(Read, error, 0, 16, 0, + 2) /* Response */ +UAVCAN_DSDL_BIT_DEF(Read, data, 0, 8, 2, + 256) + +/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */ +UAVCAN_DSDL_MESG_DEF(LogMessage, 16383, 0x4570, 7, MailBox0, Fifo0, + NA, SingleFrameTailInit) /* Message */ +/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */ +UAVCAN_DSDL_BIT_DEF(LogMessage, level, 5, 3, 0, + 1) +UAVCAN_DSDL_BIT_DEF(LogMessage, source_length, 0, 5, 0, + 1) +UAVCAN_DSDL_BIT_DEF(LogMessage, source, 0, 8, 1, + 4) // Bootloader specific uses is 4 +UAVCAN_DSDL_BIT_DEF(LogMessage, text, 0, 8, 5, + 2) // Bootloader specific uses is 2 diff --git a/platforms/nuttx/src/canbootloader/include/visibility.h b/platforms/nuttx/src/canbootloader/include/visibility.h new file mode 100644 index 000000000000..00fa1a78e707 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/visibility.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file visibility.h + * + * Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#pragma once + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif + + +#ifdef __PX4_NUTTX +/* On NuttX we call clearenv() so we cannot use getenv() and others (see px4_task_spawn_cmd() in px4_nuttx_tasks.c). + * We need to include the headers declaring getenv() before the pragma, otherwise it will trigger a poison error. + */ +#include +#ifdef __cplusplus +#include +#endif +#pragma GCC poison getenv setenv putenv +#endif /* __PX4_NUTTX */ diff --git a/platforms/nuttx/src/canbootloader/protocol/uavcan.c b/platforms/nuttx/src/canbootloader/protocol/uavcan.c new file mode 100644 index 000000000000..57441eab4ac0 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/protocol/uavcan.c @@ -0,0 +1,814 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include "boot_config.h" + +#include +#include +#include + +#include "chip.h" +#include "stm32.h" + +#include "timer.h" +#include "uavcan.h" +#include "can.h" +#include "crc.h" + +#define CAN_REQUEST_TIMEOUT 1000 +#define ANY_NODE_ID 0 +#define THIS_NODE 1 +#define FW_SERVER 2 +#define REQ_NODE 3 + +typedef begin_packed_struct struct dsdl_t { + uavcan_protocol_t prototype; + uint16_t signature_crc16; + uint8_t intail; + uint8_t outtail; + uint8_t mailbox : 2; + uint8_t fifo : 2; +} end_packed_struct dsdl_t; + +/* Values used in filter initialization */ + +typedef enum uavcan_transfer_stage_t { + Initialization, + OnStartOfTransfer +} uavcan_transfer_stage_t; + +typedef enum uavcan_dsdl_ignore_t { + sizeDSDLTransfers = SizeDSDL - SizeDSDLComponents + 1 +} uavcan_dsdl_ignore_t; + + +/* Forward declaration */ +extern const dsdl_t dsdl_table[sizeDSDLTransfers]; + +uint8_t g_uavcan_priority = PriorityAllocation; +uint8_t g_this_node_id = 0; +uint8_t g_server_node_id = 0; + +static inline uavcan_dsdl_t getDSDLOffset(uavcan_dsdl_t full) { return full - (SizeDSDLComponents + 1); } + +/**************************************************************************** + * Name: uavcan_init_comapare_masks + * + * Description: + * This function builds the masks needed for filtering Transfers + * + * Input Parameters: + * stage - Either Initialization, OnStartOfTransfer + * protocol - A pointer to a uavcan_protocol_t to configure the filters + * masks - A pointer to a uavcan_protocol_t return the filters + * + * Returned value: + * None + * + ****************************************************************************/ +static void uavcan_init_comapare_masks(uavcan_transfer_stage_t stage, uavcan_protocol_t *protocol, + uavcan_protocol_t *masks) +{ + switch (stage) { + default: + break; + + case OnStartOfTransfer: + masks->id.u32 |= uavcan_protocol_mask(UavCanMessageSourceNodeID); + masks->tail_init.u8 |= uavcan_protocol_mask(UavCanTransferID); + break; + + + case Initialization: + + masks->tail_init.u8 = uavcan_protocol_mask(UavCanToggle) + | uavcan_protocol_mask(UavCanStartOfTransfer); + + if (protocol->msg.service_not_message) { + + /* Specific Filter initialization for a Services */ + + + masks->id.u32 = uavcan_protocol_mask(UavCanServiceTypeID) + | uavcan_protocol_mask(UavCanServiceRequestNotResponse) + | uavcan_protocol_mask(UavCanServiceDestinationNodeID) + | uavcan_protocol_mask(UavCanServiceServiceNotMessage); + + if (protocol->ser.source_node_id != ANY_NODE_ID) { + + masks->id.u32 |= uavcan_protocol_mask(UavCanMessageSourceNodeID); + + } + + + } else { + + /* Specific Filter initialization for a Message */ + + /* Is it anonymous? */ + + if (protocol->msg.source_node_id == ANY_NODE_ID) { + + /* Intentional use of UavCanMessageTypeID because the response + * to the anonymous message is a message and the discriminator + * will be 0 + */ + masks->id.u32 = uavcan_protocol_mask(UavCanMessageTypeID) + | uavcan_protocol_mask(UavCanAnonMessageServiceNotMessage); + + } else { + + + masks->id.u32 = uavcan_protocol_mask(UavCanMessageTypeID) + | uavcan_protocol_mask(UavCanMessageServiceNotMessage) + | uavcan_protocol_mask(UavCanMessageSourceNodeID); + } + } + + break; + } +} + +/**************************************************************************** + * Name: load_dsdl_protocol + * + * Description: + * This function builds the protocol for a given dsdl + * N.B The value set for a service is request. + * + * Input Parameters: + * dsdl - An uavcan DSDL Identifier (Auto Generated) + * direction_in_not_out - in or out bound + * protocol - A pointer to a uavcan_protocol_t to configure the filters + * masks - A pointer to a uavcan_protocol_t return the filters + * + * Returned value: + * None + * + ****************************************************************************/ +static const dsdl_t *load_dsdl_protocol(uavcan_dsdl_t dsdl, uavcan_direction_t rx_not_tx, uavcan_protocol_t *protocol, + uint8_t that_node_id) +{ + const dsdl_t *pdsdl = &dsdl_table[getDSDLOffset(dsdl)]; + + protocol->id.u32 = pdsdl->prototype.id.u32; + protocol->msg.priority = g_uavcan_priority; + + /* Preserver the transfer_id */ + protocol->tail_init.u8 &= uavcan_protocol_mask(UavCanTransferID); + + if (rx_not_tx) { + /* Rx */ + protocol->tail_init.u8 |= pdsdl->intail; + + /* Service */ + if (pdsdl->prototype.msg.service_not_message) { + + protocol->ser.dest_node_id = g_this_node_id; + protocol->ser.source_node_id = that_node_id; + } + + } else { + /* + * Tx + * All Transfers sent have .source_node_id set + * to this node's id + * During allocation this value will be + * 0 + */ + protocol->tail_init.u8 |= pdsdl->outtail; + protocol->msg.source_node_id = g_this_node_id; + + /* + * All Service Transfer sent have + * The ser.dest_node_id + */ + if (pdsdl->prototype.msg.service_not_message) { + protocol->ser.dest_node_id = that_node_id; + } + } + + return pdsdl; +} + +/**************************************************************************** + * Name: uavcan_tx + * + * Description: + * This function sends a single uavcan protocol based frame applying + * the tail byte. + * + * Input Parameters: + * protocol - A pointer to a uavcan_protocol_t to configure the send + * frame_data - A pointer to 8 bytes of data to be sent (all 8 bytes will be + * loaded into the CAN transmitter but only length bytes will + * be sent. + * length - The number of bytes of the frame_date (DLC field) + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * The UavcanOk of the data sent. Anything else indicates if a timeout + * occurred. + * + ****************************************************************************/ +CCASSERT((int)UavcanOk == (int)CAN_OK); + +static uavcan_error_t uavcan_tx(uavcan_protocol_t *protocol, uint8_t *frame_data, size_t length, uint8_t mailbox) +{ + frame_data[length++] = protocol->tail_init.u8; + return can_tx(protocol->id.u32, length, frame_data, mailbox); +} + +/**************************************************************************** + * Name: uavcan_tx_dsdl + * + * Description: + * This function sends a uavcan protocol transfer. For a Service + * The natural case for a Service Response is to send it to the Requestor + * + * Therefore, this function assumes that id this is a Service Transfer, + * the protocol object has the destination node id set in + * protocol->ser.source_node_id. + * + * Input Parameters: + * dsdl - An Uavcan DSDL Identifier (Auto Generated) + * protocol - A pointer to a uavcan_protocol_t to configure the send, + * For a service transfer the desination node id set in + * ser.source_node_id to that of the node we are sending to. + * transfer - A pointer to the packed data of the transfer to be sent. + * length - The number of bytes of data + * + * Returned value: + * The UavcanOk of the data sent. Anything else indicates if a timeout + * occurred. + * + ****************************************************************************/ + +uavcan_error_t uavcan_tx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol, const uint8_t *transfer, + size_t transfer_length) +{ + /* + * Since we do not discriminate between sending a Message or a + * Service (Request or Response) + * We assume this is a response from a rx request and msg.source_node_id + * is that of the requester and that will become the ser.dest_node_id + */ + const dsdl_t *pdsdl = load_dsdl_protocol(dsdl, OutBound, protocol, protocol->ser.source_node_id); + + uint8_t payload[CanPayloadLength]; + + /* + * Only prepend CRC if the transfer will not fit within a single frame + */ + uint32_t m = 0; + + if (transfer_length > MaxUserPayloadLength) { + uint16_t transfer_crc = crc16_signature(pdsdl->signature_crc16, transfer_length, transfer); + payload[PayloadOffsetCRClsb] = (uint8_t)transfer_crc; + payload[PayloadOffsetCRCmsb] = (uint8_t)(transfer_crc >> 8u); + m = PayloadOffsetCRCdata; + } + + for (uint32_t i = 0; i < transfer_length; i++) { + + payload[m++] = transfer[i]; + + /* Is this the last byte? */ + + protocol->tail.eot = (i == (transfer_length - 1)); + + /* Either end of user portion of payload or last byte */ + if (m == MaxUserPayloadLength || protocol->tail.eot) { + uavcan_error_t rv = uavcan_tx(protocol, payload, m, pdsdl->mailbox); + + if (rv != UavcanOk) { + return rv; + } + + /* Increment 1 bit frame sequence number */ + + protocol->tail.toggle ^= true; + protocol->tail.sot = false; + + m = 0; + } + } + + return UavcanOk; +} + +/**************************************************************************** + * Name: uavcan_rx + * + * Description: + * This function is called to receive a single CAN frame from the supplied + * fifo. It does not block if there is not available, but returns 0 + * + * Input Parameters: + * protocol - A pointer to a uavcan_protocol_t to return the CAN ID and + * Tail byte + * frame_data - A pointer to return 8 bytes of the frame's data + * (all 8 bytes will be read from the CAN fifo but + * only length bytes will valid. + * length - A pointer to return the number of bytes of the frame_data + * (DLC field) + * fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo. + * + * Returned value: + * The length of the data read or 0 if the fifo was empty + * + ****************************************************************************/ +//#define DEBUG_DTID 1 +#define DEBUG_DTID_TRIGGER 4 +#ifdef DEBUG_DTID +int trigger = DEBUG_DTID_TRIGGER; +int id = DEBUG_DTID; +bool msg = true; +bool svc = false; + +#endif + +static uint8_t uavcan_rx(uavcan_protocol_t *protocol, uint8_t *frame_data, size_t *length, uint8_t fifo) +{ + uint8_t rv = can_rx(&protocol->id.u32, length, frame_data, fifo); + + if (rv) { + + /* Remove the tail byte from length */ + + *length -= 1; + + /* Save the tail byte from the last byte of frame*/ + + protocol->tail_init.u8 = frame_data[*length]; +#ifdef DEBUG_DTID +#pragma message("!!!!!!! DEBUG_DTID is enabled !!!!") + static volatile int count = 0; + + if ((msg && protocol->msg.type_id == id) + || (svc && protocol->ser.type_id == id) + ) { + if (count++ == trigger) { + count = 0; + static volatile int j = 0; + j++; + } + } + +#endif + } + + return rv; +} + + +/**************************************************************************** + * Name: uavcan_rx_dsdl + * + * Description: + * This function receives a uavcan protocol transfer. + * For a Service the natural case for a Service Request is to receive from + * 1) Any Node + * 2) The established Server. + * + * Therefore, this function assumes that id this is a Service Transfer, + * the protocol object has the destination node id set in + * protocol->ser.source_node_id. + * + * + * Input Parameters: + * dsdl - An Uavcan DSDL Identifier (Auto Generated) + * protocol - A pointer to a uavcan_protocol_t to configure the receive, + * based the dsdl for the DTID Service. + * If the request must come from a specific server + * then protocol->ser.source_node_id, should be set + * to that node id; + * + * in_out_transfer_length - The number of bytes of data to receive and the + * number received. + * timeout_ms - The amount of time in mS to wait for the initial transfer + * + * Returned value: + * uavcan_error_t + * + ****************************************************************************/ + +uavcan_error_t uavcan_rx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol, + uint8_t *transfer, size_t *in_out_transfer_length, + uint32_t timeout_ms) +{ + const dsdl_t *pdsdl = load_dsdl_protocol(dsdl, InBound, protocol, protocol->msg.source_node_id); + bl_timer_id timer = timer_allocate(modeTimeout | modeStarted, timeout_ms, 0); + + /* + * Set up comparison masks + * In the tail we care about the initial state sot, eot and toggle + * In the CAN ID We care about the Service Type ID, RequestNotResponse, + * ServiceNotMessage, and the destination id + * If the source id is not ANY_NODE_ID, that that must match too. + */ + + uavcan_protocol_t masks; + + uavcan_init_comapare_masks(Initialization, protocol, &masks); + + uint8_t timeout = false; + uint16_t transfer_crc = 0; + size_t total_rx = 0; + uavcan_protocol_t rx_protocol; + + do { + uint8_t payload[CanPayloadLength]; + size_t rx_length; + + if (!uavcan_rx(&rx_protocol, payload, &rx_length, pdsdl->fifo) + || BadTailState == (rx_protocol.tail_init.u8 & BadTailState) + || ((rx_protocol.id.u32 ^ protocol->id.u32) & masks.id.u32) + || ((rx_protocol.tail_init.u8 ^ protocol->tail_init.u8) & masks.tail_init.u8)) { + continue; + } + + + timer_restart(timer, timeout_ms); + + /* + If this is the first frame, capture the actual source node ID and + transfer ID for future comparisons + */ + size_t payload_index = 0; + + /* Is this the start of transfer? */ + if (rx_protocol.tail.sot) { + + /* + * We expect only one Start Of Transfer per transfer + * So knock it down + */ + protocol->tail.sot = false; + + /* Discard any data */ + total_rx = 0; + + /* + * Capture the source node id + * and the Transfer Id + */ + protocol->msg.source_node_id = rx_protocol.msg.source_node_id; + protocol->tail.transfer_id = rx_protocol.tail.transfer_id; + + /* + * This is the start of transfer - update the + * masks for this phase or the transfer were + * source_node_id is known and transfer_id + * matters. From now on match source node and + * transfer ID too + */ + uavcan_init_comapare_masks(OnStartOfTransfer, protocol, &masks); + + /* Is this a multi-frame transfer? */ + if (rx_protocol.tail.eot == false) { + + /* Capture the frame CRC */ + transfer_crc = uavcan_make_uint16(payload[PayloadOffsetCRClsb], payload[PayloadOffsetCRCmsb]); + + /* + * When the CRC is prepended to the payload + * the index of the data is past the CRC + */ + payload_index = PayloadOffsetCRCdata; + rx_length -= PayloadOffsetCRCdata; + } + } + + + if (rx_protocol.tail.transfer_id > protocol->tail.transfer_id + || total_rx >= *in_out_transfer_length) { + + uavcan_init_comapare_masks(Initialization, protocol, &masks); + protocol->tail.sot = true; + protocol->tail.toggle = false; + + } else if (rx_protocol.tail.transfer_id < protocol->tail.transfer_id) { + continue; + } + + /* Copy transfer bytes to the transfer */ + + if (total_rx + rx_length <= *in_out_transfer_length) { + memcpy(&transfer[total_rx], &payload[payload_index], rx_length); + total_rx += rx_length; + } + + /* Increment 1 bit frame sequence number */ + + protocol->tail.toggle ^= true; + + /* Is this the end of the transfer ?*/ + + if (rx_protocol.tail.eot) { + + /* Return length of data received */ + + *in_out_transfer_length = total_rx; + + break; + } + } while (!(timeout = timer_expired(timer))); + + timer_free(timer); + + return (!timeout && (rx_protocol.tail.sot + || transfer_crc == crc16_signature(pdsdl->signature_crc16, total_rx, transfer)) + ? UavcanOk : UavcanError); +} + +/**************************************************************************** + * Name: uavcan_pack_GetNodeInfo_response + * + * Description: + * This function packs the data of a uavcan_NodeStatus_t into + * a uavcan_GetNodeInfo_response_t structure as array of bytes. + * Then it packs the uavcan_GetNodeInfo_response_t + * + * Input Parameters: + * response The uavcan_GetNodeInfo_response_t to be packed + * node_status - The uavcan_NodeStatus_t that is part of the composition + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ +size_t uavcan_pack_GetNodeInfo_response(uavcan_GetNodeInfo_response_t *response) +{ + size_t contiguous_length = FixedSizeGetNodeInfo + \ + response->hardware_version.certificate_of_authenticity_length; + /* + * Move name so it's contiguous with the end of certificate_of_authenticity[length] + * which very well may be just after certificate_of_authenticity_length if the length + * of the certificate_of_authenticity is 0 + */ + memcpy(&((uint8_t *)response)[contiguous_length], response->name, response->name_length); + return contiguous_length + response->name_length; +} + +/**************************************************************************** + * Name: uavcan_pack_LogMessage + * + * Description: + * This function formats the data of a uavcan_logmessage_t structure into + * an array of bytes. + * + * Input Parameters: + * external - The array of bytes to populate. + * internal - The uavcan_logmessage_t to pack into the data + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ +static size_t uavcan_pack_LogMessage(uint8_t *external, const uavcan_LogMessage_t *internal) +{ + /* Pack the 3 bit level in top bits followed by the length of source */ + external[uavcan_byte_offset(LogMessage, level)] = uavcan_ppack(internal, LogMessage, level) \ + | uavcan_pack(uavcan_byte_count(LogMessage, source), LogMessage, source_length); + memcpy(&external[uavcan_byte_offset(LogMessage, source)], internal->source, + PackedSizeMsgLogMessage - sizeof_member(uavcan_LogMessage_t, level)); + return PackedSizeMsgLogMessage; +} + +/**************************************************************************** + * Name: uavcan_tx_log_message + * + * Description: + * This functions sends uavcan LogMessage type data. The Source will be + * taken from the application defined debug_log_source + * + * Input Parameters: + * level - Log Level of the LogMessage Constants DEBUG, INFO, WARN, ERROR + * stage - The Stage the application is at. see Aplication defined + * LOGMESSAGE_STAGE_x + * status - The status of that stage. See Application defined + * LOGMESSAGE_RESULT_x + * + * Returned Value: + * None + * + ****************************************************************************/ +void uavcan_tx_log_message(uavcan_LogMessageConsts_t level, uint8_t stage, uint8_t status) +{ + static uint8_t transfer_id; + + uavcan_LogMessage_t message; + uavcan_protocol_t protocol; + + protocol.tail.transfer_id = transfer_id++; + + const dsdl_t *dsdl = load_dsdl_protocol(DSDLMsgLogMessage, MessageOut, &protocol, 0); + + message.level = level; + memcpy(&message.source, debug_log_source, sizeof(message.source)); + message.text[0] = stage; + message.text[1] = status; + + uint8_t payload[CanPayloadLength]; + size_t frame_len = uavcan_pack_LogMessage(payload, &message); + uavcan_tx(&protocol, payload, frame_len, dsdl->mailbox); +} + +/**************************************************************************** + * Name: uavcan_tx_allocation_message + * + * Description: + * This function sends a uavcan allocation message. + * + * Input Parameters: + * requested_node_id - This node's preferred node id 0 for no preference. + * unique_id_length - This node's length of it's unique identifier. + * unique_id - A pointer to the bytes that represent unique + * identifier. + * unique_id_offset - The offset equal 0 or the number of bytes in the + * the last received message that matched the unique ID + * field. + * random - The value to use as the discriminator of the + * anonymous message + * + * Returned value: + * None + * + ****************************************************************************/ +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, uint16_t random) +{ + static uint8_t transfer_id; + uavcan_protocol_t protocol; + protocol.tail.transfer_id = transfer_id++; + + const dsdl_t *dsdl = load_dsdl_protocol(DSDLMsgAllocation, MessageOut, &protocol, 0); + + /* Override defaults */ + protocol.ana.discriminator = random; + + uint8_t payload[CanPayloadLength]; + + size_t max_copy = MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + size_t remaining = unique_id_length - unique_id_offset; + + if (max_copy > remaining) { + max_copy = remaining; + } + + payload[uavcan_byte_offset(Allocation, node_id)] = uavcan_pack(requested_node_id, Allocation, node_id) | + (unique_id_offset ? 0 : uavcan_bit_mask(Allocation, first_part_of_unique_id)); + /* + * Copy in the remaining bytes of payload, either filling it + * or on the last chunk partially filling it + */ + memcpy(&payload[uavcan_byte_offset(Allocation, unique_id)], &unique_id[unique_id_offset], max_copy); + + /* Account for the payload[0] */ + max_copy++; + uavcan_tx(&protocol, payload, max_copy, dsdl->mailbox); + can_cancel_on_error(dsdl->mailbox); +} + +/**************************************************************************** + * Private Data - This table is positioned here to not mess up the line + * numbers for the debugger. + ****************************************************************************/ +#define NA 0 +#define END_COMPONENTS + +#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) + +#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + { \ + {\ + .msg = \ + { \ + .source_node_id = 0, \ + .service_not_message = (false), \ + .type_id = (dtid), \ + .priority = 0, \ + }, \ + { \ + .tail_init = \ + { \ + .u8 = (outbound), \ + } \ + } \ + }, \ + signature, \ + inbound, \ + outbound, \ + mailbox, \ + fifo, \ + }, + +#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + { \ + {\ + .ser = \ + { \ + .source_node_id = 0, \ + .service_not_message = (true), \ + .dest_node_id = 0, \ + .request_not_response = (true), \ + .type_id = (dtid), \ + .priority = 0, \ + }, \ + { \ + .tail_init = \ + { \ + .u8 = (outbound), \ + } \ + } \ + }, \ + signature, \ + inbound, \ + outbound, \ + mailbox, \ + fifo, \ + }, + +#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \ + { \ + {\ + .ser = \ + { \ + .source_node_id = 0, \ + .service_not_message = (true), \ + .dest_node_id = 0, \ + .request_not_response = (false), \ + .type_id = (dtid), \ + .priority = 0, \ + }, \ + { \ + .tail_init = \ + { \ + .u8 = (outbound), \ + } \ + } \ + }, \ + signature, \ + inbound, \ + outbound, \ + mailbox, \ + fifo, \ + }, + + +#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) + +const dsdl_t dsdl_table[sizeDSDLTransfers] = { +#include "uavcan_dsdl_defs.h" +}; +#undef UAVCAN_DSDL_TYPE_DEF +#undef UAVCAN_DSDL_SRSP_DEF +#undef UAVCAN_DSDL_SREQ_DEF +#undef UAVCAN_DSDL_MESG_DEF +#undef UAVCAN_DSDL_BIT_DEF +#undef FW_SERVER +#undef REQ_NODE +#undef THIS_NODE +#undef ANY_NODE_ID +#undef END_COMPONENTS +#undef NA diff --git a/platforms/nuttx/src/canbootloader/sched/timer.c b/platforms/nuttx/src/canbootloader/sched/timer.c new file mode 100644 index 000000000000..626a4bcc6fe8 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/sched/timer.c @@ -0,0 +1,434 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +// Turn off Probes in this module +#undef CONFIG_BOARD_USE_PROBES + +#include + +#include +#include +#include + +#include +#include +#include + +#include "px4_macros.h" +#include "timer.h" +#include "nvic.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; + +static time_ms_t sys_tic; +static bl_timer_t timers[OPT_BL_NUMBER_TIMERS]; + +/* Use to initialize */ +const bl_timer_cb_t null_cb = { 0, 0 }; + +/* We use the linker --wrap ability to wrap the NuttX stm32 call out to + * the sceduler's sched_process_timer and service it here. Thus replacing + * the NuttX scheduler with are timer driven scheduling. + */ +void __wrap_sched_process_timer(void); + +/**************************************************************************** + * Name: timer_tic + * + * Description: + * Returns the system tic counter that counts in units of + * (CONFIG_USEC_PER_TICK/1000). By default 10 Ms. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +time_ms_t timer_tic(void) +{ + return sys_tic; +} + +/**************************************************************************** + * Name: sched_process_timer + * + * Description: + * Called by Nuttx on the ISR of the SysTic. This function run the list of + * timers. It deducts that amount of the time of a system tick from the + * any timers that are in use and running. + * + * Depending on the mode of the timer, the appropriate actions is taken on + * expiration. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +__EXPORT void __wrap_sched_process_timer(void) +{ + //PROBE(1, true); + //PROBE(1, false); + + /* Increment the per-tick system counter */ + sys_tic++; + + /* todo:May need a X tick here is threads run long */ + time_ms_t ms_elapsed = (CONFIG_USEC_PER_TICK / 1000); + + /* Walk the time list from High to low and */ + bl_timer_id t; + + for (t = arraySize(timers) - 1; (int8_t) t >= 0; t--) { + + /* Timer in use and running */ + if ((timers[t].ctl & (inuse | running)) == (inuse | running)) { + + /* Is it NOT already expired nor about to expire ?*/ + if (timers[t].count != 0) { + + /* Is it off in future */ + if (timers[t].count > ms_elapsed) { + + /* Just remove the amount attributed to the tick */ + + timers[t].count -= ms_elapsed; + continue; + } + + /* it has expired now or less than a tick ago */ + + /* Mark it expired */ + + timers[t].count = 0; + + /* Now perform action based on mode */ + switch (timers[t].ctl & ~(inuse | running)) { + + 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; + + /* FALLTHROUGH */ + /* to callback */ + case Timeout: + if (timers[t].usr.cb) { + timers[t].usr.cb(t, timers[t].usr.context); + } + + break; + + default: + break; + } + } + } + } +} + +/**************************************************************************** + * Name: timer_allocate + * + * Description: + * Is used to allocate a timer. Allocation does not involve memory + * allocation as the data for the timer are compile time generated. + * See OPT_BL_NUMBER_TIMERS + * + * There is an inherent priority to the timers in that the first timer + * allocated is the first timer run per tick. + * + * There are 3 modes of operation for the timers. All modes support an + * optional call back on expiration. + * + * modeOneShot - Specifies a one-shot timer. After notification timer + * is resource is freed. + * modeRepeating - Specifies a repeating timer that will reload and + * call an optional. + * modeTimeout - Specifies a persistent start / stop timer. + * + * modeStarted - Or'ed in to start the timer when allocated + * + * + * Input Parameters: + * mode - One of bl_timer_modes_t with the Optional modeStarted + * msfromnow - The reload and initial value for the timer in Ms. + * fc - A pointer or NULL (0). If it is non null it can be any + * of the following: + * + * a) A bl_timer_cb_t populated on the users stack or + * in the data segment. The values are copied into the + * internal data structure of the timer and therefore do + * not have to persist after the call to timer_allocate + * + * b) The address of null_cb. This is identical to passing + * null for the value of fc. + * + * Returned Value: + * On success a value from 0 - OPT_BL_NUMBER_TIMERS-1 that is + * the bl_timer_id for subsequent timer operations + * -1 on failure. This indicates there are no free timers. + * + ****************************************************************************/ +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 = enter_critical_section(); + + 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; + } + } + + leave_critical_section(s); + return t; +} + +/**************************************************************************** + * Name: timer_free + * + * Description: + * Is used to free a timer. Freeing a timer does not involve memory + * deallocation as the data for the timer are compile time generated. + * See OPT_BL_NUMBER_TIMERS + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_free(bl_timer_id id) +{ + DEBUGASSERT(id >= 0 && id < arraySize(timers)); + irqstate_t s = enter_critical_section(); + memset(&timers[id], 0, sizeof(timers[id])); + leave_critical_section(s); +} + +/**************************************************************************** + * Name: timer_start + * + * Description: + * Is used to Start a timer. The reload value is copied to the counter. + * And the running bit it set. There is no problem in Starting a running + * timer. But it will restart the timeout. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_start(bl_timer_id id) +{ + DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = enter_critical_section(); + timers[id].count = timers[id].reload; + timers[id].ctl |= running; + leave_critical_section(s); +} + +/**************************************************************************** + * Name: timer_stop + * + * Description: + * Is used to stop a timer. It is Ok to stop a stopped timer. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_stop(bl_timer_id id) +{ + DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = enter_critical_section(); + timers[id].ctl &= ~running; + leave_critical_section(s); + +} + +/**************************************************************************** + * Name: timer_expired + * + * Description: + * Test if a timer that was configured as a modeTimeout timer is expired. + * To be expired the time has to be running and have a count of 0. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * Non Zero if the timer is expired otherwise zero. + * + ****************************************************************************/ +int timer_expired(bl_timer_id id) +{ + DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = enter_critical_section(); + int rv = ((timers[id].ctl & running) && timers[id].count == 0); + leave_critical_section(s); + return rv; +} + +/**************************************************************************** + * Name: timer_restart + * + * Description: + * Is used to re start a timer with a new reload count. The reload value + * is copied to the counter and the running bit it set. There is no + * problem in restarting a running timer. + * + * Input Parameters: + * id - Returned from timer_allocate; + * ms - Is a time_ms_t and the new reload value to use. + * + * Returned Value: + * None. + * + ****************************************************************************/ +void timer_restart(bl_timer_id id, time_ms_t ms) +{ + DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = enter_critical_section(); + timers[id].count = timers[id].reload = ms; + timers[id].ctl |= running; + leave_critical_section(s); +} + +/**************************************************************************** + * Name: timer_ref + * + * Description: + * Returns an time_ref_t that is a reference (ponter) to the internal counter + * of the timer selected by id. It should only be used with calls to + * timer_ref_expired. + * + * Input Parameters: + * id - Returned from timer_allocate; + * + * Returned Value: + * An internal reference that should be treated as opaque by the caller and + * should only be used with calls to timer_ref_expired. + * There is no reference counting on the reference and therefore does not + * require any operation to free it. + * + *************************************************************************/ +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; +} + +/**************************************************************************** + * Name: timer_init + * + * Description: + * Called early in os_start to initialize the data associated with + * the timers + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ +__EXPORT void timer_init(void) +{ + /* For system timing probing see bord.h and + * CONFIG_BOARD_USE_PROBES + */ + //PROBE_INIT(7); + //PROBE(1, true); + //PROBE(2, true); + //PROBE(3, true); + //PROBE(1, false); + //PROBE(2, false); + //PROBE(3, false); + /* This is the lowlevel IO if needed to instrument timing + * with the smallest impact + * *((uint32_t *)0x40011010) = 0x100; // PROBE(3,true); + * *((uint32_t *)0x40011014) = 0x100; // PROBE(3,false); + */ + + /* Initialize timer data */ + + sys_tic = 0; + memset(timers, 0, sizeof(timers)); +} diff --git a/platforms/nuttx/src/canbootloader/uavcan/main.c b/platforms/nuttx/src/canbootloader/uavcan/main.c new file mode 100644 index 000000000000..0405c81518a9 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/uavcan/main.c @@ -0,0 +1,1354 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "boot_config.h" + +#include + +#include +#include +#include +#include + +#include "chip.h" +#include "stm32.h" +#include "nvic.h" + +#include "board.h" +#include "flash.h" +#include "timer.h" +#include "blsched.h" +#include "can.h" +#include "uavcan.h" +#include "random.h" +#include "crc.h" +#include "boot_app_shared.h" + +//#define DEBUG_APPLICATION_INPLACE 1 /* Never leave defined */ +#define DEBUG_NO_FW_UPDATE 1 /* With DEBUG_APPLICATION_INPLACE + * prevents fw update + */ + +/* Using 2 character text in LogMessage */ + +#define LOGMESSAGE_STAGE_INIT 'I' +#define LOGMESSAGE_STAGE_GET_INFO 'G' +#define LOGMESSAGE_STAGE_ERASE 'E' +#define LOGMESSAGE_STAGE_READ 'R' +#define LOGMESSAGE_STAGE_PROGRAM 'P' +#define LOGMESSAGE_STAGE_VALIDATE 'V' +#define LOGMESSAGE_STAGE_FINALIZE 'F' + +#define LOGMESSAGE_RESULT_START 's' +#define LOGMESSAGE_RESULT_FAIL 'f' +#define LOGMESSAGE_RESULT_OK 'o' + +#if defined(DEBUG_APPLICATION_INPLACE) +#pragma message "******** DANGER DEBUG_APPLICATION_INPLACE is DEFINED ******" +#endif + +typedef volatile struct bootloader_t { + can_speed_t bus_speed; + volatile uint8_t health; + volatile uint8_t mode; + volatile uint8_t sub_mode; + 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; + union { + uint32_t l; + uint8_t b[sizeof(uint32_t)]; + } fw_word0; + +} bootloader_t; + +bootloader_t bootloader; + +const uint8_t debug_log_source[uavcan_byte_count(LogMessage, source)] = {'B', 'o', 'o', 't'}; + +/**************************************************************************** + * Name: start_the_watch_dog + * + * Description: + * This function will start the hardware watchdog. Once stated the code must + * kick it before it time out a reboot will occur. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +static inline void start_the_watch_dog(void) +{ +#ifdef OPT_ENABLE_WD +#endif +} +/**************************************************************************** + * Name: kick_the_watch_dog + * + * Description: + * This function will reset the watchdog timeout + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +static inline void kick_the_watch_dog(void) +{ +#ifdef OPT_ENABLE_WD +#endif +} + +/**************************************************************************** + * Name: uptime_process + * + * Description: + * Run as a timer callback off the system tick ISR. This function counts + * Seconds of up time. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ +static void uptime_process(bl_timer_id id, void *context) +{ + bootloader.uptime++; +} + +/**************************************************************************** + * Name: node_info_process + * + * Description: + * Run as a timer callback off the system tick ISR. + * Once we have a node id, this process is started and handles the + * GetNodeInfo replies. This function uses the fifoGetNodeInfo and + * MBGetNodeInfo of the CAN so it can coexist regardless of + * application state and what is it doing. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ +static void node_info_process(bl_timer_id id, void *context) +{ + uavcan_GetNodeInfo_response_t response; + uavcan_GetNodeInfo_request_t request; + + uavcan_protocol_t protocol; + + response.nodes_status.uptime_sec = bootloader.uptime; + response.nodes_status.u8 = uavcan_pack(bootloader.sub_mode, NodeStatus, sub_mode) + | uavcan_pack(bootloader.mode, NodeStatus, mode) + | uavcan_pack(bootloader.health, NodeStatus, health); + response.nodes_status.vendor_specific_status_code = 0u; + + (void)board_get_hardware_version(&response.hardware_version); + response.name_length = board_get_product_name(response.name, sizeof(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.vcs_commit = + bootloader.fw_image_descriptor->vcs_commit; + + response.software_version.image_crc = + bootloader.fw_image_descriptor->image_crc; + + response.software_version.optional_field_flags = OPTIONAL_FIELD_FLAG_IMAGE_CRC | OPTIONAL_FIELD_FLAG_VCS_COMMIT; + } + + size_t length = sizeof(uavcan_GetNodeInfo_request_t); + size_t send_length = uavcan_pack_GetNodeInfo_response(&response); + + /* + * Do a passive receive attempt on the GetNodeInfo fifo + * If it matches send the GetNodeInfo response + */ + + protocol.id.u32 = ANY_NODE_ID; + + if (UavcanOk == uavcan_rx_dsdl(DSDLReqGetNodeInfo, &protocol, (uint8_t *) &request, &length, 0)) { + if (UavcanOk == uavcan_tx_dsdl(DSDLRspGetNodeInfo, &protocol, (const uint8_t *) &response, send_length)) { + bootloader.sent_node_info_response = true; + } + } +} + +/**************************************************************************** + * Name: node_status_process + * + * Description: + * Run as a timer callback off the system tick ISR. + * Once we have a node id, this process is started and sends the + * NodeStatus messages. This function uses the MBNodeStatus + * of the CAN so it can coexist regardless of application state ant + * what is it doing. + * + * Input Parameters: + * Not Used. + * + * Returned Value: + * None + * + ****************************************************************************/ +static void node_status_process(bl_timer_id id, void *context) +{ + static uint8_t transfer_id; + + uavcan_NodeStatus_t message; + uavcan_protocol_t protocol; + + protocol.tail.transfer_id = transfer_id++; + + message.uptime_sec = bootloader.uptime; + message.u8 = uavcan_pack(bootloader.sub_mode, NodeStatus, sub_mode) + | uavcan_pack(bootloader.mode, NodeStatus, mode) + | uavcan_pack(bootloader.health, NodeStatus, health); + message.vendor_specific_status_code = 0u; + uavcan_tx_dsdl(DSDLMsgNodeStatus, &protocol, (const uint8_t *) &message, sizeof(uavcan_NodeStatus_t)); +} + +/**************************************************************************** + * Name: find_descriptor + * + * Description: + * This functions looks through the application image in flash on 8 byte + * aligned boundaries to find the Application firmware descriptor. + * Once it is found the bootloader.fw_image_descriptor is set to point to + * it. + * + * + * Input Parameters: + * None + * + * Returned State: + * If found bootloader.fw_image_descriptor points to the app_descriptor_t + * of the application firmware image. + * If not found bootloader.fw_image_descriptor = NULL + * + ****************************************************************************/ +static void find_descriptor(void) +{ + uint64_t *p = (uint64_t *)APPLICATION_LOAD_ADDRESS; + app_descriptor_t *descriptor = NULL; + union { + uint64_t ull; + char text[sizeof(uint64_t)]; + } sig = { + .text = {APP_DESCRIPTOR_SIGNATURE} + }; + + do { + if (*p == sig.ull) { + descriptor = (app_descriptor_t *)p; + break; + } + } while (++p < APPLICATION_LAST_64BIT_ADDRRESS); + + bootloader.fw_image_descriptor = (volatile app_descriptor_t *)descriptor; +} + +/**************************************************************************** + * Name: is_app_valid + * + * Description: + * This functions validates the applications image based on the validity of + * the Application firmware descriptor's crc and the value of the first word + * in the FLASH image. + * + * + * Input Parameters: + * first_word - the value read from the first word of the Application's + * in FLASH image. + * + * Returned Value: + * true if the application in flash is valid., false otherwise. + * + ****************************************************************************/ +static bool is_app_valid(uint32_t first_word) +{ + uint64_t crc; + size_t i, length, crc_offset; + uint32_t word; + + find_descriptor(); + + if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu) { + return false; + } + + length = bootloader.fw_image_descriptor->image_size; + + if (length > APPLICATION_SIZE) { + return false; + } + + crc_offset = (size_t)(&bootloader.fw_image_descriptor->image_crc) - + (size_t) bootloader.fw_image; + crc_offset >>= 2u; + length >>= 2u; + + crc = crc64_add_word(CRC64_INITIAL, first_word); + + for (i = 1u; i < length; i++) { + if (i == crc_offset || i == crc_offset + 1u) { + /* Zero out the CRC field while computing the CRC */ + word = 0u; + + } else { + word = bootloader.fw_image[i]; + } + + crc = crc64_add_word(crc, word); + } + + crc ^= CRC64_OUTPUT_XOR; + +#if defined(DEBUG_APPLICATION_INPLACE) + return true; +#endif + + return crc == bootloader.fw_image_descriptor->image_crc; +} + +/**************************************************************************** + * Name: get_dynamic_node_id + * + * Description: + * This functions performs The allocatee side of the uavcan specification + * for dynamic node allocation. + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * allocated_node_id - A pointer to the location that should receive the + * allocated node id. + * Returned Value: + * CAN_OK - Indicates the a node id has been allocated to this + * node. + * CAN_BOOT_TIMEOUT - Indicates that tboot expired before an allocation + * was done. + * + ****************************************************************************/ +static int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) +{ + uavcan_HardwareVersion_t hw_version; + + struct { + uint8_t node_id; + uavcan_Allocation_t allocation_message; + } server; + + /* Get the Hw info, (struct will be zeroed by board_get_hardware_version ) */ + + size_t rx_len = board_get_hardware_version(&hw_version); + uint16_t random = (uint16_t) timer_hrt_read(); + random = crc16_signature(random, rx_len, hw_version.unique_id); + util_srand(random); + memset(&server, 0, sizeof(server)); + + /* + * Rule A: on initialization, The allocatee subscribes to uavcan.protocol.dynamic_node_id.Allocation and + * starts a Request Timer with interval of Trequestinterval = the random value between MIN_REQUEST_PERIOD_MS + * MAX_REQUEST_PERIOD_MS + * + */ + + bl_timer_id trequest = timer_allocate(modeTimeout | modeStarted, util_random(MIN_REQUEST_PERIOD_MS, + MAX_REQUEST_PERIOD_MS), 0); + + uavcan_protocol_t protocol; + + do { + /* + * Rule B. On expiration of trequest: + * 1. Request Timer restarts with a random interval of Trequest. + * 2. The allocatee broadcasts a first-stage Allocation request message, where the fields are assigned following values: + * node_id - preferred node ID, or zero if the allocatee doesn't have any preference + * first_part_of_unique_id - true + * unique_id - first MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of unique ID + */ + + if (timer_expired(trequest)) { + uavcan_tx_allocation_message(*allocated_node_id, sizeof_member(uavcan_HardwareVersion_t, unique_id), + hw_version.unique_id, + 0u, random); +restart: + timer_restart(trequest, util_random(MIN_REQUEST_PERIOD_MS, MAX_REQUEST_PERIOD_MS)); + server.node_id = ANY_NODE_ID; + } + + /* + * Do we have a frame and it is of DTIDAllocation that can be Anonymous or a Message + * (It can not be s service) It is possibly not to us. + */ + + protocol.ana.source_node_id = server.node_id; + rx_len = sizeof(server.allocation_message); + + if (UavcanOk == uavcan_rx_dsdl(DSDLMsgAllocation, &protocol, (uint8_t *) &server.allocation_message, &rx_len, 50)) { + + rx_len -= uavcan_byte_count(Allocation, node_id); + + /* + * Rule C. On any Allocation message, even if other rules also match: + * 1. Request Timer restarts with a random interval of Trequest. + */ + + timer_restart(trequest, util_random(MIN_REQUEST_PERIOD_MS, MAX_REQUEST_PERIOD_MS)); + + /* + Skip this message if it's anonymous (from another client), + */ + if (protocol.msg.source_node_id == UavcanAnonymousNodeID) { + + continue; + + /* + * If we do not have a server set or the transfer id + * does not match and this is the first frame + */ + + } else if (0 == server.node_id) { + server.node_id = protocol.msg.source_node_id; + } + + /* + * + * 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 waits for Tfollowup units of time, while listening for other Allocation messages (anon or otherwise). + * If an Allocation message is received during this time, the execution of this rule will be terminated. + * Also see rule C. + * 2. 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 MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST 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 */ + + size_t max_compare = uavcan_byte_count(Allocation, unique_id); + + if (max_compare > rx_len) { + max_compare = rx_len; + } + + uint8_t unique_id_matched; + + for (unique_id_matched = 0; unique_id_matched < max_compare + && hw_version.unique_id[unique_id_matched] == server.allocation_message.unique_id[unique_id_matched]; + unique_id_matched++); + + if (unique_id_matched < rx_len) { + + /* Abort if we didn't match the whole unique ID */ + + goto restart; + + /* All frames are received, yet what was sent by was not the complete id yet */ + + } else if (unique_id_matched == uavcan_byte_count(Allocation, unique_id)) { + + /* Case E */ + *allocated_node_id = uavcan_runpack(server.allocation_message, Allocation, node_id); + break; + + } else { + /* Case D */ + uint8_t rx_payload[CanPayloadLength]; + + /* Case D.1 */ + protocol.id.u32 = ANY_NODE_ID; + + if (UavcanOk == uavcan_rx_dsdl(DSDLMsgAllocation, &protocol, (uint8_t *) &rx_payload, &rx_len, + util_random(MIN_FOLLOWUP_DELAY_MS, MAX_FOLLOWUP_DELAY_MS))) { + goto restart; + } + + /* Sending the next chunk */ + + uavcan_tx_allocation_message(*allocated_node_id, sizeof_member(uavcan_HardwareVersion_t, unique_id), + hw_version.unique_id, + unique_id_matched, random); + + } + } + } while (!timer_expired(tboot)); + + timer_free(trequest); + return *allocated_node_id == ANY_NODE_ID ? CAN_BOOT_TIMEOUT : CAN_OK; +} + +/**************************************************************************** + * Name: wait_for_beginfirmwareupdate + * + * Description: + * This functions performs The allocatee side of the uavcan specification + * for begin firmware update. + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * fw_path - A pointer to the location that should receive the + * path of the firmware file to read. + * fw_path_length - A pointer to return the path length in. + * + * Returned Value: + * UavcanOk - Indicates the a beginfirmwareupdate was received and + * processed successful. + * UavcanBootTimeout - Indicates that tboot expired before a + * beginfirmwareupdate was received. + * + ****************************************************************************/ +static uavcan_error_t wait_for_beginfirmwareupdate(bl_timer_id tboot, uavcan_Path_t *fw_path, size_t *fw_path_length) +{ + uavcan_BeginFirmwareUpdate_request request; + uavcan_protocol_t protocol; + + uavcan_error_t status = UavcanError; + size_t rx_length; + fw_path->u8[0] = 0; + *fw_path_length = 0; + + g_server_node_id = ANY_NODE_ID; + + while (status != UavcanOk) { + + if (timer_expired(tboot)) { + return UavcanBootTimeout; + } + + protocol.id.u32 = ANY_NODE_ID; + rx_length = sizeof(uavcan_BeginFirmwareUpdate_request); + status = uavcan_rx_dsdl(DSDLReqBeginFirmwareUpdate, &protocol, + (uint8_t *) &request, &rx_length, + UavcanServiceTimeOutMs); + } + + if (UavcanOk == status) { + /* Update the priority on the BeginFirmwareUpdate received Request */ + g_uavcan_priority = protocol.ser.priority; + + /* Send an ERROR_OK response */ + uavcan_BeginFirmwareUpdate_response response; + + response.error = ERROR_OK; + + /* We do not care if this send fails */ + uavcan_tx_dsdl(DSDLRspBeginFirmwareUpdate, &protocol, + (uint8_t *)&response, sizeof(uavcan_BeginFirmwareUpdate_response)); + + rx_length = rx_length - uavcan_byte_count(BeginFirmwareUpdate, source_node_id); + memcpy(fw_path, &request.image_file_remote_path, sizeof(uavcan_Path_t)); + g_server_node_id = request.source_node_id; + *fw_path_length = rx_length; + } + + return status; +} + +/**************************************************************************** + * Name: file_getinfo + * + * Description: + * This functions performs The allocatee side of the uavcan specification + * for getinfo (for a file). + * + * Input Parameters: + * + * fw_path - A pointer to the path of the firmware file that's + * info is being requested. + * fw_path_length - The path length of the firmware file that's + * info is being requested. + * fw_image_size - A pointer to the location that should receive the + * firmware image size. + * + * Returned Value: + * None. + * + ****************************************************************************/ +static void file_getinfo(const uavcan_Path_t *fw_path, size_t fw_path_length, size_t *fw_image_size) +{ + uavcan_GetInfo_request_t request; + uavcan_GetInfo_response_t response; + uavcan_protocol_t protocol; + + protocol.tail_init.u8 = 0; + + uint8_t retries = UavcanServiceRetries; + + memcpy(&request.path, fw_path, sizeof(uavcan_Path_t)); + + *fw_image_size = 0; + + while (retries--) { + + protocol.ser.source_node_id = g_server_node_id; + size_t length = FixedSizeGetInfoRequest + fw_path_length; + + if (UavcanOk == uavcan_tx_dsdl(DSDLReqGetInfo, &protocol, + (uint8_t *)&request, length)) { + + length = sizeof(response); + protocol.ser.source_node_id = g_server_node_id; + uavcan_error_t status = uavcan_rx_dsdl(DSDLRspGetInfo, + &protocol, + (uint8_t *) &response, + &length, + UavcanServiceTimeOutMs); + + protocol.tail.transfer_id++; + + /* UAVCANBootloader_v0.3 #27: validateFileInfo(file_info, &errorcode) */ + if (status == UavcanOk && response.error.value == FILE_ERROR_OK && + (response.entry_type.flags & (ENTRY_TYPE_FLAG_FILE | ENTRY_TYPE_FLAG_READABLE)) && + response.size > 0 && response.size < OPT_APPLICATION_IMAGE_LENGTH) { + *fw_image_size = response.size; + break; + } + } + } +} + +/**************************************************************************** + * Name: file_read_and_program + * + * Description: + * This functions performs The allocatee side of the uavcan specification + * for file read and programs the flash. + * + * Input Parameters: + * + * fw_path - A pointer to the path of the firmware file that + * is being read. + * fw_path_length - The path length of the firmware file that is + * being read. + * fw_image_size - The size the fw image file should be. + * + * Returned Value: + * FLASH_OK - Indicates that the correct amount of data has + * been programmed to the FLASH. + * processed successful. + * FLASH_ERROR - Indicates that an error occurred + * + * From Read 218.Read.uavcan updated 5/16/2015 + * + * There are two possible outcomes of a successful service call: + * 1. Data array size equals its capacity. This means that the + * end of the file is not reached yet. + * 2. Data array size is less than its capacity, possibly zero. This + * means that the end of file is reached. + * + * Thus, if The allocatee needs to fetch the entire file, it should + * repeatedly call this service while increasing the offset, + * until incomplete data is returned. + * + * If the object pointed by 'path' cannot be read (e.g. it is a + * directory or it does not exist), appropriate error code + * will be returned, and data array will be empty. + * + ****************************************************************************/ +static flash_error_t file_read_and_program(const uavcan_Path_t *fw_path, uint8_t fw_path_length, size_t fw_image_size) +{ + uavcan_Read_request_t request; + uavcan_Read_response_t response; + uavcan_protocol_t protocol; + + uavcan_error_t uavcan_status; + flash_error_t flash_status; + + uint8_t *data; + uint32_t flash_address = (uint32_t) bootloader.fw_image; + + memset(&request, 0, sizeof(request)); + memset(&response, 0, sizeof(response)); + + /* Set up the read request */ + memcpy(&request.path, fw_path, sizeof(uavcan_Path_t)); + + uint8_t retries = UavcanServiceRetries; + + /* + * Rate limiting on read requests: + * + * 2/sec (500 ms) on a 125 Kbaud bus Speed = 1 1000/2 + * 4/sec (250 ms) on a 250 Kbaud bus Speed = 2 1000/4 + * 8/sec (125 ms) on a 500 Kbaud bus Speed = 3 1000/8 + * 16/sec (62.5 ms) on a 1 Mbaud bus Speed = 4 1000/16 + */ + + uint32_t read_ms = 1000 >> bootloader.bus_speed; + size_t length; + + protocol.tail_init.u8 = 0; + bl_timer_id tread = timer_allocate(modeTimeout | modeStarted, read_ms, 0); + + do { + /* reset the rate limit */ + retries = UavcanServiceRetries; + uavcan_status = UavcanError; + + while (retries && uavcan_status != UavcanOk) { + + timer_restart(tread, read_ms); + + length = FixedSizeReadRequest + fw_path_length; + protocol.ser.source_node_id = g_server_node_id; + uavcan_status = uavcan_tx_dsdl(DSDLReqRead, &protocol, + (uint8_t *)&request, length); + + if (uavcan_status == UavcanOk) { + length = sizeof(uavcan_Read_response_t); + protocol.ser.source_node_id = g_server_node_id; + uavcan_status = uavcan_rx_dsdl(DSDLRspRead, + &protocol, + (uint8_t *) &response, + &length, + UavcanServiceTimeOutMs); + + protocol.tail.transfer_id++; + } + + if (uavcan_status != UavcanOk) { + + retries--; + + } else { + + if (length > sizeof_member(uavcan_Read_response_t, error)) { + + length -= sizeof_member(uavcan_Read_response_t, error); + + } else if (response.error.value != FILE_ERROR_OK) { + uavcan_status = UavcanError; + + retries--; + + board_indicate(fw_update_invalid_response); + + uavcan_tx_log_message(LOGMESSAGE_LEVELERROR, + LOGMESSAGE_STAGE_PROGRAM, + LOGMESSAGE_RESULT_FAIL); + } + } + } + + /* Exhausted retries */ + if (uavcan_status != UavcanOk) { + board_indicate(fw_update_timeout); + break; + } + + data = response.data; + + /* + * STM32 flash addresses must be word aligned If the packet is the + * last and an odd length add an 0xff but do not count it in length + * The is OK to do because the uavcan Read will fill all data payloads + * until the last one + */ + if (length & 1) { + data[length] = 0xff; + } + + /* Save the first word off */ + if (request.offset == 0u) { + bootloader.fw_word0.l = *(uint32_t *)data; + *(uint32_t *)data = 0xffffffff; + } + + flash_status = bl_flash_write(flash_address + request.offset, + data, + length + (length & 1)); + + request.offset += length; + + /* rate limit */ + while (!timer_expired(tread)) { + ; + } + + } while (request.offset < fw_image_size && + length == sizeof(response.data) && + flash_status == FLASH_OK); + + timer_free(tread); + + /* + * Return success if the last read succeeded, the last write succeeded, the + * correct number of bytes were written, and the length of the last response + * was not. */ + if (uavcan_status == UavcanOk && flash_status == FLASH_OK + && request.offset == fw_image_size && length != 0) { + + return FLASH_OK; + + } else { + return FLASH_ERROR; + } +} + +/**************************************************************************** + * Name: do_jump + * + * Description: + * This functions begins the execution of the application firmware. + * + * Input Parameters: + * stacktop - The value that should be loaded into the stack pointer. + * entrypoint - The address to jump to. + * + * Returned Value: + * Does not return. + * + ****************************************************************************/ +static void do_jump(uint32_t stacktop, uint32_t entrypoint) +{ + asm volatile("msr msp, %[stacktop] \n" + "bx %[entrypoint] \n" + :: [stacktop] "r"(stacktop), [entrypoint] "r"(entrypoint):); + + // just to keep noreturn happy + for (;;); +} + +/**************************************************************************** + * Name: application_run + * + * Description: + * This functions will test the application image is valid by + * checking the value of the first word for != 0xffffffff and the + * second word for an address that lies inside od the application + * fw image in flash. + * + * Input Parameters: + * fw_image_size - The size the fw image is. + * + * Returned Value: + * If the Image is valid this code does not return, but runs the application + * via do_jump. + * If the image is invalid the function returns. + * + ****************************************************************************/ +static void application_run(size_t fw_image_size, bootloader_app_shared_t *common) +{ + /* + * 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 defined(DEBUG_APPLICATION_INPLACE) + fw_image_size = FLASH_SIZE - OPT_BOOTLOADER_SIZE_IN_K; +#endif + + uint32_t fw_image[2] = {bootloader.fw_image[0], bootloader.fw_image[1]}; + + if (fw_image[0] != 0xffffffff + && fw_image[1] > APPLICATION_LOAD_ADDRESS + && fw_image[1] < (APPLICATION_LOAD_ADDRESS + fw_image_size)) { + + /* We want to disable interrupts regardless of whether NuttX + * is configured for CONFIG_ARMV7M_USEBASEPRI + */ + + __asm__ __volatile__("\tcpsid i\n"); + + stm32_boarddeinitialize(); + + /* kill the systick interrupt */ + putreg32(0, NVIC_SYSTICK_CTRL); + + /* and set a specific LED pattern */ + board_indicate(jump_to_app); + + /* Update the shared memory and make it valid to tell the + * App are node ID and Can bit rate. + */ + + if (common->crc.valid) { + bootloader_app_shared_write(common, BootLoader); + } + + + /* 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(fw_image[0], fw_image[1]); + } +} + +/**************************************************************************** + * Name: autobaud_and_get_dynamic_node_id + * + * Description: + * This helper functions wraps the auto baud and node id allocation + * + * Input Parameters: + * tboot - The id of the timer to use at the tBoot timeout. + * speed - A pointer to the location to receive the speed detected by + * the auto baud function. + * node_id - A pointer to the location to receive the allocated node_id + * from the dynamic node allocation. + * + * Returned Value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, uint32_t *node_id) +{ + board_indicate(autobaud_start); + + int rv = can_autobaud(speed, tboot); + + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(autobaud_end); + board_indicate(allocation_start); +#if defined(DEBUG_APPLICATION_INPLACE) + *node_id = 125; + return rv; +#endif + rv = get_dynamic_node_id(tboot, node_id); + + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(allocation_end); + } + } + + return rv; +} + +/**************************************************************************** + * Name: main + * + * Description: + * Called by the os_start code + * + * Input Parameters: + * Not used. + * + * Returned Value: + * Does not return. + * + ****************************************************************************/ +__EXPORT int main(int argc, char *argv[]) +{ + size_t fw_image_size = 0; + uavcan_Path_t fw_path; + size_t fw_path_length; + uint8_t error_log_stage; + flash_error_t status; + bootloader_app_shared_t common; + + start_the_watch_dog(); + + /* Begin with all data zeroed */ + memset((void *)&bootloader, 0, sizeof(bootloader)); + + /* Begin with a node id of zero for Allocation */ + g_this_node_id = ANY_NODE_ID; + + bootloader.health = HEALTH_OK; + bootloader.mode = MODE_INITIALIZATION; + bootloader.sub_mode = 0; + + error_log_stage = LOGMESSAGE_STAGE_INIT; + + bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS); + + /* + * This Option is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + */ + bootloader.wait_for_getnodeinfo = OPT_WAIT_FOR_GETNODEINFO; + + /* + * This Option allows the compile timed option to be overridden + * by the state of the jumper + * + */ +#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 + + /* Is the memory in the Application space occupied by a valid application? */ + bootloader.app_valid = is_app_valid(bootloader.fw_image[0]); + + board_indicate(reset); + + /* Was this boot a result of the Application being told it has a FW update ? */ + bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && + common.bus_speed && common.node_id; + + /* + * Mark CRC to say this is not from + * auto baud and Node Allocation + */ + common.crc.valid = false; + + /* Either way prevent Deja vu by invalidating the struct*/ + bootloader_app_shared_invalidate(); + + /* Set up the Timers */ + bl_timer_cb_t p = null_cb; + p.cb = uptime_process; + + /* Uptime is always on*/ + timer_allocate(modeRepeating | modeStarted, 1000, &p); + + /* + * NodeInfo is a controlled process that will be run once a node Id is + * established to process the received NodeInfo response. + */ + p.cb = node_info_process; + bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p); + + /* + * NodeStatus is a controlled process that will sent the requisite NodeStatus + * once a node Id is established + */ + p.cb = node_status_process; + bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p); + + /* + * tBoot is a controlled timer, that is run to allow the application, if + * valid to be booted. tBoot is gated on by the application being valid + * and the OPT_WAIT_FOR_GETNODEINFOxxxx settings + * + */ + bl_timer_id tboot = timer_allocate(modeTimeout, OPT_TBOOT_MS, 0); + + /* + * If the Application is Valid and we are not in a reboot from the + * running Application requesting to Bootload or we are not configured + * to wait for NodeStatus then start tBoot + */ + if (bootloader.app_valid && !bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request) { + timer_start(tboot); + } + + /* + * If this is a reboot from the running Application requesting to Bootload + * then use the CAN parameters supplied by the running Application to init + * the CAN device. + */ + if (bootloader.app_bl_request) { + + bootloader.bus_speed = common.bus_speed; + can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); + + } else { + + /* + * It is a regular boot, So we need to autobaud and get a node ID + * If the tBoot was started, we will boot normal if the auto baud + * or the Node allocation runs longer the tBoot + */ + + /* Preferred Node Address */ + + common.node_id = OPT_PREFERRED_NODE_ID; + + if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, (can_speed_t *)&bootloader.bus_speed, &common.node_id)) { + + /* + * It is OK that node ID is set to the preferred Node ID because + * common.crc.valid is not true yet + */ + + goto boot; + } + + /* We have autobauded and got a Node ID. So reset uptime + * and save the speed and node_id in both the common and + * and bootloader data sets + */ + + bootloader.uptime = 0; + common.bus_speed = can_speed2freq(bootloader.bus_speed); + + /* + * Mark CRC to say this is from + * auto baud and Node Allocation + */ + common.crc.valid = true; + + /* Auto bauding may have taken a long time, so restart the tboot time*/ + + if (bootloader.app_valid && !bootloader.wait_for_getnodeinfo) { + timer_start(tboot); + } + + + } + + /* Now that we have a node Id configure the uavcan library */ + g_this_node_id = common.node_id; + + /* Now start the processes that were defendant on a node ID */ + timer_start(tinfo); + timer_start(tstatus); + + /* + * Now since we are sending NodeStatus messages the Node Status monitor on the + * bus will see us as a new node or one whose uptime has gone backwards + * So we wait for the NodeInfoRequest and our response to be sent of if + * tBoot is running a time tBoot out. + */ + while (!bootloader.sent_node_info_response) { + if (timer_expired(tboot)) { + goto boot; + } + } + + /* + * Now we have seen and responded to the NodeInfoRequest + * If we have a Valid App and we are processing and the running App's + * re-boot to bootload request or we are configured to wait + * we start the tBoot time out. + * + * This effectively extends the time the FirmwareServer has to tell us + * to Begin a FW update. To tBoot from NodeInfoRequest as opposed to + * tBoot from Reset. + */ + if (bootloader.app_valid && + (bootloader.wait_for_getnodeinfo || + bootloader.app_bl_request)) { + + timer_start(tboot); + } + +#if defined(DEBUG_APPLICATION_INPLACE) && defined(DEBUG_NO_FW_UPDATE) + goto boot; +#endif + + /* + * Now We wait up to tBoot for a begin firmware update from the FirmwareServer + * on the bus. tBoot will only be running if the app is valid. + */ + + do { + if (UavcanBootTimeout == wait_for_beginfirmwareupdate(tboot, &fw_path, + &fw_path_length)) { + goto boot; + } + } while (fw_path_length == 0); + + /* We received a begin firmware update */ + timer_stop(tboot); + + board_indicate(fw_update_start); + bootloader.mode = MODE_SOFTWARE_UPDATE; + + file_getinfo(&fw_path, fw_path_length, &fw_image_size); + + //todo:Check this + if (fw_image_size < sizeof(app_descriptor_t)) { + error_log_stage = LOGMESSAGE_STAGE_GET_INFO; + goto failure; + } + + /* LogMessage the Erase */ + uavcan_tx_log_message(LOGMESSAGE_LEVELINFO, + LOGMESSAGE_STAGE_ERASE, + LOGMESSAGE_RESULT_START); + + + /* Need to signal that the app is no longer valid if Node Info Request are done */ + bootloader.app_valid = false; + + status = bl_flash_erase(APPLICATION_LOAD_ADDRESS, APPLICATION_SIZE); + + 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 = LOGMESSAGE_STAGE_ERASE; + goto failure; + } + + status = file_read_and_program(&fw_path, fw_path_length, fw_image_size); + + if (status != FLASH_OK) { + error_log_stage = LOGMESSAGE_STAGE_PROGRAM; + goto failure; + } + + /* Did we program a valid image ?*/ + if (!is_app_valid(bootloader.fw_word0.l)) { + bootloader.app_valid = 0u; + + board_indicate(fw_update_invalid_crc); + + error_log_stage = LOGMESSAGE_STAGE_VALIDATE; + goto failure; + } + + /* Yes Commit the first word to location 0 of the Application image in flash */ + status = bl_flash_write((uint32_t) bootloader.fw_image, (uint8_t *) &bootloader.fw_word0.b[0], + sizeof(bootloader.fw_word0.b)); + + if (status != FLASH_OK) { + error_log_stage = LOGMESSAGE_STAGE_FINALIZE; + goto failure; + } + + /* Send a completion log allocation_message */ + uavcan_tx_log_message(LOGMESSAGE_LEVELINFO, + LOGMESSAGE_STAGE_FINALIZE, + LOGMESSAGE_RESULT_OK); + + + + /* Boot the application */ + +boot: + + kick_the_watch_dog(); + + application_run(bootloader.fw_image_descriptor->image_size, &common); + + /* We will fall thru if the Image is bad */ + +failure: + + uavcan_tx_log_message(LOGMESSAGE_LEVELERROR, + error_log_stage, + LOGMESSAGE_RESULT_FAIL); + + + bootloader.health = HEALTH_CRITICAL; + + bl_timer_id tmr = timer_allocate(modeTimeout | modeStarted, OPT_RESTART_TIMEOUT_MS, 0); + + while (!timer_expired(tmr)) { + ; + } + + timer_free(tmr); + up_systemreset(); +} + +/**************************************************************************** + * Name: sched_yield() + * + * Description: + * This function should be called in situation were the cpu may be + * busy for a long time without interrupts or the ability to run code + * to insure that the timer based process will be run. + * + * + * Input Parameters: + * None + * + * Returned value: + * None + * + ****************************************************************************/ +#if defined(OPT_USE_YIELD) +void bl_sched_yield(void) +{ + /* + * TODO: The uptime will be stalled so consider having the caller or + * this code track the stall time and accumulate it to kee the uptime + * moving + */ + + node_status_process(0, 0); +} +#endif diff --git a/platforms/nuttx/src/canbootloader/util/crc.c b/platforms/nuttx/src/canbootloader/util/crc.c new file mode 100644 index 000000000000..f1d74d688b28 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/util/crc.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include "crc.h" + +/**************************************************************************** + * Name: crc16_add + * + * Description: + * Use to calculates a CRC-16-CCITT using the polynomial of + * 0x1021 by adding a value successive values. + * + * Input Parameters: + * crc - The running total of the crc 16 + * value - The value to add + * + * Returned Value: + * The current crc16 with the value processed. + * + ****************************************************************************/ +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; +} + +/**************************************************************************** + * Name: crc16_signature + * + * Description: + * Calculates a CRC-16-CCITT using the crc16_add + * function + * + * Input Parameters: + * initial - The Initial value to uses as the crc's starting point + * length - The number of bytes to add to the crc + * bytes - A pointer to any array of length bytes + * + * Returned Value: + * The crc16 of the array of bytes + * + ****************************************************************************/ +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; +} + +/**************************************************************************** + * Name: crc64_add_word + * + * Description: + * Calculates a CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693 + * See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 + * Check: 0x62EC59E3F1A4F00A + * + * Input Parameters: + * crc - The running total of the crc 64 + * value - The value to add + * + * Returned Value: + * The current crc64 with the value processed. + * + ****************************************************************************/ +__EXPORT uint64_t crc64_add_word(uint64_t crc, uint32_t value) +{ + uint32_t i, j; + uint8_t byte; + const uint64_t poly = 0x42F0E1EBA9EA3693ull; + + for (j = 0; j < 4; j++) { + byte = ((uint8_t *) &value)[j]; + crc ^= (uint64_t) byte << 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/platforms/nuttx/src/canbootloader/util/random.c b/platforms/nuttx/src/canbootloader/util/random.c new file mode 100644 index 000000000000..73982cf5ad0e --- /dev/null +++ b/platforms/nuttx/src/canbootloader/util/random.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "random.h" + +#define RAND_MAX_32 ((1U << 31) - 1) + +static uint32_t rseed; + +/**************************************************************************** + * Name: util_srand + * + * Description: + * This function seeds the random number generator + * + * + * Input Parameters: + * seed - The seed + * + * Returned value: + * None + * + ****************************************************************************/ +void util_srand(uint16_t seed) +{ + rseed = seed; +} + +/**************************************************************************** + * Name: util_random + * + * Description: + * This function returns a random number between min and max + * + * + * Input Parameters: + * min - The minimum value the return value can be. + * max - The maximum value the return value can be. + * + * Returned value: + * A random number + * + ****************************************************************************/ +uint16_t util_random(uint16_t min, uint16_t max) +{ + uint16_t rand = (rseed = (rseed * 214013 + 2531011) & RAND_MAX_32) >> 16; + return rand % (max - min) + min; +}