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