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