Document pass on main.c

1 files changed, 657 insertions, 384 deletions

diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
index d7859ea7a..27075a769 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/main.c
@@ -1,3 +1,43 @@
+/****************************************************************************
+ *
+ *   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 <nuttx/config.h>
 #include "board_config.h"
 
@@ -21,36 +61,15 @@
 
 #include "board_config.h"
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
 
-uint64_t get_short_unique_id(void);
-void send_log_message(uint8_t node_id, uint8_t level, uint8_t stage,
-                                 uint8_t status);
-
-int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id);
-int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed,
-                                    uint32_t *node_id);
-void poll_getnodeinfo(uint8_t node_id, uint32_t uptime, uint8_t status);
-
-int wait_for_beginfirmwareupdate(bl_timer_id tboot,
-                                             uint8_t * fw_path,
-                                             uint8_t * fw_path_length,
-                                             uint8_t * fw_source_node_id);
-
-void file_getinfo(size_t * fw_image_size, uint64_t * fw_image_crc,
-                             const uint8_t * fw_path, uint8_t fw_path_length,
-                             uint8_t fw_source_node_id);
-
-flash_error_t file_read_and_program(uint8_t fw_source_node_id,
-                                               uint8_t fw_path_length,
-                                               const uint8_t * fw_path,
-                                               size_t fw_image_size,
-                                               uint32_t * fw_word0);
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
 
-static uint8_t is_app_valid(uint32_t first_word);
-void find_descriptor(void);
-void application_run(size_t fw_image_size);
-
-volatile struct {
+typedef volatile struct bootloader_t {
   can_speed_t bus_speed;
   volatile uint8_t node_id;
   volatile uint8_t status_code;
@@ -62,18 +81,64 @@ volatile struct {
   bool  app_bl_request;
   bool sent_node_info_response;
 
-} bootloader;
-
+} bootloader_t;
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+bootloader_t bootloader;
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: uptime_process
+ *
+ * Description:
+ *   Run as a timer callback off the system tick ISR. This function counts
+ *   Seconds of up time.
+ *
+ * Input Parameters:
+ *   Not Used.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
 
 static void uptime_process(bl_timer_id id, void *context)
 {
   bootloader.uptime++;
 }
 
-/*
- * Once we have a noide id
- * Handle GetNodeInfo replies regardless of application state
- */
+
+/****************************************************************************
+ * Name: node_info_process
+ *
+ * Description:
+ *   Run as a timer callback off the system tick ISR.
+ *   Once we have a node id, this process is started and handles the
+ *   GetNodeInfo replies.  This function uses the fifoGetNodeInfo and
+ *   MBGetNodeInfo of the CAN so it can coexist regardless of
+ *   application state and what is it doing.
+ *
+ * Input Parameters:
+ *   Not Used.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
 
 static void node_info_process(bl_timer_id id, void *context)
 {
@@ -118,287 +183,186 @@ static void node_info_process(bl_timer_id id, void *context)
 
       bootloader.sent_node_info_response = true;
   }
-
-
 }
 
-/*
- * Once we have a noide id
- * Handle GetNodeInfo replies regardless of application state
- */
+/****************************************************************************
+ * Name: node_status_process
+ *
+ * Description:
+ *   Run as a timer callback off the system tick ISR.
+ *   Once we have a node id, this process is started and sends the
+ *   NodeStatus messages.  This function uses the MBNodeStatus
+ *   of the CAN so it can coexist regardless of application state ant
+ *   what is it doing.
+ *
+ * Input Parameters:
+ *   Not Used.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
 static void node_status_process(bl_timer_id id, void *context)
 {
   uavcan_tx_nodestatus(bootloader.node_id, bootloader.uptime, bootloader.status_code);
 }
 
-__EXPORT int main(int argc, char *argv[])
+/****************************************************************************
+ * Name: send_log_message
+ *
+ * Description:
+ *   This functions sends uavcan logmessage type data. See uavcan/protocol.h
+ *   UAVCAN_LOGMESSAGE_xxx defines.
+ *
+ * Input Parameters:
+ *   node_id - This node's node id
+ *   level   - Log Level of the logmessage DEBUG, INFO, WARN, ERROR
+ *   stage   - The Stage the application is at. see UAVCAN_LOGMESSAGE_STAGE_x
+ *   status  - The status of that stage. Start, Fail OK
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+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;
 
-  uint64_t fw_image_crc;
-  size_t fw_image_size;
-  uint32_t fw_word0;
-  uint8_t fw_path[200];
-  uint8_t fw_path_length;
-  uint8_t fw_source_node_id;
-  uint8_t error_log_stage;
-  flash_error_t status;
-  bootloader_app_shared_t common;
-
-#ifdef OPT_ENABLE_WD
-  /* ... */
-#endif
-
-  memset((void *)&bootloader, 0, sizeof(bootloader));
-
-  bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING;
-
-  error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT;
-
-  bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS);
-
-#if defined(OPT_WAIT_FOR_GETNODEINFO)
-  bootloader.wait_for_getnodeinfo = 1;
-#endif
-
-#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
-  bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT;
-#endif
-
-
-  bootloader.app_valid = is_app_valid(bootloader.fw_image[0]);
-
-  board_indicate(reset);
-
-
-  bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) &&
-    common.bus_speed && common.node_id;
-  bootloader_app_shared_invalidate();
-
-  bl_timer_cb_t p = null_cb;
-  p.cb = uptime_process;
-  timer_allocate(modeRepeating|modeStarted, 1000, &p);
-
-  p.cb = node_info_process;
-  bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p);
-
-  p.cb = node_status_process;
-  bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p);
-
-
-
-  bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0);
-
-  if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) {
-      timer_start(tboot);
-  }
-
-  if (bootloader.app_bl_request) {
-
-      bootloader.bus_speed = common.bus_speed;
-      bootloader.node_id = (uint8_t) common.node_id;
-      can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal);
-
-  } else {
-
-      can_speed_t speed;
-
-      if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) {
-          goto boot;
-      }
-
-      /* reset uptime */
-      bootloader.uptime = 0;
-      common.bus_speed = can_speed2freq(speed);
-      bootloader.node_id = (uint8_t) common.node_id;
-
-  }
-  // Start Processes that requier Node Id
-  timer_start(tinfo);
-  timer_start(tstatus);
-
-  while (!bootloader.sent_node_info_response) {
-      if (timer_expired(tboot))
-        {
-          goto boot;
-        }
-  }
-
-  if (bootloader.app_valid &&
-     (bootloader.wait_for_getnodeinfo ||
-      bootloader.app_bl_request)) {
+  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;
 
-      timer_start(tboot);
-  }
+  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);
+}
 
