radio/src/bluetooth.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22 #include "io/frsky_firmware_update.h"
  23
  24 #if defined(PCBX9E)
  25 #define BLUETOOTH_COMMAND_NAME         "TTM:REN-"
  26 #define BLUETOOTH_ANSWER_NAME          "TTM:REN"
  27 #define BLUETOOTH_COMMAND_BAUD_115200  "TTM:BPS-115200"
  28 #else
  29 #define BLUETOOTH_COMMAND_NAME         "AT+NAME"
  30 #define BLUETOOTH_ANSWER_NAME          "OK+"
  31 #define BLUETOOTH_COMMAND_BAUD_115200  "AT+BAUD115200"
  32 #endif
  33
  34 #if defined(_MSC_VER)
  35   #define SWAP32(val)      (_byteswap_ulong(val))
  36 #elif defined(__GNUC__ )
  37   #define SWAP32(val)      (__builtin_bswap32(val))
  38 #endif
  39
  40 extern Fifo<uint8_t, BT_TX_FIFO_SIZE> btTxFifo;
  41 extern Fifo<uint8_t, BT_RX_FIFO_SIZE> btRxFifo;
  42
  43 Bluetooth bluetooth;
  44
  45 void Bluetooth::write(const uint8_t * data, uint8_t length)
  46 {
  47   TRACE_NOCRLF("BT>");
  48   for (int i=0; i<length; i++) {
  49     TRACE_NOCRLF(" %02X", data[i]);
  50     while (btTxFifo.isFull()) {
  51       if (!bluetoothIsWriting())
  52         bluetoothWriteWakeup();
  53       RTOS_WAIT_MS(1);
  54     }
  55     btTxFifo.push(data[i]);
  56   }
  57   TRACE_NOCRLF("\r\n");
  58   bluetoothWriteWakeup();
  59 }
  60
  61 void Bluetooth::writeString(const char * str)
  62 {
  63   TRACE("BT> %s", str);
  64   while (*str != 0) {
  65     btTxFifo.push(*str++);
  66   }
  67   btTxFifo.push('\r');
  68   btTxFifo.push('\n');
  69   bluetoothWriteWakeup();
  70 }
  71
  72 char * Bluetooth::readline(bool error_reset)
  73 {
  74   uint8_t byte;
  75
  76   while (true) {
  77     if (!btRxFifo.pop(byte)) {
  78 #if defined(PCBX9E)
  79       // X9E BT module can get unresponsive
  80       TRACE("NO RESPONSE FROM BT MODULE");
  81 #endif
  82       return nullptr;
  83     }
  84
  85     TRACE_NOCRLF("%02X ", byte);
  86
  87 #if 0
  88     if (error_reset && byte == 'R' && bufferIndex == 4 && memcmp(buffer, "ERRO", 4)) {
  89 #if defined(PCBX9E)  // X9E enter BT reset loop if following code is implemented
  90       TRACE("BT Error...");
  91 #else
  92       TRACE("BT Reset...");
  93       bufferIndex = 0;
  94       bluetoothDisable();
  95       state = BLUETOOTH_STATE_OFF;
  96       wakeupTime = get_tmr10ms() + 100; /* 1s */
  97 #endif
  98       return nullptr;
  99     }
 100     else
 101 #endif
 102
 103     if (byte == '\n') {
 104       if (bufferIndex > 2 && buffer[bufferIndex-1] == '\r') {
 105         buffer[bufferIndex-1] = '\0';
 106         bufferIndex = 0;
 107         TRACE("BT< %s", buffer);
 108         if (error_reset && !strcmp((char *)buffer, "ERROR")) {
 109 #if defined(PCBX9E) // X9E enter BT reset loop if following code is implemented
 110           TRACE("BT Error...");
 111 #else
 112           TRACE("BT Reset...");
 113           bluetoothDisable();
 114           state = BLUETOOTH_STATE_OFF;
 115           wakeupTime = get_tmr10ms() + 100; /* 1s */
 116 #endif
 117           return nullptr;
 118         }
 119         else {
 120           if (!memcmp(buffer, "Central:", 8))
 121             strcpy(localAddr, (char *) buffer + 8);
 122           else if (!memcmp(buffer, "Peripheral:", 11))
 123             strcpy(localAddr, (char *) buffer + 11);
 124           return (char *) buffer;
 125         }
 126       }
 127       else {
 128         bufferIndex = 0;
 129       }
 130     }
 131     else {
 132       buffer[bufferIndex++] = byte;
 133       bufferIndex &= (BLUETOOTH_LINE_LENGTH-1);
 134     }
 135   }
 136 }
 137
 138 void Bluetooth::processTrainerFrame(const uint8_t * buffer)
 139 {
 140   TRACE("");
 141
 142   for (uint8_t channel=0, i=1; channel<8; channel+=2, i+=3) {
 143     // +-500 != 512, but close enough.
 144     ppmInput[channel] = buffer[i] + ((buffer[i+1] & 0xf0) << 4) - 1500;
 145     ppmInput[channel+1] = ((buffer[i+1] & 0x0f) << 4) + ((buffer[i+2] & 0xf0) >> 4) + ((buffer[i+2] & 0x0f) << 8) - 1500;
 146   }
 147
 148   ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
 149 }
 150
 151 void Bluetooth::appendTrainerByte(uint8_t data)
 152 {
 153   if (bufferIndex < BLUETOOTH_LINE_LENGTH) {
 154     buffer[bufferIndex++] = data;
 155     // we check for "DisConnected", but the first byte could be altered (if received in state STATE_DATA_XOR)
 156     if (data == '\n') {
 157       if (!strncmp((char *)&buffer[bufferIndex-13], "isConnected", 11)) {
 158         TRACE("BT< DisConnected");
 159         state = BLUETOOTH_STATE_DISCONNECTED;
 160         bufferIndex = 0;
 161         wakeupTime += 200; // 1s
 162       }
 163     }
 164   }
 165 }
 166
 167 void Bluetooth::processTrainerByte(uint8_t data)
 168 {
 169   static uint8_t dataState = STATE_DATA_IDLE;
 170
 171   switch (dataState) {
 172     case STATE_DATA_START:
 173       if (data == START_STOP) {
 174         dataState = STATE_DATA_IN_FRAME ;
 175         bufferIndex = 0;
 176       }
 177       else {
 178         appendTrainerByte(data);
 179       }
 180       break;
 181
 182     case STATE_DATA_IN_FRAME:
 183       if (data == BYTE_STUFF) {
 184         dataState = STATE_DATA_XOR; // XOR next byte
 185       }
 186       else if (data == START_STOP) {
 187         dataState = STATE_DATA_IN_FRAME ;
 188         bufferIndex = 0;
 189       }
 190       else {
 191         appendTrainerByte(data);
 192       }
 193       break;
 194
 195     case STATE_DATA_XOR:
 196       appendTrainerByte(data ^ STUFF_MASK);
 197       dataState = STATE_DATA_IN_FRAME;
 198       break;
 199
 200     case STATE_DATA_IDLE:
 201       if (data == START_STOP) {
 202         bufferIndex = 0;
 203         dataState = STATE_DATA_START;
 204       }
 205       else {
 206         appendTrainerByte(data);
 207       }
 208       break;
 209   }
 210
 211   if (bufferIndex >= BLUETOOTH_PACKET_SIZE) {
 212     uint8_t crc = 0x00;
 213     for (int i=0; i<13; i++) {
 214       crc ^= buffer[i];
 215     }
 216     if (crc == buffer[13]) {
 217       if (buffer[0] == 0x80) {
 218         processTrainerFrame(buffer);
 219       }
 220     }
 221     dataState = STATE_DATA_IDLE;
 222   }
 223 }
 224
 225 void Bluetooth::pushByte(uint8_t byte)
 226 {
 227   crc ^= byte;
 228   if (byte == START_STOP || byte == BYTE_STUFF) {
 229     buffer[bufferIndex++] = BYTE_STUFF;
 230     byte ^= STUFF_MASK;
 231   }
 232   buffer[bufferIndex++] = byte;
 233 }
 234
 235 void Bluetooth::sendTrainer()
 236 {
 237   int16_t PPM_range = g_model.extendedLimits ? 640*2 : 512*2;
 238
 239   int firstCh = g_model.trainerData.channelsStart;
 240   int lastCh = firstCh + 8;
 241
 242   uint8_t * cur = buffer;
 243   bufferIndex = 0;
 244   crc = 0x00;
 245
 246   buffer[bufferIndex++] = START_STOP; // start byte
 247   pushByte(0x80); // trainer frame type?
 248   for (int channel=0; channel<lastCh; channel+=2, cur+=3) {
 249     uint16_t channelValue1 = PPM_CH_CENTER(channel) + limit((int16_t)-PPM_range, channelOutputs[channel], (int16_t)PPM_range) / 2;
 250     uint16_t channelValue2 = PPM_CH_CENTER(channel+1) + limit((int16_t)-PPM_range, channelOutputs[channel+1], (int16_t)PPM_range) / 2;
 251     pushByte(channelValue1 & 0x00ff);
 252     pushByte(((channelValue1 & 0x0f00) >> 4) + ((channelValue2 & 0x00f0) >> 4));
 253     pushByte(((channelValue2 & 0x000f) << 4) + ((channelValue2 & 0x0f00) >> 8));
 254   }
 255   buffer[bufferIndex++] = crc;
 256   buffer[bufferIndex++] = START_STOP; // end byte
 257
 258   write(buffer, bufferIndex);
 259   bufferIndex = 0;
 260 }
 261
 262 void Bluetooth::forwardTelemetry(const uint8_t * packet)
 263 {
 264   crc = 0x00;
 265
 266   buffer[bufferIndex++] = START_STOP; // start byte
 267   for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
 268     pushByte(packet[i]);
 269   }
 270   buffer[bufferIndex++] = crc;
 271   buffer[bufferIndex++] = START_STOP; // end byte
 272
 273   if (bufferIndex >= 2*FRSKY_SPORT_PACKET_SIZE) {
 274     write(buffer, bufferIndex);
 275     bufferIndex = 0;
 276   }
 277 }
 278
 279 void Bluetooth::receiveTrainer()
 280 {
 281   uint8_t byte;
 282
 283   while (true) {
 284     if (!btRxFifo.pop(byte)) {
 285       return;
 286     }
 287
 288     TRACE_NOCRLF("%02X ", byte);
 289
 290     processTrainerByte(byte);
 291   }
 292 }
 293
 294 #if defined(PCBX9E)
 295 void Bluetooth::wakeup(void)
 296 {
 297 #if !defined(SIMU)
 298   if (!g_eeGeneral.bluetoothMode) {
 299     if (state != BLUETOOTH_INIT) {
 300       bluetoothDisable();
 301       state = BLUETOOTH_INIT;
 302     }
 303   }
 304   else {
 305     static tmr10ms_t waitEnd = 0;
 306     if (state != BLUETOOTH_STATE_IDLE) {
 307       if (state == BLUETOOTH_INIT) {
 308         bluetoothInit(BLUETOOTH_DEFAULT_BAUDRATE, true);
 309         char command[32];
 310         char * cur = strAppend(command, BLUETOOTH_COMMAND_NAME);
 311         uint8_t len = ZLEN(g_eeGeneral.bluetoothName);
 312         if (len > 0) {
 313           for (int i = 0; i < len; i++) {
 314             *cur++ = char2lower(zchar2char(g_eeGeneral.bluetoothName[i]));
 315           }
 316           *cur = '\0';
 317         }
 318         else {
 319           cur = strAppend(cur, FLAVOUR);
 320         }
 321         writeString(command);
 322         state = BLUETOOTH_WAIT_TTM;
 323         waitEnd = get_tmr10ms() + 25; // 250ms
 324       }
 325       else if (state == BLUETOOTH_WAIT_TTM) {
 326         if (get_tmr10ms() > waitEnd) {
 327           char * line = readline();
 328           if (strncmp(line, "OK+", 3)) {
 329             state = BLUETOOTH_STATE_IDLE;
 330           }
 331           else {
 332             bluetoothInit(BLUETOOTH_FACTORY_BAUDRATE, true);
 333             writeString("TTM:BPS-115200");
 334             state = BLUETOOTH_WAIT_BAUDRATE_CHANGE;
 335             waitEnd = get_tmr10ms() + 250; // 2.5s
 336           }
 337         }
 338       }
 339       else if (state == BLUETOOTH_WAIT_BAUDRATE_CHANGE) {
 340         if (get_tmr10ms() > waitEnd) {
 341           state = BLUETOOTH_INIT;
 342         }
 343       }
 344     }
 345     else if (IS_BLUETOOTH_TRAINER()){
 346       state = BLUETOOTH_STATE_CONNECTED;
 347       bluetoothWriteWakeup();
 348       sendTrainer();
 349     }
 350   }
 351 #endif
 352 }
 353 #else // PCBX9E
 354 void Bluetooth::wakeup()
 355 {
 356   if (state != BLUETOOTH_STATE_OFF) {
 357     bluetoothWriteWakeup();
 358     if (bluetoothIsWriting()) {
 359       return;
 360     }
 361   }
 362
 363   tmr10ms_t now = get_tmr10ms();
 364
 365   if (now < wakeupTime)
 366     return;
 367
 368   wakeupTime = now + 5; /* 50ms default */
 369
 370   if (state == BLUETOOTH_STATE_FLASH_FIRMWARE) {
 371     return;
 372   }
 373
 374   if (g_eeGeneral.bluetoothMode == BLUETOOTH_OFF || (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && !IS_BLUETOOTH_TRAINER())) {
 375     if (state != BLUETOOTH_STATE_OFF) {
 376       bluetoothDisable();
 377       state = BLUETOOTH_STATE_OFF;
 378     }
 379     wakeupTime = now + 10; /* 100ms */
 380   }
 381   else if (state == BLUETOOTH_STATE_OFF) {
 382     bluetoothInit(BLUETOOTH_FACTORY_BAUDRATE, true);
 383     state = BLUETOOTH_STATE_FACTORY_BAUDRATE_INIT;
 384   }
 385
 386   if (state == BLUETOOTH_STATE_FACTORY_BAUDRATE_INIT) {
 387     writeString("AT+BAUD4");
 388     state = BLUETOOTH_STATE_BAUDRATE_SENT;
 389     wakeupTime = now + 10; /* 100ms */
 390   }
 391   else if (state == BLUETOOTH_STATE_BAUDRATE_SENT) {
 392     bluetoothInit(BLUETOOTH_DEFAULT_BAUDRATE, true);
 393     state = BLUETOOTH_STATE_BAUDRATE_INIT;
 394     readline(false);
 395     wakeupTime = now + 10; /* 100ms */
 396   }
 397   else if (state == BLUETOOTH_STATE_CONNECTED) {
 398     if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH) {
 399       receiveTrainer();
 400     }
 401     else {
 402       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH) {
 403         sendTrainer();
 404         wakeupTime = now + 2; /* 20ms */
 405       }
 406       readline(); // to deal with "ERROR"
 407     }
 408   }
 409   else {
 410     char * line = readline();
 411     if (state == BLUETOOTH_STATE_BAUDRATE_INIT) {
 412       char command[32];
 413       char * cur = strAppend(command, BLUETOOTH_COMMAND_NAME);
 414       uint8_t len = ZLEN(g_eeGeneral.bluetoothName);
 415       if (len > 0) {
 416         for (int i = 0; i < len; i++) {
 417           *cur++ = char2lower(zchar2char(g_eeGeneral.bluetoothName[i]));
 418         }
 419         *cur = '\0';
 420       }
 421       else {
 422         cur = strAppend(cur, FLAVOUR);
 423       }
 424       writeString(command);
 425       state = BLUETOOTH_STATE_NAME_SENT;
 426     }
 427     else if (state == BLUETOOTH_STATE_NAME_SENT && (!strncmp(line, "OK+", 3) || !strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 428       writeString("AT+TXPW0");
 429       state = BLUETOOTH_STATE_POWER_SENT;
 430     }
 431     else if (state == BLUETOOTH_STATE_POWER_SENT && (!strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 432       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH)
 433         writeString("AT+ROLE1");
 434       else
 435         writeString("AT+ROLE0");
 436       state = BLUETOOTH_STATE_ROLE_SENT;
 437     }
 438     else if (state == BLUETOOTH_STATE_ROLE_SENT && (!strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 439       state = BLUETOOTH_STATE_IDLE;
 440     }
 441     else if (state == BLUETOOTH_STATE_DISCOVER_REQUESTED) {
 442       writeString("AT+DISC?");
 443       state = BLUETOOTH_STATE_DISCOVER_SENT;
 444     }
 445     else if (state == BLUETOOTH_STATE_DISCOVER_SENT && !strcmp(line, "OK+DISCS")) {
 446       state = BLUETOOTH_STATE_DISCOVER_START;
 447     }
 448     else if (state == BLUETOOTH_STATE_DISCOVER_START && !strncmp(line, "OK+DISC:", 8)) {
 449       if (strlen(line) < 8 + LEN_BLUETOOTH_ADDR && reusableBuffer.moduleSetup.bt.devicesCount < MAX_BLUETOOTH_DISTANT_ADDR) {
 450         strncpy(reusableBuffer.moduleSetup.bt.devices[reusableBuffer.moduleSetup.bt.devicesCount], &line[8], LEN_BLUETOOTH_ADDR);
 451         ++reusableBuffer.moduleSetup.bt.devicesCount;
 452       }
 453     }
 454 #if defined(PCBHORUS)
 455     else if (state == BLUETOOTH_STATE_DISCOVER_START && !strcmp(line, "OK+DISCE")) {
 456       state = BLUETOOTH_STATE_DISCOVER_END;
 457     }
 458 #endif
 459     else if (state == BLUETOOTH_STATE_CLEAR_REQUESTED) {
 460       char command[] = "AT+CLEAR";
 461       writeString(command);
 462       state = BLUETOOTH_STATE_IDLE;
 463     }
 464     else if (state == BLUETOOTH_STATE_BIND_REQUESTED) {
 465       char command[32];
 466       strAppend(strAppend(command, "AT+CON"), distantAddr);
 467       writeString(command);
 468       state = BLUETOOTH_STATE_CONNECT_SENT;
 469     }
 470     else if ((state == BLUETOOTH_STATE_IDLE || state == BLUETOOTH_STATE_DISCONNECTED || state == BLUETOOTH_STATE_CONNECT_SENT) && !strncmp(line, "Connected:", 10)) {
 471       strcpy(distantAddr, &line[10]); // TODO quick & dirty
 472       state = BLUETOOTH_STATE_CONNECTED;
 473       if (g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH) {
 474         wakeupTime += 500; // it seems a 5s delay is needed before sending the 1st frame
 475       }
 476     }
 477     else if (state == BLUETOOTH_STATE_DISCONNECTED && !line) {
 478       char command[32];
 479       strAppend(strAppend(command, "AT+CON"), distantAddr);
 480       writeString(command);
 481       wakeupTime = now + 200; /* 2s */
 482     }
 483   }
 484 }
 485 #endif
 486
 487 uint8_t Bluetooth::bootloaderChecksum(uint8_t command, const uint8_t * data, uint8_t size)
 488 {
 489   uint8_t sum = command;
 490   for (uint8_t i = 0; i < size; i++) {
 491     sum += data[i];
 492   }
 493   return sum;
 494 }
 495
 496 uint8_t Bluetooth::read(uint8_t * data, uint8_t size, uint32_t timeout)
 497 {
 498   watchdogSuspend(timeout / 10);
 499
 500   uint8_t len = 0;
 501   while (len < size) {
 502     uint32_t elapsed = 0;
 503     uint8_t byte;
 504     while (!btRxFifo.pop(byte)) {
 505       if (elapsed++ >= timeout) {
 506         return len;
 507       }
 508       RTOS_WAIT_MS(1);
 509     }
 510     data[len++] = byte;
 511   }
 512   return len;
 513 }
 514
 515 #define BLUETOOTH_ACK   0xCC
 516 #define BLUETOOTH_NACK  0x33
 517
 518 const char * Bluetooth::bootloaderWaitCommandResponse(uint32_t timeout)
 519 {
 520   uint8_t response[2];
 521   if (read(response, sizeof(response), timeout) != sizeof(response)) {
 522     return "Bluetooth timeout";
 523   }
 524
 525   if (response[0] != 0x00) {
 526     return "Bluetooth error";
 527   }
 528
 529   if (response[1] == BLUETOOTH_ACK || response[1] == BLUETOOTH_NACK) {
 530     return nullptr;
 531   }
 532
 533   return "Bluetooth error";
 534 }
 535
 536 const char * Bluetooth::bootloaderWaitResponseData(uint8_t *data, uint8_t size)
 537 {
 538   uint8_t header[2];
 539   if (read(header, 2) != 2) {
 540     return "Bluetooth timeout";
 541   }
 542
 543   uint8_t len = header[0] - 2;
 544   uint8_t checksum = header[1];
 545
 546   if (len > size) {
 547     return "Bluetooth error";
 548   }
 549
 550   if (read(data, len) != len) {
 551     return "Bluetooth timeout";
 552   }
 553
 554   if (bootloaderChecksum(0, data, len) != checksum) {
 555     return "Bluetooth CRC error";
 556   }
 557
 558   return nullptr;
 559 }
 560
 561 const char * Bluetooth::bootloaderSetAutoBaud()
 562 {
 563   uint8_t packet[2] = {
 564     0x55,
 565     0x55
 566   };
 567   write(packet, sizeof(packet));
 568   return bootloaderWaitCommandResponse();
 569 }
 570
 571 void Bluetooth::bootloaderSendCommand(uint8_t command, const void *data, uint8_t size)
 572 {
 573   uint8_t packet[3] = {
 574     uint8_t(3 + size),
 575     bootloaderChecksum(command, (uint8_t *) data, size),
 576     command
 577   };
 578   write(packet, sizeof(packet));
 579   if (size > 0) {
 580     write((uint8_t *)data, size);
 581   }
 582 }
 583
 584 void Bluetooth::bootloaderSendCommandResponse(uint8_t response)
 585 {
 586   uint8_t packet[2] = {
 587     0x00,
 588     response
 589   };
 590   write(packet, sizeof(packet));
 591 }
 592
 593 enum {
 594   CMD_DOWNLOAD = 0x21,
 595   CMD_GET_STATUS = 0x23,
 596   CMD_SEND_DATA = 0x24,
 597   CMD_SECTOR_ERASE = 0x26,
 598   CMD_GET_CHIP_ID = 0x28,
 599 };
 600
 601 enum {
 602   CMD_RET_SUCCESS = 0x40,
 603 };
 604
 605 constexpr uint32_t CC26XX_BOOTLOADER_SIZE = 0x00001000;
 606 constexpr uint32_t CC26XX_FIRMWARE_BASE = CC26XX_BOOTLOADER_SIZE;
 607
 608 constexpr uint32_t CC26XX_PAGE_ERASE_SIZE = 0x1000;
 609 constexpr uint32_t CC26XX_MAX_BYTES_PER_TRANSFER = 252;
 610
 611 const char * Bluetooth::bootloaderSendData(const uint8_t * data, uint8_t size)
 612 {
 613   bootloaderSendCommand(CMD_SEND_DATA, data, size);
 614   return bootloaderWaitCommandResponse();
 615 }
 616
 617 const char * Bluetooth::bootloaderReadStatus(uint8_t &status)
 618 {
 619   bootloaderSendCommand(CMD_GET_STATUS);
 620   const char * result = bootloaderWaitCommandResponse();
 621   if (result)
 622     return result;
 623   result = bootloaderWaitResponseData(&status, 1);
 624   bootloaderSendCommandResponse(result == nullptr ? BLUETOOTH_ACK : BLUETOOTH_NACK);
 625   return result;
 626 }
 627
 628 const char * Bluetooth::bootloaderCheckStatus()
 629 {
 630   uint8_t status;
 631   const char * result = bootloaderReadStatus(status);
 632   if (result)
 633     return result;
 634   if (status != CMD_RET_SUCCESS)
 635     return "Wrong status";
 636   return nullptr;
 637 }
 638
 639 const char * Bluetooth::bootloaderEraseFlash(uint32_t start, uint32_t size)
 640 {
 641   uint32_t address = start;
 642   uint32_t end = start + size;
 643   while (address < end) {
 644     uint32_t addressBigEndian = SWAP32(address);
 645     bootloaderSendCommand(CMD_SECTOR_ERASE, &addressBigEndian, sizeof(addressBigEndian));
 646     const char * result = bootloaderWaitCommandResponse();
 647     if (result)
 648       return result;
 649     result = bootloaderCheckStatus();
 650     if (result)
 651       return result;
 652     address += CC26XX_PAGE_ERASE_SIZE;
 653   }
 654   return nullptr;
 655 }
 656
 657 const char * Bluetooth::bootloaderStartWriteFlash(uint32_t start, uint32_t size)
 658 {
 659   uint32_t cmdArgs[2] = {
 660     SWAP32(start),
 661     SWAP32(size),
 662   };
 663   bootloaderSendCommand(CMD_DOWNLOAD, cmdArgs, sizeof(cmdArgs));
 664   const char * result = bootloaderWaitCommandResponse();
 665   if (result)
 666     return result;
 667   result = bootloaderCheckStatus();
 668   if (result)
 669     return result;
 670   return result;
 671 }
 672
 673 const char * Bluetooth::bootloaderWriteFlash(const uint8_t * data, uint32_t size)
 674 {
 675   while (size > 0) {
 676     uint32_t len = min<uint32_t>(size, CC26XX_MAX_BYTES_PER_TRANSFER);
 677     const char * result = bootloaderSendData(data, len);
 678     if (result)
 679       return result;
 680     result = bootloaderCheckStatus();
 681     if (result)
 682       return result;
 683     data += len;
 684     size -= len;
 685   }
 686   return nullptr;
 687 }
 688
 689 const char * Bluetooth::doFlashFirmware(const char * filename)
 690 {
 691   const char * result;
 692   FIL file;
 693   uint8_t buffer[CC26XX_MAX_BYTES_PER_TRANSFER * 4];
 694   UINT count;
 695
 696   // Dummy command
 697   bootloaderSendCommand(0);
 698   result = bootloaderWaitCommandResponse(0);
 699   if (result)
 700     result = bootloaderSetAutoBaud();
 701   if (result)
 702     return result;
 703
 704   // Get chip ID
 705   bootloaderSendCommand(CMD_GET_CHIP_ID);
 706   result = bootloaderWaitCommandResponse();
 707   if (result)
 708     return result;
 709   uint8_t id[4];
 710   result = bootloaderWaitResponseData(id, 4);
 711   bootloaderSendCommandResponse(result == nullptr ? BLUETOOTH_ACK : BLUETOOTH_NACK);
 712
 713   if (f_open(&file, filename, FA_READ) != FR_OK) {
 714     return "Error opening file";
 715   }
 716
 717   FrSkyFirmwareInformation * information = (FrSkyFirmwareInformation *)buffer;
 718   if (f_read(&file, buffer, sizeof(FrSkyFirmwareInformation), &count) != FR_OK || count != sizeof(FrSkyFirmwareInformation)) {
 719     f_close(&file);
 720     return "Format error";
 721   }
 722
 723   drawProgressScreen(getBasename(filename), STR_FLASH_ERASE, 0, 0);
 724
 725   result = bootloaderEraseFlash(CC26XX_FIRMWARE_BASE, information->size);
 726   if (result) {
 727     f_close(&file);
 728     return result;
 729   }
 730
 731   uint32_t size = information->size;
 732   drawProgressScreen(getBasename(filename), STR_FLASH_WRITE, 0, size);
 733
 734   result = bootloaderStartWriteFlash(CC26XX_FIRMWARE_BASE, size);
 735   if (result)
 736     return result;
 737
 738   uint32_t done = 0;
 739   while (1) {
 740     drawProgressScreen(getBasename(filename), STR_FLASH_WRITE, done, size);
 741     if (f_read(&file, buffer, min<uint32_t>(sizeof(buffer), size - done), &count) != FR_OK) {
 742       f_close(&file);
 743       return "Error reading file";
 744     }
 745     result = bootloaderWriteFlash(buffer, count);
 746     if (result)
 747       return result;
 748     done += count;
 749     if (done >= size) {
 750       f_close(&file);
 751       return nullptr;
 752     }
 753   }
 754 }
 755
 756 const char * Bluetooth::flashFirmware(const char * filename)
 757 {
 758   drawProgressScreen(getBasename(filename), STR_MODULE_RESET, 0, 0);
 759
 760   state = BLUETOOTH_STATE_FLASH_FIRMWARE;
 761
 762   pausePulses();
 763
 764   bluetoothInit(BLUETOOTH_BOOTLOADER_BAUDRATE, true); // normal mode
 765   watchdogSuspend(500 /*5s*/);
 766   RTOS_WAIT_MS(1000);
 767
 768   bluetoothInit(BLUETOOTH_BOOTLOADER_BAUDRATE, false); // bootloader mode
 769   watchdogSuspend(500 /*5s*/);
 770   RTOS_WAIT_MS(1000);
 771
 772   const char * result = doFlashFirmware(filename);
 773
 774   AUDIO_PLAY(AU_SPECIAL_SOUND_BEEP1 );
 775   BACKLIGHT_ENABLE();
 776
 777   if (result) {
 778     POPUP_WARNING(STR_FIRMWARE_UPDATE_ERROR);
 779     SET_WARNING_INFO(result, strlen(result), 0);
 780   }
 781   else {
 782     POPUP_INFORMATION(STR_FIRMWARE_UPDATE_SUCCESS);
 783   }
 784
 785   drawProgressScreen(getBasename(filename), STR_MODULE_RESET, 0, 0);
 786
 787   /* wait 1s off */
 788   watchdogSuspend(500 /*5s*/);
 789   RTOS_WAIT_MS(1000);
 790
 791   state = BLUETOOTH_STATE_OFF;
 792   resumePulses();
 793
 794   return result;
 795 }