From aedeed0c36b17222cf77c6acb7cac2974336e8ff Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Mon, 25 Apr 2016 17:32:54 +0200 Subject: [PATCH] Cleanup 4way interface - implement cleaner interface into Cleanfligh - separate initialization and start - allow direct access to command processing, skipping BLHeli serial framing - remove mspPort_t dependency - refactor naming to CF style - use serial_4way_impl.h for internal interface to bootloader implementation - use uint16_t instead of hi/lo bytes ; use integer lengths instead of 0=256 encoding - fix buffer overflow in stkv2 code --- src/main/io/serial_4way.c | 1059 ++++++++++-------------- src/main/io/serial_4way.h | 149 +++- src/main/io/serial_4way_avrootloader.c | 343 ++++---- src/main/io/serial_4way_avrootloader.h | 11 +- src/main/io/serial_4way_impl.h | 47 ++ src/main/io/serial_4way_stk500v2.c | 393 ++++----- src/main/io/serial_4way_stk500v2.h | 2 +- src/main/io/serial_msp.c | 4 +- 8 files changed, 927 insertions(+), 1081 deletions(-) create mode 100644 src/main/io/serial_4way_impl.h diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 70e678c574..3244a83667 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -23,114 +23,117 @@ #include #include + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + #include "drivers/serial.h" -#include "drivers/buf_writer.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" -#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" +#include "io/serial_4way_impl.h" #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#include "io/serial_4way_avrootloader.h" +# include "io/serial_4way_avrootloader.h" #endif -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) -#include "io/serial_4way_stk500v2.h" +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER +# include "io/serial_4way_stk500v2.h" #endif #define USE_TXRX_LED -#ifdef USE_TXRX_LED -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON -#ifdef LED1 -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON +#if defined(USE_TXRX_LED) && defined(LED0) +# define RX_LED_OFF LED0_OFF +# define RX_LED_ON LED0_ON +# ifdef LED1 +# define TX_LED_OFF LED1_OFF +# define TX_LED_ON LED1_ON +# else +# define TX_LED_OFF LED0_OFF +# define TX_LED_ON LED0_ON +# endif #else -#define TX_LED_OFF LED0_OFF -#define TX_LED_ON LED0_ON -#endif -#else -#define RX_LED_OFF -#define RX_LED_ON -#define TX_LED_OFF -#define TX_LED_ON +# define RX_LED_OFF do {} while(0) +# define RX_LED_ON do {} while(0) +# define TX_LED_OFF do {} while(0) +# define TX_LED_ON do {} while(0) #endif #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" -// *** change to adapt Revision -#define SERIAL_4WAY_VER_MAIN 14 -#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 -#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 - +#define SERIAL_4WAY_VER_MAIN 14 +#define SERIAL_4WAY_VER_SUB_1 4 +#define SERIAL_4WAY_VER_SUB_2 4 #define SERIAL_4WAY_PROTOCOL_VER 106 -// *** end -#if (SERIAL_4WAY_VER_MAIN > 24) -#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" +#if SERIAL_4WAY_VER_MAIN > 24 +# error "SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1 must fit in uint8_t" +#endif +#if SERIAL_4WAY_VER_SUB_1 >= 10 +# warning "SERIAL_4WAY_VER_SUB_1 should be 0-9" #endif -#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) +#if SERIAL_4WAY_VER_SUB_2 >= 100 +# warning "SERIAL_4WAY_VER_SUB_2 should be <= 99 (9.9)" +#endif -#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) -#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) +#define SERIAL_4WAY_VERSION_HI (uint8_t)(SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1) +#define SERIAL_4WAY_VERSION_LO (uint8_t)(SERIAL_4WAY_VER_SUB_2) static uint8_t escCount; +uint8_t escSelected; escHardware_t escHardware[MAX_PWM_MOTORS]; -uint8_t selected_esc; +static escDeviceInfo_t deviceInfo; -uint8_32_u DeviceInfo; - -#define DeviceInfoSize 4 - -inline bool isMcuConnected(void) +static bool isMcuConnected(void) { - return (DeviceInfo.bytes[0] > 0); + return deviceInfo.signature != 0; } -inline bool isEscHi(uint8_t selEsc) +static void setDisconnected(void) { + deviceInfo.signature = 0; +} + +bool isEscHi(uint8_t selEsc) { return (digitalIn(escHardware[selEsc].gpio, escHardware[selEsc].pin) != Bit_RESET); } -inline bool isEscLo(uint8_t selEsc) + +bool isEscLo(uint8_t selEsc) { return (digitalIn(escHardware[selEsc].gpio, escHardware[selEsc].pin) == Bit_RESET); } -inline void setEscHi(uint8_t selEsc) +void setEscHi(uint8_t selEsc) { digitalHi(escHardware[selEsc].gpio, escHardware[selEsc].pin); } -inline void setEscLo(uint8_t selEsc) +void setEscLo(uint8_t selEsc) { digitalLo(escHardware[selEsc].gpio, escHardware[selEsc].pin); } -inline void setEscInput(uint8_t selEsc) +void setEscInput(uint8_t selEsc) { gpioInit(escHardware[selEsc].gpio, &escHardware[selEsc].gpio_config_INPUT); } -inline void setEscOutput(uint8_t selEsc) +void setEscOutput(uint8_t selEsc) { gpioInit(escHardware[selEsc].gpio, &escHardware[selEsc].gpio_config_OUTPUT); } -static uint32_t GetPinPos(uint32_t pin) +static uint32_t getPinPos(uint32_t pin) { - uint32_t pinPos; - for (pinPos = 0; pinPos < 16; pinPos++) { + for (int pinPos = 0; pinPos < 16; pinPos++) { uint32_t pinMask = (0x1 << pinPos); if (pin & pinMask) { return pinPos; @@ -139,152 +142,73 @@ static uint32_t GetPinPos(uint32_t pin) return 0; } -uint8_t Initialize4WayInterface(void) +// Initialize 4way ESC interface +// initializes internal structures +// returns number of ESCs available +int esc4wayInit(void) { // StopPwmAllMotors(); - pwmDisableMotors(); - escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmIOConfiguration_t *pwmIOConfiguration = pwmGetOutputConfiguration(); - for (volatile uint8_t i = 0; i < pwmIOConfiguration->ioCount; i++) { + int escIdx = 0; + for (int i = 0; i < pwmIOConfiguration->ioCount; i++) { if ((pwmIOConfiguration->ioConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmIOConfiguration->ioConfigurations[i].index] > 0) { - escHardware[escCount].gpio = pwmIOConfiguration->ioConfigurations[i].timerHardware->gpio; - escHardware[escCount].pin = pwmIOConfiguration->ioConfigurations[i].timerHardware->pin; - escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin); - escHardware[escCount].gpio_config_INPUT.pin = escHardware[escCount].pin; - escHardware[escCount].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() - escHardware[escCount].gpio_config_INPUT.mode = Mode_IPU; - escHardware[escCount].gpio_config_OUTPUT = escHardware[escCount].gpio_config_INPUT; - escHardware[escCount].gpio_config_OUTPUT.mode = Mode_Out_PP; - setEscInput(escCount); - setEscHi(escCount); - escCount++; + escHardware[escIdx].gpio = pwmIOConfiguration->ioConfigurations[i].timerHardware->gpio; + escHardware[escIdx].pin = pwmIOConfiguration->ioConfigurations[i].timerHardware->pin; + escHardware[escIdx].pinpos = getPinPos(escHardware[escIdx].pin); + escHardware[escIdx].gpio_config_INPUT.pin = escHardware[escIdx].pin; + escHardware[escIdx].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() + escHardware[escIdx].gpio_config_INPUT.mode = Mode_IPU; + escHardware[escIdx].gpio_config_OUTPUT = escHardware[escIdx].gpio_config_INPUT; + escHardware[escIdx].gpio_config_OUTPUT.mode = Mode_Out_PP; + escIdx++; } } } + escCount = escIdx; return escCount; } -void DeInitialize4WayInterface(void) +// stat BLHeli 4way interface +// sets all ESC lines as output + hi +void esc4wayStart(void) { - while (escCount > 0) { - escCount--; - escHardware[escCount].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() - setEscOutput(escCount); - setEscLo(escCount); + pwmDisableMotors(); // prevent updating PWM registers + for (int i = 0; i < escCount; i++) { + setEscInput(i); + setEscHi(i); } +} + +// stops BLHeli 4way interface +// returns all claimed pins back to PWM drivers, reenables PWM +void esc4wayRelease(void) +{ + for(int i = 0; i < escCount; i++) { + escHardware[i].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() + setEscOutput(i); + setEscLo(i); + } + escCount = 0; pwmEnableMotors(); } - -#define SET_DISCONNECTED DeviceInfo.words[0] = 0 - -#define INTF_MODE_IDX 3 // index for DeviceInfostate - -// Interface related only -// establish and test connection to the Interface - +// BLHeliSuite packet framing +// for reference, see 'Manuals/BLHeliSuite 4w-if protocol.pdf' from BLHeliSuite // Send Structure -// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo +// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) CRC16_Hi CRC16_Lo // Return -// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo +// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo -#define cmd_Remote_Escape 0x2E // '.' -#define cmd_Local_Escape 0x2F // '/' +// esc4wayCmd_e in public header -// Test Interface still present -#define cmd_InterfaceTestAlive 0x30 // '0' alive -// RETURN: ACK +typedef enum { + // not commands, but keep naming consistent with BLHeli suite + cmd_Remote_Escape = 0x2E, // '.' + cmd_Local_Escape = 0x2F, // '/' +} syn_4way_e; -// get Protocol Version Number 01..255 -#define cmd_ProtocolGetVersion 0x31 // '1' version -// RETURN: uint8_t VersionNumber + ACK - -// get Version String -#define cmd_InterfaceGetName 0x32 // '2' name -// RETURN: String + ACK - -//get Version Number 01..255 -#define cmd_InterfaceGetVersion 0x33 // '3' version -// RETURN: uint8_t AVersionNumber + ACK - - -// Exit / Restart Interface - can be used to switch to Box Mode -#define cmd_InterfaceExit 0x34 // '4' exit -// RETURN: ACK - -// Reset the Device connected to the Interface -#define cmd_DeviceReset 0x35 // '5' reset -// RETURN: ACK - -// Get the Device ID connected -// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 -// RETURN: uint8_t DeviceID + ACK - -// Initialize Flash Access for Device connected -#define cmd_DeviceInitFlash 0x37 // '7' init flash access -// RETURN: ACK - -// Erase the whole Device Memory of connected Device -#define cmd_DeviceEraseAll 0x38 // '8' erase all -// RETURN: ACK - -// Erase one Page of Device Memory of connected Device -#define cmd_DevicePageErase 0x39 // '9' page erase -// PARAM: uint8_t APageNumber -// RETURN: ACK - -// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes -// BuffLen = 0 means 256 Bytes -#define cmd_DeviceRead 0x3A // ':' read Device -// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] -// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK - -// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes -// BuffLen = 0 means 256 Bytes -#define cmd_DeviceWrite 0x3B // ';' write -// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] -// RETURN: ACK - -// Set C2CK low infinite ) permanent Reset state -#define cmd_DeviceC2CK_LOW 0x3C // '<' -// RETURN: ACK - -// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes -// BuffLen = 0 means 256 Bytes -#define cmd_DeviceReadEEprom 0x3D // '=' read Device -// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] -// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK - -// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes -// BuffLen = 0 means 256 Bytes -#define cmd_DeviceWriteEEprom 0x3E // '>' write -// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] -// RETURN: ACK - -// Set Interface Mode -#define cmd_InterfaceSetMode 0x3F // '?' -// #define imC2 0 -// #define imSIL_BLB 1 -// #define imATM_BLB 2 -// #define imSK 3 -// PARAM: uint8_t Mode -// RETURN: ACK or ACK_I_INVALID_CHANNEL - -// responses -#define ACK_OK 0x00 -// #define ACK_I_UNKNOWN_ERROR 0x01 -#define ACK_I_INVALID_CMD 0x02 -#define ACK_I_INVALID_CRC 0x03 -#define ACK_I_VERIFY_ERROR 0x04 -// #define ACK_D_INVALID_COMMAND 0x05 -// #define ACK_D_COMMAND_FAILED 0x06 -// #define ACK_D_UNKNOWN_ERROR 0x07 - -#define ACK_I_INVALID_CHANNEL 0x08 -#define ACK_I_INVALID_PARAM 0x09 -#define ACK_D_GENERAL_ERROR 0x0F /* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz Copyright (c) 2005, 2007 Joerg Wunsch @@ -322,7 +246,7 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { int i; crc = crc ^ ((uint16_t)data << 8); - for (i=0; i < 8; i++){ + for (i = 0; i < 8; i++){ if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -332,509 +256,382 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { } // * End copyright +static uint16_t signaturesAtmel[] = {0x9307, 0x930A, 0x930F, 0x940B, 0}; +static uint16_t signaturesSilabs[] = {0xF310, 0xF330, 0xF410, 0xF390, 0xF850, 0xE8B1, 0xE8B2, 0}; -#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ - (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) - -#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ - (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ - (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ - (pDeviceInfo->words[0] == 0xE8B2)) - -static uint8_t CurrentInterfaceMode; - -static uint8_t Connect(uint8_32_u *pDeviceInfo) +static bool signatureMatch(uint16_t signature, uint16_t *list) { - for (uint8_t I = 0; I < 3; ++I) { - #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) - if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { - CurrentInterfaceMode = imSK; + for(; *list; list++) + if(signature == *list) + return true; + return false; +} + +static uint8_t currentInterfaceMode; + +// Try connecting to device +// 3 attempts are made, trying both STK and BL protocols. +static uint8_t connect(escDeviceInfo_t *pDeviceInfo) +{ + for (int try = 0; try < 3; try++) { +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo) && signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { + currentInterfaceMode = imSK; return 1; - } else { - if (BL_ConnectEx(pDeviceInfo)) { - if SILABS_DEVICE_MATCH { - CurrentInterfaceMode = imSIL_BLB; - return 1; - } else if ATMEL_DEVICE_MATCH { - CurrentInterfaceMode = imATM_BLB; - return 1; - } - } } - #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) +#endif +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if (BL_ConnectEx(pDeviceInfo)) { - if SILABS_DEVICE_MATCH { - CurrentInterfaceMode = imSIL_BLB; + if(signatureMatch(pDeviceInfo->signature, signaturesSilabs)) { + currentInterfaceMode = imSIL_BLB; return 1; - } else if ATMEL_DEVICE_MATCH { - CurrentInterfaceMode = imATM_BLB; + } + if(signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { + currentInterfaceMode = imATM_BLB; return 1; } } - #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (Stk_ConnectEx(pDeviceInfo)) { - CurrentInterfaceMode = imSK; - if ATMEL_DEVICE_MATCH return 1; - } - #endif +#endif } return 0; } -static mspPort_t *_mspPort; -static bufWriter_t *_writer; +static serialPort_t *port; +static uint16_t crcIn, crcOut; -static uint8_t ReadByte(void) { +static uint8_t readByte(void) +{ // need timeout? - while (!serialRxBytesWaiting(_mspPort->port)); - return serialRead(_mspPort->port); + while (!serialRxBytesWaiting(port)); + return serialRead(port); } -static uint8_16_u CRC_in; -static uint8_t ReadByteCrc(void) { - uint8_t b = ReadByte(); - CRC_in.word = _crc_xmodem_update(CRC_in.word, b); +static uint8_t readByteCrc(void) +{ + uint8_t b = readByte(); + crcIn = _crc_xmodem_update(crcIn, b); return b; } -static void WriteByte(uint8_t b){ - bufWriterAppend(_writer, b); + +static void writeByte(uint8_t b) +{ + serialWrite(port, b); } -static uint8_16_u CRCout; -static void WriteByteCrc(uint8_t b){ - WriteByte(b); - CRCout.word = _crc_xmodem_update(CRCout.word, b); +static void writeByteCrc(uint8_t b) +{ + writeByte(b); + crcOut = _crc_xmodem_update(crcOut, b); } -void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { +// handle 4way interface on serial port +// esc4wayStart / esc4wayRelease in called internally +// 256 bytes buffer is allocated on stack +void esc4wayProcess(serialPort_t *serial) { + uint8_t command; + uint16_t addr; + int inLen; + int outLen; + uint8_t paramBuf[256]; + uint8_t replyAck; - uint8_t ParamBuf[256]; - uint8_t ESC; - uint8_t I_PARAM_LEN; - uint8_t CMD; - uint8_t ACK_OUT; - uint8_16_u CRC_check; - uint8_16_u Dummy; - uint8_t O_PARAM_LEN; - uint8_t *O_PARAM; - uint8_t *InBuff; - ioMem_t ioMem; + esc4wayStart(); - _mspPort = mspPort; - _writer = bufwriter; + port = serial; - // Start here with UART Main loop - #ifdef BEEPER +#ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); - #endif - bool isExitScheduled = false; +#endif - while(1) { + esc4wayExitRequested = false; + while(!esc4wayExitRequested) { // restart looking for new sequence from host - do { - CRC_in.word = 0; - ESC = ReadByteCrc(); - } while (ESC != cmd_Local_Escape); + crcIn = 0; + uint8_t esc = readByteCrc(); + if(esc != cmd_Local_Escape) + continue; // wait for sync character RX_LED_ON; - Dummy.word = 0; - O_PARAM = &Dummy.bytes[0]; - O_PARAM_LEN = 1; - CMD = ReadByteCrc(); - ioMem.D_FLASH_ADDR_H = ReadByteCrc(); - ioMem.D_FLASH_ADDR_L = ReadByteCrc(); - I_PARAM_LEN = ReadByteCrc(); + command = readByteCrc(); + addr = readByteCrc() << 8; + addr |= readByteCrc(); - InBuff = ParamBuf; - uint8_t i = I_PARAM_LEN; - do { - *InBuff = ReadByteCrc(); - InBuff++; - i--; - } while (i != 0); + inLen = readByteCrc(); + if(inLen == 0) + inLen = 0x100; // len ==0 -> param is 256B - CRC_check.bytes[1] = ReadByte(); - CRC_check.bytes[0] = ReadByte(); + for(int i = 0; i < inLen; i++) + paramBuf[i] = readByteCrc(); + readByteCrc(); readByteCrc(); // update input CRC RX_LED_OFF; - if(CRC_check.word == CRC_in.word) { - ACK_OUT = ACK_OK; - } else { - ACK_OUT = ACK_I_INVALID_CRC; + outLen = 0; // output handling code will send single zero byte if necessary + replyAck = esc4wayAck_OK; + + if(crcIn != 0) // CRC of correct message == 0 + replyAck = esc4wayAck_I_INVALID_CRC; + + if (replyAck == esc4wayAck_OK) + replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); + + // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) + if(!outLen) { + paramBuf[0] = 0; + outLen = 1; } - if (ACK_OUT == ACK_OK) - { - // wtf.D_FLASH_ADDR_H=Adress_H; - // wtf.D_FLASH_ADDR_L=Adress_L; - ioMem.D_PTR_I = ParamBuf; - - switch(CMD) { - // ******* Interface related stuff ******* - case cmd_InterfaceTestAlive: - { - if (isMcuConnected()){ - switch(CurrentInterfaceMode) - { - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - case imSIL_BLB: - { - if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - if (!Stk_SignOn()) { // SetStateDisconnected(); - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - default: - ACK_OUT = ACK_D_GENERAL_ERROR; - } - if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; - } - break; - } - case cmd_ProtocolGetVersion: - { - // Only interface itself, no matter what Device - Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; - break; - } - - case cmd_InterfaceGetName: - { - // Only interface itself, no matter what Device - // O_PARAM_LEN=16; - O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); - O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; - break; - } - - case cmd_InterfaceGetVersion: - { - // Only interface itself, no matter what Device - // Dummy = iUart_res_InterfVersion; - O_PARAM_LEN = 2; - Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; - Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; - break; - } - case cmd_InterfaceExit: - { - isExitScheduled = true; - break; - } - case cmd_InterfaceSetMode: - { - #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { - #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { - #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (ParamBuf[0] == imSK) { - #endif - CurrentInterfaceMode = ParamBuf[0]; - } else { - ACK_OUT = ACK_I_INVALID_PARAM; - } - break; - } - - case cmd_DeviceReset: - { - if (ParamBuf[0] < escCount) { - // Channel may change here - selected_esc = ParamBuf[0]; - } - else { - ACK_OUT = ACK_I_INVALID_CHANNEL; - break; - } - switch (CurrentInterfaceMode) - { - case imSIL_BLB: - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - { - BL_SendCMDRunRestartBootloader(&DeviceInfo); - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - break; - } - #endif - } - SET_DISCONNECTED; - break; - } - case cmd_DeviceInitFlash: - { - SET_DISCONNECTED; - if (ParamBuf[0] < escCount) { - //Channel may change here - //ESC_LO or ESC_HI; Halt state for prev channel - selected_esc = ParamBuf[0]; - } else { - ACK_OUT = ACK_I_INVALID_CHANNEL; - break; - } - O_PARAM_LEN = DeviceInfoSize; //4 - O_PARAM = (uint8_t *)&DeviceInfo; - if(Connect(&DeviceInfo)) { - DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; - } else { - SET_DISCONNECTED; - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case cmd_DeviceEraseAll: - { - switch(CurrentInterfaceMode) - { - case imSK: - { - if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; - break; - } - default: - ACK_OUT = ACK_I_INVALID_CMD; - } - break; - } - #endif - - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case cmd_DevicePageErase: - { - switch (CurrentInterfaceMode) - { - case imSIL_BLB: - { - Dummy.bytes[0] = ParamBuf[0]; - //Address = Page * 512 - ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); - ioMem.D_FLASH_ADDR_L = 0; - if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; - break; - } - default: - ACK_OUT = ACK_I_INVALID_CMD; - } - break; - } - #endif - - //*** Device Memory Read Ops *** - case cmd_DeviceRead: - { - ioMem.D_NUM_BYTES = ParamBuf[0]; - /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; - */ - switch(CurrentInterfaceMode) - { - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - case imATM_BLB: - { - if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) - { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - if(!Stk_ReadFlash(&ioMem)) - { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - default: - ACK_OUT = ACK_I_INVALID_CMD; - } - if (ACK_OUT == ACK_OK) - { - O_PARAM_LEN = ioMem.D_NUM_BYTES; - O_PARAM = (uint8_t *)&ParamBuf; - } - break; - } - - case cmd_DeviceReadEEprom: - { - ioMem.D_NUM_BYTES = ParamBuf[0]; - /* - D_FLASH_ADDR_H = Adress_H; - D_FLASH_ADDR_L = Adress_L; - D_PTR_I = BUF_I; - */ - switch (CurrentInterfaceMode) - { - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - { - if (!BL_ReadEEprom(&ioMem)) - { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - if (!Stk_ReadEEprom(&ioMem)) - { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - default: - ACK_OUT = ACK_I_INVALID_CMD; - } - if(ACK_OUT == ACK_OK) - { - O_PARAM_LEN = ioMem.D_NUM_BYTES; - O_PARAM = (uint8_t *)&ParamBuf; - } - break; - } - - //*** Device Memory Write Ops *** - case cmd_DeviceWrite: - { - ioMem.D_NUM_BYTES = I_PARAM_LEN; - /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; - */ - switch (CurrentInterfaceMode) - { - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - case imATM_BLB: - { - if (!BL_WriteFlash(&ioMem)) { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - if (!Stk_WriteFlash(&ioMem)) - { - ACK_OUT = ACK_D_GENERAL_ERROR; - } - break; - } - #endif - } - break; - } - - case cmd_DeviceWriteEEprom: - { - ioMem.D_NUM_BYTES = I_PARAM_LEN; - ACK_OUT = ACK_D_GENERAL_ERROR; - /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; - */ - switch (CurrentInterfaceMode) - { - #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - { - ACK_OUT = ACK_I_INVALID_CMD; - break; - } - case imATM_BLB: - { - if (BL_WriteEEprom(&ioMem)) - { - ACK_OUT = ACK_OK; - } - break; - } - #endif - #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - { - if (Stk_WriteEEprom(&ioMem)) - { - ACK_OUT = ACK_OK; - } - break; - } - #endif - } - break; - } - default: - { - ACK_OUT = ACK_I_INVALID_CMD; - } - } - } - - CRCout.word = 0; - + crcOut = 0; TX_LED_ON; - serialBeginWrite(_mspPort->port); - WriteByteCrc(cmd_Remote_Escape); - WriteByteCrc(CMD); - WriteByteCrc(ioMem.D_FLASH_ADDR_H); - WriteByteCrc(ioMem.D_FLASH_ADDR_L); - WriteByteCrc(O_PARAM_LEN); - - i=O_PARAM_LEN; - do { - WriteByteCrc(*O_PARAM); - O_PARAM++; - i--; - } while (i > 0); - - WriteByteCrc(ACK_OUT); - WriteByte(CRCout.bytes[1]); - WriteByte(CRCout.bytes[0]); - serialEndWrite(_mspPort->port); - bufWriterFlush(_writer); + serialBeginWrite(port); + writeByteCrc(cmd_Remote_Escape); + writeByteCrc(command); + writeByteCrc(addr >> 8); + writeByteCrc(addr & 0xff); + writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B + for(int i = 0; i < outLen; i++) + writeByteCrc(paramBuf[i]); + writeByteCrc(replyAck); + writeByte(crcOut >> 8); + writeByte(crcOut & 0xff); + serialEndWrite(port); TX_LED_OFF; - if (isExitScheduled) { - DeInitialize4WayInterface(); - return; + } + + esc4wayRelease(); +} + +// handle 4Way interface command +// command - received command, will be sent back in reply +// addr - from received header +// data - buffer used both for received parameters and returned data. +// Should be 256B long ; TODO - implement limited buffer size +// inLen - received input length +// outLen - size of data to return, max 256, initialized to zero +// single '\0' byte will be send if outLen is zero (protocol limitation) +esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen) +{ + ioMem_t ioMem; + ioMem.addr = addr; // default flash operation address + ioMem.data = data; // command data buffer is used for read and write commands + + switch(command) { +// ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + if (!isMcuConnected()) + return esc4wayAck_OK; + + switch(currentInterfaceMode) { +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + if (BL_SendCMDKeepAlive()) + return esc4wayAck_OK; + break; +#endif +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + if (Stk_SignOn()) + return esc4wayAck_OK; + break; +#endif + } + setDisconnected(); + return esc4wayAck_D_GENERAL_ERROR; + + case cmd_ProtocolGetVersion: + // Only interface itself, no matter what Device + data[0] = SERIAL_4WAY_PROTOCOL_VER; + *outLen = 1; + return esc4wayAck_OK; + + case cmd_InterfaceGetName: + // Only interface itself, no matter what Device + // outLen=16; + memcpy(data, SERIAL_4WAY_INTERFACE_NAME_STR, strlen(SERIAL_4WAY_INTERFACE_NAME_STR)); + *outLen = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + return esc4wayAck_OK; + + case cmd_InterfaceGetVersion: + // Only interface itself, no matter what Device + data[0] = SERIAL_4WAY_VERSION_HI; + data[1] = SERIAL_4WAY_VERSION_LO; + *outLen = 2; + return esc4wayAck_OK; + + case cmd_InterfaceExit: + esc4wayExitRequested = true; + return esc4wayAck_OK; + + case cmd_InterfaceSetMode: + switch(data[0]) { +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + case imSIL_BLB: + case imATM_BLB: +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + case imSK: +#endif + currentInterfaceMode = data[0]; + break; + default: + return esc4wayAck_I_INVALID_PARAM; + } + return esc4wayAck_OK; + + case cmd_DeviceReset: + if(data[0] >= escCount) + return esc4wayAck_I_INVALID_CHANNEL; + // Channel may change here + escSelected = data[0]; + switch (currentInterfaceMode) { +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + BL_SendCMDRunRestartBootloader(); + break; +#endif +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + break; +#endif + } + setDisconnected(); + return esc4wayAck_OK; + + case cmd_DeviceInitFlash: { + setDisconnected(); + if (data[0] >= escCount) + return esc4wayAck_I_INVALID_CHANNEL; + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + int replyAck = esc4wayAck_OK; + escSelected = data[0]; + if(!connect(&deviceInfo)) { + setDisconnected(); + replyAck = esc4wayAck_D_GENERAL_ERROR; + } + deviceInfo.interfaceMode = currentInterfaceMode; + memcpy(data, &deviceInfo, sizeof(deviceInfo)); + *outLen = sizeof(deviceInfo); + return replyAck; } - }; + +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + switch(currentInterfaceMode) { + case imSK: + if (!Stk_Chip_Erase()) + return esc4wayAck_D_GENERAL_ERROR; + break; + default: + return esc4wayAck_I_INVALID_CMD; + } + return esc4wayAck_OK; +#endif + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + switch (currentInterfaceMode) { + case imSIL_BLB: + *outLen = 1; // return block number (from incoming packet) + // Address = Page * 512 + ioMem.addr = data[0] << 9; + if (!BL_PageErase(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + default: + return esc4wayAck_I_INVALID_CMD; + } + return esc4wayAck_OK; +#endif + +//*** Device Memory Read Ops *** + +// macros to mix interface with (single bit) memory type for switch statement +#define M_FLASH 0 +#define M_EEPROM 1 +#define INTFMEM(interface, memory) (((interface) << 1) | (memory)) + + case cmd_DeviceReadEEprom: + case cmd_DeviceRead: { + int len = data[0]; + if(len == 0) + len = 0x100; + ioMem.len = len; + switch(INTFMEM(currentInterfaceMode, (command == cmd_DeviceRead) ? M_FLASH : M_EEPROM)) { +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case INTFMEM(imSIL_BLB, M_FLASH): + if(!BL_ReadFlashSIL(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + case INTFMEM(imATM_BLB, M_FLASH): + if(!BL_ReadFlashATM(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + case INTFMEM(imATM_BLB, M_EEPROM): + if(!BL_ReadEEprom(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + // INTFMEM(imSIL_BLB, M_EEPROM): no eeprom on Silabs +#endif +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case INTFMEM(imSK, M_FLASH): + if(!Stk_ReadFlash(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + case INTFMEM(imSK, M_EEPROM): + if (!Stk_ReadEEprom(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; +#endif + default: + return esc4wayAck_I_INVALID_CMD; + } + *outLen = ioMem.len; + return esc4wayAck_OK; + } + +//*** Device Memory Write Ops *** + case cmd_DeviceWrite: + case cmd_DeviceWriteEEprom: + ioMem.len = inLen; + switch (INTFMEM(currentInterfaceMode, (command == cmd_DeviceWrite) ? M_FLASH : M_EEPROM)) { +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case INTFMEM(imSIL_BLB, M_FLASH): + case INTFMEM(imATM_BLB, M_FLASH): + if (!BL_WriteFlash(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + case INTFMEM(imATM_BLB, M_EEPROM): + if (!BL_WriteEEprom(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; +#endif +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case INTFMEM(imSK, M_FLASH): + if (!Stk_WriteFlash(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; + case INTFMEM(imSK, M_EEPROM): + if (!Stk_WriteEEprom(&ioMem)) + return esc4wayAck_D_GENERAL_ERROR; + break; +#endif + default: + return esc4wayAck_I_INVALID_CMD; + } + return esc4wayAck_OK; +#undef M_FLASH +#undef M_EEPROM +#undef INTFMEM + default: + return esc4wayAck_I_INVALID_CMD; + } + // should not get here } #endif diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index 1a350e6823..3e31523b68 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -19,58 +19,117 @@ #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER -#define imC2 0 -#define imSIL_BLB 1 -#define imATM_BLB 2 -#define imSK 3 +typedef enum { + imC2 = 0, + imSIL_BLB = 1, + imATM_BLB = 2, + imSK = 3, +} esc4wayInterfaceMode_e; -typedef struct { - GPIO_TypeDef* gpio; - uint16_t pinpos; - uint16_t pin; - gpio_config_t gpio_config_INPUT; - gpio_config_t gpio_config_OUTPUT; -} escHardware_t; +typedef enum { +// Test Interface still present + cmd_InterfaceTestAlive = 0x30, // '0' alive +// RETURN: ACK -extern uint8_t selected_esc; +// get Protocol Version Number 01..255 + cmd_ProtocolGetVersion = 0x31, // '1' version +// RETURN: uint8_t VersionNumber + ACK -bool isEscHi(uint8_t selEsc); -bool isEscLo(uint8_t selEsc); -void setEscHi(uint8_t selEsc); -void setEscLo(uint8_t selEsc); -void setEscInput(uint8_t selEsc); -void setEscOutput(uint8_t selEsc); +// get Version String + cmd_InterfaceGetName = 0x32, // '2' name +// RETURN: String + ACK -#define ESC_IS_HI isEscHi(selected_esc) -#define ESC_IS_LO isEscLo(selected_esc) -#define ESC_SET_HI setEscHi(selected_esc) -#define ESC_SET_LO setEscLo(selected_esc) -#define ESC_INPUT setEscInput(selected_esc) -#define ESC_OUTPUT setEscOutput(selected_esc) +//get Version Number 01..255 + cmd_InterfaceGetVersion = 0x33, // '3' version +// RETURN: uint16_t VersionNumber + ACK -typedef struct ioMem_s { - uint8_t D_NUM_BYTES; - uint8_t D_FLASH_ADDR_H; - uint8_t D_FLASH_ADDR_L; - uint8_t *D_PTR_I; -} ioMem_t; +// Exit / Restart Interface - can be used to switch to Box Mode + cmd_InterfaceExit = 0x34, // '4' exit +// RETURN: ACK -extern ioMem_t ioMem; +// Reset the Device connected to the Interface + cmd_DeviceReset = 0x35, // '5' reset +// PARAM: uint8_t escId +// RETURN: ACK -typedef union __attribute__ ((packed)) { - uint8_t bytes[2]; - uint16_t word; -} uint8_16_u; +// Get the Device ID connected +// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106 +// RETURN: uint8_t DeviceID + ACK -typedef union __attribute__ ((packed)) { - uint8_t bytes[4]; - uint16_t words[2]; - uint32_t dword; -} uint8_32_u; +// Initialize Flash Access for Device connected +// Autodetects interface protocol; retruns device signature and protocol + cmd_DeviceInitFlash = 0x37, // '7' init flash access +// PARAM: uint8_t escId +// RETURN: uint8_t deviceInfo[4] + ACK -//extern uint8_32_u DeviceInfo; +// Erase the whole Device Memory of connected Device + cmd_DeviceEraseAll = 0x38, // '8' erase all +// RETURN: ACK -bool isMcuConnected(void); -uint8_t Initialize4WayInterface(void); -void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter); -void DeInitialize4WayInterface(void); +// Erase one Page of Device Memory of connected Device + cmd_DevicePageErase = 0x39, // '9' page erase +// PARAM: uint8_t PageNumber (512B pages) +// RETURN: APageNumber ACK + +// Read to Buffer from FLASH Memory of connected Device +// Buffer Len is Max 256 Bytes, 0 means 256 Bytes + cmd_DeviceRead = 0x3A, // ':' read Device +// PARAM: [ADRESS] uint8_t BuffLen +// RETURN: [ADRESS, len] Buffer[0..256] ACK + +// Write Buffer to FLASH Memory of connected Device +// Buffer Len is Max 256 Bytes, 0 means 256 Bytes + cmd_DeviceWrite = 0x3B, // ';' write +// PARAM: [ADRESS + BuffLen] Buffer[1..256] +// RETURN: ACK + +// Set C2CK low infinite - permanent Reset state (unimplemented) + cmd_DeviceC2CK_LOW = 0x3C, // '<' +// RETURN: ACK + +// Read to Buffer from EEPROM Memory of connected Device +// Buffer Len is Max 256 Bytes, 0 means 256 Bytes + cmd_DeviceReadEEprom = 0x3D, // '=' read Device +// PARAM: [ADRESS] uint8_t BuffLen +// RETURN: [ADRESS + BuffLen] + Buffer[1..256] ACK + +// Write Buffer to EEPROM Memory of connected Device +// Buffer Len is Max 256 Bytes, 0 means 256 Bytes + cmd_DeviceWriteEEprom = 0x3E, // '>' write +// PARAM: [ADRESS + BuffLen] Buffer[1..256] +// RETURN: ACK + +// Set Interface Mode + cmd_InterfaceSetMode = 0x3F, // '?' +// PARAM: uint8_t Mode (interfaceMode_e) +// RETURN: ACK +} esc4wayCmd_e; + +// responses +typedef enum { + esc4wayAck_OK = 0x00, +// esc4wayAck_I_UNKNOWN_ERROR = 0x01, + esc4wayAck_I_INVALID_CMD = 0x02, + esc4wayAck_I_INVALID_CRC = 0x03, + esc4wayAck_I_VERIFY_ERROR = 0x04, +// esc4wayAck_D_INVALID_COMMAND = 0x05, +// esc4wayAck_D_COMMAND_FAILED = 0x06, +// esc4wayAck_D_UNKNOWN_ERROR = 0x07, + esc4wayAck_I_INVALID_CHANNEL = 0x08, + esc4wayAck_I_INVALID_PARAM = 0x09, + esc4wayAck_D_GENERAL_ERROR = 0x0f, +} esc4wayAck_e; + +typedef struct escDeviceInfo_s { + uint16_t signature; // lower 16 bit of signature + uint8_t signature2; // top 8 bit of signature for SK / BootMsg last char from BL + uint8_t interfaceMode; +} escDeviceInfo_t; + +bool esc4wayExitRequested; // flag that exit was requested. Set by esc4wayProcessCmd, used internally by esc4wayProcess + +int esc4wayInit(void); +void esc4wayStart(void); +void esc4wayRelease(void); +void esc4wayProcess(serialPort_t *serial); +esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 199c448bd5..acc05f9225 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,6 +25,8 @@ #include #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + +#include "common/utils.h" #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/buf_writer.h" @@ -33,281 +35,259 @@ #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" +#include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - // Bootloader commands // RunCmd #define RestartBootloader 0 -#define ExitBootloader 1 +#define ExitBootloader 1 -#define CMD_RUN 0x00 -#define CMD_PROG_FLASH 0x01 -#define CMD_ERASE_FLASH 0x02 +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 #define CMD_READ_FLASH_SIL 0x03 -#define CMD_VERIFY_FLASH 0x03 -#define CMD_READ_EEPROM 0x04 -#define CMD_PROG_EEPROM 0x05 -#define CMD_READ_SRAM 0x06 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 #define CMD_READ_FLASH_ATM 0x07 -#define CMD_KEEP_ALIVE 0xFD -#define CMD_SET_ADDRESS 0xFF -#define CMD_SET_BUFFER 0xFE +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE -#define CMD_BOOTINIT 0x07 -#define CMD_BOOTSIGN 0x08 +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 // Bootloader result codes -#define brSUCCESS 0x30 -#define brERRORCOMMAND 0xC1 -#define brERRORCRC 0xC2 -#define brNONE 0xFF +#define BR_SUCCESS 0x30 +#define BR_ERRORCOMMAND 0xC1 +#define BR_ERRORCRC 0xC2 +#define BR_NONE 0xFF +#define START_BIT_TIMEOUT 2000 // 2ms -#define START_BIT_TIMEOUT_MS 2 +#define BIT_TIME 52 // 52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS +#define START_BIT_TIME (BIT_TIME_HALVE + 1) -#define BIT_TIME (52) //52uS -#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS -#define START_BIT_TIME (BIT_TIME_HALVE + 1) -//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) - -static uint8_t suart_getc_(uint8_t *bt) +static int suart_getc(void) { uint32_t btime; uint32_t start_time; - uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; + uint32_t wait_time = micros() + START_BIT_TIMEOUT; while (ESC_IS_HI) { // check for startbit begin - if (millis() >= wait_time) { - return 0; + if (micros() >= wait_time) { + return -1; } } // start bit start_time = micros(); btime = start_time + START_BIT_TIME; uint16_t bitmask = 0; - uint8_t bit = 0; - while (micros() < btime); - while(1) { + for(int bit = 0; bit < 10; bit++) { + while (cmp32(micros(), btime) < 0); if (ESC_IS_HI) - { bitmask |= (1 << bit); - } btime = btime + BIT_TIME; - bit++; - if (bit == 10) break; - while (micros() < btime); } // check start bit and stop bit - if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { - return 0; + if ((bitmask & (1 << 0)) || (!(bitmask & (1 << 9)))) { + return -1; } - *bt = bitmask >> 1; - return 1; + return bitmask >> 1; } -static void suart_putc_(uint8_t *tx_b) +static void suart_putc(uint8_t byte) { - // shift out stopbit first - uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); + // send one idle bit first (stopbit from previous byte) + uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10); uint32_t btime = micros(); while(1) { - if(bitmask & 1) { + if(bitmask & 1) ESC_SET_HI; // 1 - } - else { + else ESC_SET_LO; // 0 - } btime = btime + BIT_TIME; - bitmask = (bitmask >> 1); - if (bitmask == 0) break; // stopbit shifted out - but don't wait - while (micros() < btime); + bitmask >>= 1; + if (bitmask == 0) + break; // stopbit shifted out - but don't wait + while (cmp32(micros(), btime) < 0); } } -static uint8_16_u CRC_16; -static uint8_16_u LastCRC_16; - -static void ByteCrc(uint8_t *bt) +static uint16_t crc16Byte(uint16_t from, uint8_t byte) { - uint8_t xb = *bt; - for (uint8_t i = 0; i < 8; i++) - { - if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) { - CRC_16.word = CRC_16.word >> 1; - CRC_16.word = CRC_16.word ^ 0xA001; + uint16_t crc16 = from; + for (int i = 0; i < 8; i++) { + if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) { + crc16 >>= 1; + crc16 ^= 0xA001; } else { - CRC_16.word = CRC_16.word >> 1; + crc16 >>= 1; } - xb = xb >> 1; + byte >>= 1; } + return crc16; } -static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) +static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc) { - // len 0 means 256 - CRC_16.word = 0; - LastCRC_16.word = 0; - uint8_t LastACK = brNONE; - do { - if(!suart_getc_(pstring)) goto timeout; - ByteCrc(pstring); - pstring++; - len--; - } while(len > 0); + int crc = 0; + int c; - if(isMcuConnected()) { - //With CRC read 3 more - if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; - if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; - if(!suart_getc_(&LastACK)) goto timeout; - if (CRC_16.word != LastCRC_16.word) { - LastACK = brERRORCRC; + uint8_t lastACK = BR_NONE; + for(int i = 0; i < len; i++) { + int c; + if ((c = suart_getc()) < 0) goto timeout; + crc = crc16Byte(crc, c); + pstring[i] = c; + } + + if(checkCrc) { + // With CRC read 3 more + for(int i = 0; i < 2; i++) { // checksum 2 CRC bytes + if ((c = suart_getc()) < 0) goto timeout; + crc = crc16Byte(crc, c); } + if((c = suart_getc()) < 0) goto timeout; + lastACK = c; + if (crc != 0) // CRC of correct message is 0 + lastACK = BR_ERRORCRC; } else { - if(!suart_getc_(&LastACK)) goto timeout; + if((c = suart_getc()) < 0) goto timeout; + lastACK = c; } timeout: - return (LastACK == brSUCCESS); + return (lastACK == BR_SUCCESS); } -static void BL_SendBuf(uint8_t *pstring, uint8_t len) +static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc) { ESC_OUTPUT; - CRC_16.word=0; - do { - suart_putc_(pstring); - ByteCrc(pstring); - pstring++; - len--; - } while (len > 0); - - if (isMcuConnected()) { - suart_putc_(&CRC_16.bytes[0]); - suart_putc_(&CRC_16.bytes[1]); + uint16_t crc = 0; + for(int i = 0; i < len; i++) { + suart_putc(pstring[i]); + crc = crc16Byte(crc, pstring[i]); + } + if (appendCrc) { + suart_putc(crc & 0xff); + suart_putc(crc >> 8); } ESC_INPUT; } -uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) +uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo) { - #define BootMsgLen 4 - #define DevSignHi (BootMsgLen) - #define DevSignLo (BootMsgLen+1) +#define BOOT_MSG_LEN 4 +#define DevSignHi (BOOT_MSG_LEN) +#define DevSignLo (BOOT_MSG_LEN + 1) - //DeviceInfo.dword=0; is set before - uint8_t BootInfo[9]; - uint8_t BootMsg[BootMsgLen-1] = "471"; + memset(pDeviceInfo, 0, sizeof(*pDeviceInfo)); + uint8_t bootInfo[BOOT_MSG_LEN + 4]; + static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471"; // x * 0 + 9 #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; - BL_SendBuf(BootInit, 21); + // SK message was sent during autodetection, use longer preamble + uint8_t bootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; #else - uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; - BL_SendBuf(BootInit, 17); + uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; #endif - if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { + BL_SendBuf(bootInit, sizeof(bootInit), false); + if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false)) return 0; - } // BootInfo has no CRC (ACK byte already analyzed... ) // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) - for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK - if (BootInfo[i] != BootMsg[i]) { - return (0); - } - } - - //only 2 bytes used $1E9307 -> 0x9307 - pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1]; - pDeviceInfo->bytes[1] = BootInfo[DevSignHi]; - pDeviceInfo->bytes[0] = BootInfo[DevSignLo]; - return (1); -} - -static uint8_t BL_GetACK(uint32_t Timeout) -{ - uint8_t LastACK = brNONE; - while (!(suart_getc_(&LastACK)) && (Timeout)) { - Timeout--; - } ; - return (LastACK); -} - -uint8_t BL_SendCMDKeepAlive(void) -{ - uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; - BL_SendBuf(sCMD, 2); - if (BL_GetACK(1) != brERRORCOMMAND) { + if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK return 0; - } + + pDeviceInfo->signature2 = bootInfo[BOOT_MSG_LEN - 1]; // taken from bootloaderMsg part, ascii 'c' now + pDeviceInfo->signature = (bootInfo[DevSignHi] << 8) | bootInfo[DevSignLo]; // SIGNATURE_001, SIGNATURE_002 return 1; } -void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) +static uint8_t BL_GetACK(int timeout) +{ + int c; + while ((c = suart_getc()) < 0) + if(--timeout < 0) // timeout=1 -> 1 retry + return BR_NONE; + return c; +} + +uint8_t BL_SendCMDKeepAlive(void) +{ + uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; + BL_SendBuf(sCMD, sizeof(sCMD), true); + if (BL_GetACK(1) != BR_ERRORCOMMAND) + return 0; + return 1; +} + +void BL_SendCMDRunRestartBootloader(void) { uint8_t sCMD[] = {RestartBootloader, 0}; - pDeviceInfo->bytes[0] = 1; - BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) + BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00) return; } static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; - uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; - BL_SendBuf(sCMD, 4); - return (BL_GetACK(2) == brSUCCESS); + if((pMem->addr == 0xffff)) + return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff }; + BL_SendBuf(sCMD, sizeof(sCMD), true); + return BL_GetACK(2) == BR_SUCCESS; } static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) { - uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES}; - if (pMem->D_NUM_BYTES == 0) { - // set high byte - sCMD[2] = 1; - } - BL_SendBuf(sCMD, 4); - if (BL_GetACK(2) != brNONE) return 0; - BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES); - return (BL_GetACK(40) == brSUCCESS); + uint16_t len = pMem->len; + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, len >> 8, len & 0xff}; + BL_SendBuf(sCMD, sizeof(sCMD), true); + if (BL_GetACK(2) != BR_NONE) + return 0; + BL_SendBuf(pMem->data, len, true); + return BL_GetACK(40) == BR_SUCCESS; } static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) { - if (BL_SendCMDSetAddress(pMem)) { - uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES}; - BL_SendBuf(sCMD, 2); - return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES )); - } - return 0; + if(!BL_SendCMDSetAddress(pMem)) + return 0; + unsigned len = pMem->len; + uint8_t sCMD[] = {cmd, len & 0xff}; // 0x100 is sent a 0x00 here + BL_SendBuf(sCMD, sizeof(sCMD), true); + return BL_ReadBuf(pMem->data, len, true); } static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) { - if (BL_SendCMDSetAddress(pMem)) { - if (!BL_SendCMDSetBuffer(pMem)) return 0; - uint8_t sCMD[] = {cmd, 0x01}; - BL_SendBuf(sCMD, 2); - return (BL_GetACK(timeout) == brSUCCESS); - } - return 0; + if(!BL_SendCMDSetAddress(pMem)) + return 0; + if (!BL_SendCMDSetBuffer(pMem)) + return 0; + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, sizeof(sCMD), true); + return BL_GetACK(timeout) == BR_SUCCESS; } -uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) +uint8_t BL_ReadFlashATM(ioMem_t *pMem) { - if(interface_mode == imATM_BLB) { - return BL_ReadA(CMD_READ_FLASH_ATM, pMem); - } else { - return BL_ReadA(CMD_READ_FLASH_SIL, pMem); - } + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); } - - + +uint8_t BL_ReadFlashSIL(ioMem_t *pMem) +{ + return BL_ReadA(CMD_READ_FLASH_SIL, pMem); +} + + uint8_t BL_ReadEEprom(ioMem_t *pMem) { return BL_ReadA(CMD_READ_EEPROM, pMem); @@ -315,23 +295,22 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem) uint8_t BL_PageErase(ioMem_t *pMem) { - if (BL_SendCMDSetAddress(pMem)) { - uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; - BL_SendBuf(sCMD, 2); - return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS); - } - return 0; + if(!BL_SendCMDSetAddress(pMem)) + return 0; + + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, sizeof(sCMD), true); + return BL_GetACK(40 * 1000 / START_BIT_TIMEOUT) == BR_SUCCESS; } uint8_t BL_WriteEEprom(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); + return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT); } uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); + return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT); } #endif -#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 39cfaaa3d9..4a788fa37c 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -21,11 +21,12 @@ #pragma once void BL_SendBootInit(void); -uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); +uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo); uint8_t BL_SendCMDKeepAlive(void); -uint8_t BL_PageErase(ioMem_t *pMem); -uint8_t BL_ReadEEprom(ioMem_t *pMem); uint8_t BL_WriteEEprom(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_PageErase(ioMem_t *pMem); uint8_t BL_WriteFlash(ioMem_t *pMem); -uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); -void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); +uint8_t BL_ReadFlashATM(ioMem_t *pMem); +uint8_t BL_ReadFlashSIL(ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(void); diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h new file mode 100644 index 0000000000..c9adc7f916 --- /dev/null +++ b/src/main/io/serial_4way_impl.h @@ -0,0 +1,47 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + * Author: 4712 +*/ + +typedef struct { + GPIO_TypeDef* gpio; + uint16_t pinpos; + uint16_t pin; + gpio_config_t gpio_config_INPUT; + gpio_config_t gpio_config_OUTPUT; +} escHardware_t; + +extern uint8_t escSelected; + +bool isEscHi(uint8_t selEsc); +bool isEscLo(uint8_t selEsc); +void setEscHi(uint8_t selEsc); +void setEscLo(uint8_t selEsc); +void setEscInput(uint8_t selEsc); +void setEscOutput(uint8_t selEsc); + +#define ESC_IS_HI isEscHi(escSelected) +#define ESC_IS_LO isEscLo(escSelected) +#define ESC_SET_HI setEscHi(escSelected) +#define ESC_SET_LO setEscLo(escSelected) +#define ESC_INPUT setEscInput(escSelected) +#define ESC_OUTPUT setEscOutput(escSelected) + +typedef struct ioMem_s { + uint16_t len; + uint16_t addr; + uint8_t *data; +} ioMem_t; diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 80aa5d1ddd..24a9293d88 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -17,42 +17,45 @@ * have a look at https://github.com/sim-/tgy/blob/master/boot.inc * for info about the stk500v2 implementation */ -#include + #include #include #include #include #include -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "common/utils.h" #include "drivers/gpio.h" #include "drivers/buf_writer.h" #include "drivers/pwm_mapping.h" #include "drivers/serial.h" +#include "drivers/system.h" #include "config/config.h" #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" +#include "io/serial_4way_impl.h" #include "io/serial_4way_stk500v2.h" -#include "drivers/system.h" -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER -#define BIT_LO_US (32) //32uS -#define BIT_HI_US (2*BIT_LO_US) +#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) -static uint8_t StkInBuf[16]; -#define STK_BIT_TIMEOUT 250 // micro seconds +#define BIT_LO_US 32 //32uS +#define BIT_HI_US (2 * BIT_LO_US) + +static uint8_t stkInBuf[16]; + +#define STK_BIT_TIMEOUT 250 // micro seconds #define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms #define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms -#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms -#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s -#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} -#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinLo while (ESC_IS_HI) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } +#define WaitPinHi while (ESC_IS_LO) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } -static uint32_t LastBitTime; -static uint32_t HiLoTsh; +static uint32_t lastBitTime; +static uint32_t hiLoTsh; static uint8_t SeqNumber; static uint8_t StkCmd; @@ -75,10 +78,10 @@ static uint8_t ckSumOut; #define STATUS_CMD_OK 0x00 -#define CmdFlashEepromRead 0xA0 -#define EnterIspCmd1 0xAC -#define EnterIspCmd2 0x53 -#define signature_r 0x30 +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define SPI_SIGNATURE_READ 0x30 #define delay_us(x) delayMicroseconds(x) #define IRQ_OFF // dummy @@ -86,30 +89,30 @@ static uint8_t ckSumOut; static void StkSendByte(uint8_t dat) { - ckSumOut ^= dat; - for (uint8_t i = 0; i < 8; i++) { - if (dat & 0x01) { - // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). - ESC_SET_HI; - delay_us(BIT_HI_US); - ESC_SET_LO; - delay_us(BIT_HI_US); - } else { - // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total) - ESC_SET_HI; - delay_us(BIT_LO_US); - ESC_SET_LO; - delay_us(BIT_LO_US); - ESC_SET_HI; - delay_us(BIT_LO_US); - ESC_SET_LO; - delay_us(BIT_LO_US); - } - dat >>= 1; - } + ckSumOut ^= dat; + for (uint8_t i = 0; i < 8; i++) { + if (dat & 0x01) { + // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). + ESC_SET_HI; + delay_us(BIT_HI_US); + ESC_SET_LO; + delay_us(BIT_HI_US); + } else { + // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total) + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + } + dat >>= 1; + } } -static void StkSendPacketHeader(void) +static void StkSendPacketHeader(uint16_t len) { IRQ_OFF; ESC_OUTPUT; @@ -119,6 +122,9 @@ static void StkSendPacketHeader(void) ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); + StkSendByte(len >> 8); + StkSendByte(len & 0xff); + StkSendByte(TOKEN); } static void StkSendPacketFooter(void) @@ -130,16 +136,14 @@ static void StkSendPacketFooter(void) IRQ_ON; } - - static int8_t ReadBit(void) { uint32_t btimer = micros(); uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; - LastBitTime = micros() - btimer; - if (LastBitTime <= HiLoTsh) { + lastBitTime = micros() - btimer; + if (lastBitTime <= hiLoTsh) { timeout_timer = timeout_timer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; @@ -152,30 +156,27 @@ timeout: return -1; } -static uint8_t ReadByte(uint8_t *bt) +static int ReadByte(void) { - *bt = 0; - for (uint8_t i = 0; i < 8; i++) { + uint8_t byte = 0; + for (int i = 0; i < 8; i++) { int8_t bit = ReadBit(); - if (bit == -1) goto timeout; - if (bit == 1) { - *bt |= (1 << i); - } + if (bit < 0) + return -1; // timeout + byte |= bit << i; } - ckSumIn ^=*bt; - return 1; -timeout: - return 0; + ckSumIn ^= byte; + return byte; } static uint8_t StkReadLeader(void) { // Reset learned timing - HiLoTsh = BIT_HI_US + BIT_LO_US; + hiLoTsh = BIT_HI_US + BIT_LO_US; // Wait for the first bit - uint32_t waitcycl; //250uS each + int waitcycl; //250uS each if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; @@ -184,57 +185,54 @@ static uint8_t StkReadLeader(void) } else { waitcycl= STK_WAITCYLCES; } - for ( ; waitcycl >0 ; waitcycl--) { - //check is not timeout - if (ReadBit() >- 1) break; - } - - //Skip the first bits - if (waitcycl == 0){ + while(ReadBit() < 0 && --waitcycl > 0); + if (waitcycl <= 0) goto timeout; + + // Skip the first bits + for (int i = 0; i < 10; i++) { + if (ReadBit() < 0) + goto timeout; } - for (uint8_t i = 0; i < 10; i++) { - if (ReadBit() == -1) goto timeout; - } - - // learn timing - HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); + // learn timing (0.75 * lastBitTime) + hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2); // Read until we get a 0 bit - int8_t bit; + int bit; do { bit = ReadBit(); - if (bit == -1) goto timeout; + if (bit < 0) + goto timeout; } while (bit > 0); return 1; timeout: return 0; } -static uint8_t StkRcvPacket(uint8_t *pstring) +static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen) { - uint8_t bt = 0; - uint8_16_u Len; + int byte; + int len; - IRQ_OFF; - if (!StkReadLeader()) goto Err; + IRQ_OFF; + if (!StkReadLeader()) goto Err; ckSumIn=0; - if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err; - if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err; - ReadByte(&Len.bytes[1]); - if (Len.bytes[1] > 1) goto Err; - ReadByte(&Len.bytes[0]); - if (Len.bytes[0] < 1) goto Err; - if (!ReadByte(&bt) || (bt != TOKEN)) goto Err; - if (!ReadByte(&bt) || (bt != StkCmd)) goto Err; - if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err; - for (uint16_t i = 0; i < (Len.word - 2); i++) - { - if (!ReadByte(pstring)) goto Err; - pstring++; + if ((byte = ReadByte()) < 0 || (byte != MESSAGE_START)) goto Err; + if ((byte = ReadByte()) < 0 || (byte != SeqNumber)) goto Err; + len = ReadByte() << 8; + len |= ReadByte(); + if(len < 1 || len >= 256 + 4) // will catch timeout too; limit length to max expected size + goto Err; + if ((byte = ReadByte()) < 0 || (byte != TOKEN)) goto Err; + if ((byte = ReadByte()) < 0 || (byte != StkCmd)) goto Err; + if ((byte = ReadByte()) < 0 || (byte != STATUS_CMD_OK)) goto Err; + for (int i = 0; i < len - 2; i++) { + if ((byte = ReadByte()) < 0) goto Err; + if(i < maxLen) // limit saved length (buffer is only 256B, but memory read reply contains additional status + 1 unknown byte) + pstring[i] = byte; } - ReadByte(&bt); + ReadByte(); // read checksum if (ckSumIn != 0) goto Err; IRQ_ON; return 1; @@ -243,89 +241,64 @@ Err: return 0; } -static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) +static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr) { - StkCmd= CMD_SPI_MULTI; - StkSendPacketHeader(); - StkSendByte(0); // hi byte Msg len - StkSendByte(8); // lo byte Msg len - StkSendByte(TOKEN); - StkSendByte(CMD_SPI_MULTI); - StkSendByte(4); // NumTX - StkSendByte(4); // NumRX - StkSendByte(0); // RxStartAdr - StkSendByte(Cmd); // {TxData} Cmd - StkSendByte(AdrHi); // {TxData} AdrHi - StkSendByte(AdrLo); // {TxData} AdrLoch - StkSendByte(0); // {TxData} 0 - StkSendPacketFooter(); - if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3 - if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) { - *ResByte = StkInBuf[3]; - } - return 1; - } - return 0; + StkCmd = CMD_SPI_MULTI; + StkSendPacketHeader(8); + StkSendByte(CMD_SPI_MULTI); + StkSendByte(4); // NumTX + StkSendByte(4); // NumRX + StkSendByte(0); // RxStartAdr + StkSendByte(subcmd); // {TxData} Cmd + StkSendByte(addr >> 8); // {TxData} AdrHi + StkSendByte(addr & 0xff); // {TxData} AdrLow + StkSendByte(0); // {TxData} 0 + StkSendPacketFooter(); + if (StkRcvPacket(stkInBuf, sizeof(stkInBuf))) { // NumRX + 3 + if ((stkInBuf[0] == 0x00) + && ((stkInBuf[1] == subcmd) || (stkInBuf[1] == 0x00 /* ignore zero returns */)) + && (stkInBuf[2] == 0x00)) { + *resByte = stkInBuf[3]; + } + return 1; + } + return 0; } static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { - // ignore 0xFFFF - // assume address is set before and we read or write the immediately following package - if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; - StkCmd = CMD_LOAD_ADDRESS; - StkSendPacketHeader(); - StkSendByte(0); // hi byte Msg len - StkSendByte(5); // lo byte Msg len - StkSendByte(TOKEN); - StkSendByte(CMD_LOAD_ADDRESS); - StkSendByte(0); - StkSendByte(0); - StkSendByte(pMem->D_FLASH_ADDR_H); - StkSendByte(pMem->D_FLASH_ADDR_L); - StkSendPacketFooter(); - return (StkRcvPacket((void *)StkInBuf)); + // ignore 0xFFFF + // assume address is set before and we read or write the immediately following memory + if((pMem->addr == 0xffff)) + return 1; + StkCmd = CMD_LOAD_ADDRESS; + StkSendPacketHeader(5); + StkSendByte(CMD_LOAD_ADDRESS); + StkSendByte(0); + StkSendByte(0); + StkSendByte(pMem->addr >> 8); + StkSendByte(pMem->addr & 0xff); + StkSendPacketFooter(); + return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); } static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) { - uint8_t LenHi; - if (pMem->D_NUM_BYTES>0) { - LenHi=0; - } else { - LenHi=1; - } - StkSendPacketHeader(); - StkSendByte(0); // hi byte Msg len - StkSendByte(4); // lo byte Msg len - StkSendByte(TOKEN); - StkSendByte(StkCmd); - StkSendByte(LenHi); - StkSendByte(pMem->D_NUM_BYTES); - StkSendByte(CmdFlashEepromRead); - StkSendPacketFooter(); - return (StkRcvPacket(pMem->D_PTR_I)); + StkSendPacketHeader(4); + StkSendByte(StkCmd); + StkSendByte(pMem->len >> 8); + StkSendByte(pMem->len & 0xff); + StkSendByte(CmdFlashEepromRead); + StkSendPacketFooter(); + return StkRcvPacket(pMem->data, pMem->len); } static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) { - uint8_16_u Len; - uint8_t LenLo = pMem->D_NUM_BYTES; - uint8_t LenHi; - if (LenLo) { - LenHi = 0; - Len.word = LenLo + 10; - } else { - LenHi = 1; - Len.word = 256 + 10; - } - StkSendPacketHeader(); - StkSendByte(Len.bytes[1]); // high byte Msg len - StkSendByte(Len.bytes[0]); // low byte Msg len - StkSendByte(TOKEN); + StkSendPacketHeader(pMem->len + 10); StkSendByte(StkCmd); - StkSendByte(LenHi); - StkSendByte(LenLo); + StkSendByte(pMem->len >> 8); + StkSendByte(pMem->len & 0xff); StkSendByte(0); // mode StkSendByte(0); // delay StkSendByte(0); // cmd1 @@ -333,92 +306,82 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) StkSendByte(0); // cmd3 StkSendByte(0); // poll1 StkSendByte(0); // poll2 - do { - StkSendByte(*pMem->D_PTR_I); - pMem->D_PTR_I++; - LenLo--; - } while (LenLo); + for(int i = 0; i < pMem->len; i++) + StkSendByte(pMem->data[i]); StkSendPacketFooter(); - return StkRcvPacket((void *)StkInBuf); + return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); } uint8_t Stk_SignOn(void) { - StkCmd=CMD_SIGN_ON; - StkSendPacketHeader(); - StkSendByte(0); // hi byte Msg len - StkSendByte(1); // lo byte Msg len - StkSendByte(TOKEN); + StkCmd = CMD_SIGN_ON; + StkSendPacketHeader(1); StkSendByte(CMD_SIGN_ON); StkSendPacketFooter(); - return (StkRcvPacket((void *) StkInBuf)); + return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); } -uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) +uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo) { - if (Stk_SignOn()) { - if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) { - if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) { - return 1; - } - } + if (!Stk_SignOn()) + return 0; + uint8_t signature[3]; // device signature, MSB first + for(unsigned i = 0; i < sizeof(signature); i++) { + if (!_CMD_SPI_MULTI_EX(&signature[i], SPI_SIGNATURE_READ, i)) + return 0; } - return 0; + // convert signature to little endian + pDeviceInfo->signature = (signature[1] << 8) | signature[2]; + pDeviceInfo->signature2 = signature[0]; + return 1; } uint8_t Stk_Chip_Erase(void) { - StkCmd = CMD_CHIP_ERASE_ISP; - StkSendPacketHeader(); - StkSendByte(0); // high byte Msg len - StkSendByte(7); // low byte Msg len - StkSendByte(TOKEN); - StkSendByte(CMD_CHIP_ERASE_ISP); - StkSendByte(20); // ChipErase_eraseDelay atmega8 - StkSendByte(0); // ChipErase_pollMethod atmega8 - StkSendByte(0xAC); - StkSendByte(0x88); - StkSendByte(0x13); - StkSendByte(0x76); - StkSendPacketFooter(); - return (StkRcvPacket(StkInBuf)); + StkCmd = CMD_CHIP_ERASE_ISP; + StkSendPacketHeader(7); + StkSendByte(StkCmd); + StkSendByte(20); // ChipErase_eraseDelay atmega8 + StkSendByte(0); // ChipErase_pollMethod atmega8 + StkSendByte(0xAC); + StkSendByte(0x88); + StkSendByte(0x13); + StkSendByte(0x76); + StkSendPacketFooter(); + return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); } uint8_t Stk_ReadFlash(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS(pMem)) { - StkCmd = CMD_READ_FLASH_ISP; - return (_CMD_READ_MEM_ISP(pMem)); - } - return 0; + if (!_CMD_LOAD_ADDRESS(pMem)) + return 0; + StkCmd = CMD_READ_FLASH_ISP; + return _CMD_READ_MEM_ISP(pMem); } uint8_t Stk_ReadEEprom(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS(pMem)) { - StkCmd = CMD_READ_EEPROM_ISP; - return (_CMD_READ_MEM_ISP(pMem)); - } - return 0; + if (!_CMD_LOAD_ADDRESS(pMem)) + return 0; + StkCmd = CMD_READ_EEPROM_ISP; + return _CMD_READ_MEM_ISP(pMem); } uint8_t Stk_WriteFlash(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS(pMem)) { - StkCmd = CMD_PROGRAM_FLASH_ISP; - return (_CMD_PROGRAM_MEM_ISP(pMem)); - } - return 0; + if (!_CMD_LOAD_ADDRESS(pMem)) + return 0; + StkCmd = CMD_PROGRAM_FLASH_ISP; + return _CMD_PROGRAM_MEM_ISP(pMem); } uint8_t Stk_WriteEEprom(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS(pMem)) { - StkCmd = CMD_PROGRAM_EEPROM_ISP; - return (_CMD_PROGRAM_MEM_ISP(pMem)); - } - return 0; + if (!_CMD_LOAD_ADDRESS(pMem)) + return 0; + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return _CMD_PROGRAM_MEM_ISP(pMem); } -#endif + #endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index 80ca89826d..8be51b667c 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -18,7 +18,7 @@ #pragma once uint8_t Stk_SignOn(void); -uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); +uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo); uint8_t Stk_ReadEEprom(ioMem_t *pMem); uint8_t Stk_WriteEEprom(ioMem_t *pMem); uint8_t Stk_ReadFlash(ioMem_t *pMem); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ac75b2afb3..22625ccb73 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1568,7 +1568,7 @@ static bool processInCommand(void) // switch all motor lines HI // reply the count of ESC found headSerialReply(1); - serialize8(Initialize4WayInterface()); + serialize8(esc4wayInit()); // because we do not come back after calling Process4WayInterface // proceed with a success reply first tailSerialReply(); @@ -1579,7 +1579,7 @@ static bool processInCommand(void) // rem: App: Wait at least appx. 500 ms for BLHeli to jump into // bootloader mode before try to connect any ESC // Start to activate here - Process4WayInterface(currentPort, writer); + esc4wayProcess(currentPort->port); // former used MSP uart is still active // proceed as usual with MSP commands break;