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