src/main/io/serial_4way.c

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