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