diff options
| author | David Sidrane <david_s5@nscdg.com> | 2015-04-21 17:28:14 -1000 |
|---|---|---|
| committer | David Sidrane <david_s5@nscdg.com> | 2015-04-22 02:30:14 -1000 |
| commit | f6a937dc5acf4f9a43876293e9181e45f38c8bec (patch) | |
| tree | f39c44171465e417380f080f8331565ef03d745e | |
| parent | 139439c0e05511f9dd4de44399020e512e2a6097 (diff) | |
| download | px4-firmware-f6a937dc5acf4f9a43876293e9181e45f38c8bec.tar.gz px4-firmware-f6a937dc5acf4f9a43876293e9181e45f38c8bec.tar.bz2 px4-firmware-f6a937dc5acf4f9a43876293e9181e45f38c8bec.zip | |
Ran astyle
13 files changed, 1061 insertions, 1061 deletions
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c index 8a8958c4d..5cd524e35 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.c @@ -128,7 +128,7 @@ static inline uint32_t read_msr_rx(void) { - return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX; + return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX; } /**************************************************************************** @@ -137,11 +137,11 @@ static inline uint32_t read_msr_rx(void) 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; + __asm__ __volatile__ ("\tcpsid i\n"); + *now = timer_hrt_read(); + uint32_t msr = read_msr_rx(); + __asm__ __volatile__ ("\tcpsie i\n"); + return msr; } /**************************************************************************** @@ -150,22 +150,22 @@ static uint32_t read_msr(time_hrt_cycles_t *now) 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; + 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; } /**************************************************************************** @@ -189,7 +189,7 @@ static int read_bits_times(time_hrt_cycles_t *times, size_t max) int can_speed2freq(can_speed_t speed) { - return 1000000 >> (CAN_1MBAUD-speed); + return 1000000 >> (CAN_1MBAUD-speed); } /**************************************************************************** @@ -209,7 +209,7 @@ int can_speed2freq(can_speed_t speed) can_speed_t can_freq2speed(int freq) { - return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); + return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); } @@ -237,26 +237,26 @@ can_speed_t can_freq2speed(int freq) void can_tx(uint32_t message_id, size_t length, const uint8_t * message, uint8_t mailbox) { - uint32_t data[2]; + uint32_t data[2]; - memcpy(data, message, sizeof(data)); + memcpy(data, message, sizeof(data)); - /* - * Just block while waiting for the mailbox. Give it an extra 0.75 ms per - * frame to avoid an issue Ben was seeing with packets going missing on a USBtin. */ + /* + * Just block while waiting for the mailbox. Give it an extra 0.75 ms per + * frame to avoid an issue Ben was seeing with packets going missing on a USBtin. */ - uint32_t mask = CAN_TSR_TME0 << mailbox; - time_hrt_cycles_t begin = timer_hrt_read(); + uint32_t mask = CAN_TSR_TME0 << mailbox; + time_hrt_cycles_t begin = timer_hrt_read(); - while (((getreg32(STM32_CAN1_TSR) & mask) == 0) || - timer_hrt_elapsed(begin, timer_hrt_read()) < WAIT_TX_READY_MS)); + while (((getreg32(STM32_CAN1_TSR) & mask) == 0) || + timer_hrt_elapsed(begin, timer_hrt_read()) < WAIT_TX_READY_MS)); - putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox)); - putreg32(data[0], STM32_CAN1_TDLR(mailbox)); - putreg32(data[1], STM32_CAN1_TDHR(mailbox)); - putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ, - STM32_CAN1_TIR(mailbox)); -} + 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)); + } /**************************************************************************** * Name: can_rx @@ -279,31 +279,31 @@ void can_tx(uint32_t message_id, size_t length, const uint8_t * message, 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 }; + 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) + if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK) { - rv = 1; - /* If so, process it */ + 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)); + *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]); + putreg32(CAN_RFR_RFOM, fifos[fifo & 1]); - memcpy(message, data, sizeof(data)); + memcpy(message, data, sizeof(data)); } - return rv; + return rv; } @@ -335,71 +335,71 @@ uint8_t can_rx(uint32_t * message_id, size_t * length, uint8_t * message, int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) { - *can_speed = CAN_UNKNOWN; + *can_speed = CAN_UNKNOWN; - volatile int attempt = 0; - /* Threshold are at 1.5 Bit times */ + 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. - */ + /* + * 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 bit_time; + time_hrt_cycles_t min_cycles; + int sample; + can_speed_t speed = CAN_125KBAUD; - time_hrt_cycles_t samples[128]; + time_hrt_cycles_t samples[128]; - while(1) { + while(1) { - while(1) { + while(1) { - min_cycles = ULONG_MAX; - int samplecnt = read_bits_times(samples, arraySize(samples)); + min_cycles = ULONG_MAX; + int samplecnt = read_bits_times(samples, arraySize(samples)); - if (timer_expired(timeout)) { - return CAN_BOOT_TIMEOUT; - } + 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 ((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; - } + 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; - } + 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; - } + 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); + attempt++; + can_init(speed, CAN_Mode_Silent); - } /* while(1) */ + } /* while(1) */ - return CAN_OK; + return CAN_OK; } /**************************************************************************** @@ -421,104 +421,104 @@ int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) 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 << 24) | - (CAN_125KBAUD_BS1 << 16) | - (CAN_125KBAUD_BS2 << 20) | (CAN_125KBAUD_PRESCALER - 1), - - (CAN_250KBAUD_SJW << 24) | - (CAN_250KBAUD_BS1 << 16) | - (CAN_250KBAUD_BS2 << 20) | (CAN_250KBAUD_PRESCALER - 1), - - (CAN_500KBAUD_SJW << 24) | - (CAN_500KBAUD_BS1 << 16) | - (CAN_500KBAUD_BS2 << 20) | (CAN_500KBAUD_PRESCALER - 1), - - (CAN_1MBAUD_SJW << 24) | - (CAN_1MBAUD_BS1 << 16) | - (CAN_1MBAUD_BS2 << 20) | (CAN_1MBAUD_PRESCALER - 1) - }; - /* Remove Unknow Offset */ - if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) { - return -EINVAL; - } - uint32_t timeout; - /* - * Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP - * knock down Sleep and raise CAN_MCR_INRQ - */ - - putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR); - - /* Wait until initialization mode is acknowledged */ - - for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) + int speedndx = speed -1; + /* + * TODO: use full-word writes to reduce the number of loads/stores. + * + * Also consider filter use -- maybe set filters for all the message types we + * want. */ + + + const uint32_t bitrates[] = { + (CAN_125KBAUD_SJW << 24) | + (CAN_125KBAUD_BS1 << 16) | + (CAN_125KBAUD_BS2 << 20) | (CAN_125KBAUD_PRESCALER - 1), + + (CAN_250KBAUD_SJW << 24) | + (CAN_250KBAUD_BS1 << 16) | + (CAN_250KBAUD_BS2 << 20) | (CAN_250KBAUD_PRESCALER - 1), + + (CAN_500KBAUD_SJW << 24) | + (CAN_500KBAUD_BS1 << 16) | + (CAN_500KBAUD_BS2 << 20) | (CAN_500KBAUD_PRESCALER - 1), + + (CAN_1MBAUD_SJW << 24) | + (CAN_1MBAUD_BS1 << 16) | + (CAN_1MBAUD_BS2 << 20) | (CAN_1MBAUD_PRESCALER - 1) + }; + /* Remove Unknow Offset */ + if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) { + return -EINVAL; + } + uint32_t timeout; + /* + * Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP + * knock down Sleep and raise CAN_MCR_INRQ + */ + + putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR); + + /* Wait until initialization mode is acknowledged */ + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { - if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) { - /* We are in initialization mode */ + /* We are in initialization mode */ - break; + break; } } - if (timeout < 1) + if (timeout < 1) { - /* - * Initialization failed, not much we can do now other than try a normal - * startup. */ - return -ETIME; + /* + * 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(bitrates[speedndx]| mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR); - putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF, STM32_CAN1_MCR); + putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF, STM32_CAN1_MCR); - for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { - if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) { - /* We are in initialization mode */ + /* We are in initialization mode */ - break; + break; } } - if (timeout < 1) + if (timeout < 1) { - return -ETIME; + return -ETIME; } - /* - * CAN filter initialization -- accept everything on RX FIFO 0, and only - * GetNodeInfo requests on RX FIFO 1. */ - putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); - putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ - putreg32(3, STM32_CAN1_FS1R); /* Enable 32-bit mode for filters 0 and 1 */ + /* + * CAN filter initialization -- accept everything on RX FIFO 0, and only + * GetNodeInfo requests on RX FIFO 1. */ + putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); + putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ + putreg32(3, STM32_CAN1_FS1R); /* Enable 32-bit mode for filters 0 and 1 */ - /* Filter 0 masks -- data type ID 551 only */ - putreg32(UAVCAN_GETNODEINFO_DTID << 22, STM32_CAN1_FIR(0, 1)); - /* Top 10 bits of ID */ - putreg32(0xFFC00000, STM32_CAN1_FIR(0, 2)); + /* Filter 0 masks -- data type ID 551 only */ + putreg32(UAVCAN_GETNODEINFO_DTID << 22, STM32_CAN1_FIR(0, 1)); + /* Top 10 bits of ID */ + putreg32(0xFFC00000, STM32_CAN1_FIR(0, 2)); - /* Filter 1 masks -- everything is don't-care */ - putreg32(0, STM32_CAN1_FIR(1, 1)); - putreg32(0, STM32_CAN1_FIR(1, 2)); + /* Filter 1 masks -- everything is don't-care */ + putreg32(0, STM32_CAN1_FIR(1, 1)); + putreg32(0, STM32_CAN1_FIR(1, 2)); - putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */ - putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the + putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */ + putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the * rest of filters */ - putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */ + putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */ - putreg32(0, STM32_CAN1_FMR); /* Leave init Mode */ + putreg32(0, STM32_CAN1_FMR); /* Leave init Mode */ - return OK; + return OK; } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h index a83cab86b..a149b44e6 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/can/driver.h @@ -53,19 +53,19 @@ typedef enum { - CAN_UNKNOWN = 0, - CAN_125KBAUD = 1, - CAN_250KBAUD = 2, - CAN_500KBAUD = 3, - CAN_1MBAUD = 4 + 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_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; @@ -78,12 +78,12 @@ typedef enum typedef enum { - fifoAll = 0, - MBAll = 0, + fifoAll = 0, + MBAll = 0, - fifoGetNodeInfo = 1, - MBGetNodeInfo = 1, - MBNodeStatus = 1, + fifoGetNodeInfo = 1, + MBGetNodeInfo = 1, + MBNodeStatus = 1, } can_fifo_mailbox_t; diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c index d9e7ca5e9..bcf8178d0 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c @@ -99,11 +99,11 @@ 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); + 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); } @@ -113,11 +113,11 @@ inline static void read(bootloader_app_shared_t * pshared) 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); + 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); } @@ -127,16 +127,16 @@ inline static void write(bootloader_app_shared_t * pshared) static uint64_t calulate_signature(bootloader_app_shared_t * pshared) { - size_t size = sizeof(bootloader_app_shared_t) - sizeof(pshared->crc); - uint64_t crc = CRC64_INITIAL; - uint8_t *protected = (uint8_t *) & pshared->signature; + size_t size = sizeof(bootloader_app_shared_t) - sizeof(pshared->crc); + uint64_t crc = CRC64_INITIAL; + uint8_t *protected = (uint8_t *) & pshared->signature; - while (size--) + while (size--) { - crc = crc64_add(crc, *protected++); + crc = crc64_add(crc, *protected++); } - crc ^= CRC64_OUTPUT_XOR; - return crc; + crc ^= CRC64_OUTPUT_XOR; + return crc; } @@ -145,13 +145,13 @@ static uint64_t calulate_signature(bootloader_app_shared_t * pshared) ****************************************************************************/ 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) + memset(pshared, 0, sizeof(bootloader_app_shared_t)); + if (role != Invalid) { - pshared->signature = - (role == - App ? BOOTLOADER_COMMON_APP_SIGNATURE : - BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); + pshared->signature = + (role == + App ? BOOTLOADER_COMMON_APP_SIGNATURE : + BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE); } } @@ -190,19 +190,19 @@ static void bootloader_app_shared_init(bootloader_app_shared_t * pshared, eRole_ int bootloader_app_shared_read(bootloader_app_shared_t * shared, eRole_t role) { - int rv = -EBADR; - bootloader_app_shared_t working; + int rv = -EBADR; + bootloader_app_shared_t working; - read(&working); + read(&working); - if ((role == App ? working.signature == BOOTLOADER_COMMON_APP_SIGNATURE - : working.signature == BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE) - && (working.crc.ull == calulate_signature(&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; + *shared = working; + rv = OK; } - return rv; + return rv; } /**************************************************************************** @@ -232,13 +232,13 @@ int bootloader_app_shared_read(bootloader_app_shared_t * shared, 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); + 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); } @@ -262,7 +262,7 @@ void bootloader_app_shared_write(bootloader_app_shared_t * shared, void bootloader_app_shared_invalidate(void) { - bootloader_app_shared_t working; - bootloader_app_shared_init(&working, Invalid); - write(&working); + bootloader_app_shared_t working; + bootloader_app_shared_init(&working, Invalid); + write(&working); } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h index 12f683f58..937282894 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h @@ -93,10 +93,10 @@ typedef enum eRole { typedef struct packed_struct bootloader_app_shared_t { union - { + { uint64_t ull; uint32_t ul[2]; - } crc; + } crc; uint32_t signature; uint32_t bus_speed; uint32_t node_id; @@ -128,7 +128,7 @@ typedef struct packed_struct bootloader_app_shared_t { ****************************************************************************/ typedef struct packed_struct app_descriptor_t - { +{ uint64_t signature; uint64_t image_crc; uint32_t image_size; @@ -174,7 +174,7 @@ typedef struct packed_struct app_descriptor_t ****************************************************************************/ int bootloader_app_shared_read(bootloader_app_shared_t * shared, - eRole_t role); + eRole_t role); /**************************************************************************** * Name: bootloader_app_shared_write diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h b/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h index d15e318f9..0f8657274 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h @@ -35,7 +35,7 @@ * @file board_config.h * * PX4CANNODEv1 for the bootloader internal definitions - * This file is related to the parrent folder version but defines + * This file is related to the parrent folder version but defines * differnet usages of the hardware for bootloading */ @@ -112,17 +112,17 @@ typedef enum { - 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, + 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; /************************************************************************************ * Public data diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c index def248819..079b2fa3f 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c @@ -88,17 +88,17 @@ __EXPORT void stm32_boardinitialize(void) { - putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); - stm32_configgpio(GPIO_CAN1_RX); - stm32_configgpio(GPIO_CAN1_TX); - stm32_configgpio(GPIO_CAN_CTRL); - putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, - STM32_RCC_APB1RSTR); - putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, - STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN_CTRL); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); #if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) - stm32_configgpio(GPIO_GETNODEINFO_JUMPER); + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); #endif } @@ -115,8 +115,8 @@ __EXPORT void stm32_boardinitialize(void) void stm32_boarddeinitialize(void) { - putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, - STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, + STM32_RCC_APB1RSTR); } /**************************************************************************** @@ -138,11 +138,11 @@ void stm32_boarddeinitialize(void) uint8_t board_get_product_name(uint8_t * product_name, size_t maxlen) { - DEBUGASSERT(maxlen > 3); - product_name[0] = 'h'; - product_name[1] = 'i'; - product_name[2] = '!'; - return 3u; + DEBUGASSERT(maxlen > 3); + product_name[0] = 'h'; + product_name[1] = 'i'; + product_name[2] = '!'; + return 3u; } /**************************************************************************** @@ -161,27 +161,27 @@ uint8_t board_get_product_name(uint8_t * product_name, size_t maxlen) void board_get_hardware_version(uavcan_hardwareversion_t * hw_version) { - uint32_t i; - volatile uint8_t *stm32f_uid = (volatile uint8_t *)STM32_SYSMEM_UID; + uint32_t i; + volatile uint8_t *stm32f_uid = (volatile uint8_t *)STM32_SYSMEM_UID; - hw_version->major = 1u; - hw_version->minor = 0u; + hw_version->major = 1u; + hw_version->minor = 0u; - for (i = 0u; i < 12u; i++) + for (i = 0u; i < 12u; i++) { - hw_version->unique_id[i] = stm32f_uid[i]; + hw_version->unique_id[i] = stm32f_uid[i]; } - for (; i < 16u; i++) + for (; i < 16u; i++) { - hw_version->unique_id[i] = 0u; + hw_version->unique_id[i] = 0u; } - for (i = 0u; i < 255u; i++) + for (i = 0u; i < 255u; i++) { - hw_version->certificate_of_authenticity[i] = 0; + hw_version->certificate_of_authenticity[i] = 0; } - hw_version->certificate_of_authenticity_length = 0u; + hw_version->certificate_of_authenticity_length = 0u; } /**************************************************************************** diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c index 882829ff1..34d852bc5 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c @@ -88,21 +88,21 @@ 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++) + 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)) + if (crc & (1u << 15u)) { - crc = (uint16_t) ((crc << 1u) ^ poly); + crc = (uint16_t) ((crc << 1u) ^ poly); } - else + else { - crc = (uint16_t) (crc << 1u); + crc = (uint16_t) (crc << 1u); } } - return crc; + return crc; } /**************************************************************************** @@ -151,19 +151,19 @@ uint16_t crc16_signature(uint16_t initial, size_t length, uint64_t crc64_add(uint64_t crc, uint8_t value) { - uint32_t i; - const uint64_t poly = 0x42F0E1EBA9EA3693ull; - crc ^= (uint64_t) value << 56u; - for (i = 0; i < 8; i++) + uint32_t i; + const uint64_t poly = 0x42F0E1EBA9EA3693ull; + crc ^= (uint64_t) value << 56u; + for (i = 0; i < 8; i++) { - if (crc & (1ull << 63u)) + if (crc & (1ull << 63u)) { - crc = (uint64_t) (crc << 1u) ^ poly; + crc = (uint64_t) (crc << 1u) ^ poly; } - else + else { - crc = (uint64_t) (crc << 1u); + crc = (uint64_t) (crc << 1u); } } - return crc; + return crc; } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c index dffd2c3c5..bab054e26 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c @@ -95,41 +95,41 @@ flash_error_t bl_flash_erase(size_t address) { - /* - * 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 */ + /* + * 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; + flash_error_t status = FLASH_ERROR_AFU; - ssize_t bllastpage = up_progmem_getpage(address - 1); + ssize_t bllastpage = up_progmem_getpage(address - 1); - if (bllastpage >= 0) + if (bllastpage >= 0) { - status = FLASH_ERROR_SUICIDE; - ssize_t appfirstpage = up_progmem_getpage(address); + status = FLASH_ERROR_SUICIDE; + ssize_t appfirstpage = up_progmem_getpage(address); - if (appfirstpage > bllastpage) + if (appfirstpage > bllastpage) { - size_t pagecnt = up_progmem_npages() - (bllastpage + 1); + size_t pagecnt = up_progmem_npages() - (bllastpage + 1); - /* Erase the whole application flash region */ - status = FLASH_OK; + /* Erase the whole application flash region */ + status = FLASH_OK; - while (status == FLASH_OK && pagecnt--) + while (status == FLASH_OK && pagecnt--) { - ssize_t ps = up_progmem_erasepage(appfirstpage); - if (ps <= 0) + ssize_t ps = up_progmem_erasepage(appfirstpage); + if (ps <= 0) { - status = FLASH_ERASE_ERROR; + status = FLASH_ERASE_ERROR; } } } } - return status; + return status; } /**************************************************************************** @@ -151,15 +151,15 @@ flash_error_t bl_flash_erase(size_t address) flash_error_t bl_flash_write_word(uint32_t flash_address, const uint8_t data[4]) { - flash_error_t status = FLASH_ERROR; - if (flash_address >= APPLICATION_LOAD_ADDRESS && - (flash_address + sizeof(data)) <= (uint32_t) APPLICATION_LAST_32BIT_ADDRRESS) + flash_error_t status = FLASH_ERROR; + if (flash_address >= APPLICATION_LOAD_ADDRESS && + (flash_address + sizeof(data)) <= (uint32_t) APPLICATION_LAST_32BIT_ADDRRESS) { - if (sizeof(data) == - up_progmem_write((size_t) flash_address, (void *)data, sizeof(data))) + if (sizeof(data) == + up_progmem_write((size_t) flash_address, (void *)data, sizeof(data))) { - status = FLASH_OK; + status = FLASH_OK; } } - return status; + return status; } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c index 9867ae28e..e53d3ba9b 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.c @@ -65,21 +65,21 @@ ****************************************************************************/ typedef enum { - OneShot = modeOneShot, - Repeating = modeRepeating, - Timeout = modeTimeout, + OneShot = modeOneShot, + Repeating = modeRepeating, + Timeout = modeTimeout, - modeMsk = 0x3 , - running = modeStarted, - inuse = 0x80, + 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_cb_t usr; + time_ms_t count; + time_ms_t reload; + bl_timer_ctl_t ctl; } bl_timer_t; /**************************************************************************** @@ -125,7 +125,7 @@ const bl_timer_cb_t null_cb = { 0, 0 }; time_ms_t timer_tic(void) { - return sys_tic; + return sys_tic; } /**************************************************************************** @@ -149,69 +149,69 @@ time_ms_t timer_tic(void) void sched_process_timer(void) { - PROBE(1,true); - PROBE(1,false); + PROBE(1,true); + PROBE(1,false); - /* Increment the per-tick system counter */ + /* Increment the per-tick system counter */ - sys_tic++; + sys_tic++; - /* todo:May need a X tick here is threads run long */ + /* todo:May need a X tick here is threads run long */ - time_ms_t ms_elapsed = (CONFIG_USEC_PER_TICK/1000); + time_ms_t ms_elapsed = (CONFIG_USEC_PER_TICK/1000); - /* Walk the time list from High to low and */ + /* Walk the time list from High to low and */ - bl_timer_id t; - for( t = arraySize(timers)-1; (int8_t) t >= 0; t--) { + bl_timer_id t; + for( t = arraySize(timers)-1; (int8_t) t >= 0; t--) { - /* Timer in use and running */ + /* Timer in use and running */ - if ((timers[t].ctl & (inuse|running)) == (inuse|running)) { + if ((timers[t].ctl & (inuse|running)) == (inuse|running)) { - /* Is it NOT already expired nor about to expire ?*/ + /* Is it NOT already expired nor about to expire ?*/ - if (timers[t].count != 0 && timers[t].count > ms_elapsed) { + if (timers[t].count != 0 && timers[t].count > ms_elapsed) { - /* So just remove the amount attributed to the tick */ + /* So just remove the amount attributed to the tick */ - timers[t].count -= ms_elapsed; + timers[t].count -= ms_elapsed; - } else { + } else { - /* it has expired now or less than a tick ago */ + /* it has expired now or less than a tick ago */ - /* Mark it expired */ + /* Mark it expired */ - timers[t].count = 0; + timers[t].count = 0; - /* Now perform action based on mode */ + /* Now perform action based on mode */ - switch(timers[t].ctl & ~(inuse|running)) { + 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); + 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: - case Timeout: - timers[t].count = timers[t].reload; - if (timers[t].usr.cb) { - timers[t].usr.cb(t, timers[t].usr.context); - } - break; - default: break; - } - } - } - } + case Repeating: + case Timeout: + timers[t].count = timers[t].reload; + if (timers[t].usr.cb) { + timers[t].usr.cb(t, timers[t].usr.context); + } + break; + default: + break; + } + } + } + } } /**************************************************************************** @@ -260,23 +260,23 @@ void sched_process_timer(void) bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, bl_timer_cb_t *fc) { - bl_timer_id t; - irqstate_t s = irqsave(); + bl_timer_id t; + irqstate_t s = irqsave(); - for(t = arraySize(timers)-1; (int8_t)t >= 0; t--) { + for(t = arraySize(timers)-1; (int8_t)t >= 0; t--) { - if ((timers[t].ctl & inuse) == 0 ) { + 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; - } - } + timers[t].reload = msfromnow; + timers[t].count = msfromnow; + timers[t].usr = fc ? *fc : null_cb; + timers[t].ctl = (mode & (modeMsk|running)) | (inuse); + break; + } + } - irqrestore(s); - return t; + irqrestore(s); + return t; } @@ -298,10 +298,10 @@ bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, void timer_free(bl_timer_id id) { - DEBUGASSERT(id>=0 && id < arraySize(timers)); - irqstate_t s = irqsave(); - memset(&timers[id], 0, sizeof(timers[id])); - irqrestore(s); + DEBUGASSERT(id>=0 && id < arraySize(timers)); + irqstate_t s = irqsave(); + memset(&timers[id], 0, sizeof(timers[id])); + irqrestore(s); } /**************************************************************************** @@ -321,11 +321,11 @@ void timer_free(bl_timer_id id) ****************************************************************************/ void timer_start(bl_timer_id id) { - DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); - irqstate_t s = irqsave(); - timers[id].count = timers[id].reload; - timers[id].ctl |= running; - irqrestore(s); + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].count = timers[id].reload; + timers[id].ctl |= running; + irqrestore(s); } @@ -345,10 +345,10 @@ void timer_start(bl_timer_id id) void timer_stop(bl_timer_id id) { - DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); - irqstate_t s = irqsave(); - timers[id].ctl &= ~running; - irqrestore(s); + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].ctl &= ~running; + irqrestore(s); } @@ -369,11 +369,11 @@ void timer_stop(bl_timer_id id) int timer_expired(bl_timer_id id) { - DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); - irqstate_t s = irqsave(); - int rv = ((timers[id].ctl & running) && timers[id].count == 0); - irqrestore(s); - return rv; + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + int rv = ((timers[id].ctl & running) && timers[id].count == 0); + irqrestore(s); + return rv; } /**************************************************************************** @@ -395,11 +395,11 @@ int timer_expired(bl_timer_id id) void timer_restart(bl_timer_id id, time_ms_t ms) { - DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); - irqstate_t s = irqsave(); - timers[id].count = timers[id].reload = ms; - timers[id].ctl |= running; - irqrestore(s); + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + irqstate_t s = irqsave(); + timers[id].count = timers[id].reload = ms; + timers[id].ctl |= running; + irqrestore(s); } /**************************************************************************** @@ -423,8 +423,8 @@ void timer_restart(bl_timer_id id, time_ms_t ms) 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; + DEBUGASSERT(id>=0 && id < arraySize(timers) && (timers[id].ctl & inuse)); + return (time_ref_t) &timers[id].count; } /**************************************************************************** @@ -445,25 +445,25 @@ time_ref_t timer_ref(bl_timer_id id) __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)); + /* 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/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h index 33729918c..247659836 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h @@ -78,14 +78,14 @@ typedef uint32_t time_hrt_cycles_t; /* A timer value type of the hrt */ * */ 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 + /* 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; @@ -110,8 +110,8 @@ typedef void (*bl_timer_ontimeout)(bl_timer_id id, void *context); */ typedef struct { - void *context; - bl_timer_ontimeout cb; + void *context; + bl_timer_ontimeout cb; } bl_timer_cb_t; @@ -315,7 +315,7 @@ time_ref_t timer_ref(bl_timer_id id); static inline int timer_ref_expired(time_ref_t ref) { - return *ref == 0; + return *ref == 0; } /**************************************************************************** @@ -354,7 +354,7 @@ time_ms_t timer_tic(void); static inline time_hrt_cycles_t timer_hrt_read(void) { - return getreg32(NVIC_SYSTICK_CURRENT); + return getreg32(NVIC_SYSTICK_CURRENT); } /**************************************************************************** @@ -374,7 +374,7 @@ static inline time_hrt_cycles_t timer_hrt_read(void) static inline time_hrt_cycles_t timer_hrt_max(void) { - return getreg32(NVIC_SYSTICK_RELOAD) + 1; + return getreg32(NVIC_SYSTICK_RELOAD) + 1; } /**************************************************************************** @@ -395,17 +395,17 @@ static inline time_hrt_cycles_t timer_hrt_max(void) 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 */ + /* It is a down count from NVIC_SYSTICK_RELOAD */ - time_hrt_cycles_t elapsed = begin - end; - time_hrt_cycles_t reload = timer_hrt_max(); + time_hrt_cycles_t elapsed = begin - end; + time_hrt_cycles_t reload = timer_hrt_max(); - /* Did it wrap */ - if (elapsed > reload) { - elapsed += reload; - } + /* Did it wrap */ + if (elapsed > reload) { + elapsed += reload; + } - return elapsed; + return elapsed; } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c index 27075a769..3f890474d 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c @@ -70,16 +70,16 @@ ****************************************************************************/ typedef volatile struct bootloader_t { - can_speed_t bus_speed; - volatile uint8_t node_id; - volatile uint8_t status_code; - volatile bool app_valid; - volatile uint32_t uptime; - volatile app_descriptor_t *fw_image_descriptor; - volatile uint32_t *fw_image; - bool wait_for_getnodeinfo; - bool app_bl_request; - bool sent_node_info_response; + can_speed_t bus_speed; + volatile uint8_t node_id; + volatile uint8_t status_code; + volatile bool app_valid; + volatile uint32_t uptime; + volatile app_descriptor_t *fw_image_descriptor; + volatile uint32_t *fw_image; + bool wait_for_getnodeinfo; + bool app_bl_request; + bool sent_node_info_response; } bootloader_t; @@ -118,7 +118,7 @@ bootloader_t bootloader; static void uptime_process(bl_timer_id id, void *context) { - bootloader.uptime++; + bootloader.uptime++; } @@ -143,46 +143,46 @@ static void uptime_process(bl_timer_id id, void *context) static void node_info_process(bl_timer_id id, void *context) { - uavcan_getnodeinfo_response_t response; - uavcan_nodestatus_t node_status; - uavcan_frame_id_t frame_id; - size_t frame_len; - uint32_t rx_message_id; - uint8_t frame_payload[8]; - - node_status.uptime_sec = bootloader.uptime; - node_status.status_code = bootloader.status_code; - node_status.vendor_specific_status_code = 0u; - uavcan_pack_nodestatus(response.nodestatus, &node_status); - - board_get_hardware_version(&response.hardware_version); - response.name_length = board_get_product_name(response.name, 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.optional_field_mask = 3u; - bootloader.fw_image_descriptor->minor_version; - response.software_version.vcs_commit = - bootloader.fw_image_descriptor->vcs_commit; - response.software_version.image_crc = - bootloader.fw_image_descriptor->image_crc; - } - - rx_message_id = 0u; - if (can_rx(&rx_message_id, &frame_len, frame_payload, fifoGetNodeInfo) && - uavcan_parse_message_id(&frame_id, rx_message_id, UAVCAN_GETNODEINFO_DTID) && - frame_payload[0] == bootloader.node_id && - frame_id.last_frame) { - uavcan_tx_getnodeinfo_response(bootloader.node_id, &response, - frame_id.source_node_id, - frame_id.transfer_id); - - bootloader.sent_node_info_response = true; - } + uavcan_getnodeinfo_response_t response; + uavcan_nodestatus_t node_status; + uavcan_frame_id_t frame_id; + size_t frame_len; + uint32_t rx_message_id; + uint8_t frame_payload[8]; + + node_status.uptime_sec = bootloader.uptime; + node_status.status_code = bootloader.status_code; + node_status.vendor_specific_status_code = 0u; + uavcan_pack_nodestatus(response.nodestatus, &node_status); + + board_get_hardware_version(&response.hardware_version); + response.name_length = board_get_product_name(response.name, 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.optional_field_mask = 3u; + bootloader.fw_image_descriptor->minor_version; + response.software_version.vcs_commit = + bootloader.fw_image_descriptor->vcs_commit; + response.software_version.image_crc = + bootloader.fw_image_descriptor->image_crc; + } + + rx_message_id = 0u; + if (can_rx(&rx_message_id, &frame_len, frame_payload, fifoGetNodeInfo) && + uavcan_parse_message_id(&frame_id, rx_message_id, UAVCAN_GETNODEINFO_DTID) && + frame_payload[0] == bootloader.node_id && + frame_id.last_frame) { + uavcan_tx_getnodeinfo_response(bootloader.node_id, &response, + frame_id.source_node_id, + frame_id.transfer_id); + + bootloader.sent_node_info_response = true; + } } /**************************************************************************** @@ -205,7 +205,7 @@ static void node_info_process(bl_timer_id id, void *context) static void node_status_process(bl_timer_id id, void *context) { - uavcan_tx_nodestatus(bootloader.node_id, bootloader.uptime, bootloader.status_code); + uavcan_tx_nodestatus(bootloader.node_id, bootloader.uptime, bootloader.status_code); } /**************************************************************************** @@ -228,23 +228,23 @@ static void node_status_process(bl_timer_id id, void *context) static void send_log_message(uint8_t node_id, uint8_t level, uint8_t stage, uint8_t status) { - uavcan_logmessage_t message; - uavcan_frame_id_t frame_id; - uint8_t payload[8]; - size_t frame_len; - - frame_id.transfer_id = 0; - frame_id.last_frame = 1u; - frame_id.frame_index = 0; - frame_id.source_node_id = node_id; - frame_id.transfer_type = MESSAGE_BROADCAST; - frame_id.data_type_id = UAVCAN_LOGMESSAGE_DTID; - - message.level = level; - message.message[0] = stage; - message.message[1] = status; - frame_len = uavcan_pack_logmessage(payload, &message); - can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBAll); + uavcan_logmessage_t message; + uavcan_frame_id_t frame_id; + uint8_t payload[8]; + size_t frame_len; + + frame_id.transfer_id = 0; + frame_id.last_frame = 1u; + frame_id.frame_index = 0; + frame_id.source_node_id = node_id; + frame_id.transfer_type = MESSAGE_BROADCAST; + frame_id.data_type_id = UAVCAN_LOGMESSAGE_DTID; + + message.level = level; + message.message[0] = stage; + message.message[1] = status; + frame_len = uavcan_pack_logmessage(payload, &message); + can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBAll); } /**************************************************************************** @@ -269,18 +269,18 @@ static void send_log_message(uint8_t node_id, uint8_t level, uint8_t stage, static void find_descriptor(void) { - bootloader.fw_image_descriptor = NULL; - uint64_t *p = (uint64_t *) APPLICATION_LOAD_ADDRESS; - union - { - uint64_t ull; - char text[sizeof(uint64_t)]; - } sig = { - .text = {APP_DESCRIPTOR_SIGNATURE} - }; - do { - if (*p == sig.ull) { - bootloader.fw_image_descriptor = (volatile app_descriptor_t *) p; + bootloader.fw_image_descriptor = NULL; + uint64_t *p = (uint64_t *) APPLICATION_LOAD_ADDRESS; + union + { + uint64_t ull; + char text[sizeof(uint64_t)]; + } sig = { + .text = {APP_DESCRIPTOR_SIGNATURE} + }; + do { + if (*p == sig.ull) { + bootloader.fw_image_descriptor = (volatile app_descriptor_t *) p; } } while(bootloader.fw_image_descriptor == NULL && ++p < APPLICATION_LAST_64BIT_ADDRRESS); } @@ -305,42 +305,42 @@ static void find_descriptor(void) static bool is_app_valid(uint32_t first_word) { - uint64_t crc; - size_t i, length, crc_offset; - uint8_t byte; + uint64_t crc; + size_t i, length, crc_offset; + uint8_t byte; - find_descriptor(); + find_descriptor(); - if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu) + if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu) { - return false; + return false; } - length = bootloader.fw_image_descriptor->image_size; - crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) - - (size_t) bootloader.fw_image; + length = bootloader.fw_image_descriptor->image_size; + crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) - + (size_t) bootloader.fw_image; - crc = CRC64_INITIAL; - for (i = 0u; i < 4u; i++) + crc = CRC64_INITIAL; + for (i = 0u; i < 4u; i++) { - crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u))); + crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u))); } - for (i = 4u; i < length; i++) + for (i = 4u; i < length; i++) { - if (crc_offset <= i && i < crc_offset + 8u) + if (crc_offset <= i && i < crc_offset + 8u) { - /* Zero out the CRC field while computing the CRC */ - byte = 0u; + /* Zero out the CRC field while computing the CRC */ + byte = 0u; } - else + else { - byte = ((volatile uint8_t *)bootloader.fw_image)[i]; + byte = ((volatile uint8_t *)bootloader.fw_image)[i]; } - crc = crc64_add(crc, byte); + crc = crc64_add(crc, byte); } - crc ^= CRC64_OUTPUT_XOR; + crc ^= CRC64_OUTPUT_XOR; - return crc == bootloader.fw_image_descriptor->image_crc; + return crc == bootloader.fw_image_descriptor->image_crc; } /**************************************************************************** @@ -373,10 +373,10 @@ static int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) uint16_t rx_crc; struct { - uint8_t node_id; - uint8_t transfer_id; - uint8_t allocated_node_id; - uint8_t frame_index; + uint8_t node_id; + uint8_t transfer_id; + uint8_t allocated_node_id; + uint8_t frame_index; } server; *allocated_node_id = 0; @@ -421,92 +421,92 @@ static int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id) } if (can_rx(&rx_message_id, &rx_len, rx_payload, fifoAll) && - uavcan_parse_message_id(&rx_frame_id, rx_message_id, UAVCAN_DYNAMICNODEIDALLOCATION_DTID)) { - - /* - Rule C. On any Allocation message, even if other rules also match: - 1) Request Timer restarts. - */ - - timer_start(trequest); - - /* - Skip this message if it's anonymous (from another client), or if it's - from a different server to the one we're listening to at the moment - */ - if (!rx_frame_id.source_node_id || (server.node_id && - rx_frame_id.source_node_id != server.node_id)) { - continue; - } else if (!server.node_id || - rx_frame_id.transfer_id != server.transfer_id || - rx_frame_id.frame_index == 0) { - /* First (only?) frame of the transfer */ - if (rx_frame_id.last_frame) { - offset = 1u; - } else { - rx_crc = (uint16_t)(rx_payload[0] | (rx_payload[1] << 8u)); - server.allocated_node_id = rx_payload[2]; - offset = 3u; - } - server.node_id = rx_frame_id.source_node_id; - server.transfer_id = rx_frame_id.transfer_id; - unique_id_matched = 0u; - server.frame_index = 1u; - } else if (rx_frame_id.frame_index != server.frame_index) { - /* Abort if the frame index is wrong */ - server.node_id = 0u; - continue; - } else { - offset = 0u; - server.frame_index++; - } - - /* - Rule D. On an Allocation message WHERE (source node ID is - non-anonymous) AND (client's unique ID starts with the bytes available - in the field unique_id) AND (unique_id is less than 16 bytes long): - 1) The client broadcasts a second-stage Allocation request message, - where the fields are assigned following values: - node_id - same value as in the first-stage - first_part_of_unique_id - false - unique_id - at most 7 bytes of local unique ID - with an offset equal to number of - bytes in the received unique ID - - Rule E. On an Allocation message WHERE (source node ID is - non-anonymous) AND (unique_id fully matches client's unique ID) AND - (node_id in the received message is not zero): - 1) Request Timer stops. - 2) The client initializes its node_id with the received value. - 3) The client terminates subscription to Allocation messages. - 4) Exit. - */ - - /* Count the number of unique ID bytes matched */ - for (i = offset; i < rx_len && unique_id_matched < 16u && - hw_version.unique_id[unique_id_matched] == rx_payload[i]; - unique_id_matched++, i++); - - if (i < rx_len) { - /* Abort if we didn't match the whole unique ID */ - server.node_id = 0u; - } else if (rx_frame_id.last_frame && unique_id_matched < 16u) { - /* Case D */ - uavcan_tx_allocation_message(*allocated_node_id, 16u, hw_version.unique_id, - unique_id_matched, transfer_id++); - } else if (rx_frame_id.last_frame) { - /* Validate CRC */ - expected_crc = UAVCAN_ALLOCATION_CRC; - expected_crc = crc16_add(expected_crc, server.allocated_node_id); - expected_crc = crc16_signature(expected_crc, 16u, - hw_version.unique_id); - - /* Case E */ - if (rx_crc == expected_crc) { - *allocated_node_id = server.allocated_node_id >> 1u; - break; - } - } + uavcan_parse_message_id(&rx_frame_id, rx_message_id, UAVCAN_DYNAMICNODEIDALLOCATION_DTID)) { + + /* + Rule C. On any Allocation message, even if other rules also match: + 1) Request Timer restarts. + */ + + timer_start(trequest); + + /* + Skip this message if it's anonymous (from another client), or if it's + from a different server to the one we're listening to at the moment + */ + if (!rx_frame_id.source_node_id || (server.node_id && + rx_frame_id.source_node_id != server.node_id)) { + continue; + } else if (!server.node_id || + rx_frame_id.transfer_id != server.transfer_id || + rx_frame_id.frame_index == 0) { + /* First (only?) frame of the transfer */ + if (rx_frame_id.last_frame) { + offset = 1u; + } else { + rx_crc = (uint16_t)(rx_payload[0] | (rx_payload[1] << 8u)); + server.allocated_node_id = rx_payload[2]; + offset = 3u; + } + server.node_id = rx_frame_id.source_node_id; + server.transfer_id = rx_frame_id.transfer_id; + unique_id_matched = 0u; + server.frame_index = 1u; + } else if (rx_frame_id.frame_index != server.frame_index) { + /* Abort if the frame index is wrong */ + server.node_id = 0u; + continue; + } else { + offset = 0u; + server.frame_index++; + } + + /* + Rule D. On an Allocation message WHERE (source node ID is + non-anonymous) AND (client's unique ID starts with the bytes available + in the field unique_id) AND (unique_id is less than 16 bytes long): + 1) The client broadcasts a second-stage Allocation request message, + where the fields are assigned following values: + node_id - same value as in the first-stage + first_part_of_unique_id - false + unique_id - at most 7 bytes of local unique ID + with an offset equal to number of + bytes in the received unique ID + + Rule E. On an Allocation message WHERE (source node ID is + non-anonymous) AND (unique_id fully matches client's unique ID) AND + (node_id in the received message is not zero): + 1) Request Timer stops. + 2) The client initializes its node_id with the received value. + 3) The client terminates subscription to Allocation messages. + 4) Exit. + */ + + /* Count the number of unique ID bytes matched */ + for (i = offset; i < rx_len && unique_id_matched < 16u && + hw_version.unique_id[unique_id_matched] == rx_payload[i]; + unique_id_matched++, i++); + + if (i < rx_len) { + /* Abort if we didn't match the whole unique ID */ + server.node_id = 0u; + } else if (rx_frame_id.last_frame && unique_id_matched < 16u) { + /* Case D */ + uavcan_tx_allocation_message(*allocated_node_id, 16u, hw_version.unique_id, + unique_id_matched, transfer_id++); + } else if (rx_frame_id.last_frame) { + /* Validate CRC */ + expected_crc = UAVCAN_ALLOCATION_CRC; + expected_crc = crc16_add(expected_crc, server.allocated_node_id); + expected_crc = crc16_signature(expected_crc, 16u, + hw_version.unique_id); + + /* Case E */ + if (rx_crc == expected_crc) { + *allocated_node_id = server.allocated_node_id >> 1u; + break; + } + } } } while (!timer_expired(tboot)); @@ -540,50 +540,50 @@ static int wait_for_beginfirmwareupdate(bl_timer_id tboot, uint8_t * fw_path, uint8_t * fw_path_length, uint8_t * fw_source_node_id) { - uavcan_beginfirmwareupdate_request_t request; - uavcan_frame_id_t frame_id; - size_t i; - uint8_t frame_payload[8]; - can_error_t status; + uavcan_beginfirmwareupdate_request_t request; + uavcan_frame_id_t frame_id; + size_t i; + uint8_t frame_payload[8]; + can_error_t status; - status = CAN_ERROR; - fw_path[0] = 0; - *fw_source_node_id = 0; + status = CAN_ERROR; + fw_path[0] = 0; + *fw_source_node_id = 0; - while (status != CAN_OK) + while (status != CAN_OK) { - if (timer_expired(tboot)) { - return CAN_BOOT_TIMEOUT; - } - status = uavcan_rx_beginfirmwareupdate_request(bootloader.node_id, - &request, &frame_id); + if (timer_expired(tboot)) { + return CAN_BOOT_TIMEOUT; + } + status = uavcan_rx_beginfirmwareupdate_request(bootloader.node_id, + &request, &frame_id); } - if (status == CAN_OK) + if (status == CAN_OK) { - /* UAVCANBootloader_v0.3 #22.2: Resp580.BeginFirmwareUpdate.uavcan */ + /* UAVCANBootloader_v0.3 #22.2: Resp580.BeginFirmwareUpdate.uavcan */ - /* Send an ERROR_OK response */ - frame_payload[0] = frame_id.source_node_id; - frame_payload[1] = 0u; + /* Send an ERROR_OK response */ + frame_payload[0] = frame_id.source_node_id; + frame_payload[1] = 0u; - frame_id.last_frame = 1u; - frame_id.frame_index = 0u; - frame_id.source_node_id = bootloader.node_id; - frame_id.transfer_type = SERVICE_RESPONSE; + frame_id.last_frame = 1u; + frame_id.frame_index = 0u; + frame_id.source_node_id = bootloader.node_id; + frame_id.transfer_type = SERVICE_RESPONSE; - can_tx(uavcan_make_message_id(&frame_id), 2u, frame_payload, MBAll); + can_tx(uavcan_make_message_id(&frame_id), 2u, frame_payload, MBAll); - /* UAVCANBootloader_v0.3 #22.3: fwPath = image_file_remote_path */ - for (i = 0u; i < request.path_length; i++) + /* UAVCANBootloader_v0.3 #22.3: fwPath = image_file_remote_path */ + for (i = 0u; i < request.path_length; i++) { - fw_path[i] = request.path[i]; + fw_path[i] = request.path[i]; } - *fw_path_length = request.path_length; - /* UAVCANBootloader_v0.3 #22.4: fwSourceNodeID = source_node_id */ - *fw_source_node_id = request.source_node_id; + *fw_path_length = request.path_length; + /* UAVCANBootloader_v0.3 #22.4: fwSourceNodeID = source_node_id */ + *fw_source_node_id = request.source_node_id; } - return status; + return status; } /**************************************************************************** @@ -613,53 +613,53 @@ static void file_getinfo(size_t * fw_image_size, uint64_t * fw_image_crc, const uint8_t * fw_path, uint8_t fw_path_length, uint8_t fw_source_node_id) { - uavcan_getinfo_request_t request; - uavcan_getinfo_response_t response; - uint8_t transfer_id, retries, i; - can_error_t status; + uavcan_getinfo_request_t request; + uavcan_getinfo_response_t response; + uint8_t transfer_id, retries, i; + can_error_t status; - for (i = 0; i < fw_path_length; i++) + for (i = 0; i < fw_path_length; i++) { - request.path[i] = fw_path[i]; + request.path[i] = fw_path[i]; } - request.path_length = fw_path_length; + request.path_length = fw_path_length; - /* UAVCANBootloader_v0.3 #25: SetRetryAndTimeout(3,1000MS) */ - retries = UAVCAN_SERVICE_RETRIES; - transfer_id = 0; + /* UAVCANBootloader_v0.3 #25: SetRetryAndTimeout(3,1000MS) */ + retries = UAVCAN_SERVICE_RETRIES; + transfer_id = 0; - *fw_image_size = 0; - *fw_image_crc = 0; + *fw_image_size = 0; + *fw_image_crc = 0; - while (retries) + while (retries) { - /* UAVCANBootloader_v0.3 #26: 585.GetInfo.uavcan(path) */ - uavcan_tx_getinfo_request(bootloader.node_id, &request, - fw_source_node_id, transfer_id); - - /* UAVCANBootloader_v0.3 #28: - * 585.GetInfo.uavcan(fw_path,fw_crc,fw_size...), */ - status = - uavcan_rx_getinfo_response(bootloader.node_id, &response, - fw_source_node_id, transfer_id, - UAVCAN_SERVICE_TIMEOUT_MS); - - transfer_id++; - - /* UAVCANBootloader_v0.3 #27: validateFileInfo(file_info, &errorcode) */ - if (status == CAN_OK && response.error == UAVCAN_FILE_ERROR_OK && - (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE) && - (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE) && - response.size > 0u && response.size < OPT_APPLICATION_IMAGE_LENGTH) + /* UAVCANBootloader_v0.3 #26: 585.GetInfo.uavcan(path) */ + uavcan_tx_getinfo_request(bootloader.node_id, &request, + fw_source_node_id, transfer_id); + + /* UAVCANBootloader_v0.3 #28: + * 585.GetInfo.uavcan(fw_path,fw_crc,fw_size...), */ + status = + uavcan_rx_getinfo_response(bootloader.node_id, &response, + fw_source_node_id, transfer_id, + UAVCAN_SERVICE_TIMEOUT_MS); + + transfer_id++; + + /* UAVCANBootloader_v0.3 #27: validateFileInfo(file_info, &errorcode) */ + if (status == CAN_OK && response.error == UAVCAN_FILE_ERROR_OK && + (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE) && + (response.entry_type & UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE) && + response.size > 0u && response.size < OPT_APPLICATION_IMAGE_LENGTH) { - /* UAVCANBootloader_v0.3 #28.4: save(file_info) */ - *fw_image_size = response.size; - *fw_image_crc = response.crc64; - break; + /* UAVCANBootloader_v0.3 #28.4: save(file_info) */ + *fw_image_size = response.size; + *fw_image_crc = response.crc64; + break; } - else + else { - retries--; + retries--; } } } @@ -689,152 +689,152 @@ static void file_getinfo(size_t * fw_image_size, uint64_t * fw_image_crc, ****************************************************************************/ static flash_error_t file_read_and_program(uint8_t fw_source_node_id, - uint8_t fw_path_length, - const uint8_t * fw_path, - size_t fw_image_size, - uint32_t * fw_word0) + uint8_t fw_path_length, + const uint8_t * fw_path, + size_t fw_image_size, + uint32_t * fw_word0) { - uavcan_read_request_t request; - uavcan_read_response_t response; - size_t bytes_written, i, write_length; - can_error_t can_status; - flash_error_t flash_status; - uint32_t next_read_deadline; - uint8_t transfer_id; - uint8_t retries; - uint8_t write_remainder[4]; - uint8_t write_remainder_length; - uint8_t *data; - - /* Set up the read request */ - for (i = 0; i < fw_path_length; i++) + uavcan_read_request_t request; + uavcan_read_response_t response; + size_t bytes_written, i, write_length; + can_error_t can_status; + flash_error_t flash_status; + uint32_t next_read_deadline; + uint8_t transfer_id; + uint8_t retries; + uint8_t write_remainder[4]; + uint8_t write_remainder_length; + uint8_t *data; + + /* Set up the read request */ + for (i = 0; i < fw_path_length; i++) { - request.path[i] = fw_path[i]; + request.path[i] = fw_path[i]; } - request.path_length = fw_path_length; - request.offset = 0u; + request.path_length = fw_path_length; + request.offset = 0u; - bytes_written = 0u; - write_remainder_length = 0u; - transfer_id = 0u; - next_read_deadline = 0u; + bytes_written = 0u; + write_remainder_length = 0u; + transfer_id = 0u; + next_read_deadline = 0u; - do + do { - /* - * Rate limiting on read requests: - 2/sec on a 125 Kbaud bus - 4/sec on - * a 250 Kbaud bus - 8/sec on a 500 Kbaud bus - 16/sec on a 1 Mbaud bus */ - while (bootloader.uptime < next_read_deadline); - next_read_deadline = bootloader.uptime + 6u; - - /* UAVCANBootloader_v0.3 #28.11: SetRetryAndTimeout(3,1000MS) */ - retries = UAVCAN_SERVICE_RETRIES; - can_status = CAN_ERROR; - while (retries && can_status != CAN_OK) + /* + * Rate limiting on read requests: - 2/sec on a 125 Kbaud bus - 4/sec on + * a 250 Kbaud bus - 8/sec on a 500 Kbaud bus - 16/sec on a 1 Mbaud bus */ + while (bootloader.uptime < next_read_deadline); + next_read_deadline = bootloader.uptime + 6u; + + /* UAVCANBootloader_v0.3 #28.11: SetRetryAndTimeout(3,1000MS) */ + retries = UAVCAN_SERVICE_RETRIES; + can_status = CAN_ERROR; + while (retries && can_status != CAN_OK) { - /* UAVCANBootloader_v0.3 #28.12: 588.Read.uavcan(0, path) */ - /* UAVCANBootloader_v0.3 #33: 588.Read.uavcan(N,path) */ - request.offset = bytes_written + write_remainder_length; - uavcan_tx_read_request(bootloader.node_id, &request, - fw_source_node_id, transfer_id); - - /* UAVCANBootloader_v0.3 #28.12.1: 588.Read.uavcan(0,path,data) */ - /* UAVCANBootloader_v0.3 #33.1: 583.Read.uavcan(0,path,data) */ - can_status = uavcan_rx_read_response(bootloader.node_id, - &response, fw_source_node_id, - transfer_id, - UAVCAN_SERVICE_TIMEOUT_MS); - - transfer_id++; - - /* UAVCANBootloader_v0.3 #34: ValidateReadResp(resp) */ - if (can_status != CAN_OK || response.error != UAVCAN_FILE_ERROR_OK) + /* UAVCANBootloader_v0.3 #28.12: 588.Read.uavcan(0, path) */ + /* UAVCANBootloader_v0.3 #33: 588.Read.uavcan(N,path) */ + request.offset = bytes_written + write_remainder_length; + uavcan_tx_read_request(bootloader.node_id, &request, + fw_source_node_id, transfer_id); + + /* UAVCANBootloader_v0.3 #28.12.1: 588.Read.uavcan(0,path,data) */ + /* UAVCANBootloader_v0.3 #33.1: 583.Read.uavcan(0,path,data) */ + can_status = uavcan_rx_read_response(bootloader.node_id, + &response, fw_source_node_id, + transfer_id, + UAVCAN_SERVICE_TIMEOUT_MS); + + transfer_id++; + + /* UAVCANBootloader_v0.3 #34: ValidateReadResp(resp) */ + if (can_status != CAN_OK || response.error != UAVCAN_FILE_ERROR_OK) { - can_status = CAN_ERROR; - - /* UAVCANBootloader_v0.3 #35: [(retries != 0 && timeout) || - * !ValidReadReadResponse]:INDICATE_FW_UPDATE_INVALID_RESPONSE */ - board_indicate(fw_update_invalid_response); - - /* UAVCANBootloader_v0.3 #36: [(retries != 0 && timeout) || - * !ValidReadReadResponse]:1023.LogMessage.uavcan */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_ERROR, - UAVCAN_LOGMESSAGE_STAGE_PROGRAM, - UAVCAN_LOGMESSAGE_RESULT_FAIL); - retries--; + can_status = CAN_ERROR; + + /* UAVCANBootloader_v0.3 #35: [(retries != 0 && timeout) || + * !ValidReadReadResponse]:INDICATE_FW_UPDATE_INVALID_RESPONSE */ + board_indicate(fw_update_invalid_response); + + /* UAVCANBootloader_v0.3 #36: [(retries != 0 && timeout) || + * !ValidReadReadResponse]:1023.LogMessage.uavcan */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_ERROR, + UAVCAN_LOGMESSAGE_STAGE_PROGRAM, + UAVCAN_LOGMESSAGE_RESULT_FAIL); + retries--; } } - if (can_status != CAN_OK) + if (can_status != CAN_OK) { - /* UAVCANBootloader_v0.3 #37: [(retries == 0 && - * timeout]:INDICATE_FW_UPDATE_TIMEOUT */ - board_indicate(fw_update_timeout); - break; + /* UAVCANBootloader_v0.3 #37: [(retries == 0 && + * timeout]:INDICATE_FW_UPDATE_TIMEOUT */ + board_indicate(fw_update_timeout); + break; } - data = response.data; - write_length = response.data_length; + data = response.data; + write_length = response.data_length; - /* - * If this is the last read (zero length) and there are bytes left to - * write, pad the firmware image out with zeros to ensure a full-word - * write. */ - if (write_length == 0u && write_remainder_length > 0u) + /* + * If this is the last read (zero length) and there are bytes left to + * write, pad the firmware image out with zeros to ensure a full-word + * write. */ + if (write_length == 0u && write_remainder_length > 0u) { - write_length = 4u - write_remainder_length; - data[0] = data[1] = data[2] = 0u; + write_length = 4u - write_remainder_length; + data[0] = data[1] = data[2] = 0u; } - /* - * If the length of a previous read was not a multiple of 4, we'll have a - * few bytes left over which need to be combined with the next write as - * all writes must be word-aligned and a whole number of words long. */ - flash_status = FLASH_OK; - while (write_length && flash_status == FLASH_OK) + /* + * If the length of a previous read was not a multiple of 4, we'll have a + * few bytes left over which need to be combined with the next write as + * all writes must be word-aligned and a whole number of words long. */ + flash_status = FLASH_OK; + while (write_length && flash_status == FLASH_OK) { - write_remainder[write_remainder_length++] = *(data++); - write_length--; + write_remainder[write_remainder_length++] = *(data++); + write_length--; - if (write_remainder_length == 4u) + if (write_remainder_length == 4u) { - if (bytes_written == 0u) + if (bytes_written == 0u) { - /* UAVCANBootloader_v0.3 #30: SaveWord0 */ - ((uint8_t *) fw_word0)[0] = write_remainder[0]; - ((uint8_t *) fw_word0)[1] = write_remainder[1]; - ((uint8_t *) fw_word0)[2] = write_remainder[2]; - ((uint8_t *) fw_word0)[3] = write_remainder[3]; + /* UAVCANBootloader_v0.3 #30: SaveWord0 */ + ((uint8_t *) fw_word0)[0] = write_remainder[0]; + ((uint8_t *) fw_word0)[1] = write_remainder[1]; + ((uint8_t *) fw_word0)[2] = write_remainder[2]; + ((uint8_t *) fw_word0)[3] = write_remainder[3]; } - else + else { - flash_status = - bl_flash_write_word((uint32_t) - (&bootloader.fw_image[bytes_written >> 2u]), - write_remainder); + flash_status = + bl_flash_write_word((uint32_t) + (&bootloader.fw_image[bytes_written >> 2u]), + write_remainder); } - bytes_written += 4u; - write_remainder_length = 0u; + bytes_written += 4u; + write_remainder_length = 0u; } } } - while (bytes_written <= fw_image_size && response.data_length != 0u && - flash_status == FLASH_OK); - - /* - * 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 zero. */ - if (can_status == CAN_OK && flash_status == FLASH_OK && - bytes_written == fw_image_size && response.data_length == 0u) + while (bytes_written <= fw_image_size && response.data_length != 0u && + flash_status == FLASH_OK); + + /* + * 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 zero. */ + if (can_status == CAN_OK && flash_status == FLASH_OK && + bytes_written == fw_image_size && response.data_length == 0u) { - return FLASH_OK; + return FLASH_OK; } - else + else { - return FLASH_ERROR; + return FLASH_ERROR; } } @@ -855,10 +855,10 @@ static flash_error_t file_read_and_program(uint8_t fw_source_node_id, static void do_jump(uint32_t stacktop, uint32_t entrypoint) { - asm volatile ("msr msp, %0 \n" - "bx %1 \n"::"r" (stacktop), "r"(entrypoint):); - // just to keep noreturn happy - for (;;); + asm volatile ("msr msp, %0 \n" + "bx %1 \n"::"r" (stacktop), "r"(entrypoint):); + // just to keep noreturn happy + for (;;); } /**************************************************************************** @@ -882,39 +882,39 @@ static void do_jump(uint32_t stacktop, uint32_t entrypoint) static void application_run(size_t fw_image_size) { - /* - * We refuse to program the first word of the app until the upload is marked - * complete by the host. So if it's not 0xffffffff, we should try booting it. - - * The second word of the app is the entrypoint; it must point within the - * flash area (or we have a bad flash). - */ - if (bootloader.fw_image[0] != 0xffffffff - && bootloader.fw_image[1] > APPLICATION_LOAD_ADDRESS - && bootloader.fw_image[1] < (APPLICATION_LOAD_ADDRESS + fw_image_size)) + /* + * We refuse to program the first word of the app until the upload is marked + * complete by the host. So if it's not 0xffffffff, we should try booting it. + + * The second word of the app is the entrypoint; it must point within the + * flash area (or we have a bad flash). + */ + if (bootloader.fw_image[0] != 0xffffffff + && bootloader.fw_image[1] > APPLICATION_LOAD_ADDRESS + && bootloader.fw_image[1] < (APPLICATION_LOAD_ADDRESS + fw_image_size)) { - (void)irqsave(); + (void)irqsave(); - stm32_boarddeinitialize(); + stm32_boarddeinitialize(); - /* kill the systick interrupt */ - up_disable_irq(STM32_IRQ_SYSTICK); - putreg32((NVIC_SYSTICK_CTRL_CLKSOURCE | NVIC_SYSTICK_CTRL_TICKINT), - NVIC_SYSTICK_CTRL); + /* kill the systick interrupt */ + up_disable_irq(STM32_IRQ_SYSTICK); + putreg32((NVIC_SYSTICK_CTRL_CLKSOURCE | NVIC_SYSTICK_CTRL_TICKINT), + NVIC_SYSTICK_CTRL); - /* and set a specific LED pattern */ - board_indicate(jump_to_app); + /* and set a specific LED pattern */ + board_indicate(jump_to_app); - /* the interface */ + /* the interface */ - /* switch exception handlers to the application */ - __asm volatile ("dsb"); - __asm volatile ("isb"); - putreg32(APPLICATION_LOAD_ADDRESS, NVIC_VECTAB); - __asm volatile ("dsb"); - /* extract the stack and entrypoint from the app vector table and go */ - do_jump(bootloader.fw_image[0], bootloader.fw_image[1]); + /* switch exception handlers to the application */ + __asm volatile ("dsb"); + __asm volatile ("isb"); + putreg32(APPLICATION_LOAD_ADDRESS, NVIC_VECTAB); + __asm volatile ("dsb"); + /* extract the stack and entrypoint from the app vector table and go */ + do_jump(bootloader.fw_image[0], bootloader.fw_image[1]); } } @@ -936,23 +936,23 @@ static void application_run(size_t fw_image_size) * ****************************************************************************/ static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, - can_speed_t *speed, - uint32_t *node_id) + can_speed_t *speed, + uint32_t *node_id) { - board_indicate(autobaud_start); + board_indicate(autobaud_start); - int rv = can_autobaud(speed, tboot); + int rv = can_autobaud(speed, tboot); - if (rv != CAN_BOOT_TIMEOUT) { - board_indicate(autobaud_end); - board_indicate(allocation_start); - rv = get_dynamic_node_id(tboot, node_id); - if (rv != CAN_BOOT_TIMEOUT) { - board_indicate(allocation_end); - } - } - return rv; + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(autobaud_end); + board_indicate(allocation_start); + rv = get_dynamic_node_id(tboot, node_id); + if (rv != CAN_BOOT_TIMEOUT) { + board_indicate(allocation_end); + } + } + return rv; } /**************************************************************************** @@ -976,232 +976,232 @@ static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, __EXPORT int main(int argc, char *argv[]) { - uint64_t fw_image_crc; - size_t fw_image_size = 0; - uint32_t fw_word0; - uint8_t fw_path[200]; - uint8_t fw_path_length; - uint8_t fw_source_node_id; - uint8_t error_log_stage; - flash_error_t status; - bootloader_app_shared_t common; + uint64_t fw_image_crc; + size_t fw_image_size = 0; + uint32_t fw_word0; + uint8_t fw_path[200]; + uint8_t fw_path_length; + uint8_t fw_source_node_id; + uint8_t error_log_stage; + flash_error_t status; + bootloader_app_shared_t common; #ifdef OPT_ENABLE_WD - /* ... */ + /* ... */ #endif - memset((void *)&bootloader, 0, sizeof(bootloader)); + memset((void *)&bootloader, 0, sizeof(bootloader)); - bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING; + bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING; - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT; + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT; - bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS); + bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS); #if defined(OPT_WAIT_FOR_GETNODEINFO) - bootloader.wait_for_getnodeinfo = 1; + bootloader.wait_for_getnodeinfo = 1; #endif #if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) - bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT; + bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT; #endif - bootloader.app_valid = is_app_valid(bootloader.fw_image[0]); + bootloader.app_valid = is_app_valid(bootloader.fw_image[0]); - board_indicate(reset); + board_indicate(reset); - bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && - common.bus_speed && common.node_id; - bootloader_app_shared_invalidate(); + bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && + common.bus_speed && common.node_id; + bootloader_app_shared_invalidate(); - bl_timer_cb_t p = null_cb; - p.cb = uptime_process; - timer_allocate(modeRepeating|modeStarted, 1000, &p); + bl_timer_cb_t p = null_cb; + p.cb = uptime_process; + timer_allocate(modeRepeating|modeStarted, 1000, &p); - p.cb = node_info_process; - bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p); + p.cb = node_info_process; + bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p); - p.cb = node_status_process; - bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p); + p.cb = node_status_process; + bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p); - bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0); + bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0); - if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) { - timer_start(tboot); - } + if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) { + timer_start(tboot); + } - if (bootloader.app_bl_request) { + if (bootloader.app_bl_request) { - bootloader.bus_speed = common.bus_speed; - bootloader.node_id = (uint8_t) common.node_id; - can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); + bootloader.bus_speed = common.bus_speed; + bootloader.node_id = (uint8_t) common.node_id; + can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); - } else { + } else { - can_speed_t speed; + can_speed_t speed; - if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) { - goto boot; - } + if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) { + goto boot; + } - /* reset uptime */ - bootloader.uptime = 0; - common.bus_speed = can_speed2freq(speed); - bootloader.node_id = (uint8_t) common.node_id; + /* reset uptime */ + bootloader.uptime = 0; + common.bus_speed = can_speed2freq(speed); + bootloader.node_id = (uint8_t) common.node_id; - } - // Start Processes that requier Node Id - timer_start(tinfo); - timer_start(tstatus); + } + // Start Processes that requier Node Id + timer_start(tinfo); + timer_start(tstatus); - while (!bootloader.sent_node_info_response) { - if (timer_expired(tboot)) + while (!bootloader.sent_node_info_response) { + if (timer_expired(tboot)) { - goto boot; + goto boot; } - } + } - if (bootloader.app_valid && - (bootloader.wait_for_getnodeinfo || - bootloader.app_bl_request)) { + if (bootloader.app_valid && + (bootloader.wait_for_getnodeinfo || + bootloader.app_bl_request)) { - timer_start(tboot); - } + timer_start(tboot); + } - do + do { - if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path, - &fw_path_length, - &fw_source_node_id)) + if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path, + &fw_path_length, + &fw_source_node_id)) { - goto boot; + goto boot; } } - while (!fw_source_node_id); + while (!fw_source_node_id); - timer_stop(tboot); - board_indicate(fw_update_start); + timer_stop(tboot); + board_indicate(fw_update_start); - file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length, - fw_source_node_id); + file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length, + fw_source_node_id); - //todo:Check this - if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc) + //todo:Check this + if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc) { - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO; - goto failure; + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO; + goto failure; } - /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_INFO, - UAVCAN_LOGMESSAGE_STAGE_ERASE, - UAVCAN_LOGMESSAGE_RESULT_START); + /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_INFO, + UAVCAN_LOGMESSAGE_STAGE_ERASE, + UAVCAN_LOGMESSAGE_RESULT_START); - /* Need to signal that the app is no longer valid if Node Info Request are done */ + /* Need to signal that the app is no longer valid if Node Info Request are done */ - bootloader.app_valid = false; + bootloader.app_valid = false; - status = bl_flash_erase(APPLICATION_LOAD_ADDRESS); - if (status != FLASH_OK) + status = bl_flash_erase(APPLICATION_LOAD_ADDRESS); + if (status != FLASH_OK) { - /* UAVCANBootloader_v0.3 #28.8: [Erase - * Failed]:INDICATE_FW_UPDATE_ERASE_FAIL */ - board_indicate(fw_update_erase_fail); + /* UAVCANBootloader_v0.3 #28.8: [Erase + * Failed]:INDICATE_FW_UPDATE_ERASE_FAIL */ + board_indicate(fw_update_erase_fail); - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE; - goto failure; + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE; + goto failure; } - status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path, - fw_image_size, &fw_word0); - if (status != FLASH_OK) + status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path, + fw_image_size, &fw_word0); + if (status != FLASH_OK) { - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM; - goto failure; + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM; + goto failure; } - /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */ - if (!is_app_valid(fw_word0)) + /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */ + if (!is_app_valid(fw_word0)) { - bootloader.app_valid = 0u; + bootloader.app_valid = 0u; - /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */ - board_indicate(fw_update_invalid_crc); + /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */ + board_indicate(fw_update_invalid_crc); - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE; - goto failure; + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE; + goto failure; } - /* UAVCANBootloader_v0.3 #46: [crc == fw_crc]:write word0 */ - status = bl_flash_write_word((uint32_t) bootloader.fw_image, (uint8_t *) & fw_word0); - if (status != FLASH_OK) + /* UAVCANBootloader_v0.3 #46: [crc == fw_crc]:write word0 */ + status = bl_flash_write_word((uint32_t) bootloader.fw_image, (uint8_t *) & fw_word0); + if (status != FLASH_OK) { - /* Not specifically listed in UAVCANBootloader_v0.3: - * 1023.LogMessage.uavcan */ - error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE; - goto failure; + /* Not specifically listed in UAVCANBootloader_v0.3: + * 1023.LogMessage.uavcan */ + error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE; + goto failure; } - /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */ - bootloader_app_shared_write(&common, BootLoader); + /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */ + bootloader_app_shared_write(&common, BootLoader); - /* Send a completion log message */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_INFO, - UAVCAN_LOGMESSAGE_STAGE_FINALIZE, - UAVCAN_LOGMESSAGE_RESULT_OK); + /* Send a completion log message */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_INFO, + UAVCAN_LOGMESSAGE_STAGE_FINALIZE, + UAVCAN_LOGMESSAGE_RESULT_OK); - /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */ + /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */ boot: - /* UAVCANBootloader_v0.3 #50: jump_to_app */ - application_run(fw_image_size); + /* UAVCANBootloader_v0.3 #50: jump_to_app */ + application_run(fw_image_size); - /* We will fall thru if the Iamage is bad */ + /* We will fall thru if the Iamage is bad */ failure: - /* UAVCANBootloader_v0.3 #28.2: - * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */ - /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #38: [(retries == 0 && - * timeout)]:1023.LogMessage.uavcan */ - /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */ - send_log_message(bootloader.node_id, - UAVCAN_LOGMESSAGE_LEVEL_ERROR, - error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL); - - /* UAVCANBootloader_v0.3 #28.3: - * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #28.10: [Erase - * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #39: [(retries == 0 && - * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */ - /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t, - * STATUS_CRITICAL) */ - bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL; - - /* UAVCANBootloader_v0.3 #28.1: - * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ - /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(), - * RestartWithDelay(20,000ms) */ - /* UAVCANBootloader_v0.3 #45: [crc - * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ - - bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0); - while(!timer_expired(tmr)) + /* UAVCANBootloader_v0.3 #28.2: + * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */ + /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #38: [(retries == 0 && + * timeout)]:1023.LogMessage.uavcan */ + /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */ + send_log_message(bootloader.node_id, + UAVCAN_LOGMESSAGE_LEVEL_ERROR, + error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL); + + /* UAVCANBootloader_v0.3 #28.3: + * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #28.10: [Erase + * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #39: [(retries == 0 && + * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */ + /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t, + * STATUS_CRITICAL) */ + bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL; + + /* UAVCANBootloader_v0.3 #28.1: + * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ + /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(), + * RestartWithDelay(20,000ms) */ + /* UAVCANBootloader_v0.3 #45: [crc + * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */ + + bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0); + while(!timer_expired(tmr)) { - ; + ; } - timer_free(tmr); - up_systemreset(); + timer_free(tmr); + up_systemreset(); } diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c index 1db00dc8f..bb8e08d73 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c @@ -130,11 +130,11 @@ static void uavcan_tx_multiframe_(uavcan_frame_id_t *frame_id, ****************************************************************************/ static can_error_t uavcan_rx_multiframe_(uint8_t node_id, - uavcan_frame_id_t *frame_id, - size_t *message_length, - uint8_t *message, - uint16_t initial_crc, - uint32_t timeout_ms) + uavcan_frame_id_t *frame_id, + size_t *message_length, + uint8_t *message, + uint16_t initial_crc, + uint32_t timeout_ms) { uavcan_frame_id_t rx_id; size_t rx_length; @@ -372,24 +372,24 @@ int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, void uavcan_tx_nodestatus(uint8_t node_id, uint32_t uptime_sec, uint8_t status_code) { - uavcan_nodestatus_t message; - uavcan_frame_id_t frame_id; - uint8_t payload[8]; - size_t frame_len; - - frame_id.transfer_id = 0; - frame_id.last_frame = 1u; - frame_id.frame_index = 0; - frame_id.source_node_id = node_id; - frame_id.transfer_type = MESSAGE_BROADCAST; - frame_id.data_type_id = UAVCAN_NODESTATUS_DTID; - - message.uptime_sec = uptime_sec; - message.status_code = status_code; - message.vendor_specific_status_code = 0u; - frame_len = uavcan_pack_nodestatus(payload, &message); - - can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBNodeStatus); + uavcan_nodestatus_t message; + uavcan_frame_id_t frame_id; + uint8_t payload[8]; + size_t frame_len; + + frame_id.transfer_id = 0; + frame_id.last_frame = 1u; + frame_id.frame_index = 0; + frame_id.source_node_id = node_id; + frame_id.transfer_type = MESSAGE_BROADCAST; + frame_id.data_type_id = UAVCAN_NODESTATUS_DTID; + + message.uptime_sec = uptime_sec; + message.status_code = status_code; + message.vendor_specific_status_code = 0u; + frame_len = uavcan_pack_nodestatus(payload, &message); + + can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBNodeStatus); } /**************************************************************************** @@ -488,7 +488,7 @@ void uavcan_tx_getnodeinfo_response(uint8_t node_id, fixed_length = 6u + sizeof(uavcan_softwareversion_t) + 2u + 16u + 1u; contiguous_length = fixed_length + - response->hardware_version.certificate_of_authenticity_length; + response->hardware_version.certificate_of_authenticity_length; packet_length = contiguous_length + response->name_length; /* Move name so it's contiguous with the start of the packet */ @@ -528,8 +528,8 @@ void uavcan_tx_getnodeinfo_response(uint8_t node_id, ****************************************************************************/ can_error_t uavcan_rx_beginfirmwareupdate_request(uint8_t node_id, - uavcan_beginfirmwareupdate_request_t *request, - uavcan_frame_id_t *frame_id) + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *frame_id) { size_t length; can_error_t status; @@ -657,9 +657,9 @@ can_error_t uavcan_rx_read_response(uint8_t node_id, ****************************************************************************/ void uavcan_tx_getinfo_request(uint8_t node_id, - const uavcan_getinfo_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id) + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id) { uavcan_frame_id_t frame_id; diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h index b59c58a90..f14dbd685 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h @@ -58,13 +58,13 @@ ****************************************************************************/ typedef enum - { +{ CAN_OK = 0, CAN_BOOT_TIMEOUT, CAN_ERROR - } can_error_t; +} can_error_t; - /* UAVCAN message formats */ +/* UAVCAN message formats */ typedef enum { SERVICE_RESPONSE = 0, SERVICE_REQUEST = 1, @@ -73,12 +73,12 @@ typedef enum { } uavcan_transfertype_t; typedef struct packed_struct uavcan_frame_id_t { - uint8_t transfer_id; - uint8_t last_frame; - uint8_t frame_index; - uint8_t source_node_id; - uavcan_transfertype_t transfer_type; - uint16_t data_type_id; + uint8_t transfer_id; + uint8_t last_frame; + uint8_t frame_index; + uint8_t source_node_id; + uavcan_transfertype_t transfer_type; + uint16_t data_type_id; } uavcan_frame_id_t; typedef struct packed_struct uavcan_nodestatus_t { @@ -397,8 +397,8 @@ void uavcan_tx_getnodeinfo_response(uint8_t node_id, ****************************************************************************/ can_error_t uavcan_rx_beginfirmwareupdate_request(uint8_t node_id, - uavcan_beginfirmwareupdate_request_t *request, - uavcan_frame_id_t *frame_id); + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *frame_id); /**************************************************************************** * Name: uavcan_tx_read_request @@ -470,9 +470,9 @@ can_error_t uavcan_rx_read_response(uint8_t node_id, ****************************************************************************/ void uavcan_tx_getinfo_request(uint8_t node_id, - const uavcan_getinfo_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id); + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id); /**************************************************************************** * Name: uavcan_rx_getinfo_response |