-  do
-    {
-      if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path,
-                                                          &fw_path_length,
-                                                          &fw_source_node_id))
-        {
-          goto boot;
+/****************************************************************************
+ * Name: find_descriptor
+ *
+ * Description:
+ *   This functions looks through the application image in flash on 8 byte
+ *   aligned boundaries to find the Application firmware descriptor.
+ *   Once it is found the bootloader.fw_image_descriptor is set to point to
+ *   it.
+ *
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned State:
+ *   If found bootloader.fw_image_descriptor points to the app_descriptor_t
+ *   of the application firmware image.
+ *   If not found bootloader.fw_image_descriptor = NULL
+ *
+ ****************************************************************************/
+
+static void find_descriptor(void)
+{
+  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 (!fw_source_node_id);
-
-  timer_stop(tboot);
-  board_indicate(fw_update_start);
-
-  file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length,
-               fw_source_node_id);
-
-  //todo:Check this
-  if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc)
-    {
-      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO;
-      goto failure;
-    }
-
-  /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */
-  send_log_message(bootloader.node_id,
-                              UAVCAN_LOGMESSAGE_LEVEL_INFO,
-                              UAVCAN_LOGMESSAGE_STAGE_ERASE,
-                              UAVCAN_LOGMESSAGE_RESULT_START);
-
-
-  /* Need to signal that the app is no longer valid  if Node Info Request are done */
-
-  bootloader.app_valid = false;
-
-  status = bl_flash_erase(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);
-
-      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE;
-      goto failure;
-    }
-
-  status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path,
-                                 fw_image_size, &fw_word0);
-  if (status != FLASH_OK)
-    {
-      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM;
-      goto failure;
-    }
-
-  /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */
-  if (!is_app_valid(fw_word0))
-    {
-      bootloader.app_valid = 0u;
+    } while(bootloader.fw_image_descriptor == NULL && ++p < APPLICATION_LAST_64BIT_ADDRRESS);
+}
 
