From fe4aa4bbc75fd46efe1c08baae8347cf47f127b2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 25 Jun 2018 13:39:22 +1000
Subject: [PATCH] AP_Bootloader: support uarts and usb for bootloading

---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  12 ++-
 Tools/AP_Bootloader/bl_protocol.cpp   |  54 ++++++-------
 Tools/AP_Bootloader/support.cpp       | 112 ++++++++++++++++++++++----
 Tools/AP_Bootloader/support.h         |   3 +-
 4 files changed, 132 insertions(+), 49 deletions(-)

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index d3b3ac2eea..2c0de92ee5 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -35,15 +35,13 @@ extern "C" {
 
 struct boardinfo board_info;
 
+#ifndef BOOTLOADER_BAUDRATE
+#define BOOTLOADER_BAUDRATE 115200
+#endif
+
 int main(void)
 {
-    sduObjectInit(&SDU1);
-    sduStart(&SDU1, &serusbcfg);
-
-    usbDisconnectBus(serusbcfg.usbp);
-    chThdSleepMilliseconds(1000);
-    usbStart(serusbcfg.usbp, &usbcfg);
-    usbConnectBus(serusbcfg.usbp);
+    init_uarts();
 
     board_info.board_type = APJ_BOARD_ID;
     board_info.board_rev = 0;
diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp
index 148e7ad60f..b6f90287d1 100644
--- a/Tools/AP_Bootloader/bl_protocol.cpp
+++ b/Tools/AP_Bootloader/bl_protocol.cpp
@@ -97,6 +97,7 @@
 #define PROTO_GET_CHIP_DES			0x2e    // read chip version In ASCII
 #define PROTO_BOOT					0x30    // boot the application
 #define PROTO_DEBUG					0x31    // emit debug information - format not defined
+#define PROTO_SET_BAUD				0x33    // baud rate on uart
 
 #define PROTO_PROG_MULTI_MAX    64	// maximum PROG_MULTI size
 #define PROTO_READ_MULTI_MAX    255	// size of the size field
@@ -216,14 +217,8 @@ jump_to_app()
     
     led_set(LED_OFF);
 
-    /* kill the systick timer */
-    chVTReset(&systick_vt);
-
-    // stop USB driver
-    //cfini();
-
     // disable all interrupt sources
-    //port_disable();
+    port_disable();
 
     /* switch exception handlers to the application */
     *(volatile uint32_t *)SCB_VTOR = APP_START_ADDRESS;
@@ -243,19 +238,6 @@ sync_response(void)
     cout(data, sizeof(data));
 }
 
-#if defined(TARGET_HW_PX4_FMU_V4)
-static void
-bad_silicon_response(void)
-{
-    uint8_t data[] = {
-        PROTO_INSYNC,			// "in sync"
-        PROTO_BAD_SILICON_REV	// "issue with < Rev 3 silicon"
-    };
-
-    cout(data, sizeof(data));
-}
-#endif
-
 static void
 invalid_response(void)
 {
@@ -736,6 +718,31 @@ bootloader(unsigned timeout)
             // XXX reserved for ad-hoc debugging as required
             break;
 
+		case PROTO_SET_BAUD: {
+			/* expect arg then EOC */
+            uint32_t baud = 0;
+
+            if (cin_word(&baud, 100)) {
+                goto cmd_bad;
+            }
+
+			if (!wait_for_eoc(2)) {
+                goto cmd_bad;
+			}
+
+            // send the sync response for this command
+            sync_response();
+
+            // set the baudrate
+            port_setbaud(baud);
+
+            // this is different to what every other case in this
+            // switch does!  Most go through sync_response down the
+            // bottom, but we need to undertake an action after
+            // returning the response...
+            continue;
+        }
+            
         default:
             continue;
         }
@@ -759,12 +766,5 @@ cmd_fail:
         // send a 'command failed' response but don't kill the timeout - could be garbage
         failure_response();
         continue;
-
-#if defined(TARGET_HW_PX4_FMU_V4)
-bad_silicon:
-        // send the bad silicon response but don't kill the timeout - could be garbage
-        bad_silicon_response();
-        continue;
-#endif
     }
 }
diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp
index cb4c0814e8..9d22618e1f 100644
--- a/Tools/AP_Bootloader/support.cpp
+++ b/Tools/AP_Bootloader/support.cpp
@@ -13,34 +13,44 @@
 #include "mcu_f4.h"
 #include "mcu_f7.h"
 
