aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDavid Sidrane <david_s5@nscdg.com>2015-04-20 17:56:16 -1000
committerDavid Sidrane <david_s5@nscdg.com>2015-04-22 02:30:14 -1000
commit254aa39e2bab1a9ce3eab105e44290b389ba002c (patch)
tree71d2c3e1a06f5467599b697304cb040bad0becb2
parentd89ef3b0dc83c5f3510d6eb95d7b4bbed9f3c690 (diff)
downloadpx4-firmware-254aa39e2bab1a9ce3eab105e44290b389ba002c.tar.gz
px4-firmware-254aa39e2bab1a9ce3eab105e44290b389ba002c.tar.bz2
px4-firmware-254aa39e2bab1a9ce3eab105e44290b389ba002c.zip
Documantation pass
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h258
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/boot.c170
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/crc.c133
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/crc.h114
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/flash.c36
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/flash.h94
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/src/timer.h61
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c26
8 files changed, 576 insertions, 316 deletions
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 3d2a802eb..674621a76 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/board_config.h
@@ -1,9 +1,6 @@
-/************************************************************************************
- * configs/olimexino-stm32/src/olimexino-stm32.h
+/****************************************************************************
*
- * Copyright (C) 2015 Gregory Nutt. All rights reserved.
- * Author: Gregory Nutt <gnutt@nuttx.org>
- * David_s5 <david_s5@nscdg.com>
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,252 +29,131 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- ************************************************************************************/
+ ****************************************************************************/
+
+/*
+ * @file board_config.h
+ *
+ * PX4CANNODEv1 for the bootloader internal definitions
+ * This file is related to the parrent folder version but defines
+ * differnet usages of the hardware for bootloading
+ */
+
+#pragma once
-#ifndef __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H
-#define __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H
/************************************************************************************
* Included Files
************************************************************************************/
-#include <nuttx/config.h>
+#include "../../board_config.h"
#include <nuttx/compiler.h>
#include <stdint.h>
-/************************************************************************************
+/****************************************************************************
* Pre-processor Definitions
- ************************************************************************************/
-
-#if STM32_NSPI < 1
-# undef CONFIG_STM32_SPI1
-# undef CONFIG_STM32_SPI2
-#elif STM32_NSPI < 2
-# undef CONFIG_STM32_SPI2
-#endif
-
-/* LEDs *****************************************************************************
- *
- * GPIO Function MPU Board
- * Pin # Name
- * -- ----- -------------------------------- ----------------------------
- *
- * PA[05] PA5/SPI1_SCK/ADC5 21 D13(SCK1/LED1)
- * PA[01] PA1/USART2_RTS/ADC1/TIM2_CH2 15 D3(LED2)
- */
-
-#define GPIO_LED1 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTA | GPIO_PIN5 | GPIO_OUTPUT_CLEAR)
-#define GPIO_LED_GREEN GPIO_LED1
-#define GPIO_LED2 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTA | GPIO_PIN1 | GPIO_OUTPUT_CLEAR)
-#define GPIO_LED_YELLOW GPIO_LED2
-
-/* BUTTON ***************************************************************************
- *
- * GPIO Function MPU Board
- * Pin # Name
- * -- ----- -------------------------------- ----------------------------
- *
- * PC[09] PC9/TIM3_CH4 40 BOOT0
- *
- */
-
-#define BUTTON_BOOT0n (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN9 | \
- GPIO_EXTI)
-#define IRQBUTTON BUTTON_BOOT0_BIT
-
-/* USBs *****************************************************************************
- *
- * GPIO Function MPU Board
- * Pin # Name
- * -- ----- -------------------------------- ----------------------------
- *
- * PC[11] PC11/USART3_RX 52 USB_P
- * PC[12] PC12/USART3_CK 53 DISC
- *
- */
-
-#define GPIO_USB_VBUS (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN11)
-#define GPIO_USB_PULLUPn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTC | GPIO_PIN12 | GPIO_OUTPUT_SET)
-
-/* SPI ***************************************************************************
- *
- * GPIO Function MPU Board
- * Pin # Name
- * -- ----- -------------------------------- ----------------------------
- *
- * PC[09] PA4/SPI1_NSS/USART2_CK/ADC4 20 D10(#SS1)
- * PD[02] PD2/TIM3_ETR 54 D25(MMC_CS)
- */
-
-#define GPIO_SPI1_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTC | GPIO_PIN9 | GPIO_OUTPUT_SET)
-#define USER_CSn GPIO_SPI1_SSn
-
-#define GPIO_SPI2_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTD | GPIO_PIN2 | GPIO_OUTPUT_SET)
-#define MMCSD_CSn GPIO_SPI2_SSn
+ ****************************************************************************/
-/* CAN ***************************************************************************
+/* Bootloader Option*****************************************************************
*
* GPIO Function MPU Board
* Pin # Name
* -- ----- -------------------------------- ----------------------------
- *
- * PB[08] PB8/TIM4_CH3/I2C1_SCL/CANRX 61 D14(CANRX)
- * PB[09] PB9/TIM4_CH4/I2C1_SDA/CANTX 62 D24(CANTX)
- * PC[13] PC13/ANTI_TAMP 2 D21(CAN_CTRL)
+ * * PC[09] PC9/TIM3_CH4 40 BOOT0
*/
-
-#define GPIO_CAN_CTRL (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | \
- GPIO_PORTC | GPIO_PIN13 | GPIO_OUTPUT_CLEAR)
-
#define GPIO_GETNODEINFO_JUMPER (BUTTON_BOOT0n&~(GPIO_EXTI))
-/************************************************************************************
- * Public Types
- ************************************************************************************/
+/****************************************************************************
+ * Public Type Definitions
+ ****************************************************************************/
+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,
+} uiindication_t;
/************************************************************************************
* Public data
************************************************************************************/
#ifndef __ASSEMBLY__
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
/************************************************************************************
- * Name: stm32_spiinitialize
+ * Name: stm32_boarddeinitialize
*
* Description:
- * Called to configure SPI chip select GPIO pins.
+ * This function is called by the bootloader code priore to booting
+ * the application. Is should place the HW into an benign initialized state.
*
************************************************************************************/
-#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \
- defined(CONFIG_STM32_SPI3)
-void weak_function stm32_spiinitialize(void);
-#endif
+void stm32_boarddeinitialize(void);
-/************************************************************************************
- * Name: stm32_usbinitialize
+/****************************************************************************
+ * Name: board_get_product_name
*
* Description:
- * Called to setup USB-related GPIO pins.
+ * Called to retrive the product name. The retuned alue is a assumed
+ * to be written to a pascal style string that will be length prefixed
+ * and not null terminated
*
- ************************************************************************************/
-
-void stm32_usbinitialize(void);
-
-/************************************************************************************
- * Name: stm32_usb_set_pwr_callback()
- *
- * Description:
- * Called to setup set a call back for USB power state changes.
+ * Input Parameters:
+ * product_name - A pointer to a buffer to write the name.
+ * maxlen - The imum number of chatater that can be written
*
- * Inputs:
- * pwr_changed_handler: An interrupt handler that will be called on VBUS power
- * state changes.
+ * Returned Value:
+ * The length of charaacters written to the buffer.
*
- ************************************************************************************/
+ ****************************************************************************/
-void stm32_usb_set_pwr_callback(xcpt_t pwr_changed_handler);
+uint8_t board_get_product_name(uint8_t * product_name, size_t maxlen);
/****************************************************************************
- * Name: stm32_led_initialize
+ * Name: board_get_hardware_version
*
* Description:
- * This functions is called very early in initialization to perform board-
- * specific initialization of LED-related resources. This includes such
- * things as, for example, configure GPIO pins to drive the LEDs and also
- * putting the LEDs in their correct initial state.
- *
- * NOTE: In most architectures, LED initialization() is called from
- * board-specific initialization and should, therefore, have the name
- * <arch>_led_intialize(). But there are a few architectures where the
- * LED initialization function is still called from common chip
- * architecture logic. This interface is not, however, a common board
- * interface in any event and the name board_led_initialization is
- * deprecated.
+ * Called to retrive the hardware version information.
*
* Input Parameters:
- * None
+ * hw_version - A pointer to a uavcan_hardwareversion_t.
*
* Returned Value:
* None
*
****************************************************************************/
-#ifdef CONFIG_ARCH_LEDS
-void stm32_led_initialize(void);
-#endif
-
-/************************************************************************************
- * Name: stm32_can_initialize
- *
- * Description:
- * Called at application startup time to initialize the CAN functionality.
- *
- ************************************************************************************/
-
-#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2))
-int stm32_can_initialize(void);
-#endif
+void board_get_hardware_version(uavcan_hardwareversion_t * hw_version);
/****************************************************************************
- * Name: usbmsc_archinitialize
+ * Name: board_indicate
*
* Description:
- * Called from the application system/usbmc or the boards_nsh if the
- * application is not included.
- * Perform architecture specific initialization. This function must
- * configure the block device to export via USB. This function must be
- * provided by architecture-specific logic in order to use this add-on.
+ * Provides User feedback to indicate the state of the bootloader
+ * on board specific hardware.
*
- ****************************************************************************/
-
-#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_USBMSC)
-int usbmsc_archinitialize(void);
-#endif
-
-/****************************************************************************
- * Name: composite_archinitialize
+ * Input Parameters:
+ * indication - A member of the uiindication_t
*
- * Description:
- * Called from the application system/composite or the boards_nsh if the
- * application is not included.
- * Perform architecture specific initialization. This function must
- * configure the block device to export via USB. This function must be
- * provided by architecture-specific logic in order to use this add-on.
+ * Returned Value:
+ * None
*
****************************************************************************/
-#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_COMPOSITE)
-extern int composite_archinitialize(void);
-#endif
-
-uint8_t board_get_product_name(uint8_t * product_name);
-void board_get_hardware_version(uavcan_hardwareversion_t * hw_version);
-void board_initialize(void);
-void board_indicate_reset(void);
-void board_indicate_autobaud_start(void);
-void board_indicate_autobaud_end(void);
-void board_indicate_allocation_start(void);
-void board_indicate_allocation_end(void);
-void board_indicate_fw_update_start(void);
-void board_indicate_fw_update_erase_fail(void);
-void board_indicate_fw_update_invalid_response(void);
-void board_indicate_fw_update_timeout(void);
-void board_indicate_fw_update_invalid_crc(void);
-void board_indicate_jump_to_app(void);
-void stm32_boarddeinitialize(void);
-uint8_t board_get_wait_for_getnodeinfo_flag(void);
+void board_indicate(uiindication_t indication);
#endif /* __ASSEMBLY__ */
-#endif /* __CONFIGS_OLIMEXINO_STM32_SRC_OLIMEXINO_STM32_H */
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c
index 6f1a07d13..57897910e 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/boot.c
@@ -1,9 +1,9 @@
-/************************************************************************************
- * configs/olimexino-stm32/src/bl_boot.c
+/****************************************************************************
*
- * Copyright (C) 2015 Gregory Nutt. All rights reserved.
- * Author: Gregory Nutt <gnutt@nuttx.org>
- * David Sidrane <david_s5@nscdg.com>
+ * Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ * Author: Ben Dyer <ben_dyer@mac.com>
+ * Pavel Kirienko <pavel.kirienko@zubax.com>
+ * David Sidrane <david_s5@nscdg.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -15,7 +15,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,11 +32,11 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- ************************************************************************************/
+ ****************************************************************************/
-/************************************************************************************
+/****************************************************************************
* Included Files
- ************************************************************************************/
+ ****************************************************************************/
#include <nuttx/config.h>
#include "app_config.h"
@@ -46,21 +46,35 @@
#include <nuttx/board.h>
-
#include "board_config.h"
-
-/************************************************************************************
+/****************************************************************************
* Pre-processor Definitions
- ************************************************************************************/
+ ****************************************************************************/
-/************************************************************************************
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
* Private Functions
- ************************************************************************************/
+ ****************************************************************************/
-/************************************************************************************
+/****************************************************************************
* Public Functions
- ************************************************************************************/
+ ****************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
@@ -89,6 +103,15 @@ __EXPORT void stm32_boardinitialize(void)
}
+/************************************************************************************
+ * Name: stm32_boarddeinitialize
+ *
+ * Description:
+ * This function is called by the bootloader code priore to booting
+ * the application. Is should place the HW into an benign initialized state.
+ *
+ ************************************************************************************/
+
void stm32_boarddeinitialize(void)
{
@@ -96,23 +119,46 @@ void stm32_boarddeinitialize(void)
STM32_RCC_APB1RSTR);
}
-#include <nuttx/config.h>
-#include "app_config.h"
-
-#include <stdint.h>
-
-#include "stm32.h"
-
-
+/****************************************************************************
+ * Name: board_get_product_name
+ *
+ * Description:
+ * Called to retrive the product name. The retuned alue is a assumed
+ * to be written to a pascal style string that will be length prefixed
+ * and not null terminated
+ *
+ * Input Parameters:
+ * product_name - A pointer to a buffer to write the name.
+ * maxlen - The imum number of chatater that can be written
+ *
+ * Returned Value:
+ * The length of charaacters written to the buffer.
+ *
+ ****************************************************************************/
-uint8_t board_get_product_name(uint8_t * product_name)
+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;
}
+/****************************************************************************
+ * Name: board_get_hardware_version
+ *
+ * Description:
+ * Called to retrive the hardware version information.
+ *
+ * Input Parameters:
+ * hw_version - A pointer to a uavcan_hardwareversion_t.
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
void board_get_hardware_version(uavcan_hardwareversion_t * hw_version)
{
uint32_t i;
@@ -138,62 +184,22 @@ void board_get_hardware_version(uavcan_hardwareversion_t * hw_version)
hw_version->certificate_of_authenticity_length = 0u;
}
-void board_indicate_reset(void)
-{
-
-}
-
-void board_indicate_autobaud_start(void)
-{
-
-}
-
-void board_indicate_autobaud_end(void)
-{
-
-}
-
-void board_indicate_allocation_start(void)
-{
-
-}
-
-void board_indicate_allocation_end(void)
-{
-
-}
-
-void board_indicate_fw_update_start(void)
-{
-
-}
-
-void board_indicate_fw_update_erase_fail(void)
-{
-
-}
-
-void board_indicate_fw_update_invalid_response(void)
-{
-
-}
-
-void board_indicate_fw_update_timeout(void)
-{
-
-}
-
-void board_indicate_fw_update_invalid_crc(void)
-{
-
-}
-
-void board_indicate_jump_to_app(void)
-{
-
-}
+/****************************************************************************
+ * Name: board_indicate
+ *
+ * Description:
+ * Provides User feedback to indicate the state of the bootloader
+ * on board specific hardware.
+ *
+ * Input Parameters:
+ * indication - A member of the uiindication_t
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
-uint8_t board_get_wait_for_getnodeinfo_flag(void)
+void board_indicate(uiindication_t indication)
{
- return 1u;
+//todo:Indicate state on Led
}
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c
index cee1dae3f..882829ff1 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.c
@@ -1,14 +1,91 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ * Author: Ben Dyer <ben_dyer@mac.com>
+ * Pavel Kirienko <pavel.kirienko@zubax.com>
+ * David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
#include <stdint.h>
#include <stdlib.h>
#include "crc.h"
-/*
-CRC-16-CCITT
-Initial value: 0xFFFF
-Poly: 0x1021
-Reverse: no
-Output xor: 0
-*/
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: crc16_add
+ *
+ * Description:
+ * Use to caculates a CRC-16-CCITT using the polynomial of
+ * 0x1021 by adding a value successive values.
+ *
+ * Input Parameters:
+ * crc - The running total of the crc 16
+ * value - The value to add
+ *
+ * Returned Value:
+ * The current crc16 with the value processed.
+ *
+ ****************************************************************************/
+
uint16_t crc16_add(uint16_t crc, uint8_t value)
{
uint32_t i;
@@ -28,6 +105,23 @@ uint16_t crc16_add(uint16_t crc, uint8_t value)
return crc;
}
+/****************************************************************************
+ * Name: crc16_signature
+ *
+ * Description:
+ * Caculates a CRC-16-CCITT using the crc16_add
+ * function
+ *
+ * Input Parameters:
+ * initial - The Inital value to uses as the crc's statrting point
+ * length - The number of bytes to add to the crc
+ * bytes - A pointer to any array of length bytes
+ *
+ * Returned Value:
+ * The crc16 of the array of bytes
+ *
+ ****************************************************************************/
+
uint16_t crc16_signature(uint16_t initial, size_t length,
const uint8_t *bytes)
{
@@ -38,16 +132,23 @@ uint16_t crc16_signature(uint16_t initial, size_t length,
return initial ^ CRC16_OUTPUT_XOR;
}
+/****************************************************************************
+ * Name: crc64_add
+ *
+ * Description:
+ * Caculates a CRC-64-WE usinsg the polynomial of 0x42F0E1EBA9EA3693
+ * See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64
+ * Check: 0x62EC59E3F1A4F00A
+ *
+ * Input Parameters:
+ * crc - The running total of the crc 64
+ * value - The value to add
+ *
+ * Returned Value:
+ * The current crc64 with the value processed.
+ *
+ ****************************************************************************/
-/*
-CRC-64-WE
-Description: http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64
-Initial value: 0xFFFFFFFFFFFFFFFF
-Poly: 0x42F0E1EBA9EA3693
-Reverse: no
-Output xor: 0xFFFFFFFFFFFFFFFF
-Check: 0x62EC59E3F1A4F00A
-*/
uint64_t crc64_add(uint64_t crc, uint8_t value)
{
uint32_t i;
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h
index 8eaf39c04..2ca756b9c 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/crc.h
@@ -1,11 +1,117 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ * Author: Ben Dyer <ben_dyer@mac.com>
+ * Pavel Kirienko <pavel.kirienko@zubax.com>
+ * David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
#pragma once
-#define CRC16_INITIAL 0xFFFFu
-#define CRC16_OUTPUT_XOR 0x0000u
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+#define CRC16_INITIAL 0xFFFFu
+#define CRC16_OUTPUT_XOR 0x0000u
+#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull
+#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull
+
+/****************************************************************************
+ * Public Type Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+/****************************************************************************
+ * Name: crc16_add
+ *
+ * Description:
+ * Use to caculates a CRC-16-CCITT using the polynomial of
+ * 0x1021 by adding a value successive values.
+ *
+ * Input Parameters:
+ * crc - The running total of the crc 16
+ * value - The value to add
+ *
+ * Returned Value:
+ * The current crc16 with the value processed.
+ *
+ ****************************************************************************/
+
uint16_t crc16_add(uint16_t crc, uint8_t value);
+
+/****************************************************************************
+ * Name: crc16_signature
+ *
+ * Description:
+ * Caculates a CRC-16-CCITT using the crc16_add
+ * function
+ *
+ * Input Parameters:
+ * initial - The Inital value to uses as the crc's statrting point
+ * length - The number of bytes to add to the crc
+ * bytes - A pointer to any array of length bytes
+ *
+ * Returned Value:
+ * The crc16 of the array of bytes
+ *
+ ****************************************************************************/
+
uint16_t crc16_signature(uint16_t initial, size_t length,
const uint8_t *bytes);
-#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull
-#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull
+/****************************************************************************
+ * Name: crc64_add
+ *
+ * Description:
+ * Caculates a CRC-64-WE usinsg the polynomial of 0x42F0E1EBA9EA3693
+ * See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64
+ * Check: 0x62EC59E3F1A4F00A
+ *
+ * Input Parameters:
+ * crc - The running total of the crc 64
+ * value - The value to add
+ *
+ * Returned Value:
+ * The current crc64 with the value processed.
+ *
+ ****************************************************************************/
+
uint64_t crc64_add(uint64_t crc, uint8_t value);
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c
index cdf177ac6..8c28b1fbd 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.c
@@ -12,7 +12,21 @@
#include "flash.h"
-flash_error_t bl_flash_erase(void)
+/****************************************************************************
+ * Name: bl_flash_erase
+ *
+ * Description:
+ * This function erases the flash starting at the given address
+ *
+ * Input Parameters:
+ * address - The address of the flash to erase
+ *
+ * Returned value:
+ * On sucess FLASH_OK On Error one of the flash_error_t
+ *
+ ****************************************************************************/
+
+flash_error_t bl_flash_erase(size_t address)
{
/*
* FIXME (?): this may take a long time, and while flash is being erased it
@@ -21,13 +35,13 @@ flash_error_t bl_flash_erase(void)
flash_error_t status = FLASH_ERROR_AFU;
- ssize_t bllastpage = up_progmem_getpage(APPLICATION_LOAD_ADDRESS - 1);
+ ssize_t bllastpage = up_progmem_getpage(address - 1);
if (bllastpage >= 0)
{
status = FLASH_ERROR_SUICIDE;
- ssize_t appfirstpage = up_progmem_getpage(APPLICATION_LOAD_ADDRESS);
+ ssize_t appfirstpage = up_progmem_getpage(address);
if (appfirstpage > bllastpage)
{
@@ -51,6 +65,22 @@ flash_error_t bl_flash_erase(void)
return status;
}
+/****************************************************************************
+ * Name: bl_flash_write_word
+ *
+ * Description:
+ * This function erases the flash starting at the given address
+ *
+ * Input Parameters:
+ * flash_address - The address of the flash to write
+ * data - A pointer to a buffer of 4 bytes to be written
+ * to the flash.
+ *
+ * Returned value:
+ * On sucess FLASH_OK On Error one of the flash_error_t
+ *
+ ****************************************************************************/
+
flash_error_t bl_flash_write_word(uint32_t flash_address, const uint8_t data[4])
{
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h
index 2be47848b..db5f5827b 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/flash.h
@@ -1,7 +1,54 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ * Author: Ben Dyer <ben_dyer@mac.com>
+ * Pavel Kirienko <pavel.kirienko@zubax.com>
+ * David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
#pragma once
-typedef enum
- {
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Type Definitions
+ ****************************************************************************/
+
+typedef enum {
FLASH_OK = 0,
FLASH_ERROR,
FLASH_ERASE_ERROR,
@@ -9,8 +56,45 @@ typedef enum
FLASH_ERROR_SUICIDE,
FLASH_ERROR_AFU,
- } flash_error_t;
+} flash_error_t;
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+/****************************************************************************
+ * Name: bl_flash_erase
+ *
+ * Description:
+ * This function erases the flash starting at the given address
+ *
+ * Input Parameters:
+ * address - The word aligned address of the flash to erase
+ *
+ * Returned value:
+ * On sucess FLASH_OK On Error one of the flash_error_t
+ *
+ ****************************************************************************/
+
+flash_error_t bl_flash_erase(size_t address);
+
+/****************************************************************************
+ * Name: bl_flash_write_word
+ *
+ * Description:
+ * This function erases the flash starting at the given address
+ *
+ * Input Parameters:
+ * flash_address - The address of the flash to write
+ * data - A pointer to a buffer of 4 bytes to be written
+ * to the flash.
+ *
+ * Returned value:
+ * On sucess FLASH_OK On Error one of the flash_error_t
+ *
+ ****************************************************************************/
-flash_error_t bl_flash_erase(void);
flash_error_t bl_flash_write_word(uint32_t flash_address, const uint8_t * word);
-uint64_t flash_crc(uint32_t flash_address, size_t length, uint64_t initial_crc);
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h
index 530a60859..65f977d3b 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/src/timer.h
@@ -1,9 +1,59 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ * Author: Ben Dyer <ben_dyer@mac.com>
+ * Pavel Kirienko <pavel.kirienko@zubax.com>
+ * David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
#pragma once
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
#include "stm32.h"
#include "nvic.h"
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000)
+#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000)
+
+/****************************************************************************
+ * Public Type Definitions
+ ****************************************************************************/
+
typedef enum {
//! Specifies a one-shot timer. After notification timer is discarded.
modeOneShot = 1,
@@ -17,6 +67,7 @@ typedef enum {
} bl_timer_modes_t;
+
typedef uint8_t bl_timer_id;
typedef uint32_t time_ms_t;
typedef volatile time_ms_t* time_ref_t;
@@ -31,8 +82,16 @@ typedef struct {
} bl_timer_cb_t;
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
extern bl_timer_cb_t null_cb;
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
void timer_init(void);
bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow, bl_timer_cb_t *fc);
@@ -50,8 +109,6 @@ static inline int timer_ref_expired(time_ref_t ref)
}
-#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000)
-#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000)
static inline time_hrt_cycles_t timer_hrt_read(void)
{
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
index a990b389c..6be947458 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
@@ -91,7 +91,7 @@ static void node_info_process(bl_timer_id id, void *context)
uavcan_pack_nodestatus(response.nodestatus, &node_status);
board_get_hardware_version(&response.hardware_version);
- response.name_length = board_get_product_name(response.name);
+ 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) {
@@ -167,7 +167,7 @@ __EXPORT int main(int argc, char *argv[])
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)) &&
@@ -242,7 +242,7 @@ __EXPORT int main(int argc, char *argv[])
while (!fw_source_node_id);
timer_stop(tboot);
- board_indicate_fw_update_start();
+ board_indicate(fw_update_start);
file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length,
fw_source_node_id);
@@ -265,12 +265,12 @@ __EXPORT int main(int argc, char *argv[])
bootloader.app_valid = false;
- status = bl_flash_erase();
+ 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();
+ board_indicate(fw_update_erase_fail);
error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE;
goto failure;
@@ -290,7 +290,7 @@ __EXPORT int main(int argc, char *argv[])
bootloader.app_valid = 0u;
/* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */
- board_indicate_fw_update_invalid_crc();
+ board_indicate(fw_update_invalid_crc);
error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE;
goto failure;
@@ -383,16 +383,16 @@ uint64_t get_short_unique_id(void)
int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, uint32_t *node_id)
{
- board_indicate_autobaud_start();
+ board_indicate(autobaud_start);
int rv = can_autobaud(speed, tboot);
if (rv != CAN_BOOT_TIMEOUT) {
can_init(*speed, CAN_Mode_Normal);
- board_indicate_autobaud_end();
- board_indicate_allocation_start();
+ 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();
+ board_indicate(allocation_end);
}
}
return rv;
@@ -721,7 +721,7 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id,
/* UAVCANBootloader_v0.3 #35: [(retries != 0 && timeout) ||
* !ValidReadReadResponse]:INDICATE_FW_UPDATE_INVALID_RESPONSE */
- board_indicate_fw_update_invalid_response();
+ board_indicate(fw_update_invalid_response);
/* UAVCANBootloader_v0.3 #36: [(retries != 0 && timeout) ||
* !ValidReadReadResponse]:1023.LogMessage.uavcan */
@@ -737,7 +737,7 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id,
{
/* UAVCANBootloader_v0.3 #37: [(retries == 0 &&
* timeout]:INDICATE_FW_UPDATE_TIMEOUT */
- board_indicate_fw_update_timeout();
+ board_indicate(fw_update_timeout);
break;
}
@@ -919,7 +919,7 @@ void application_run(size_t fw_image_size)
NVIC_SYSTICK_CTRL);
/* and set a specific LED pattern */
- board_indicate_jump_to_app();
+ board_indicate(jump_to_app);
/* the interface */