aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
1ad6de1e80
Ardupilot2/libraries/AP_HAL_F4Light/hardware/4way/serial_4way_avrootloader.cpp

329 lines
8.1 KiB
C++
Raw Blame History

/*
(c) 2017 night_ghost@ykoctpa.ru
based on:
* from Cleanflight.
*
* for info about Hagens AVRootloader:
* http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "serial_4way.h"
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include <hal.h>
#include "serial_4way_impl.h"
#include "serial_4way_avrootloader.h"
#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
// Bootloader commands
// RunCmd
#define RestartBootloader 0
#define ExitBootloader 1
#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_VERIFY_FLASH_ARM 0x04
#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_BOOTINIT 0x07
#define CMD_BOOTSIGN 0x08
#define START_BIT_TIMEOUT_MS 2
#define BIT_TIME (52) // 52uS
#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS
#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS
#define START_BIT_TIME (BIT_TIME_3_4)
//#define 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 = millis() + START_BIT_TIMEOUT_MS;
while (ESC_IS_HI) {
// check for startbit begin
if (millis() >= wait_time) {
return 0;
}
}
// start bit
start_time = micros();
btime = start_time + START_BIT_TIME;
uint16_t bitmask = 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) || (!(bitmask & (1 << 9)))) {
return 0;
}
*bt = bitmask >> 1;
return 1;
}
static void suart_putc_(const uint8_t *tx_b)
{
// shift out stopbit first
uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10);
uint32_t btime = micros();
while(1) {
if(bitmask & 1) {
ESC_SET_HI; // 1
}
else {
ESC_SET_LO; // 0
}
btime = btime + BIT_TIME;
bitmask = (bitmask >> 1);
if (bitmask == 0) break; // stopbit shifted out - but don't wait
while (micros() < btime);
}
}
static uint8_16_u CRC_16;
static uint8_16_u LastCRC_16;
static void ByteCrc(const uint8_t *bt)
{
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 {
CRC_16.word = CRC_16.word >> 1;
}
xb = xb >> 1;
}
}
static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len)
{
// 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);
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;
}
} else {
if(!suart_getc_(&LastACK)) goto timeout;
}
timeout:
return (LastACK == brSUCCESS);
}
static void BL_SendBuf(const uint8_t *pstring, uint8_t len)
{
ESC_OUTPUT;
CRC_16.word=0;
do {
suart_putc_(pstring);
ByteCrc(pstring);
pstring++;
len--;
} while (len > 0);
if (isMcuConnected()) {
suart_putc_(&CRC_16.bytes[0]);
suart_putc_(&CRC_16.bytes[1]);
}
ESC_INPUT;
}
uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo)
{
#define BootMsgLen 4
#define DevSignHi (BootMsgLen)
#define DevSignLo (BootMsgLen+1)
//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)
static const 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
static const 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
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)
for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK
if (BootInfo[i] != BootMsg[i]) {
return (0);
}
}
//only 2 bytes used $1E9307 -> 0x9307
pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1];
pDeviceInfo->bytes[1] = BootInfo[DevSignHi];
pDeviceInfo->bytes[0] = BootInfo[DevSignLo];
return (1);
}
static uint8_t BL_GetACK(uint32_t Timeout)
{
uint8_t LastACK = brNONE;
while (!(suart_getc_(&LastACK)) && (Timeout)) {
Timeout--;
} ;
return (LastACK);
}
uint8_t BL_SendCMDKeepAlive(void)
{
uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
BL_SendBuf(sCMD, 2);
if (BL_GetACK(1) != brERRORCOMMAND) {
return 0;
}
return 1;
}
void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo)
{
uint8_t sCMD[] = {RestartBootloader, 0};
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->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)
{
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)) {
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)) {
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_ReadFlash(uint8_t interface_mode, ioMem_t *pMem)
{
if(interface_mode == imATM_BLB) {
return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
} else {
return BL_ReadA(CMD_READ_FLASH_SIL, pMem);
}
}
uint8_t BL_ReadEEprom(ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_EEPROM, pMem);
}
uint8_t BL_PageErase(ioMem_t *pMem)
{
if (BL_SendCMDSetAddress(pMem)) {
uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK((1400 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
}
return 0;
}
uint8_t BL_WriteEEprom(ioMem_t *pMem)
{
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, (400 / START_BIT_TIMEOUT_MS));
}
uint8_t BL_VerifyFlash(ioMem_t *pMem)
{
if (BL_SendCMDSetAddress(pMem)) {
if (!BL_SendCMDSetBuffer(pMem)) return 0;
uint8_t sCMD[] = {CMD_VERIFY_FLASH_ARM, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK(40 / START_BIT_TIMEOUT_MS));
}
return 0;
}
#endif
#endif
Reference in New Issue View Git Blame Copy Permalink