From a40c0c04426b6a07355dac73fb89c44b7cfcfc85 Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Mon, 13 Jun 2016 00:41:45 +0200 Subject: [PATCH] Cleanup 4way-if from cleanflight --- src/main/io/serial_4way.c | 1060 ++++++++++-------------- src/main/io/serial_4way.h | 155 +++- src/main/io/serial_4way_avrootloader.c | 344 ++++---- src/main/io/serial_4way_avrootloader.h | 15 +- src/main/io/serial_4way_impl.h | 47 ++ src/main/io/serial_4way_stk500v2.c | 391 ++++----- src/main/io/serial_4way_stk500v2.h | 5 +- src/main/io/serial_msp.c | 6 +- 8 files changed, 938 insertions(+), 1085 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 5f0dfb4666..d7710cee42 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -25,112 +25,110 @@ #include "platform.h" #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" - -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" -#endif -#if defined(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; +bool esc4wayExitRequested = false; -uint8_32_u DeviceInfo; +static escDeviceInfo_t 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 +137,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)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); + int escIdx = 0; for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; - escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[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 = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; + escHardware[escIdx].pin = pwmOutputConfiguration->portConfigurations[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 @@ -318,11 +237,12 @@ void DeInitialize4WayInterface(void) CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { +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,511 +252,383 @@ 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]; - /* - wtf.D_FLASH_ADDR_H=Adress_H; - wtf.D_FLASH_ADDR_L=Adress_L; - wtf.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]; - /* - wtf.D_FLASH_ADDR_H = Adress_H; - wtf.D_FLASH_ADDR_L = Adress_L; - wtf.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; - /* - wtf.D_FLASH_ADDR_H=Adress_H; - wtf.D_FLASH_ADDR_L=Adress_L; - wtf.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; - /* - wtf.D_FLASH_ADDR_H=Adress_H; - wtf.D_FLASH_ADDR_L=Adress_L; - wtf.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..6c86b44c84 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -16,61 +16,126 @@ * Author: 4712 */ +#pragma once + +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + #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; + +extern 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); + +#endif diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index a44048b703..efb4610268 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -24,7 +24,7 @@ #include #include "platform.h" -#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 +33,262 @@ #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 +#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(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 BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS +#define START_BIT_TIME (BIT_TIME_3_4) -#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 +296,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..9ed08db61c 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -20,12 +20,17 @@ #pragma once +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + 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); + +#endif 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 f6e21d580b..872e8dce7d 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,35 +23,38 @@ #include #include "platform.h" -#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; @@ -74,10 +77,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 @@ -85,30 +88,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; @@ -118,6 +121,9 @@ static void StkSendPacketHeader(void) ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); + StkSendByte(len >> 8); + StkSendByte(len & 0xff); + StkSendByte(TOKEN); } static void StkSendPacketFooter(void) @@ -129,16 +135,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; @@ -151,30 +155,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; @@ -183,57 +184,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; @@ -242,89 +240,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 @@ -332,92 +305,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..8f2fddcf40 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -17,11 +17,14 @@ */ #pragma once +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + 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); uint8_t Stk_WriteFlash(ioMem_t *pMem); uint8_t Stk_Chip_Erase(void); +#endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b38569a8c8..31756bbe08 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -93,9 +93,7 @@ #include "serial_msp.h" -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "io/serial_4way.h" -#endif static serialPort_t *mspSerialPort; @@ -1736,7 +1734,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(); @@ -1747,7 +1745,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;