-      /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */
-      board_indicate(fw_update_invalid_crc);
+/****************************************************************************
+ * Name: is_app_valid
+ *
+ * Description:
+ *   This functions validates the applications image based on the validity of
+ *   the Application firmware descriptor's crc and the value of the first word
+ *   in the FLASH image.
+ *
+ *
+ * Input Parameters:
+ *   first_word - the value read from the first word of the Application's
+ *   in FLASH image.
+ *
+ * Returned Value:
+ *   true if the application in flash is valid., false otherwise.
+ *
+ ****************************************************************************/
+
+static bool is_app_valid(uint32_t first_word)
+{
+  uint64_t crc;
+  size_t i, length, crc_offset;
+  uint8_t byte;
 
-      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE;
-      goto failure;
-    }
+  find_descriptor();
 
-  /* 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)
+  if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu)
     {
-      /* Not specifically listed in UAVCANBootloader_v0.3:
-       * 1023.LogMessage.uavcan */
-      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE;
-      goto failure;
+      return false;
     }
 
-  /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */
-  bootloader_app_shared_write(&common, BootLoader);
-
-  /* Send a completion log message */
-  send_log_message(bootloader.node_id,
-                              UAVCAN_LOGMESSAGE_LEVEL_INFO,
-                              UAVCAN_LOGMESSAGE_STAGE_FINALIZE,
-                              UAVCAN_LOGMESSAGE_RESULT_OK);
-
-  /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */
-
-boot:
-  /* UAVCANBootloader_v0.3 #50: jump_to_app */
-  application_run(fw_image_size);
-
-  /* We will fall thru if the Iamage is bad */
-
-failure:
-  /* UAVCANBootloader_v0.3 #28.2:
-   * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */
-  /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */
-  /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */
-  /* UAVCANBootloader_v0.3 #38: [(retries == 0 &&
-   * timeout)]:1023.LogMessage.uavcan */
-  /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */
-  send_log_message(bootloader.node_id,
-                              UAVCAN_LOGMESSAGE_LEVEL_ERROR,
-                              error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL);
-
-  /* UAVCANBootloader_v0.3 #28.3:
-   * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t,
-   * STATUS_CRITICAL) */
-  /* UAVCANBootloader_v0.3 #28.10: [Erase
-   * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */
-  /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t,
-   * STATUS_CRITICAL) */
-  /* UAVCANBootloader_v0.3 #39: [(retries == 0 &&
-   * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */
-  /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t,
-   * STATUS_CRITICAL) */
-  bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL;
-
-  /* UAVCANBootloader_v0.3 #28.1:
-   * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */
-  /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(),
-   * RestartWithDelay(20,000ms) */
-  /* UAVCANBootloader_v0.3 #45: [crc
-   * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */
+  length = bootloader.fw_image_descriptor->image_size;
+  crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) -
+    (size_t) bootloader.fw_image;
 
-  bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0);
-  while(!timer_expired(tmr))
+  crc = CRC64_INITIAL;
+  for (i = 0u; i < 4u; i++)
     {
-      ;
+      crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u)));
     }
