From 62886174356a9e0a48ce6df02924e78ffca4306f Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Fri, 26 Apr 2019 10:07:15 +0200
Subject: [PATCH] Bluetooth flash firmware continued

---
 radio/src/bluetooth.cpp | 45 ++++++++++++++++++++++++++++++++++++++---
 radio/src/bluetooth.h   |  2 ++
 2 files changed, 44 insertions(+), 3 deletions(-)

diff --git a/radio/src/bluetooth.cpp b/radio/src/bluetooth.cpp
index 3074e6f99..84c9a83d7 100644
--- a/radio/src/bluetooth.cpp
+++ b/radio/src/bluetooth.cpp
@@ -492,6 +492,9 @@ uint8_t Bluetooth::read(uint8_t * data, uint8_t size, uint8_t timeout)
   return len;
 }
 
+#define BLUETOOTH_ACK   0xCC
+#define BLUETOOTH_NACK  0x33
+
 const char * Bluetooth::waitBootloaderCommandResponse()
 {
   uint8_t response[2];
@@ -503,13 +506,38 @@ const char * Bluetooth::waitBootloaderCommandResponse()
     return "Bluetooth error";
   }
 
-  if (response[1] == 0xCC || response[1] == 0x33) {
+  if (response[1] == BLUETOOTH_ACK || response[1] == BLUETOOTH_NACK) {
     return nullptr;
   }
 
   return "Bluetooth error";
 }
 
+const char * Bluetooth::waitBootloaderResponseData(uint8_t * data, uint8_t size)
+{
+  uint8_t header[2];
+  if (read(header, 2) != 2) {
+    return "Bluetooth timeout";
+  }
+
+  uint8_t len = header[0] - 2;
+  uint8_t checksum = header[1];
+
+  if (len > size) {
+    return "Bluetooth error";
+  }
+
+  if (read(data, len) != len) {
+    return "Bluetooth timeout";
+  }
+
+  if (bootloaderChecksum(0, data, len) != checksum) {
+    return "Bluetooth CRC error";
+  }
+
+  return nullptr;
+}
+
 const char * Bluetooth::sendBootloaderAutoBaud()
 {
   uint8_t packet[2] = {
@@ -535,6 +563,14 @@ void Bluetooth::sendBootloaderCommand(uint8_t command, const uint8_t *data, uint
   }
 }
 
+void Bluetooth::sendBootloaderCommandResponse(uint8_t response)
+{
+  uint8_t packet[2] = {
+    0x00, response
+  };
+  write(packet, sizeof(packet));
+}
+
 enum
 {
   CMD_GET_CHIP_ID = 0x28,
@@ -557,8 +593,9 @@ const char * Bluetooth::doFlashFirmware(const char * filename)
   result = waitBootloaderCommandResponse();
   if (result)
     return result;
-  // uint8_t id[4];
-
+  uint8_t id[4];
+  result = waitBootloaderResponseData(id, 4);
+  sendBootloaderCommandResponse(result == nullptr ? BLUETOOTH_ACK : BLUETOOTH_NACK);
 
   return result;
 }
@@ -571,6 +608,8 @@ void Bluetooth::flashFirmware(const char * filename)
 
   pausePulses();
 
+  bluetoothInit(BLUETOOTH_BOOTLOADER_BAUDRATE, true); // normal mode
+  RTOS_WAIT_MS(500);
   bluetoothInit(BLUETOOTH_BOOTLOADER_BAUDRATE, false); // bootloader mode
 
   const char * result = doFlashFirmware(filename);
diff --git a/radio/src/bluetooth.h b/radio/src/bluetooth.h
index 70546f470..1b60c4ba9 100644
--- a/radio/src/bluetooth.h
+++ b/radio/src/bluetooth.h
@@ -74,7 +74,9 @@ class Bluetooth
 
     uint8_t bootloaderChecksum(uint8_t command, const uint8_t * data, uint8_t size);
     void sendBootloaderCommand(uint8_t command, const uint8_t * data = nullptr, uint8_t size = 0);
+    void sendBootloaderCommandResponse(uint8_t response);
     const char * waitBootloaderCommandResponse();
+    const char * waitBootloaderResponseData(uint8_t * data, uint8_t size);
     const char * sendBootloaderAutoBaud();
 
     const char * doFlashFirmware(const char * filename);