+static BaseChannel *uarts[] = { BOOTLOADER_DEV_LIST };
+static SerialConfig sercfg;
+static int8_t locked_uart = -1;
+static uint8_t last_uart;
+
 int16_t cin(unsigned timeout_ms)
 {
     uint8_t b = 0;
-    if (chnReadTimeout(&SDU1, &b, 1, MS2ST(timeout_ms)) != 1) {
-        chThdSleepMicroseconds(100);
-        return -1;
+    for (uint8_t i=0; i<ARRAY_SIZE_SIMPLE(uarts); i++) {
+        if (locked_uart == -1 || locked_uart == i) {
+            if (chnReadTimeout(uarts[i], &b, 1, MS2ST(timeout_ms)) == 1) {
+                last_uart = i;
+                return b;
+            }
+        }
     }
-    return b;
+    chThdSleepMicroseconds(100);
+    return -1;
 }
 
 int cin_word(uint32_t *wp, unsigned timeout_ms)
 {
-    if (chnReadTimeout(&SDU1, (uint8_t *)wp, 4, MS2ST(timeout_ms)) != 4) {
-        chThdSleepMicroseconds(100);
-        return -1;
+    for (uint8_t i=0; i<ARRAY_SIZE_SIMPLE(uarts); i++) {
+        if (locked_uart == -1 || locked_uart == i) {
+            if (chnReadTimeout(uarts[i], (uint8_t *)wp, 4, MS2ST(timeout_ms)) == 4) {
+                last_uart = i;
+                return 0;
+            }
+        }
     }
-    return 0;
+    chThdSleepMicroseconds(100);
+    return -1;
 }
 
 
 void cout(uint8_t *data, uint32_t len)
 {
-    chnWriteTimeout(&SDU1, data, len, MS2ST(100));
-}
-
-void cfini(void)
-{
-    sduStop(&SDU1);
+    chnWriteTimeout(uarts[last_uart], data, len, MS2ST(100));
 }
 
 static uint32_t flash_base_page;
@@ -260,6 +270,16 @@ void *memcpy(void *dest, const void *src, size_t n)
     return dest;
 }
 
+//simple variant of std c function to reduce used flash space
+int strncmp(const char *s1, const char *s2, size_t n)
+{
+    while ((*s1 != 0) && (*s1 == *s2) && n--) {
+        s1++;
+        s2++;
+    }
+    return (*s1 - *s2);
+}
+
 //simple variant of std c function to reduce used flash space
 int strcmp(const char *s1, const char *s2)
 {
@@ -270,6 +290,70 @@ int strcmp(const char *s1, const char *s2)
     return (*s1 - *s2);
 }
 
+//simple variant of std c function to reduce used flash space
+size_t strlen(const char *s1)
+{
+    size_t ret = 0;
+    while (*s1++) ret++;
+    return ret;
+}
+
+//simple variant of std c function to reduce used flash space
+void *memset(void *s, int c, size_t n)
+{
+    uint8_t *b = (uint8_t *)s;
+    while (n--) {
+        *b++ = c;
+    }
+    return s;
+}
+
 void lock_bl_port(void)
 {
+    locked_uart = last_uart;
 }
+
+/*
+  initialise serial ports
+ */
+void init_uarts(void)
+{
+#ifdef HAL_USE_SERIAL_USB
+    sduObjectInit(&SDU1);
+    sduStart(&SDU1, &serusbcfg);
+    
+    usbDisconnectBus(serusbcfg.usbp);
+    chThdSleepMilliseconds(1000);
+    usbStart(serusbcfg.usbp, &usbcfg);
+    usbConnectBus(serusbcfg.usbp);
+#endif
+
+    sercfg.speed = BOOTLOADER_BAUDRATE;
+    
+    for (uint8_t i=0; i<ARRAY_SIZE_SIMPLE(uarts); i++) {
+#ifdef HAL_USE_SERIAL_USB
+        if (uarts[i] == (BaseChannel *)&SDU1) {
+            continue;
+        }
+#endif
+        sdStart((SerialDriver *)uarts[i], &sercfg);
+    }
+}
+
+
+/*
+  set baudrate on the current port
+ */
+void port_setbaud(uint32_t baudrate)
+{
+    sercfg.speed = baudrate;
+#ifdef HAL_USE_SERIAL_USB
+    if (uarts[last_uart] == (BaseChannel *)&SDU1) {
+        // can't set baudrate on USB
+        return;
+    }
+#endif
+    sdStop((SerialDriver *)uarts[last_uart]);
+    sdStart((SerialDriver *)uarts[last_uart], &sercfg);
+}
+
diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h
index 1e422c5c81..55e6bb2223 100644
--- a/Tools/AP_Bootloader/support.h
+++ b/Tools/AP_Bootloader/support.h
@@ -12,10 +12,11 @@ struct boardinfo {
 
 extern struct boardinfo board_info;
 
+void init_uarts(void);
 int16_t cin(unsigned timeout_ms);
 int cin_word(uint32_t *wp, unsigned timeout_ms);
 void cout(uint8_t *data, uint32_t len);
-void cfini(void);
+void port_setbaud(uint32_t baudrate);
 
 void flash_init();