-  timer_free(tmr);
-  up_systemreset();
-}
-
-uint64_t get_short_unique_id(void)
-{
-  uavcan_hardwareversion_t hw_version;
-  uint64_t result;
-  size_t i;
-
-  board_get_hardware_version(&hw_version);
-
-  result = CRC64_INITIAL;
-  for (i = 0; i < 16u; i++)
+  for (i = 4u; i < length; i++)
     {
-      result = crc64_add(result, hw_version.unique_id[i]);
+      if (crc_offset <= i && i < crc_offset + 8u)
+        {
+          /* Zero out the CRC field while computing the CRC */
+          byte = 0u;
+        }
+      else
+        {
+          byte = ((volatile uint8_t *)bootloader.fw_image)[i];
+        }
+      crc = crc64_add(crc, byte);
     }
+  crc ^= CRC64_OUTPUT_XOR;
 
-  return (result ^ CRC64_OUTPUT_XOR) & 0x01FFFFFFFFFFFFFFL;
-}
-
-int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, uint32_t *node_id)
-{
-  board_indicate(autobaud_start);
-  int rv = can_autobaud(speed, tboot);
-  if (rv != CAN_BOOT_TIMEOUT) {
-      can_init(*speed, CAN_Mode_Normal);
-
-      board_indicate(autobaud_end);
-      board_indicate(allocation_start);
-      rv = get_dynamic_node_id(tboot, node_id);
-      if (rv != CAN_BOOT_TIMEOUT) {
-        board_indicate(allocation_end);
-      }
-  }
-  return rv;
+  return crc == bootloader.fw_image_descriptor->image_crc;
 }
 
-int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id)
+/****************************************************************************
+ * Name: get_dynamic_node_id
+ *
+ * Description:
+ *   This functions performs the client side of the uavcan specification
+ *   for dynamic node allocation.
+ *
+ * Input Parameters:
+ *   tboot             - The id of the timer to use at the tBoot timeout.
+ *   allocated_node_id - A pointer to the location that should receive the
+ *                       allocated node id.
+ * Returned Value:
+ *   CAN_OK            - Indicates the a node id has been allocated to this
+ *                       node.
+ *   CAN_BOOT_TIMEOUT - Indicates that tboot expired before an allocation
+ *                      was done.
+ *
+ ****************************************************************************/
+
+static int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id)
 {
     uavcan_hardwareversion_t hw_version;
     uavcan_frame_id_t rx_frame_id;
@@ -550,11 +514,31 @@ int get_dynamic_node_id(bl_timer_id tboot, uint32_t *allocated_node_id)
     return *allocated_node_id == 0 ? CAN_BOOT_TIMEOUT : CAN_OK;
 }
 
-
-int wait_for_beginfirmwareupdate(bl_timer_id tboot,
-                                             uint8_t * fw_path,
-                                             uint8_t * fw_path_length,
-                                             uint8_t * fw_source_node_id)
+/****************************************************************************
+ * Name: wait_for_beginfirmwareupdate
+ *
+ * Description:
+ *   This functions performs the client side of the uavcan specification
+ *   for begin firmware update.
+ *
+ * Input Parameters:
+ *   tboot              - The id of the timer to use at the tBoot timeout.
+ *   fw_path            - A pointer to the location that should receive the
+ *                        path of the firmware file to read.
+ *   fw_path_length    -  A pointer to return the path length in.
+ *   fw_source_node_id -  A pointer to return the node id of the node to
+ *                        read the file from.
+ * Returned Value:
+ *   CAN_OK            - Indicates the a beginfirmwareupdate was received and
+ *                       processed successful.
+ *   CAN_BOOT_TIMEOUT - Indicates that tboot expired before a
+ *                      beginfirmwareupdate was received.
+ *
+ ****************************************************************************/
+
+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;
@@ -602,10 +586,32 @@ int wait_for_beginfirmwareupdate(bl_timer_id tboot,
   return status;
 }
 
