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