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 14
  74 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4
  75 #define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04
  76
  77 #define SERIAL_4WAY_PROTOCOL_VER 106
  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     pwmIOConfiguration_t *pwmIOConfiguration = pwmGetOutputConfiguration();
 140     for (volatile uint8_t i = 0; i < pwmIOConfiguration->ioCount; i++) {
 141         if ((pwmIOConfiguration->ioConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) {
 142             if (motor[pwmIOConfiguration->ioConfigurations[i].index] > 0) {
 143                 escHardware[escCount].io = IOGetByTag(pwmIOConfiguration->ioConfigurations[i].timerHardware->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 // responses
 259 #define ACK_OK                  0x00
 260 // #define ACK_I_UNKNOWN_ERROR       0x01
 261 #define ACK_I_INVALID_CMD       0x02
 262 #define ACK_I_INVALID_CRC       0x03
 263 #define ACK_I_VERIFY_ERROR      0x04
 264 // #define ACK_D_INVALID_COMMAND 0x05
 265 // #define ACK_D_COMMAND_FAILED  0x06
 266 // #define ACK_D_UNKNOWN_ERROR       0x07
 267
 268 #define ACK_I_INVALID_CHANNEL   0x08
 269 #define ACK_I_INVALID_PARAM     0x09
 270 #define ACK_D_GENERAL_ERROR     0x0F
 271
 272 /* Copyright (c) 2002, 2003, 2004  Marek Michalkiewicz
 273    Copyright (c) 2005, 2007 Joerg Wunsch
 274    Copyright (c) 2013 Dave Hylands
 275    Copyright (c) 2013 Frederic Nadeau
 276    All rights reserved.
 277
 278    Redistribution and use in source and binary forms, with or without
 279    modification, are permitted provided that the following conditions are met:
 280
 281    * Redistributions of source code must retain the above copyright
 282      notice, this list of conditions and the following disclaimer.
 283
 284    * Redistributions in binary form must reproduce the above copyright
 285      notice, this list of conditions and the following disclaimer in
 286      the documentation and/or other materials provided with the
 287      distribution.
 288
 289    * Neither the name of the copyright holders nor the names of
 290      contributors may be used to endorse or promote products derived
 291      from this software without specific prior written permission.
 292
 293   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 294   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 295   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 296   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 297   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 298   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 299   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 300   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 301   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 302   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 303   POSSIBILITY OF SUCH DAMAGE. */
 304 uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
 305         int i;
 306
 307         crc = crc ^ ((uint16_t)data << 8);
 308         for (i=0; i < 8; i++){
 309             if (crc & 0x8000)
 310                 crc = (crc << 1) ^ 0x1021;
 311             else
 312                 crc <<= 1;
 313         }
 314         return crc;
 315 }
 316 // * End copyright
 317
 318
 319 #define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \
 320         (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B))
 321
 322 #define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \
 323         (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \
 324         (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \
 325         (pDeviceInfo->words[0] == 0xE8B2))
 326
 327 static uint8_t CurrentInterfaceMode;
 328
 329 static uint8_t Connect(uint8_32_u *pDeviceInfo)
 330 {
 331     for (uint8_t I = 0; I < 3; ++I) {
 332         #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
 333         if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) {
 334             CurrentInterfaceMode = imSK;
 335             return 1;
 336         } else {
 337             if (BL_ConnectEx(pDeviceInfo)) {
 338                 if  SILABS_DEVICE_MATCH {
 339                     CurrentInterfaceMode = imSIL_BLB;
 340                     return 1;
 341                 } else if ATMEL_DEVICE_MATCH {
 342                     CurrentInterfaceMode = imATM_BLB;
 343                     return 1;
 344                 }
 345             }
 346         }
 347         #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 348         if (BL_ConnectEx(pDeviceInfo)) {
 349             if SILABS_DEVICE_MATCH {
 350                 CurrentInterfaceMode = imSIL_BLB;
 351                 return 1;
 352             } else if ATMEL_DEVICE_MATCH {
 353                 CurrentInterfaceMode = imATM_BLB;
 354                 return 1;
 355             }
 356         }
 357         #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 358         if (Stk_ConnectEx(pDeviceInfo)) {
 359             CurrentInterfaceMode = imSK;
 360             if ATMEL_DEVICE_MATCH return 1;
 361         }
 362         #endif
 363     }
 364     return 0;
 365 }
 366
 367 static serialPort_t *port;
 368
 369 static uint8_t ReadByte(void)
 370 {
 371     // need timeout?
 372     while (!serialRxBytesWaiting(port));
 373     return serialRead(port);
 374 }
 375
 376 static uint8_16_u CRC_in;
 377 static uint8_t ReadByteCrc(void)
 378 {
 379     uint8_t b = ReadByte();
 380     CRC_in.word = _crc_xmodem_update(CRC_in.word, b);
 381     return b;
 382 }
 383
 384 static void WriteByte(uint8_t b)
 385 {
 386     serialWrite(port, b);
 387 }
 388
 389 static uint8_16_u CRCout;
 390 static void WriteByteCrc(uint8_t b)
 391 {
 392     WriteByte(b);
 393     CRCout.word = _crc_xmodem_update(CRCout.word, b);
 394 }
 395
 396 void esc4wayProcess(serialPort_t *mspPort)
 397 {
 398
 399     uint8_t ParamBuf[256];
 400     uint8_t ESC;
 401     uint8_t I_PARAM_LEN;
 402     uint8_t CMD;
 403     uint8_t ACK_OUT;
 404     uint8_16_u CRC_check;
 405     uint8_16_u Dummy;
 406     uint8_t O_PARAM_LEN;
 407     uint8_t *O_PARAM;
 408     uint8_t *InBuff;
 409     ioMem_t ioMem;
 410
 411     port = mspPort;
 412
 413     // Start here  with UART Main loop
 414     #ifdef BEEPER
 415     // fix for buzzer often starts beeping continuously when the ESCs are read
 416     // switch beeper silent here
 417     beeperSilence();
 418     #endif
 419     bool isExitScheduled = false;
 420
 421     while (1) {
 422         // restart looking for new sequence from host
 423         do {
 424             CRC_in.word = 0;
 425             ESC = ReadByteCrc();
 426         } while (ESC != cmd_Local_Escape);
 427
 428         RX_LED_ON;
 429
 430         Dummy.word = 0;
 431         O_PARAM = &Dummy.bytes[0];
 432         O_PARAM_LEN = 1;
 433         CMD = ReadByteCrc();
 434         ioMem.D_FLASH_ADDR_H = ReadByteCrc();
 435         ioMem.D_FLASH_ADDR_L = ReadByteCrc();
 436         I_PARAM_LEN = ReadByteCrc();
 437
 438         InBuff = ParamBuf;
 439         uint8_t i = I_PARAM_LEN;
 440         do {
 441           *InBuff = ReadByteCrc();
 442           InBuff++;
 443           i--;
 444         } while (i != 0);
 445
 446         CRC_check.bytes[1] = ReadByte();
 447         CRC_check.bytes[0] = ReadByte();
 448
 449         if (CRC_check.word == CRC_in.word) {
 450             ACK_OUT = ACK_OK;
 451         } else {
 452             ACK_OUT = ACK_I_INVALID_CRC;
 453         }
 454
 455         TX_LED_ON;
 456
 457         if (ACK_OUT == ACK_OK)
 458         {
 459             // wtf.D_FLASH_ADDR_H=Adress_H;
 460             // wtf.D_FLASH_ADDR_L=Adress_L;
 461             ioMem.D_PTR_I = ParamBuf;
 462
 463
 464             switch (CMD) {
 465                 // ******* Interface related stuff *******
 466                 case cmd_InterfaceTestAlive:
 467                 {
 468                     if (isMcuConnected()){
 469                         switch (CurrentInterfaceMode)
 470                         {
 471                             #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 472                             case imATM_BLB:
 473                             case imSIL_BLB:
 474                             {
 475                                 if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included
 476                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 477                                 }
 478                                 break;
 479                             }
 480                             #endif
 481                             #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 482                             case imSK:
 483                             {
 484                                 if (!Stk_SignOn()) { // SetStateDisconnected();
 485                                     ACK_OUT = ACK_D_GENERAL_ERROR;
 486                                 }
 487                                 break;
 488                             }
 489                             #endif
 490                             default:
 491                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 492                         }
 493                         if ( ACK_OUT != ACK_OK) SET_DISCONNECTED;
 494                     }
 495                     break;
 496                 }
 497                 case cmd_ProtocolGetVersion:
 498                 {
 499                     // Only interface itself, no matter what Device
 500                     Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER;
 501                     break;
 502                 }
 503
 504                 case cmd_InterfaceGetName:
 505                 {
 506                     // Only interface itself, no matter what Device
 507                     // O_PARAM_LEN=16;
 508                     O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR);
 509                     O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR;
 510                     break;
 511                 }
 512
 513                 case cmd_InterfaceGetVersion:
 514                 {
 515                     // Only interface itself, no matter what Device
 516                     // Dummy = iUart_res_InterfVersion;
 517                     O_PARAM_LEN = 2;
 518                     Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI;
 519                     Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO;
 520                     break;
 521                 }
 522                 case cmd_InterfaceExit:
 523                 {
 524                     isExitScheduled = true;
 525                     break;
 526                 }
 527                 case cmd_InterfaceSetMode:
 528                 {
 529 #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 530                     if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) {
 531 #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
 532                     if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) {
 533 #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
 534                     if (ParamBuf[0] == imSK) {
 535 #endif
 536                         CurrentInterfaceMode = ParamBuf[0];
 537                     } else {
 538                         ACK_OUT = ACK_I_INVALID_PARAM;
 539                     }
 540                     break;
 541                 }
 542
 543                 case cmd_DeviceReset:
 544                 {
 545                     if (ParamBuf[0] < escCount) {
 546                         // Channel may change here
 547                         selected_esc = ParamBuf[0];
 548                     }
 549                     else {
 550                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 551                         break;
 552                     }
 553                     switch (CurrentInterfaceMode)
 554                     {
 555                     case imSIL_BLB:
 556                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 557                         case imATM_BLB:
 558                         {
 559                             BL_SendCMDRunRestartBootloader(&DeviceInfo);
 560                             break;
 561                         }
 562                         #endif
 563                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 564                         case imSK:
 565                         {
 566                             break;
 567                         }
 568                         #endif
 569                     }
 570                     SET_DISCONNECTED;
 571                     break;
 572                 }
 573                 case cmd_DeviceInitFlash:
 574                 {
 575                     SET_DISCONNECTED;
 576                     if (ParamBuf[0] < escCount) {
 577                         //Channel may change here
 578                         //ESC_LO or ESC_HI; Halt state for prev channel
 579                         selected_esc = ParamBuf[0];
 580                     } else {
 581                         ACK_OUT = ACK_I_INVALID_CHANNEL;
 582                         break;
 583                     }
 584                     O_PARAM_LEN = DeviceInfoSize; //4
 585                     O_PARAM = (uint8_t *)&DeviceInfo;
 586                     if (Connect(&DeviceInfo)) {
 587                         DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode;
 588                     } else {
 589                         SET_DISCONNECTED;
 590                         ACK_OUT = ACK_D_GENERAL_ERROR;
 591                     }
 592                     break;
 593                 }
 594
 595                 #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 596                 case cmd_DeviceEraseAll:
 597                 {
 598                     switch (CurrentInterfaceMode)
 599                     {
 600                         case imSK:
 601                         {
 602                             if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR;
 603                             break;
 604                         }
 605                         default:
 606                             ACK_OUT = ACK_I_INVALID_CMD;
 607                     }
 608                     break;
 609                 }
 610                 #endif
 611
 612                 #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 613                 case cmd_DevicePageErase:
 614                 {
 615                     switch (CurrentInterfaceMode)
 616                     {
 617                         case imSIL_BLB:
 618                         {
 619                             Dummy.bytes[0] = ParamBuf[0];
 620                             //Address = Page * 512
 621                             ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1);
 622                             ioMem.D_FLASH_ADDR_L = 0;
 623                             if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR;
 624                             break;
 625                         }
 626                         default:
 627                             ACK_OUT = ACK_I_INVALID_CMD;
 628                     }
 629                     break;
 630                 }
 631                 #endif
 632
 633                 //*** Device Memory Read Ops ***
 634                 case cmd_DeviceRead:
 635                 {
 636                     ioMem.D_NUM_BYTES = ParamBuf[0];
 637                     /*
 638                     wtf.D_FLASH_ADDR_H=Adress_H;
 639                     wtf.D_FLASH_ADDR_L=Adress_L;
 640                     wtf.D_PTR_I = BUF_I;
 641                     */
 642                     switch (CurrentInterfaceMode)
 643                     {
 644                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 645                         case imSIL_BLB:
 646                         case imATM_BLB:
 647                         {
 648                             if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem))
 649                             {
 650                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 651                             }
 652                             break;
 653                         }
 654                         #endif
 655                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 656                         case imSK:
 657                         {
 658                             if (!Stk_ReadFlash(&ioMem))
 659                             {
 660                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 661                             }
 662                             break;
 663                         }
 664                         #endif
 665                         default:
 666                             ACK_OUT = ACK_I_INVALID_CMD;
 667                     }
 668                     if (ACK_OUT == ACK_OK)
 669                     {
 670                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 671                         O_PARAM = (uint8_t *)&ParamBuf;
 672                     }
 673                     break;
 674                 }
 675
 676                 case cmd_DeviceReadEEprom:
 677                 {
 678                     ioMem.D_NUM_BYTES = ParamBuf[0];
 679                     /*
 680                     wtf.D_FLASH_ADDR_H = Adress_H;
 681                     wtf.D_FLASH_ADDR_L = Adress_L;
 682                     wtf.D_PTR_I = BUF_I;
 683                     */
 684                     switch (CurrentInterfaceMode)
 685                     {
 686                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 687                         case imATM_BLB:
 688                         {
 689                             if (!BL_ReadEEprom(&ioMem))
 690                             {
 691                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 692                             }
 693                             break;
 694                         }
 695                         #endif
 696                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 697                         case imSK:
 698                         {
 699                             if (!Stk_ReadEEprom(&ioMem))
 700                             {
 701                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 702                             }
 703                             break;
 704                         }
 705                         #endif
 706                         default:
 707                             ACK_OUT = ACK_I_INVALID_CMD;
 708                     }
 709                     if (ACK_OUT == ACK_OK)
 710                     {
 711                         O_PARAM_LEN = ioMem.D_NUM_BYTES;
 712                         O_PARAM = (uint8_t *)&ParamBuf;
 713                     }
 714                     break;
 715                 }
 716
 717                 //*** Device Memory Write Ops ***
 718                 case cmd_DeviceWrite:
 719                 {
 720                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 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 imSIL_BLB:
 730                         case imATM_BLB:
 731                         {
 732                             if (!BL_WriteFlash(&ioMem)) {
 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_WriteFlash(&ioMem))
 742                             {
 743                                 ACK_OUT = ACK_D_GENERAL_ERROR;
 744                             }
 745                             break;
 746                         }
 747                         #endif
 748                     }
 749                     break;
 750                 }
 751
 752                 case cmd_DeviceWriteEEprom:
 753                 {
 754                     ioMem.D_NUM_BYTES = I_PARAM_LEN;
 755                     ACK_OUT = ACK_D_GENERAL_ERROR;
 756                     /*
 757                     wtf.D_FLASH_ADDR_H=Adress_H;
 758                     wtf.D_FLASH_ADDR_L=Adress_L;
 759                     wtf.D_PTR_I = BUF_I;
 760                     */
 761                     switch (CurrentInterfaceMode)
 762                     {
 763                         #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 764                         case imSIL_BLB:
 765                         {
 766                             ACK_OUT = ACK_I_INVALID_CMD;
 767                             break;
 768                         }
 769                         case imATM_BLB:
 770                         {
 771                             if (BL_WriteEEprom(&ioMem))
 772                             {
 773                                 ACK_OUT = ACK_OK;
 774                             }
 775                             break;
 776                         }
 777                         #endif
 778                         #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
 779                         case imSK:
 780                         {
 781                             if (Stk_WriteEEprom(&ioMem))
 782                             {
 783                                 ACK_OUT = ACK_OK;
 784                             }
 785                             break;
 786                         }
 787                         #endif
 788                     }
 789                     break;
 790                 }
 791                 default:
 792                 {
 793                     ACK_OUT = ACK_I_INVALID_CMD;
 794                 }
 795             }
 796         }
 797
 798         CRCout.word = 0;
 799
 800         RX_LED_OFF;
 801
 802         serialBeginWrite(port);
 803         WriteByteCrc(cmd_Remote_Escape);
 804         WriteByteCrc(CMD);
 805         WriteByteCrc(ioMem.D_FLASH_ADDR_H);
 806         WriteByteCrc(ioMem.D_FLASH_ADDR_L);
 807         WriteByteCrc(O_PARAM_LEN);
 808
 809         i=O_PARAM_LEN;
 810         do {
 811             while (!serialTxBytesFree(port));
 812
 813             WriteByteCrc(*O_PARAM);
 814             O_PARAM++;
 815             i--;
 816         } while (i > 0);
 817
 818         WriteByteCrc(ACK_OUT);
 819         WriteByte(CRCout.bytes[1]);
 820         WriteByte(CRCout.bytes[0]);
 821         serialEndWrite(port);
 822
 823         TX_LED_OFF;
 824         if (isExitScheduled) {
 825             esc4wayRelease();
 826             return;
 827         }
 828     };
 829 }
 830
 831
 832
 833 #endif