src/main/io/serial_4way.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  * Author: 4712
  23 */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <string.h>
  28
  29 #include "platform.h"
  30
  31 #ifdef  USE_SERIAL_4WAY_BLHELI_INTERFACE
  32
  33 #include "drivers/buf_writer.h"
  34 #include "drivers/io.h"
  35 #include "drivers/serial.h"
  36 #include "drivers/time.h"
  37 #include "drivers/timer.h"
  38 #include "drivers/light_led.h"
  39
  40 #include "flight/mixer.h"
  41
  42 #include "io/beeper.h"
  43 #include "io/serial_4way.h"
  44
  45 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
  46 #include "io/serial_4way_avrootloader.h"
  47 #endif
  48 #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
  49 #include "io/serial_4way_stk500v2.h"
  50 #endif
  51
  52 #if defined(USE_HAL_DRIVER)
  53 #define Bit_RESET GPIO_PIN_RESET
  54 #endif
  55
  56 #define USE_TXRX_LED
  57
  58 #ifdef  USE_TXRX_LED
  59 #define RX_LED_OFF LED0_OFF
  60 #define RX_LED_ON LED0_ON
  61 #ifdef  LED1
  62 #define TX_LED_OFF LED1_OFF
  63 #define TX_LED_ON LED1_ON
  64 #else
  65 #define TX_LED_OFF LED0_OFF
  66 #define TX_LED_ON LED0_ON
  67 #endif
  68 #else
  69 #define RX_LED_OFF
  70 #define RX_LED_ON
  71 #define TX_LED_OFF
  72 #define TX_LED_ON
  73 #endif
  74
  75 #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf"
  76 // *** change to adapt Revision
  77 #define SERIAL_4WAY_VER_MAIN 20
  78 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
  79 #define SERIAL_4WAY_VER_SUB_2 (uint8_t) 05
  80
  81 #define SERIAL_4WAY_PROTOCOL_VER 108
  82 // *** end
  83
  84 #if (SERIAL_4WAY_VER_MAIN > 24)
  85 #error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t"
  86 #endif
  87
  88 #define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2)
  89
  90 #define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100)
  91 #define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100)
  92
  93 static uint8_t escCount;
  94
  95 escHardware_t escHardware[MAX_SUPPORTED_MOTORS];
  96
  97 uint8_t selected_esc;
  98
  99 uint8_32_u DeviceInfo;
 100
 101 #define DeviceInfoSize 4
 102
 103 inline bool isMcuConnected(void)
 104 {
 105     return (DeviceInfo.bytes[0] > 0);
 106 }
 107
 108 inline bool isEscHi(uint8_t selEsc)
 109 {
 110     return (IORead(escHardware[selEsc].io) != Bit_RESET);
 111 }
 112 inline bool isEscLo(uint8_t selEsc)
 113 {
 114     return (IORead(escHardware[selEsc].io) == Bit_RESET);
 115 }
 116
 117 inline void setEscHi(uint8_t selEsc)
 118 {
 119     IOHi(escHardware[selEsc].io);
 120 }
 121
 122 inline void setEscLo(uint8_t selEsc)
 123 {
 124     IOLo(escHardware[selEsc].io);
 125 }
 126
 127 inline void setEscInput(uint8_t selEsc)
 128 {
 129     IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU);
 130 }
 131
 132 inline void setEscOutput(uint8_t selEsc)
 133 {
 134     IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP);
 135 }
 136
 137 uint8_t esc4wayInit(void)
 138 {
 139     // StopPwmAllMotors();
 140     // XXX Review effect of motor refactor
 141     //pwmDisableMotors();
 142     escCount = 0;
 143     memset(&escHardware, 0, sizeof(escHardware));
 144     pwmOutputPort_t *pwmMotors = pwmGetMotors();
 145     for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 146         if (pwmMotors[i].enabled) {
 147             if (pwmMotors[i].io != IO_NONE) {
 148                 escHardware[escCount].io = pwmMotors[i].io;
 149                 setEscInput(escCount);
 150                 setEscHi(escCount);
 151                 escCount++;
 152             }
 153         }
 154     }
 155     motorDisable();
 156     return escCount;
 157 }
 158
 159 void esc4wayRelease(void)
 160 {
 161     while (escCount > 0) {
 162         escCount--;
 163         IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP);
 164         setEscLo(escCount);
 165     }
 166     motorEnable();
 167 }
 168
 169
 170 #define SET_DISCONNECTED DeviceInfo.words[0] = 0
 171
 172 #define INTF_MODE_IDX 3  // index for DeviceInfostate
 173
 174 // Interface related only
 175 // establish and test connection to the Interface
 176
 177 // Send Structure
 178 // ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo
 179 // Return
 180 // ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo
 181
 182 #define cmd_Remote_Escape 0x2E // '.'
 183 #define cmd_Local_Escape  0x2F // '/'
 184
 185 // Test Interface still present
 186 #define cmd_InterfaceTestAlive 0x30 // '0' alive
 187 // RETURN: ACK
 188
 189 // get Protocol Version Number 01..255
 190 #define cmd_ProtocolGetVersion 0x31  // '1' version
 191 // RETURN: uint8_t VersionNumber + ACK
 192
 193 // get Version String
 194 #define cmd_InterfaceGetName 0x32 // '2' name
 195 // RETURN: String + ACK
 196
 197 //get Version Number 01..255
 198 #define cmd_InterfaceGetVersion 0x33  // '3' version
 199 // RETURN: uint8_t AVersionNumber + ACK
 200
 201
 202 // Exit / Restart Interface - can be used to switch to Box Mode
 203 #define cmd_InterfaceExit 0x34       // '4' exit
 204 // RETURN: ACK
 205
 206 // Reset the Device connected to the Interface
 207 #define cmd_DeviceReset 0x35        // '5' reset
 208 // RETURN: ACK
 209
 210 // Get the Device ID connected
 211 // #define cmd_DeviceGetID 0x36      //'6' device id removed since 06/106
 212 // RETURN: uint8_t DeviceID + ACK
 213
 214 // Initialize Flash Access for Device connected
 215 #define cmd_DeviceInitFlash 0x37    // '7' init flash access
 216 // RETURN: ACK
 217
 218 // Erase the whole Device Memory of connected Device
 219 #define cmd_DeviceEraseAll 0x38     // '8' erase all
 220 // RETURN: ACK
 221
 222 // Erase one Page of Device Memory of connected Device
 223 #define cmd_DevicePageErase 0x39    // '9' page erase
 224 // PARAM: uint8_t APageNumber
 225 // RETURN: ACK
 226
 227 // Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes
 228 // BuffLen = 0 means 256 Bytes
 229 #define cmd_DeviceRead 0x3A  // ':' read Device
 230 // PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255]
 231 // RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK
 232
 233 // Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes
 234 // BuffLen = 0 means 256 Bytes
 235 #define cmd_DeviceWrite 0x3B    // ';' write
 236 // PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255]
 237 // RETURN: ACK
 238
 239 // Set C2CK low infinite ) permanent Reset state
 240 #define cmd_DeviceC2CK_LOW 0x3C // '<'
 241 // RETURN: ACK
 242
 243 // Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes
 244 // BuffLen = 0 means 256 Bytes
 245 #define cmd_DeviceReadEEprom 0x3D  // '=' read Device
 246 // PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255]
 247 // RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK
 248
 249 // Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes
 250 // BuffLen = 0 means 256 Bytes
 251 #define cmd_DeviceWriteEEprom 0x3E  // '>' write
 252 // PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255]
 253 // RETURN: ACK
 254
 255 // Set Interface Mode
 256 #define cmd_InterfaceSetMode 0x3F   // '?'
 257 // #define imC2 0
 258 // #define imSIL_BLB 1
 259 // #define imATM_BLB 2
 260 // #define imSK 3
 261 // PARAM: uint8_t Mode
 262 // RETURN: ACK or ACK_I_INVALID_CHANNEL
 263
 264 //Write to Buffer for Verify Device Memory of connected Device //Buffer Len is Max 256 Bytes
 265 //BuffLen = 0 means 256 Bytes
 266 #define cmd_DeviceVerify 0x40   //'@' write
 267 //PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255]
 268 //RETURN: ACK
 269
 270
 271 // responses
 272 #define ACK_OK                  0x00
 273 // #define ACK_I_UNKNOWN_ERROR       0x01
 274 #define ACK_I_INVALID_CMD       0x02
 275 #define ACK_I_INVALID_CRC       0x03
 276 #define ACK_I_VERIFY_ERROR      0x04
 277 // #define ACK_D_INVALID_COMMAND 0x05
 278 // #define ACK_D_COMMAND_FAILED  0x06
 279 // #define ACK_D_UNKNOWN_ERROR       0x07
 280
 281 #define ACK_I_INVALID_CHANNEL   0x08
 282 #define ACK_I_INVALID_PARAM     0x09
 283 #define ACK_D_GENERAL_ERROR     0x0F
 284
 285 /* Copyright (c) 2002, 2003, 2004  Marek Michalkiewicz
 286    Copyright (c) 2005, 2007 Joerg Wunsch
 287    Copyright (c) 2013 Dave Hylands
 288    Copyright (c) 2013 Frederic Nadeau
 289    All rights reserved.
 290
 291    Redistribution and use in source and binary forms, with or without
 292    modification, are permitted provided that the following conditions are met:
 293
 294    * Redistributions of source code must retain the above copyright
 295      notice, this list of conditions and the following disclaimer.
 296
 297    * Redistributions in binary form must reproduce the above copyright
 298      notice, this list of conditions and the following disclaimer in
 299      the documentation and/or other materials provided with the
 300      distribution.
 301
 302    * Neither the name of the copyright holders nor the names of
 303      contributors may be used to endorse or promote products derived
 304      from this software without specific prior written permission.
 305
 306   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 307   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 308   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 309   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 310   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 311   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 312   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 313   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 314   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 315   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 316   POSSIBILITY OF SUCH DAMAGE. */
 317 uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
 318         int i;
 319
 320         crc = crc ^ ((uint16_t)data << 8);
 321         for (i=0; i < 8; i++) {
 322             if (crc & 0x8000)
 323                 crc = (crc << 1) ^ 0x1021;
 324             else
 325                 crc <<= 1;
 326         }
 327         return crc;
 328 }
 329 // * End copyright
 330
 331
 332 #define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \
 333         (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B))
 334
 335 #define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310) || (pDeviceInfo->words[0] == 0xF330) || \
 336         (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \
 337         (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \
 338         (pDeviceInfo->words[0] == 0xE8B2))
 339
 340 // BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
 341 #define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06))
 342
 343 static uint8_t CurrentInterfaceMode;
 344
 345 static uint8_t Connect(uint8_32_u *pDeviceInfo)
 346 {
 347     for (uint8_t I = 0; I < 3; ++I) {
 348 #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
 349         if ((CurrentInterfaceMode != imARM_BLB) && Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) {
 350             CurrentInterfaceMode = imSK;
 351             return 1;
 352         } else {
 353             if (BL_ConnectEx(pDeviceInfo)) {
 354                 if  SILABS_DEVICE_MATCH {
 355                     CurrentInterfaceMode = imSIL_BLB;
 356                     return 1;
 357                 } else if ATMEL_DEVICE_MATCH {
 358                     CurrentInterfaceMode = imATM_BLB;
 359                     return 1;
 360                 } else if ARM_DEVICE_MATCH {
 361                     CurrentInterfaceMode = imARM_BLB;
 362                     return 1;
 363                 }
 364             }
 365         }
 366 #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 367         if (BL_ConnectEx(pDeviceInfo)) {
 368             if SILABS_DEVICE_MATCH {
 369                 CurrentInterfaceMode = imSIL_BLB;
 370                 return 1;
 371             } else if ATMEL_DEVICE_MATCH {
 372                 CurrentInterfaceMode = imATM_BLB;
 373                 return 1;
 374             }  else if ARM_DEVICE_MATCH {
 375                 CurrentInterfaceMode = imARM_BLB;
 376                 return 1;
 377             }
 378         }
 379 #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 380         if (Stk_ConnectEx(pDeviceInfo)) {
 381             CurrentInterfaceMode = imSK;
 382             if ATMEL_DEVICE_MATCH return 1;
 383         }
 384 #endif
 385     }
 386     return 0;
 387 }
 388
 389 static serialPort_t *port;
 390
 391 static uint8_t ReadByte(void)
 392 {
 393     // need timeout?
 394     while (!serialRxBytesWaiting(port));
 395     return serialRead(port);
 396 }
 397
 398 static uint8_16_u CRC_in;
 399 static uint8_t ReadByteCrc(void)
 400 {
 401     uint8_t b = ReadByte();
 402     CRC_in.word = _crc_xmodem_update(CRC_in.word, b);
 403     return b;
 404 }
 405
 406 static void WriteByte(uint8_t b)
 407 {
 408     serialWrite(port, b);
 409 }
 410
 411 static uint8_16_u CRCout;
 412 static void WriteByteCrc(uint8_t b)
 413 {
 414     WriteByte(b);
 415     CRCout.word = _crc_xmodem_update(CRCout.word, b);
 416 }
 417
 418 void esc4wayProcess(serialPort_t *mspPort)
 419 {
 420
 421     uint8_t ParamBuf[256];
 422     uint8_t ESC;
 423     uint8_t I_PARAM_LEN;
 424     uint8_t CMD;
 425     uint8_t ACK_OUT;
 426     uint8_16_u CRC_check;
 427     uint8_16_u Dummy;
 428     uint8_t O_PARAM_LEN;
 429     uint8_t *O_PARAM;
 430     uint8_t *InBuff;
 431     ioMem_t ioMem;
 432
 433     port = mspPort;
 434
 435     // Start here  with UART Main loop
 436 #ifdef USE_BEEPER
 437     // fix for buzzer often starts beeping continuously when the ESCs are read
 438     // switch beeper silent here
 439     beeperSilence();
 440 #endif
 441     bool isExitScheduled = false;
 442
 443     while (1) {
 444         // restart looking for new sequence from host
 445         do {
 446             CRC_in.word = 0;
 447             ESC = ReadByteCrc();
 448         } while (ESC != cmd_Local_Escape);
 449
 450         RX_LED_ON;
 451
 452         Dummy.word = 0;
 453         O_PARAM = &Dummy.bytes[0];
 454         O_PARAM_LEN = 1;
 455         CMD = ReadByteCrc();
 456         ioMem.D_FLASH_ADDR_H = ReadByteCrc();
 457         ioMem.D_FLASH_ADDR_L = ReadByteCrc();
 458         I_PARAM_LEN = ReadByteCrc();
 459
 460         InBuff = ParamBuf;
 461         uint8_t i = I_PARAM_LEN;
 462         do {
 463           *InBuff = ReadByteCrc();
 464           InBuff++;
 465           i--;
 466         } while (i != 0);
 467
 468         CRC_check.bytes[1] = ReadByte();
 469         CRC_check.bytes[0] = ReadByte();
 470
 471         if (CRC_check.word == CRC_in.word) {
 472             ACK_OUT = ACK_OK;
 473         } else {
 474             ACK_OUT = ACK_I_INVALID_CRC;
 475         }
 476
 477         TX_LED_ON;
 478
 479         if (ACK_OUT == ACK_OK)
 480         {
 481             // wtf.D_FLASH_ADDR_H=Adress_H;
 482             // wtf.D_FLASH_ADDR_L=Adress_L;
 483             ioMem.D_PTR_I = ParamBuf;
 484
 485
 486             switch (CMD) {
 487                 // ******* Interface related stuff *******
 488                 case cmd_InterfaceTestAlive:
 489                 {
 490                     if (isMcuConnected()) {
 491                         switch (CurrentInterfaceMode)
 492                         {
 493                             #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 494                             case imATM_BLB:
 495                             case imSIL_BLB:
 496                             case imARM_BLB:
 497                             {
 498                                 if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included
 499                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 500                                 }
 501                                 break;
 502                             }
 503                             #endif
 504                             #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 505                             case imSK:
 506                             {
 507                                 if (!Stk_SignOn()) { // SetStateDisconnected();
 508                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 509                                 }
 510                                 break;
 511                             }
 512                             #endif
 513                             default:
 514                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 515                         }
 516                         if ( ACK_OUT != ACK_OK) SET_DISCONNECTED;
 517                     }
 518                     break;
 519                 }
 520                 case cmd_ProtocolGetVersion:
 521                 {
 522                     // Only interface itself, no matter what Device
 523                     Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER;
 524                     break;
 525                 }
 526
 527                 case cmd_InterfaceGetName:
 528                 {
 529                     // Only interface itself, no matter what Device
 530                     // O_PARAM_LEN=16;
 531                     O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR);
 532                     O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR;
 533                     break;
 534                 }
 535
 536                 case cmd_InterfaceGetVersion:
 537                 {
 538                     // Only interface itself, no matter what Device
 539                     // Dummy = iUart_res_InterfVersion;
 540                     O_PARAM_LEN = 2;
 541                     Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI;
 542                     Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO;
 543                     break;
 544                 }
 545                 case cmd_InterfaceExit:
 546                 {
 547                     isExitScheduled = true;
 548                     break;
 549                 }
 550                 case cmd_InterfaceSetMode:
 551                 {
 552 #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 553                     if ((ParamBuf[0] <= imARM_BLB) && (ParamBuf[0] >= imSIL_BLB)) {
 554 #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 555                     if (((ParamBuf[0] <= imATM_BLB)||(ParamBuf[0] == imARM_BLB)) && (ParamBuf[0] >= imSIL_BLB)) {
 556 #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 557                     if (ParamBuf[0] == imSK) {
 558 #endif
 559                         CurrentInterfaceMode = ParamBuf[0];
 560                     } else {
 561                         ACK_OUT = ACK_I_INVALID_PARAM;
 562                     }
 563                     break;
 564                 }
 565
 566                 case cmd_DeviceReset:
 567                 {
 568                     bool rebootEsc = false;
 569                     if (ParamBuf[0] < escCount) {
 570                         // Channel may change here
 571                         selected_esc = ParamBuf[0];
 572                         if (ioMem.D_FLASH_ADDR_L == 1) {
 573                             rebootEsc = true;
 574                         }
 575                     }
 576                     else {
 577                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 578                         break;
 579                     }
 580                     switch (CurrentInterfaceMode)
 581                     {
 582                         case imSIL_BLB:
 583                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 584                         case imATM_BLB:
 585                         case imARM_BLB:
 586                         {
 587                             BL_SendCMDRunRestartBootloader(&DeviceInfo);
 588                             if (rebootEsc) {
 589                                 ESC_OUTPUT;
 590                                 setEscLo(selected_esc);
 591                                 timeMs_t m = millis();
 592                                 while (millis() - m < 300);
 593                                 setEscHi(selected_esc);
 594                                 ESC_INPUT;
 595                             }
 596                             break;
 597                         }
 598                         #endif
 599                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 600                         case imSK:
 601                         {
 602                             break;
 603                         }
 604                         #endif
 605                     }
 606                     SET_DISCONNECTED;
 607                     break;
 608                 }
 609                 case cmd_DeviceInitFlash:
 610                 {
 611                     SET_DISCONNECTED;
 612                     if (ParamBuf[0] < escCount) {
 613                         //Channel may change here
 614                         //ESC_LO or ESC_HI; Halt state for prev channel
 615                         selected_esc = ParamBuf[0];
 616                     } else {
 617                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 618                         break;
 619                     }
 620                     O_PARAM_LEN = DeviceInfoSize; //4
 621                     O_PARAM = (uint8_t *)&DeviceInfo;
 622                     if (Connect(&DeviceInfo)) {
 623                         DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode;
 624                     } else {
 625                         SET_DISCONNECTED;
 626                         ACK_OUT = ACK_D_GENERAL_ERROR;
 627                     }
 628                     break;
 629                 }
 630
 631                 #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 632                 case cmd_DeviceEraseAll:
 633                 {
 634                     switch (CurrentInterfaceMode)
 635                     {
 636                         case imSK:
 637                         {
 638                             if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR;
 639                             break;
 640                         }
 641                         default:
 642                             ACK_OUT = ACK_I_INVALID_CMD;
 643                     }
 644                     break;
 645                 }
 646                 #endif
 647
 648                 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 649                 case cmd_DevicePageErase:
 650                 {
 651                     switch (CurrentInterfaceMode)
 652                     {
 653                         case imSIL_BLB:
 654                         case imARM_BLB:
 655                         {
 656                             Dummy.bytes[0] = ParamBuf[0];
 657                             if  (CurrentInterfaceMode == imARM_BLB) {
 658                                 // Address =Page * 1024
 659                                 ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 2);
 660                             } else {
 661                                 // Address =Page * 512
 662                                 ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1);
 663                             }
 664                             ioMem.D_FLASH_ADDR_L = 0;
 665                             if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR;
 666                             break;
 667                         }
 668                         default:
 669                             ACK_OUT = ACK_I_INVALID_CMD;
 670                     }
 671                     break;
 672                 }
 673                 #endif
 674
 675                 //*** Device Memory Read Ops ***
 676                 case cmd_DeviceRead:
 677                 {
 678                     ioMem.D_NUM_BYTES = ParamBuf[0];
 679                     /*
 680                     wtf.D_FLASH_ADDR_H=Adress_H;
 681                     wtf.D_FLASH_ADDR_L=Adress_L;
 682                     wtf.D_PTR_I = BUF_I;
 683                     */
 684                     switch (CurrentInterfaceMode)
 685                     {
 686                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 687                         case imSIL_BLB:
 688                         case imATM_BLB:
 689                         case imARM_BLB:
 690                         {
 691                             if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem))
 692                             {
 693                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 694                             }
 695                             break;
 696                         }
 697                         #endif
 698                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 699                         case imSK:
 700                         {
 701                             if (!Stk_ReadFlash(&ioMem))
 702                             {
 703                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 704                             }
 705                             break;
 706                         }
 707                         #endif
 708                         default:
 709                             ACK_OUT = ACK_I_INVALID_CMD;
 710                     }
 711                     if (ACK_OUT == ACK_OK)
 712                     {
 713                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 714                         O_PARAM = (uint8_t *)&ParamBuf;
 715                     }
 716                     break;
 717                 }
 718
 719                 case cmd_DeviceReadEEprom:
 720                 {
 721                     ioMem.D_NUM_BYTES = ParamBuf[0];
 722                     /*
 723                     wtf.D_FLASH_ADDR_H = Adress_H;
 724                     wtf.D_FLASH_ADDR_L = Adress_L;
 725                     wtf.D_PTR_I = BUF_I;
 726                     */
 727                     switch (CurrentInterfaceMode)
 728                     {
 729                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 730                         case imATM_BLB:
 731                         {
 732                             if (!BL_ReadEEprom(&ioMem))
 733                             {
 734                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 735                             }
 736                             break;
 737                         }
 738                         #endif
 739                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 740                         case imSK:
 741                         {
 742                             if (!Stk_ReadEEprom(&ioMem))
 743                             {
 744                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 745                             }
 746                             break;
 747                         }
 748                         #endif
 749                         default:
 750                             ACK_OUT = ACK_I_INVALID_CMD;
 751                     }
 752                     if (ACK_OUT == ACK_OK)
 753                     {
 754                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 755                         O_PARAM = (uint8_t *)&ParamBuf;
 756                     }
 757                     break;
 758                 }
 759
 760                 //*** Device Memory Write Ops ***
 761                 case cmd_DeviceWrite:
 762                 {
 763                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 764                     /*
 765                     wtf.D_FLASH_ADDR_H=Adress_H;
 766                     wtf.D_FLASH_ADDR_L=Adress_L;
 767                     wtf.D_PTR_I = BUF_I;
 768                     */
 769                     switch (CurrentInterfaceMode)
 770                     {
 771                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 772                         case imSIL_BLB:
 773                         case imATM_BLB:
 774                         case imARM_BLB:
 775                         {
 776                             if (!BL_WriteFlash(&ioMem)) {
 777                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 778                             }
 779                             break;
 780                         }
 781                         #endif
 782                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 783                         case imSK:
 784                         {
 785                             if (!Stk_WriteFlash(&ioMem))
 786                             {
 787                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 788                             }
 789                             break;
 790                         }
 791                         #endif
 792                     }
 793                     break;
 794                 }
 795
 796                 case cmd_DeviceWriteEEprom:
 797                 {
 798                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 799                     ACK_OUT = ACK_D_GENERAL_ERROR;
 800                     /*
 801                     wtf.D_FLASH_ADDR_H=Adress_H;
 802                     wtf.D_FLASH_ADDR_L=Adress_L;
 803                     wtf.D_PTR_I = BUF_I;
 804                     */
 805                     switch (CurrentInterfaceMode)
 806                     {
 807                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 808                         case imSIL_BLB:
 809                         {
 810                             ACK_OUT = ACK_I_INVALID_CMD;
 811                             break;
 812                         }
 813                         case imATM_BLB:
 814                         {
 815                             if (BL_WriteEEprom(&ioMem))
 816                             {
 817                                 ACK_OUT = ACK_OK;
 818                             }
 819                             break;
 820                         }
 821                         #endif
 822                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 823                         case imSK:
 824                         {
 825                             if (Stk_WriteEEprom(&ioMem))
 826                             {
 827                                 ACK_OUT = ACK_OK;
 828                             }
 829                             break;
 830                         }
 831                         #endif
 832                     }
 833                     break;
 834                 }
 835                 //*** Device Memory Verify Ops ***
 836                 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 837                 case cmd_DeviceVerify:
 838                 {
 839                     switch (CurrentInterfaceMode)
 840                     {
 841                         case imARM_BLB:
 842                         {
 843                             ioMem.D_NUM_BYTES = I_PARAM_LEN;
 844                             /*
 845                             wtf.D_FLASH_ADDR_H=Adress_H;
 846                             wtf.D_FLASH_ADDR_L=Adress_L;
 847                             wtf.D_PTR_I = BUF_I;
 848                             */
 849
 850                             ACK_OUT = BL_VerifyFlash(&ioMem);
 851                             switch (ACK_OUT) {
 852                                 case brSUCCESS:
 853                                     ACK_OUT = ACK_OK;
 854                                     break;
 855                                 case brERRORVERIFY:
 856                                     ACK_OUT = ACK_I_VERIFY_ERROR;
 857                                     break;
 858                                 default:
 859                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 860                                     break;
 861                             }
 862                             break;
 863                         }
 864                         default:
 865                         {
 866                             ACK_OUT = ACK_I_INVALID_CMD;
 867                             break;
 868                         }
 869                     }
 870                     break;
 871                 }
 872                 #endif
 873                 default:
 874                 {
 875                     ACK_OUT = ACK_I_INVALID_CMD;
 876                 }
 877             }
 878         }
 879
 880         CRCout.word = 0;
 881
 882         RX_LED_OFF;
 883
 884         serialBeginWrite(port);
 885         WriteByteCrc(cmd_Remote_Escape);
 886         WriteByteCrc(CMD);
 887         WriteByteCrc(ioMem.D_FLASH_ADDR_H);
 888         WriteByteCrc(ioMem.D_FLASH_ADDR_L);
 889         WriteByteCrc(O_PARAM_LEN);
 890
 891         i=O_PARAM_LEN;
 892         do {
 893             while (!serialTxBytesFree(port));
 894
 895             WriteByteCrc(*O_PARAM);
 896             O_PARAM++;
 897             i--;
 898         } while (i > 0);
 899
 900         WriteByteCrc(ACK_OUT);
 901         WriteByte(CRCout.bytes[1]);
 902         WriteByte(CRCout.bytes[0]);
 903         serialEndWrite(port);
 904
 905         TX_LED_OFF;
 906         if (isExitScheduled) {
 907             esc4wayRelease();
 908             return;
 909         }
 910     };
 911 }
 912
 913
 914
 915 #endif