-void file_getinfo(size_t * fw_image_size,
-                             uint64_t * fw_image_crc,
-                             const uint8_t * fw_path,
-                             uint8_t fw_path_length, uint8_t fw_source_node_id)
+/****************************************************************************
+ * Name: file_getinfo
+ *
+ * Description:
+ *   This functions performs the client side of the uavcan specification
+ *   for getinfo (for a file).
+ *
+ * Input Parameters:
+ *   fw_image_size     - A pointer to the location that should receive the
+ *                       firmware image size.
+ *   fw_image_crc      - A pointer to the location that should receive the
+ *                       firmware image crc.
+ *   fw_path           - A pointer to the path of the firmware file that's
+ *                       info is being requested.
+ *   fw_path_length    - The path length of the firmware file that's
+ *                       info is being requested.
+ *   fw_source_node_id - The node id of the node to get the info from.
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+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;
@@ -658,7 +664,31 @@ void file_getinfo(size_t * fw_image_size,
     }
 }
 
-flash_error_t file_read_and_program(uint8_t fw_source_node_id,
+/****************************************************************************
+ * Name: file_read_and_program
+ *
+ * Description:
+ *   This functions performs the client side of the uavcan specification
+ *   for file read and programs the flash.
+ *
+ * Input Parameters:
+ *   fw_source_node_id - The node id of the node to read the file from.
+ *   fw_path_length    - The path length of the firmware file that is
+ *                       being read.
+ *   fw_path           - A pointer to the path of the firmware file that
+ *                       is being read.
+ *   fw_image_size     - The size the fw image file should be.
+ *   fw_word0          - A pointer to the loactions to store the first
+ *                       ford of the fw image.
+ * Returned Value:
+ *   FLASH_OK          - Indicates that the correct amount of data has
+ *                       ben programed to the FLASH.
+ *                       processed successful.
+ *   FLASH_ERROR       - Indicates that an error occurred
+ *
+ ****************************************************************************/
+
+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,
@@ -670,8 +700,11 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id,
   can_error_t can_status;
   flash_error_t flash_status;
   uint32_t next_read_deadline;
-  uint8_t transfer_id, retries, write_remainder[4], write_remainder_length,
-    *data;
+  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++)
@@ -791,7 +824,7 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id,
          flash_status == FLASH_OK);
 
   /* 
-   * Return success iff the last read succeeded, the last write succeeded, the
+   * 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 &&
@@ -805,87 +838,20 @@ flash_error_t file_read_and_program(uint8_t fw_source_node_id,
     }
 }
 
-void send_log_message(uint8_t node_id,
-                                 uint8_t level, uint8_t stage, uint8_t status)
-{
-  uavcan_logmessage_t message;
-  uavcan_frame_id_t frame_id;
-  uint8_t payload[8];
-  size_t frame_len;
-
-  frame_id.transfer_id = 0;
-  frame_id.last_frame = 1u;
-  frame_id.frame_index = 0;
-  frame_id.source_node_id = node_id;
-  frame_id.transfer_type = MESSAGE_BROADCAST;
-  frame_id.data_type_id = UAVCAN_LOGMESSAGE_DTID;
-
-  message.level = level;
-  message.message[0] = stage;
-  message.message[1] = status;
-  frame_len = uavcan_pack_logmessage(payload, &message);
-  can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBAll);
-}
-
-static uint8_t is_app_valid(uint32_t first_word)
-{
-  uint64_t crc;
-  size_t i, length, crc_offset;
-  uint8_t byte;
-
-  find_descriptor();
-
-  /* UAVCANBootloader_v0.3 #7: bool AppValid = (AppFlash[0] != 0xffffffff) &&
-   * ComputeAppCRC(APP_LOADADDR, APP_INFO_OFFSET) */
-  if (!bootloader.fw_image_descriptor || first_word == 0xFFFFFFFFu)
-    {
-      return 0u;
-    }
-
-  length = bootloader.fw_image_descriptor->image_size;
-  crc_offset = (size_t) (&bootloader.fw_image_descriptor->image_crc) -
-    (size_t) bootloader.fw_image;
-
-  crc = CRC64_INITIAL;
-  for (i = 0u; i < 4u; i++)
-    {
-      crc = crc64_add(crc, (uint8_t) (first_word >> (i << 3u)));
-    }
-  for (i = 4u; i < length; i++)
-    {
-      if (crc_offset <= i && i < crc_offset + 8u)
-        {
-          /* Zero out the CRC field while computing the CRC */
-          byte = 0u;
-        }
-      else
-        {
-          byte = ((volatile uint8_t *)bootloader.fw_image)[i];
-        }
-      crc = crc64_add(crc, byte);
-    }
-  crc ^= CRC64_OUTPUT_XOR;
-
-  return crc == bootloader.fw_image_descriptor->image_crc;
-}
-
-void find_descriptor(void)
-{
-  bootloader.fw_image_descriptor = NULL;
-  uint64_t *p = (uint64_t *) APPLICATION_LOAD_ADDRESS;
-  union
-     {
-       uint64_t ull;
-       char text[sizeof(uint64_t)];
-     } sig = {
-          .text = {APP_DESCRIPTOR_SIGNATURE}
-     };
-   do {
-      if (*p ==  sig.ull) {
-          bootloader.fw_image_descriptor = (volatile app_descriptor_t *) p;
-        }
-    } while(bootloader.fw_image_descriptor == NULL && ++p < APPLICATION_LAST_64BIT_ADDRRESS);
-}
+/****************************************************************************
+ * Name: do_jump
+ *
+ * Description:
+ *   This functions begins the execution of the application firmware.
+ *
+ * Input Parameters:
+ *   stacktop   - The value that should be loaded into the stack pointer.
+ *   entrypoint - The address to jump to.
+ *
+ * Returned Value:
+ *    Does not return.
+ *
+ ****************************************************************************/
 
 static void do_jump(uint32_t stacktop, uint32_t entrypoint)
 {
@@ -895,7 +861,26 @@ static void do_jump(uint32_t stacktop, uint32_t entrypoint)
   for (;;);
 }
 
