diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 7b783422fa..b065dfd91c 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -25,169 +25,247 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" +#include "drivers/buf_writer.h" #include "drivers/gpio.h" -#include "drivers/io.h" -#include "drivers/io_impl.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" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) #include "io/serial_4way_stk500v2.h" +#endif #define USE_TXRX_LED -#if defined(USE_TXRX_LED) && defined(LED0) -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON +#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 +#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 +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON #endif #else -# 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) +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON #endif #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" -#define SERIAL_4WAY_VER_MAIN 14 -#define SERIAL_4WAY_VER_SUB_1 4 -#define SERIAL_4WAY_VER_SUB_2 4 +// *** 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_PROTOCOL_VER 106 +// *** end -#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" +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" #endif -#if SERIAL_4WAY_VER_SUB_2 >= 100 -# warning "SERIAL_4WAY_VER_SUB_2 should be <= 99 (9.9)" -#endif +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) -#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) +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) static uint8_t escCount; -uint8_t escSelected; + escHardware_t escHardware[MAX_PWM_MOTORS]; -bool esc4wayExitRequested = false; +uint8_t selected_esc; -static escDeviceInfo_t deviceInfo; +uint8_32_u DeviceInfo; -static bool isMcuConnected(void) +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) { - return deviceInfo.signature != 0; + return (DeviceInfo.bytes[0] > 0); } -static void setDisconnected(void) -{ - deviceInfo.signature = 0; -} - -bool isEscHi(uint8_t selEsc) +inline bool isEscHi(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) != Bit_RESET); } - -bool isEscLo(uint8_t selEsc) +inline bool isEscLo(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) == Bit_RESET); } -void setEscHi(uint8_t selEsc) +inline void setEscHi(uint8_t selEsc) { IOHi(escHardware[selEsc].io); } -void setEscLo(uint8_t selEsc) +inline void setEscLo(uint8_t selEsc) { IOLo(escHardware[selEsc].io); } -void setEscInput(uint8_t selEsc) +inline void setEscInput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } -void setEscOutput(uint8_t selEsc) +inline void setEscOutput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } -// Initialize 4way ESC interface -// initializes internal structures -// returns number of ESCs available -int esc4wayInit(void) +uint8_t 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[escIdx].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); - escIdx++; + escHardware[escCount].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } } - escCount = escIdx; return escCount; } -// stat BLHeli 4way interface -// sets all ESC lines as output + hi -void esc4wayStart(void) -{ - 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++) { - IOConfigGPIO(escHardware[i].io, IOCFG_AF_PP); // see pwmOutConfig() - setEscOutput(i); - setEscLo(i); + while (escCount > 0) { + escCount--; + IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP); + setEscLo(escCount); } - escCount = 0; pwmEnableMotors(); } -// BLHeliSuite packet framing -// for reference, see 'Manuals/BLHeliSuite 4w-if protocol.pdf' from BLHeliSuite + +#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 + // Send Structure -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) CRC16_Hi CRC16_Lo +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo // Return -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo -// esc4wayCmd_e in public header +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' -typedef enum { - // not commands, but keep naming consistent with BLHeli suite - cmd_Remote_Escape = 0x2E, // '.' - cmd_Local_Escape = 0x2F, // '/' -} syn_4way_e; +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK +// 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 @@ -221,12 +299,11 @@ typedef enum { 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 @@ -236,388 +313,514 @@ 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}; -static bool signatureMatch(uint16_t signature, uint16_t *list) +#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) { - 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; + 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; 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; + } + } } -#endif -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if (BL_ConnectEx(pDeviceInfo)) { - if(signatureMatch(pDeviceInfo->signature, signaturesSilabs)) { - currentInterfaceMode = imSIL_BLB; + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; return 1; - } - if(signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imATM_BLB; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; return 1; } } -#endif + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo)) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } + #endif } return 0; } static serialPort_t *port; -static uint16_t crcIn, crcOut; -static uint8_t readByte(void) +static uint8_t ReadByte(void) { // need timeout? while (!serialRxBytesWaiting(port)); return serialRead(port); } -static uint8_t readByteCrc(void) +static uint8_16_u CRC_in; +static uint8_t ReadByteCrc(void) { - uint8_t b = readByte(); - crcIn = _crc_xmodem_update(crcIn, b); + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); return b; } -static void writeByte(uint8_t b) +static void WriteByte(uint8_t b) { serialWrite(port, b); } -static void writeByteCrc(uint8_t b) +static uint8_16_u CRCout; +static void WriteByteCrc(uint8_t b) { - writeByte(b); - crcOut = _crc_xmodem_update(crcOut, b); + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); } -// handle 4way interface on serial port -// esc4wayStart / esc4wayRelease in called internally -// 256 bytes buffer is allocated on stack -void esc4wayProcess(serialPort_t *serial) +void esc4wayProcess(serialPort_t *mspPort) { - uint8_t command; - uint16_t addr; - int inLen; - int outLen; - uint8_t paramBuf[256]; - uint8_t replyAck; - esc4wayStart(); + 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; - port = serial; + port = mspPort; -#ifdef BEEPER + // Start here with UART Main loop + #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); -#endif + #endif + bool isExitScheduled = false; - esc4wayExitRequested = false; - while(!esc4wayExitRequested) { + while(1) { // restart looking for new sequence from host - crcIn = 0; - - uint8_t esc = readByteCrc(); - if(esc != cmd_Local_Escape) - continue; // wait for sync character + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); RX_LED_ON; - command = readByteCrc(); - addr = readByteCrc() << 8; - addr |= readByteCrc(); + 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(); - inLen = readByteCrc(); - if(inLen == 0) - inLen = 0x100; // len ==0 -> param is 256B + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); - for(int i = 0; i < inLen; i++) - paramBuf[i] = readByteCrc(); + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); - readByteCrc(); readByteCrc(); // update input 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; - - TX_LED_ON; - if (replyAck == esc4wayAck_OK) - replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); RX_LED_OFF; - // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) - if(!outLen) { - paramBuf[0] = 0; - outLen = 1; + if(CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; } - crcOut = 0; + 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; + + TX_LED_ON; 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 % 0x100; i++) - writeByteCrc(paramBuf[i]); - writeByteCrc(replyAck); - writeByte(crcOut >> 8); - writeByte(crcOut & 0xff); + 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(port); - -#ifdef STM32F4 - delay(50); -#endif TX_LED_OFF; - } - - esc4wayRelease(); + if (isExitScheduled) { + esc4wayRelease(); + return; + } + }; } -// 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 6c86b44c84..b1bba5ff69 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -15,127 +15,36 @@ * along with Cleanflight. If not, see . * Author: 4712 */ - #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "serial_4way_impl.h" #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER -typedef enum { - imC2 = 0, - imSIL_BLB = 1, - imATM_BLB = 2, - imSK = 3, -} esc4wayInterfaceMode_e; +#define imC2 0 +#define imSIL_BLB 1 +#define imATM_BLB 2 +#define imSK 3 -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 +extern ioMem_t ioMem; -// get Version String - cmd_InterfaceGetName = 0x32, // '2' name -// RETURN: String + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[2]; + uint16_t word; +} uint8_16_u; -//get Version Number 01..255 - cmd_InterfaceGetVersion = 0x33, // '3' version -// RETURN: uint16_t VersionNumber + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[4]; + uint16_t words[2]; + uint32_t dword; +} uint8_32_u; -// Exit / Restart Interface - can be used to switch to Box Mode - cmd_InterfaceExit = 0x34, // '4' exit -// RETURN: ACK +//extern uint8_32_u DeviceInfo; -// Reset the Device connected to the Interface - cmd_DeviceReset = 0x35, // '5' reset -// PARAM: uint8_t escId -// RETURN: ACK - -// Get the Device ID connected -// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106 -// RETURN: uint8_t DeviceID + ACK - -// 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 - -// Erase the whole Device Memory of connected Device - cmd_DeviceEraseAll = 0x38, // '8' erase all -// RETURN: ACK - -// 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); +bool isMcuConnected(void); +uint8_t esc4wayInit(void); +void esc4wayProcess(serialPort_t *mspPort); 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 01a598e8b2..2a10dcdb4a 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -24,271 +24,289 @@ #include #include "platform.h" -#include "common/utils.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/buf_writer.h" #include "drivers/pwm_mapping.h" -#include "drivers/gpio.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_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 BR_SUCCESS 0x30 -#define BR_ERRORCOMMAND 0xC1 -#define BR_ERRORCRC 0xC2 -#define BR_NONE 0xFF +#define brSUCCESS 0x30 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF -#define START_BIT_TIMEOUT 2000 // 2ms -#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 START_BIT_TIMEOUT_MS 2 -static int suart_getc(void) +#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) { uint32_t btime; uint32_t start_time; - uint32_t wait_time = micros() + START_BIT_TIMEOUT; + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; while (ESC_IS_HI) { // check for startbit begin - if (micros() >= wait_time) { - return -1; + if (millis() >= wait_time) { + return 0; } } // start bit start_time = micros(); btime = start_time + START_BIT_TIME; uint16_t bitmask = 0; - for(int bit = 0; bit < 10; bit++) { - while (cmp32(micros(), btime) < 0); + uint8_t bit = 0; + while (micros() < btime); + while(1) { 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 << 0)) || (!(bitmask & (1 << 9)))) { - return -1; + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; } - return bitmask >> 1; + *bt = bitmask >> 1; + return 1; } -static void suart_putc(uint8_t byte) +static void suart_putc_(uint8_t *tx_b) { - // send one idle bit first (stopbit from previous byte) - uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10); + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (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 >>= 1; - if (bitmask == 0) - break; // stopbit shifted out - but don't wait - while (cmp32(micros(), btime) < 0); + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait + while (micros() < btime); } } -static uint16_t crc16Byte(uint16_t from, uint8_t byte) +static uint8_16_u CRC_16; +static uint8_16_u LastCRC_16; + +static void ByteCrc(uint8_t *bt) { - uint16_t crc16 = from; - for (int i = 0; i < 8; i++) { - if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) { - crc16 >>= 1; - crc16 ^= 0xA001; + 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; } else { - crc16 >>= 1; + CRC_16.word = CRC_16.word >> 1; } - byte >>= 1; + xb = xb >> 1; } - return crc16; } -static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc) +static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) { - int crc = 0; - int c; + // 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); - 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(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; } - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; - if (crc != 0) // CRC of correct message is 0 - lastACK = BR_ERRORCRC; } else { - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; + if(!suart_getc_(&LastACK)) goto timeout; } timeout: - return (lastACK == BR_SUCCESS); + return (LastACK == brSUCCESS); } -static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc) +static void BL_SendBuf(uint8_t *pstring, uint8_t len) { ESC_OUTPUT; - 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); + 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]); } ESC_INPUT; } -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) { -#define BOOT_MSG_LEN 4 -#define DevSignHi (BOOT_MSG_LEN) -#define DevSignLo (BOOT_MSG_LEN + 1) + #define BootMsgLen 4 + #define DevSignHi (BootMsgLen) + #define DevSignLo (BootMsgLen+1) - memset(pDeviceInfo, 0, sizeof(*pDeviceInfo)); - uint8_t bootInfo[BOOT_MSG_LEN + 4]; - static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471"; + //DeviceInfo.dword=0; is set before + uint8_t BootInfo[9]; + uint8_t BootMsg[BootMsgLen-1] = "471"; // x * 0 + 9 #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - // 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}; + 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); #else - uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 17); #endif - BL_SendBuf(bootInit, sizeof(bootInit), false); - if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false)) + if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { return 0; + } // BootInfo has no CRC (ACK byte already analyzed... ) // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) - if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK - return 0; + for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK + if (BootInfo[i] != BootMsg[i]) { + 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; + //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(int timeout) +static uint8_t BL_GetACK(uint32_t Timeout) { - int c; - while ((c = suart_getc()) < 0) - if(--timeout < 0) // timeout=1 -> 1 retry - return BR_NONE; - return c; + uint8_t LastACK = brNONE; + while (!(suart_getc_(&LastACK)) && (Timeout)) { + Timeout--; + } ; + return (LastACK); } -uint8_t BL_SendCMDKeepAlive(void) +uint8_t BL_SendCMDKeepAlive(void) { uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - if (BL_GetACK(1) != BR_ERRORCOMMAND) + BL_SendBuf(sCMD, 2); + if (BL_GetACK(1) != brERRORCOMMAND) { return 0; + } return 1; } -void BL_SendCMDRunRestartBootloader(void) +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) { uint8_t sCMD[] = {RestartBootloader, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00) + pDeviceInfo->bytes[0] = 1; + BL_SendBuf(sCMD, 2); //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->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; + 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); } static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) { - 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; + 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); } static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) { - 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); + 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; } static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) { - 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; + 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; } -uint8_t BL_ReadFlashATM(ioMem_t *pMem) +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { - return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + if(interface_mode == imATM_BLB) { + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + } else { + return BL_ReadA(CMD_READ_FLASH_SIL, 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); @@ -296,22 +314,23 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem) uint8_t BL_PageErase(ioMem_t *pMem) { - 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; + 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; } uint8_t BL_WriteEEprom(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); } uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } #endif +#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 9ed08db61c..39cfaaa3d9 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -20,17 +20,12 @@ #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - void BL_SendBootInit(void); -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t BL_SendCMDKeepAlive(void); -uint8_t BL_WriteEEprom(ioMem_t *pMem); -uint8_t BL_ReadEEprom(ioMem_t *pMem); uint8_t BL_PageErase(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_WriteEEprom(ioMem_t *pMem); uint8_t BL_WriteFlash(ioMem_t *pMem); -uint8_t BL_ReadFlashATM(ioMem_t *pMem); -uint8_t BL_ReadFlashSIL(ioMem_t *pMem); -void BL_SendCMDRunRestartBootloader(void); - -#endif +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index ca1012b284..a8f83bfae8 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -15,12 +15,15 @@ * along with Cleanflight. If not, see . * Author: 4712 */ +#pragma once + +#include "drivers/io.h" typedef struct { IO_t io; } escHardware_t; -extern uint8_t escSelected; +extern uint8_t selected_esc; bool isEscHi(uint8_t selEsc); bool isEscLo(uint8_t selEsc); @@ -29,15 +32,17 @@ 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) +#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) typedef struct ioMem_s { - uint16_t len; - uint16_t addr; - uint8_t *data; + uint8_t D_NUM_BYTES; + uint8_t D_FLASH_ADDR_H; + uint8_t D_FLASH_ADDR_L; + uint8_t *D_PTR_I; } ioMem_t; + diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 1c32e2c6bb..342a0c8b06 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,38 +23,33 @@ #include #include "platform.h" -#include "common/utils.h" -#include "drivers/gpio.h" -#include "drivers/buf_writer.h" -#include "drivers/pwm_mapping.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #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 -#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#define BIT_LO_US (32) //32uS +#define BIT_HI_US (2*BIT_LO_US) +static uint8_t StkInBuf[16]; -#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_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.5ms -#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s -#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; } +#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) 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; @@ -77,10 +72,10 @@ static uint8_t ckSumOut; #define STATUS_CMD_OK 0x00 -#define CmdFlashEepromRead 0xA0 -#define EnterIspCmd1 0xAC -#define EnterIspCmd2 0x53 -#define SPI_SIGNATURE_READ 0x30 +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define signature_r 0x30 #define delay_us(x) delayMicroseconds(x) #define IRQ_OFF // dummy @@ -89,7 +84,7 @@ static uint8_t ckSumOut; static void StkSendByte(uint8_t dat) { ckSumOut ^= dat; - for (uint8_t i = 0; i < 8; i++) { + 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; @@ -111,7 +106,7 @@ static void StkSendByte(uint8_t dat) } } -static void StkSendPacketHeader(uint16_t len) +static void StkSendPacketHeader(void) { IRQ_OFF; ESC_OUTPUT; @@ -121,9 +116,6 @@ static void StkSendPacketHeader(uint16_t len) ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); - StkSendByte(len >> 8); - StkSendByte(len & 0xff); - StkSendByte(TOKEN); } static void StkSendPacketFooter(void) @@ -135,14 +127,16 @@ 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; @@ -155,27 +149,30 @@ timeout: return -1; } -static int ReadByte(void) +static uint8_t ReadByte(uint8_t *bt) { - uint8_t byte = 0; - for (int i = 0; i < 8; i++) { + *bt = 0; + for (uint8_t i = 0; i < 8; i++) { int8_t bit = ReadBit(); - if (bit < 0) - return -1; // timeout - byte |= bit << i; + if (bit == -1) goto timeout; + if (bit == 1) { + *bt |= (1 << i); + } } - ckSumIn ^= byte; - return byte; + ckSumIn ^=*bt; + return 1; +timeout: + return 0; } 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 - int waitcycl; //250uS each + uint32_t waitcycl; //250uS each if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; @@ -184,54 +181,57 @@ static uint8_t StkReadLeader(void) } else { waitcycl= STK_WAITCYLCES; } - 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 ( ; waitcycl >0 ; waitcycl--) { + //check is not timeout + if (ReadBit() >- 1) break; } - // learn timing (0.75 * lastBitTime) - hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2); + //Skip the first bits + if (waitcycl == 0){ + goto timeout; + } + + for (uint8_t i = 0; i < 10; i++) { + if (ReadBit() == -1) goto timeout; + } + + // learn timing + HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); // Read until we get a 0 bit - int bit; + int8_t bit; do { bit = ReadBit(); - if (bit < 0) - goto timeout; + if (bit == -1) goto timeout; } while (bit > 0); return 1; timeout: return 0; } -static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen) +static uint8_t StkRcvPacket(uint8_t *pstring) { - int byte; - int len; + uint8_t bt = 0; + uint8_16_u Len; IRQ_OFF; if (!StkReadLeader()) goto Err; ckSumIn=0; - 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; + 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++; } - ReadByte(); // read checksum + ReadByte(&bt); if (ckSumIn != 0) goto Err; IRQ_ON; return 1; @@ -240,26 +240,27 @@ Err: return 0; } -static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr) +static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) { - StkCmd = CMD_SPI_MULTI; - StkSendPacketHeader(8); + 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(subcmd); // {TxData} Cmd - StkSendByte(addr >> 8); // {TxData} AdrHi - StkSendByte(addr & 0xff); // {TxData} AdrLow - StkSendByte(0); // {TxData} 0 + 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(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; + 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; } @@ -267,37 +268,61 @@ static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t add static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { // ignore 0xFFFF - // assume address is set before and we read or write the immediately following memory - if((pMem->addr == 0xffff)) - return 1; + // 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(5); + 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->addr >> 8); - StkSendByte(pMem->addr & 0xff); + StkSendByte(pMem->D_FLASH_ADDR_H); + StkSendByte(pMem->D_FLASH_ADDR_L); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *)StkInBuf)); } static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(4); + 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(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(pMem->D_NUM_BYTES); StkSendByte(CmdFlashEepromRead); StkSendPacketFooter(); - return StkRcvPacket(pMem->data, pMem->len); + return (StkRcvPacket(pMem->D_PTR_I)); } static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(pMem->len + 10); + 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); StkSendByte(StkCmd); - StkSendByte(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(LenLo); StkSendByte(0); // mode StkSendByte(0); // delay StkSendByte(0); // cmd1 @@ -305,82 +330,92 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) StkSendByte(0); // cmd3 StkSendByte(0); // poll1 StkSendByte(0); // poll2 - for(int i = 0; i < pMem->len; i++) - StkSendByte(pMem->data[i]); + do { + StkSendByte(*pMem->D_PTR_I); + pMem->D_PTR_I++; + LenLo--; + } while (LenLo); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return StkRcvPacket((void *)StkInBuf); } uint8_t Stk_SignOn(void) { - StkCmd = CMD_SIGN_ON; - StkSendPacketHeader(1); + StkCmd=CMD_SIGN_ON; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(1); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_SIGN_ON); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *) StkInBuf)); } -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) { - 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; + 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; + } + } } - // convert signature to little endian - pDeviceInfo->signature = (signature[1] << 8) | signature[2]; - pDeviceInfo->signature2 = signature[0]; - return 1; + return 0; } uint8_t Stk_Chip_Erase(void) { StkCmd = CMD_CHIP_ERASE_ISP; - StkSendPacketHeader(7); - StkSendByte(StkCmd); + 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(0); // ChipErase_pollMethod atmega8 StkSendByte(0xAC); StkSendByte(0x88); StkSendByte(0x13); StkSendByte(0x76); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket(StkInBuf)); } uint8_t Stk_ReadFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_FLASH_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_FLASH_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_ReadEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_EEPROM_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_EEPROM_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_FLASH_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_FLASH_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_EEPROM_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } - +#endif #endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index 8f2fddcf40..80ca89826d 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -17,14 +17,11 @@ */ #pragma once -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - uint8_t Stk_SignOn(void); -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t Stk_ConnectEx(uint8_32_u *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