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) 06
  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] > 0xE800) && (pDeviceInfo->words[0] < 0xF900))
 336
 337
 338 // BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
 339 #define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06))
 340
 341 static uint8_t CurrentInterfaceMode;
 342
 343 static uint8_t Connect(uint8_32_u *pDeviceInfo)
 344 {
 345     for (uint8_t I = 0; I < 3; ++I) {
 346 #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
 347         if ((CurrentInterfaceMode != imARM_BLB) && Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) {
 348             CurrentInterfaceMode = imSK;
 349             return 1;
 350         } else {
 351             if (BL_ConnectEx(pDeviceInfo)) {
 352                 if  SILABS_DEVICE_MATCH {
 353                     CurrentInterfaceMode = imSIL_BLB;
 354                     return 1;
 355                 } else if ATMEL_DEVICE_MATCH {
 356                     CurrentInterfaceMode = imATM_BLB;
 357                     return 1;
 358                 } else if ARM_DEVICE_MATCH {
 359                     CurrentInterfaceMode = imARM_BLB;
 360                     return 1;
 361                 }
 362             }
 363         }
 364 #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 365         if (BL_ConnectEx(pDeviceInfo)) {
 366             if SILABS_DEVICE_MATCH {
 367                 CurrentInterfaceMode = imSIL_BLB;
 368                 return 1;
 369             } else if ATMEL_DEVICE_MATCH {
 370                 CurrentInterfaceMode = imATM_BLB;
 371                 return 1;
 372             }  else if ARM_DEVICE_MATCH {
 373                 CurrentInterfaceMode = imARM_BLB;
 374                 return 1;
 375             }
 376         }
 377 #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 378         if (Stk_ConnectEx(pDeviceInfo)) {
 379             CurrentInterfaceMode = imSK;
 380             if ATMEL_DEVICE_MATCH return 1;
 381         }
 382 #endif
 383     }
 384     return 0;
 385 }
 386
 387 static serialPort_t *port;
 388
 389 static uint8_t ReadByte(void)
 390 {
 391     // need timeout?
 392     while (!serialRxBytesWaiting(port));
 393     return serialRead(port);
 394 }
 395
 396 static uint8_16_u CRC_in;
 397 static uint8_t ReadByteCrc(void)
 398 {
 399     uint8_t b = ReadByte();
 400     CRC_in.word = _crc_xmodem_update(CRC_in.word, b);
 401     return b;
 402 }
 403
 404 static void WriteByte(uint8_t b)
 405 {
 406     serialWrite(port, b);
 407 }
 408
 409 static uint8_16_u CRCout;
 410 static void WriteByteCrc(uint8_t b)
 411 {
 412     WriteByte(b);
 413     CRCout.word = _crc_xmodem_update(CRCout.word, b);
 414 }
 415
 416 void esc4wayProcess(serialPort_t *mspPort)
 417 {
 418
 419     uint8_t ParamBuf[256];
 420     uint8_t ESC;
 421     uint8_t I_PARAM_LEN;
 422     uint8_t CMD;
 423     uint8_t ACK_OUT;
 424     uint8_16_u CRC_check;
 425     uint8_16_u Dummy;
 426     uint8_t O_PARAM_LEN;
 427     uint8_t *O_PARAM;
 428     uint8_t *InBuff;
 429     ioMem_t ioMem;
 430
 431     port = mspPort;
 432
 433     // Start here  with UART Main loop
 434 #ifdef USE_BEEPER
 435     // fix for buzzer often starts beeping continuously when the ESCs are read
 436     // switch beeper silent here
 437     beeperSilence();
 438 #endif
 439     bool isExitScheduled = false;
 440
 441     while (1) {
 442         // restart looking for new sequence from host
 443         do {
 444             CRC_in.word = 0;
 445             ESC = ReadByteCrc();
 446         } while (ESC != cmd_Local_Escape);
 447
 448         RX_LED_ON;
 449
 450         Dummy.word = 0;
 451         O_PARAM = &Dummy.bytes[0];
 452         O_PARAM_LEN = 1;
 453         CMD = ReadByteCrc();
 454         ioMem.D_FLASH_ADDR_H = ReadByteCrc();
 455         ioMem.D_FLASH_ADDR_L = ReadByteCrc();
 456         I_PARAM_LEN = ReadByteCrc();
 457
 458         InBuff = ParamBuf;
 459         uint8_t i = I_PARAM_LEN;
 460         do {
 461           *InBuff = ReadByteCrc();
 462           InBuff++;
 463           i--;
 464         } while (i != 0);
 465
 466         CRC_check.bytes[1] = ReadByte();
 467         CRC_check.bytes[0] = ReadByte();
 468
 469         if (CRC_check.word == CRC_in.word) {
 470             ACK_OUT = ACK_OK;
 471         } else {
 472             ACK_OUT = ACK_I_INVALID_CRC;
 473         }
 474
 475         TX_LED_ON;
 476
 477         if (ACK_OUT == ACK_OK)
 478         {
 479             // wtf.D_FLASH_ADDR_H=Adress_H;
 480             // wtf.D_FLASH_ADDR_L=Adress_L;
 481             ioMem.D_PTR_I = ParamBuf;
 482
 483
 484             switch (CMD) {
 485                 // ******* Interface related stuff *******
 486                 case cmd_InterfaceTestAlive:
 487                 {
 488                     if (isMcuConnected()) {
 489                         switch (CurrentInterfaceMode)
 490                         {
 491                             #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 492                             case imATM_BLB:
 493                             case imSIL_BLB:
 494                             case imARM_BLB:
 495                             {
 496                                 if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included
 497                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 498                                 }
 499                                 break;
 500                             }
 501                             #endif
 502                             #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 503                             case imSK:
 504                             {
 505                                 if (!Stk_SignOn()) { // SetStateDisconnected();
 506                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 507                                 }
 508                                 break;
 509                             }
 510                             #endif
 511                             default:
 512                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 513                         }
 514                         if ( ACK_OUT != ACK_OK) SET_DISCONNECTED;
 515                     }
 516                     break;
 517                 }
 518                 case cmd_ProtocolGetVersion:
 519                 {
 520                     // Only interface itself, no matter what Device
 521                     Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER;
 522                     break;
 523                 }
 524
 525                 case cmd_InterfaceGetName:
 526                 {
 527                     // Only interface itself, no matter what Device
 528                     // O_PARAM_LEN=16;
 529                     O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR);
 530                     O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR;
 531                     break;
 532                 }
 533
 534                 case cmd_InterfaceGetVersion:
 535                 {
 536                     // Only interface itself, no matter what Device
 537                     // Dummy = iUart_res_InterfVersion;
 538                     O_PARAM_LEN = 2;
 539                     Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI;
 540                     Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO;
 541                     break;
 542                 }
 543                 case cmd_InterfaceExit:
 544                 {
 545                     isExitScheduled = true;
 546                     break;
 547                 }
 548                 case cmd_InterfaceSetMode:
 549                 {
 550 #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 551                     if ((ParamBuf[0] <= imARM_BLB) && (ParamBuf[0] >= imSIL_BLB)) {
 552 #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 553                     if (((ParamBuf[0] <= imATM_BLB)||(ParamBuf[0] == imARM_BLB)) && (ParamBuf[0] >= imSIL_BLB)) {
 554 #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 555                     if (ParamBuf[0] == imSK) {
 556 #endif
 557                         CurrentInterfaceMode = ParamBuf[0];
 558                     } else {
 559                         ACK_OUT = ACK_I_INVALID_PARAM;
 560                     }
 561                     break;
 562                 }
 563
 564                 case cmd_DeviceReset:
 565                 {
 566                     bool rebootEsc = false;
 567                     if (ParamBuf[0] < escCount) {
 568                         // Channel may change here
 569                         selected_esc = ParamBuf[0];
 570                         if (ioMem.D_FLASH_ADDR_L == 1) {
 571                             rebootEsc = true;
 572                         }
 573                     }
 574                     else {
 575                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 576                         break;
 577                     }
 578                     switch (CurrentInterfaceMode)
 579                     {
 580                         case imSIL_BLB:
 581                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 582                         case imATM_BLB:
 583                         case imARM_BLB:
 584                         {
 585                             BL_SendCMDRunRestartBootloader(&DeviceInfo);
 586                             if (rebootEsc) {
 587                                 ESC_OUTPUT;
 588                                 setEscLo(selected_esc);
 589                                 timeMs_t m = millis();
 590                                 while (millis() - m < 300);
 591                                 setEscHi(selected_esc);
 592                                 ESC_INPUT;
 593                             }
 594                             break;
 595                         }
 596                         #endif
 597                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 598                         case imSK:
 599                         {
 600                             break;
 601                         }
 602                         #endif
 603                     }
 604                     SET_DISCONNECTED;
 605                     break;
 606                 }
 607                 case cmd_DeviceInitFlash:
 608                 {
 609                     SET_DISCONNECTED;
 610                     if (ParamBuf[0] < escCount) {
 611                         //Channel may change here
 612                         //ESC_LO or ESC_HI; Halt state for prev channel
 613                         selected_esc = ParamBuf[0];
 614                     } else {
 615                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 616                         break;
 617                     }
 618                     O_PARAM_LEN = DeviceInfoSize; //4
 619                     O_PARAM = (uint8_t *)&DeviceInfo;
 620                     if (Connect(&DeviceInfo)) {
 621                         DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode;
 622                     } else {
 623                         SET_DISCONNECTED;
 624                         ACK_OUT = ACK_D_GENERAL_ERROR;
 625                     }
 626                     break;
 627                 }
 628
 629                 #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 630                 case cmd_DeviceEraseAll:
 631                 {
 632                     switch (CurrentInterfaceMode)
 633                     {
 634                         case imSK:
 635                         {
 636                             if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR;
 637                             break;
 638                         }
 639                         default:
 640                             ACK_OUT = ACK_I_INVALID_CMD;
 641                     }
 642                     break;
 643                 }
 644                 #endif
 645
 646                 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 647                 case cmd_DevicePageErase:
 648                 {
 649                     switch (CurrentInterfaceMode)
 650                     {
 651                         case imSIL_BLB:
 652                         case imARM_BLB:
 653                         {
 654                             Dummy.bytes[0] = ParamBuf[0];
 655                             if  (CurrentInterfaceMode == imARM_BLB) {
 656                                 // Address =Page * 1024
 657                                 ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 2);
 658                             } else {
 659                                 // Address =Page * 512
 660                                 ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1);
 661                             }
 662                             ioMem.D_FLASH_ADDR_L = 0;
 663                             if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR;
 664                             break;
 665                         }
 666                         default:
 667                             ACK_OUT = ACK_I_INVALID_CMD;
 668                     }
 669                     break;
 670                 }
 671                 #endif
 672
 673                 //*** Device Memory Read Ops ***
 674                 case cmd_DeviceRead:
 675                 {
 676                     ioMem.D_NUM_BYTES = ParamBuf[0];
 677                     /*
 678                     wtf.D_FLASH_ADDR_H=Adress_H;
 679                     wtf.D_FLASH_ADDR_L=Adress_L;
 680                     wtf.D_PTR_I = BUF_I;
 681                     */
 682                     switch (CurrentInterfaceMode)
 683                     {
 684                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 685                         case imSIL_BLB:
 686                         case imATM_BLB:
 687                         case imARM_BLB:
 688                         {
 689                             if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem))
 690                             {
 691                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 692                             }
 693                             break;
 694                         }
 695                         #endif
 696                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 697                         case imSK:
 698                         {
 699                             if (!Stk_ReadFlash(&ioMem))
 700                             {
 701                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 702                             }
 703                             break;
 704                         }
 705                         #endif
 706                         default:
 707                             ACK_OUT = ACK_I_INVALID_CMD;
 708                     }
 709                     if (ACK_OUT == ACK_OK)
 710                     {
 711                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 712                         O_PARAM = (uint8_t *)&ParamBuf;
 713                     }
 714                     break;
 715                 }
 716
 717                 case cmd_DeviceReadEEprom:
 718                 {
 719                     ioMem.D_NUM_BYTES = ParamBuf[0];
 720                     /*
 721                     wtf.D_FLASH_ADDR_H = Adress_H;
 722                     wtf.D_FLASH_ADDR_L = Adress_L;
 723                     wtf.D_PTR_I = BUF_I;
 724                     */
 725                     switch (CurrentInterfaceMode)
 726                     {
 727                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 728                         case imATM_BLB:
 729                         {
 730                             if (!BL_ReadEEprom(&ioMem))
 731                             {
 732                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 733                             }
 734                             break;
 735                         }
 736                         #endif
 737                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 738                         case imSK:
 739                         {
 740                             if (!Stk_ReadEEprom(&ioMem))
 741                             {
 742                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 743                             }
 744                             break;
 745                         }
 746                         #endif
 747                         default:
 748                             ACK_OUT = ACK_I_INVALID_CMD;
 749                     }
 750                     if (ACK_OUT == ACK_OK)
 751                     {
 752                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 753                         O_PARAM = (uint8_t *)&ParamBuf;
 754                     }
 755                     break;
 756                 }
 757
 758                 //*** Device Memory Write Ops ***
 759                 case cmd_DeviceWrite:
 760                 {
 761                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 762                     /*
 763                     wtf.D_FLASH_ADDR_H=Adress_H;
 764                     wtf.D_FLASH_ADDR_L=Adress_L;
 765                     wtf.D_PTR_I = BUF_I;
 766                     */
 767                     switch (CurrentInterfaceMode)
 768                     {
 769                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 770                         case imSIL_BLB:
 771                         case imATM_BLB:
 772                         case imARM_BLB:
 773                         {
 774                             if (!BL_WriteFlash(&ioMem)) {
 775                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 776                             }
 777                             break;
 778                         }
 779                         #endif
 780                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 781                         case imSK:
 782                         {
 783                             if (!Stk_WriteFlash(&ioMem))
 784                             {
 785                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 786                             }
 787                             break;
 788                         }
 789                         #endif
 790                     }
 791                     break;
 792                 }
 793
 794                 case cmd_DeviceWriteEEprom:
 795                 {
 796                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 797                     ACK_OUT = ACK_D_GENERAL_ERROR;
 798                     /*
 799                     wtf.D_FLASH_ADDR_H=Adress_H;
 800                     wtf.D_FLASH_ADDR_L=Adress_L;
 801                     wtf.D_PTR_I = BUF_I;
 802                     */
 803                     switch (CurrentInterfaceMode)
 804                     {
 805                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 806                         case imSIL_BLB:
 807                         {
 808                             ACK_OUT = ACK_I_INVALID_CMD;
 809                             break;
 810                         }
 811                         case imATM_BLB:
 812                         {
 813                             if (BL_WriteEEprom(&ioMem))
 814                             {
 815                                 ACK_OUT = ACK_OK;
 816                             }
 817                             break;
 818                         }
 819                         #endif
 820                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 821                         case imSK:
 822                         {
 823                             if (Stk_WriteEEprom(&ioMem))
 824                             {
 825                                 ACK_OUT = ACK_OK;
 826                             }
 827                             break;
 828                         }
 829                         #endif
 830                     }
 831                     break;
 832                 }
 833                 //*** Device Memory Verify Ops ***
 834                 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 835                 case cmd_DeviceVerify:
 836                 {
 837                     switch (CurrentInterfaceMode)
 838                     {
 839                         case imARM_BLB:
 840                         {
 841                             ioMem.D_NUM_BYTES = I_PARAM_LEN;
 842                             /*
 843                             wtf.D_FLASH_ADDR_H=Adress_H;
 844                             wtf.D_FLASH_ADDR_L=Adress_L;
 845                             wtf.D_PTR_I = BUF_I;
 846                             */
 847
 848                             ACK_OUT = BL_VerifyFlash(&ioMem);
 849                             switch (ACK_OUT) {
 850                                 case brSUCCESS:
 851                                     ACK_OUT = ACK_OK;
 852                                     break;
 853                                 case brERRORVERIFY:
 854                                     ACK_OUT = ACK_I_VERIFY_ERROR;
 855                                     break;
 856                                 default:
 857                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 858                                     break;
 859                             }
 860                             break;
 861                         }
 862                         default:
 863                         {
 864                             ACK_OUT = ACK_I_INVALID_CMD;
 865                             break;
 866                         }
 867                     }
 868                     break;
 869                 }
 870                 #endif
 871                 default:
 872                 {
 873                     ACK_OUT = ACK_I_INVALID_CMD;
 874                 }
 875             }
 876         }
 877
 878         CRCout.word = 0;
 879
 880         RX_LED_OFF;
 881
 882         serialBeginWrite(port);
 883         WriteByteCrc(cmd_Remote_Escape);
 884         WriteByteCrc(CMD);
 885         WriteByteCrc(ioMem.D_FLASH_ADDR_H);
 886         WriteByteCrc(ioMem.D_FLASH_ADDR_L);
 887         WriteByteCrc(O_PARAM_LEN);
 888
 889         i=O_PARAM_LEN;
 890         do {
 891             while (!serialTxBytesFree(port));
 892
 893             WriteByteCrc(*O_PARAM);
 894             O_PARAM++;
 895             i--;
 896         } while (i > 0);
 897
 898         WriteByteCrc(ACK_OUT);
 899         WriteByte(CRCout.bytes[1]);
 900         WriteByte(CRCout.bytes[0]);
 901         serialEndWrite(port);
 902
 903         TX_LED_OFF;
 904         if (isExitScheduled) {
 905             esc4wayRelease();
 906             return;
 907         }
 908     };
 909 }
 910
 911
 912
 913 #endif