-void application_run(size_t fw_image_size)
+/****************************************************************************
+ * Name: application_run
+ *
+ * Description:
+ *   This functions will test the application image is valid by
+ *   checking the value of the first word for != 0xffffffff and the
+ *   second word for an address that lies inside od the application
+ *   fw image in flash.
+ *
+ * Input Parameters:
+ *   fw_image_size   - The size the fw image is.
+ *
+ * Returned Value:
+ *    If the Image is valid this code does not return, but runs the application
+ *    via do_jump.
+ *    If the image is invalid the function returns.
+ *
+ ****************************************************************************/
+
+static void application_run(size_t fw_image_size)
 {
   /* 
    * We refuse to program the first word of the app until the upload is marked
@@ -932,3 +917,291 @@ void application_run(size_t fw_image_size)
       do_jump(bootloader.fw_image[0], bootloader.fw_image[1]);
     }
 }
+
+/****************************************************************************
+ * Name: autobaud_and_get_dynamic_node_id
+ *
+ * Description:
+ *   This helper functions wraps the auto baud and node id allocation
+ *
+ * Input Parameters:
+ *   tboot   - The id of the timer to use at the tBoot timeout.
+ *   speed   - A pointer to the location to receive the speed detected by
+ *             the auto baud function.
+ *   node_id - A pointer to the location to receive the allocated node_id
+ *             from the dynamic node allocation.
+ *
+ * Returned Value:
+ *   CAN_OK - on Success or a CAN_BOOT_TIMEOUT
+ *
+ ****************************************************************************/
+static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot,
+                                            can_speed_t *speed,
+                                            uint32_t *node_id)
+{
+
+  board_indicate(autobaud_start);
+
+  int rv = can_autobaud(speed, tboot);
+
+  if (rv != CAN_BOOT_TIMEOUT) {
+      board_indicate(autobaud_end);
+      board_indicate(allocation_start);
+      rv = get_dynamic_node_id(tboot, node_id);
+      if (rv != CAN_BOOT_TIMEOUT) {
+        board_indicate(allocation_end);
+      }
+  }
+  return rv;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: main
+ *
+ * Description:
+ *   Called by the os_start code
+ *
+ * Input Parameters:
+ *   Not used.
+ *
+ * Returned Value:
+ *   Does not return.
+ *
+ ****************************************************************************/
+
+__EXPORT int main(int argc, char *argv[])
+{
+
+  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));
+
+  bootloader.status_code = UAVCAN_NODESTATUS_STATUS_INITIALIZING;
+
+  error_log_stage = UAVCAN_LOGMESSAGE_STAGE_INIT;
+
+  bootloader.fw_image = (volatile uint32_t *)(APPLICATION_LOAD_ADDRESS);
+
+#if defined(OPT_WAIT_FOR_GETNODEINFO)
+  bootloader.wait_for_getnodeinfo = 1;
+#endif
+
+#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
+  bootloader.wait_for_getnodeinfo &= stm32_gpioread(GPIO_GETNODEINFO_JUMPER) ^ OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT;
+#endif
+
+
+  bootloader.app_valid = is_app_valid(bootloader.fw_image[0]);
+
+  board_indicate(reset);
+
+
+  bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) &&
+    common.bus_speed && common.node_id;
+  bootloader_app_shared_invalidate();
+
+  bl_timer_cb_t p = null_cb;
+  p.cb = uptime_process;
+  timer_allocate(modeRepeating|modeStarted, 1000, &p);
+
+  p.cb = node_info_process;
+  bl_timer_id tinfo = timer_allocate(modeRepeating, OPT_NODE_INFO_RATE_MS, &p);
+
+  p.cb = node_status_process;
+  bl_timer_id tstatus = timer_allocate(modeRepeating, OPT_NODE_STATUS_RATE_MS, &p);
+
+
+
+  bl_timer_id tboot = timer_allocate(modeRepeating, OPT_TBOOT_MS , 0);
+
+  if (!bootloader.wait_for_getnodeinfo && !bootloader.app_bl_request && bootloader.app_valid) {
+      timer_start(tboot);
+  }
+
+  if (bootloader.app_bl_request) {
+
+      bootloader.bus_speed = common.bus_speed;
+      bootloader.node_id = (uint8_t) common.node_id;
+      can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal);
+
+  } else {
+
+      can_speed_t speed;
+
+      if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, &speed, &common.node_id)) {
+          goto boot;
+      }
+
+      /* reset uptime */
+      bootloader.uptime = 0;
+      common.bus_speed = can_speed2freq(speed);
+      bootloader.node_id = (uint8_t) common.node_id;
+
+  }
+  // Start Processes that requier Node Id
+  timer_start(tinfo);
+  timer_start(tstatus);
+
+  while (!bootloader.sent_node_info_response) {
+      if (timer_expired(tboot))
+        {
+          goto boot;
+        }
+  }
+
+  if (bootloader.app_valid &&
+     (bootloader.wait_for_getnodeinfo ||
+      bootloader.app_bl_request)) {
+
+      timer_start(tboot);
+  }
+
+  do
+    {
+      if (CAN_BOOT_TIMEOUT == wait_for_beginfirmwareupdate(tboot,fw_path,
+                                                          &fw_path_length,
+                                                          &fw_source_node_id))
+        {
+          goto boot;
+        }
+    }
+  while (!fw_source_node_id);
+
+  timer_stop(tboot);
+  board_indicate(fw_update_start);
+
+  file_getinfo(&fw_image_size, &fw_image_crc, fw_path, fw_path_length,
+               fw_source_node_id);
+
+  //todo:Check this
+  if (fw_image_size < sizeof(app_descriptor_t) || !fw_image_crc)
+    {
+      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_GET_INFO;
+      goto failure;
+    }
+
+  /* UAVCANBootloader_v0.3 #28.6: 1023.LogMessage.uavcan("Erase") */
+  send_log_message(bootloader.node_id,
+                              UAVCAN_LOGMESSAGE_LEVEL_INFO,
+                              UAVCAN_LOGMESSAGE_STAGE_ERASE,
+                              UAVCAN_LOGMESSAGE_RESULT_START);
+
+
+  /* Need to signal that the app is no longer valid  if Node Info Request are done */
+
+  bootloader.app_valid = false;
+
+  status = bl_flash_erase(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);
+
+      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_ERASE;
+      goto failure;
+    }
+
+  status = file_read_and_program(fw_source_node_id, fw_path_length, fw_path,
+                                 fw_image_size, &fw_word0);
+  if (status != FLASH_OK)
+    {
+      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_PROGRAM;
+      goto failure;
+    }
+
+  /* UAVCANBootloader_v0.3 #41: CalulateCRC(file_info) */
+  if (!is_app_valid(fw_word0))
+    {
+      bootloader.app_valid = 0u;
+
+      /* UAVCANBootloader_v0.3 #43: [crc Fail]:INDICATE_FW_UPDATE_INVALID_CRC */
+      board_indicate(fw_update_invalid_crc);
+
+      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_VALIDATE;
+      goto failure;
+    }
+
+  /* UAVCANBootloader_v0.3 #46: [crc == fw_crc]:write word0 */
+  status = bl_flash_write_word((uint32_t) bootloader.fw_image, (uint8_t *) & fw_word0);
+  if (status != FLASH_OK)
+    {
+      /* Not specifically listed in UAVCANBootloader_v0.3:
+       * 1023.LogMessage.uavcan */
+      error_log_stage = UAVCAN_LOGMESSAGE_STAGE_FINALIZE;
+      goto failure;
+    }
+
+  /* UAVCANBootloader_v0.3 #47: ValidateBootLoaderAppCommon() */
+  bootloader_app_shared_write(&common, BootLoader);
+
+  /* Send a completion log message */
+  send_log_message(bootloader.node_id,
+                              UAVCAN_LOGMESSAGE_LEVEL_INFO,
+                              UAVCAN_LOGMESSAGE_STAGE_FINALIZE,
+                              UAVCAN_LOGMESSAGE_RESULT_OK);
+
+  /* TODO UAVCANBootloader_v0.3 #48: KickTheDog() */
+
+boot:
+  /* UAVCANBootloader_v0.3 #50: jump_to_app */
+  application_run(fw_image_size);
+
+  /* We will fall thru if the Iamage is bad */
+
+failure:
+  /* UAVCANBootloader_v0.3 #28.2:
+   * [!validateFileInfo(file_info)]:1023.LogMessage.uavcan (errorcode) */
+  /* UAVCANBootloader_v0.3 #28.9: [Erase Fail]:1023.LogMessage.uavcan */
+  /* UAVCANBootloader_v0.3 #31: [Program Fail]::1023.LogMessage.uavcan */
+  /* UAVCANBootloader_v0.3 #38: [(retries == 0 &&
+   * timeout)]:1023.LogMessage.uavcan */
+  /* UAVCANBootloader_v0.3 #42: [crc Faill]:1023.LogMessage.uavcan */
+  send_log_message(bootloader.node_id,
+                              UAVCAN_LOGMESSAGE_LEVEL_ERROR,
+                              error_log_stage, UAVCAN_LOGMESSAGE_RESULT_FAIL);
+
+  /* UAVCANBootloader_v0.3 #28.3:
+   * [!validateFileInfo(file_info)]:550.NodeStatus.uavcan(uptime=t,
+   * STATUS_CRITICAL) */
+  /* UAVCANBootloader_v0.3 #28.10: [Erase
+   * Fail]:550.NodeStatus.uavcan(uptime=t,STATUS_CRITICAL) */
+  /* UAVCANBootloader_v0.3 #32: [Program Fail]:550.NodeStatus.uavcan(uptime=t,
+   * STATUS_CRITICAL) */
+  /* UAVCANBootloader_v0.3 #39: [(retries == 0 &&
+   * timeout)]:550.NodeStatus.uavcan(uptime=t, STATUS_CRITICAL) */
+  /* UAVCANBootloader_v0.3 #44: [crc Fail]:550.NodeStatus.uavcan(uptime=t,
+   * STATUS_CRITICAL) */
+  bootloader.status_code = UAVCAN_NODESTATUS_STATUS_CRITICAL;
+
+  /* UAVCANBootloader_v0.3 #28.1:
+   * [Retries==0]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */
+  /* UAVCANBootloader_v0.3 #40: [Retries==0]:InvalidateBootLoaderAppCommon(),
+   * RestartWithDelay(20,000ms) */
+  /* UAVCANBootloader_v0.3 #45: [crc
+   * Fail]:InvalidateBootLoaderAppCommon(),RestartWithDelay(20,000ms) */
+
+  bl_timer_id tmr = timer_allocate(modeTimeout|modeStarted , OPT_RESTART_TIMEOUT_MS, 0);
+  while(!timer_expired(tmr))
+    {
+      ;
+    }
+  timer_free(tmr);
+  up_systemreset();
+}