rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-09 20:46:07 +01:00
Code Issues Releases Activity
048ac1e383
LibrePilot/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp
m_thread 076e690337 OP-1170 Got rid of more compiler warnings. 
Uncrustified the code.
2014-03-04 23:45:01 +01:00

1106 lines
30 KiB
C++
Raw Blame History

/**
******************************************************************************
*
* @file op_dfu.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup Uploader Uploader Plugin
* @{
* @brief The uploader plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "op_dfu.h"
#include <cmath>
#include <qwaitcondition.h>
#include <QMetaType>
#include <QtWidgets/QApplication>
using namespace OP_DFU;
DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) :
debug(_debug), use_serial(_use_serial), mready(true)
{
info = NULL;
numberOfDevices = 0;
qRegisterMetaType<OP_DFU::Status>("Status");
if (use_serial) {
info = new port(portname);
info->rxBuf = sspRxBuf;
info->rxBufSize = MAX_PACKET_DATA_LEN;
info->txBuf = sspTxBuf;
info->txBufSize = MAX_PACKET_DATA_LEN;
info->max_retry = 10;
info->timeoutLen = 1000;
if (info->status() != port::open) {
cout << "Could not open serial port\n";
mready = false;
return;
}
serialhandle = new qsspt(info, debug);
int count = 0;
while ((serialhandle->ssp_Synchronise() == false) && (count < 10)) {
if (debug) {
qDebug() << "SYNC failed, resending";
}
count++;
}
if (count == 10) {
mready = false;
return;
}
qDebug() << "SYNC Succeded";
serialhandle->start();
} else {
mready = false;
QEventLoop m_eventloop;
QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
m_eventloop.exec();
QList<USBPortInfo> devices;
devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader);
if (devices.length() == 1) {
if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) {
mready = true;
QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
m_eventloop.exec();
} else {
hidHandle.close(0);
}
} else {
// Wait for the board to appear on the USB bus:
USBSignalFilter filter(0x20a0, -1, -1, USBMonitor::Bootloader);
connect(&filter, SIGNAL(deviceDiscovered()), &m_eventloop, SLOT(quit()));
for (int x = 0; x < 4; ++x) {
qDebug() << "OP_DFU trying to detect bootloader:" << x;
if (x == 0) {
QTimer::singleShot(10000, &m_eventloop, SLOT(quit()));
} else {
QTimer::singleShot(2000, &m_eventloop, SLOT(quit()));
}
m_eventloop.exec();
devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader);
qDebug() << "Devices length: " << devices.length();
if (devices.length() == 1) {
qDebug() << "Opening device";
if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) {
QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
m_eventloop.exec();
qDebug() << "OP_DFU detected after delay";
mready = true;
qDebug() << "Detected";
break;
} else {
hidHandle.close(0);
}
} else {
qDebug() << devices.length() << " device(s) detected, don't know what to do!";
mready = false;
}
}
}
}
}
DFUObject::~DFUObject()
{
if (use_serial) {
if (mready) {
delete serialhandle;
delete info;
}
} else {
hidHandle.close(0);
}
}
bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array)
{
QFile file(sfile);
if (!file.open(QIODevice::WriteOnly)) {
if (debug) {
qDebug() << "Can't open file";
}
return false;
}
file.write(array);
file.close();
return true;
}
/**
Tells the mainboard to enter DFU Mode.
*/
bool DFUObject::enterDFU(int const &devNumber)
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::EnterDFU; // DFU Command
buf[2] = 0; // DFU Count
buf[3] = 0; // DFU Count
buf[4] = 0; // DFU Count
buf[5] = 0; // DFU Count
buf[6] = devNumber; // DFU Data0
buf[7] = 1; // DFU Data1
buf[8] = 1; // DFU Data2
buf[9] = 1; // DFU Data3
int result = sendData(buf, BUF_LEN);
if (result < 1) {
return false;
}
if (debug) {
qDebug() << "EnterDFU: " << result << " bytes sent";
}
return true;
}
/**
Tells the board to get ready for an upload. It will in particular
erase the memory to make room for the data. You will have to query
its status to wait until erase is done before doing the actual upload.
*/
bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc)
{
int lastPacketCount;
qint32 numberOfPackets = numberOfBytes / 4 / 14;
int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
if (pad == 0) {
lastPacketCount = 14;
} else {
++numberOfPackets;
lastPacketCount = pad;
}
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = setStartBit(OP_DFU::Upload); // DFU Command
buf[2] = numberOfPackets >> 24; // DFU Count
buf[3] = numberOfPackets >> 16; // DFU Count
buf[4] = numberOfPackets >> 8; // DFU Count
buf[5] = numberOfPackets; // DFU Count
buf[6] = (int)type; // DFU Data0
buf[7] = lastPacketCount; // DFU Data1
buf[8] = crc >> 24;
buf[9] = crc >> 16;
buf[10] = crc >> 8;
buf[11] = crc;
if (debug) {
qDebug() << "Number of packets:" << numberOfPackets << " Size of last packet:" << lastPacketCount;
}
int result = sendData(buf, BUF_LEN);
delay::msleep(1000);
if (debug) {
qDebug() << result << " bytes sent";
}
if (result > 0) {
return true;
}
return false;
}
/**
Does the actual data upload to the board. Needs to be called once the
board is ready to accept data following a StartUpload command, and it is erased.
*/
bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data)
{
int lastPacketCount;
qint32 numberOfPackets = numberOfBytes / 4 / 14;
int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
if (pad == 0) {
lastPacketCount = 14;
} else {
++numberOfPackets;
lastPacketCount = pad;
}
if (debug) {
qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes";
}
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Upload; // DFU Command
int packetsize;
float percentage;
int laspercentage = 0;
for (qint32 packetcount = 0; packetcount < numberOfPackets; ++packetcount) {
percentage = (float)(packetcount + 1) / numberOfPackets * 100;
if (laspercentage != (int)percentage) {
printProgBar((int)percentage, "UPLOADING");
}
laspercentage = (int)percentage;
if (packetcount == numberOfPackets) {
packetsize = lastPacketCount;
} else {
packetsize = 14;
}
// qDebug()<<packetcount;
buf[2] = packetcount >> 24; // DFU Count
buf[3] = packetcount >> 16; // DFU Count
buf[4] = packetcount >> 8; // DFU Count
buf[5] = packetcount; // DFU Count
char *pointer = data.data();
pointer = pointer + 4 * 14 * packetcount;
// qDebug()<<"Packet Number="<<packetcount<<"Data0="<<(int)data[0]<<" Data1="<<(int)data[1]<<" Data0="<<(int)data[2]<<" Data0="<<(int)data[3]<<" buf6="<<(int)buf[6]<<" buf7="<<(int)buf[7]<<" buf8="<<(int)buf[8]<<" buf9="<<(int)buf[9];
CopyWords(pointer, buf + 6, packetsize * 4);
// for (int y=0;y<packetsize*4;++y)
// {
// qDebug()<<y<<":"<<(int)data[packetcount*14*4+y]<<"---"<<(int)buf[6+y];
// }
// qDebug()<<" Data0="<<(int)data[0]<<" Data0="<<(int)data[1]<<" Data0="<<(int)data[2]<<" Data0="<<(int)data[3]<<" buf6="<<(int)buf[6]<<" buf7="<<(int)buf[7]<<" buf8="<<(int)buf[8]<<" buf9="<<(int)buf[9];
// delay::msleep(send_delay);
// if(StatusRequest()!=OP_DFU::uploading) return false;
int result = sendData(buf, BUF_LEN);
// qDebug()<<"sent:"<<result;
if (result < 1) {
return false;
}
// qDebug() << "UPLOAD:"<<"Data="<<(int)buf[6]<<(int)buf[7]<<(int)buf[8]<<(int)buf[9]<<";"<<result << " bytes sent";
}
cout << "\n";
// while(true){}
return true;
}
/**
Sends the firmware description to the device
*/
OP_DFU::Status DFUObject::UploadDescription(QVariant desc)
{
cout << "Starting uploading description\n";
QByteArray array;
if (desc.type() == QMetaType::QString) {
QString description = desc.toString();
if (description.length() % 4 != 0) {
int pad = description.length() / 4;
pad = (pad + 1) * 4;
pad = pad - description.length();
QString padding;
padding.fill(' ', pad);
description.append(padding);
}
array = description.toLatin1();
} else if (desc.type() == QMetaType::QByteArray) {
array = desc.toByteArray();
}
if (!StartUpload(array.length(), OP_DFU::Descript, 0)) {
return OP_DFU::abort;
}
if (!UploadData(array.length(), array)) {
return OP_DFU::abort;
}
if (!EndOperation()) {
return OP_DFU::abort;
}
OP_DFU::Status ret = StatusRequest();
if (debug) {
qDebug() << "Upload description Status=" << StatusToString(ret);
}
return ret;
}
/**
Downloads the description string for the current device.
You have to call enterDFU before calling this function.
*/
QString DFUObject::DownloadDescription(int const & numberOfChars)
{
QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
int index = arr.indexOf(255);
return QString((index == -1) ? arr : arr.left(index));
}
QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
{
QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
return arr;
}
/**
Starts a firmware download
@param firmwareArray: pointer to the location where we should store the firmware
@package device: the device to use for the download
*/
bool DFUObject::DownloadFirmware(QByteArray *firmwareArray, int device)
{
if (isRunning()) {
return false;
}
requestedOperation = OP_DFU::Download;
requestSize = devices[device].SizeOfCode;
requestTransferType = OP_DFU::FW;
requestStorage = firmwareArray;
start();
return true;
}
/**
Runs the upload or download operations.
*/
void DFUObject::run()
{
switch (requestedOperation) {
case OP_DFU::Download:
StartDownloadT(requestStorage, requestSize, requestTransferType);
emit(downloadFinished());
break;
case OP_DFU::Upload:
{
OP_DFU::Status ret = UploadFirmwareT(requestFilename, requestVerify, requestDevice);
emit(uploadFinished(ret));
break;
}
default:
break;
}
}
/**
Downloads a certain number of bytes from a certain location, and stores in an array whose
pointer is passed as an argument
*/
bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type)
{
int lastPacketCount;
// First of all, work out the number of DFU packets we should ask for:
qint32 numberOfPackets = numberOfBytes / 4 / 14;
int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
if (pad == 0) {
lastPacketCount = 14;
} else {
++numberOfPackets;
lastPacketCount = pad;
}
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Download_Req; // DFU Command
buf[2] = numberOfPackets >> 24; // DFU Count
buf[3] = numberOfPackets >> 16; // DFU Count
buf[4] = numberOfPackets >> 8; // DFU Count
buf[5] = numberOfPackets; // DFU Count
buf[6] = (int)type; // DFU Data0
buf[7] = lastPacketCount; // DFU Data1
buf[8] = 1; // DFU Data2
buf[9] = 1; // DFU Data3
int result = sendData(buf, BUF_LEN);
if (debug) {
qDebug() << "StartDownload:" << numberOfPackets << "packets" << " Last Packet Size=" << lastPacketCount << " " << result << " bytes sent";
}
float percentage;
int laspercentage = 0;
// Now get those packets:
for (qint32 x = 0; x < numberOfPackets; ++x) {
int size;
percentage = (float)(x + 1) / numberOfPackets * 100;
if (laspercentage != (int)percentage) {
printProgBar((int)percentage, "DOWNLOADING");
}
laspercentage = (int)percentage;
result = receiveData(buf, BUF_LEN);
if (debug) {
qDebug() << result << " bytes received" << " Count=" << x << "-" << (int)buf[2] << ";" << (int)buf[3] << ";" << (int)buf[4] << ";" << (int)buf[5] << " Data=" << (int)buf[6] << ";" << (int)buf[7] << ";" << (int)buf[8] << ";" << (int)buf[9];
}
if (x == numberOfPackets - 1) {
size = lastPacketCount * 4;
} else {
size = 14 * 4;
}
fw->append(buf + 6, size);
}
StatusRequest();
return true;
}
/**
Resets the device
*/
int DFUObject::ResetDevice(void)
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Reset; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
return sendData(buf, BUF_LEN);
// return hidHandle.send(0,buf, BUF_LEN, 500);
}
int DFUObject::AbortOperation(void)
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Abort_Operation; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
return sendData(buf, BUF_LEN);
}
/**
Starts the firmware (leaves bootloader and boots the main software)
*/
int DFUObject::JumpToApp(bool safeboot, bool erase)
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::JumpFW; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
if (safeboot) {
/* force system to safe boot mode (hwsettings == defaults) */
buf[8] = 0x5A;
buf[9] = 0xFE;
} else {
buf[8] = 0;
buf[9] = 0;
}
if (erase) {
// force data flash clear
buf[10] = 0x00;
buf[11] = 0x00;
buf[12] = 0xFA;
buf[13] = 0x5F;
buf[14] = 0x00;
buf[15] = 0x00;
buf[16] = 0x00;
buf[17] = 0x01;
buf[18] = 0x00;
buf[19] = 0x00;
buf[20] = 0x00;
buf[21] = 0x00;
} else {
buf[10] = 0x00;
buf[11] = 0x00;
buf[12] = 0x00;
buf[13] = 0x00;
buf[14] = 0x00;
buf[15] = 0x00;
buf[16] = 0x00;
buf[17] = 0x00;
buf[18] = 0x00;
buf[19] = 0x00;
buf[20] = 0x00;
buf[21] = 0x00;
}
return sendData(buf, BUF_LEN);
}
OP_DFU::Status DFUObject::StatusRequest()
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Status_Request; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
if (debug) {
qDebug() << "StatusRequest: " << result << " bytes sent";
}
result = receiveData(buf, BUF_LEN);
if (debug) {
qDebug() << "StatusRequest: " << result << " bytes received";
}
if (buf[1] == OP_DFU::Status_Rep) {
return (OP_DFU::Status)buf[6];
} else {
return OP_DFU::abort;
}
}
/**
Ask the bootloader for the list of devices available
*/
bool DFUObject::findDevices()
{
devices.clear();
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Req_Capabilities; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
if (result < 1) {
return false;
}
result = receiveData(buf, BUF_LEN);
if (result < 1) {
return false;
}
numberOfDevices = buf[7];
RWFlags = buf[8];
RWFlags = RWFlags << 8 | buf[9];
if (buf[1] == OP_DFU::Rep_Capabilities) {
for (int x = 0; x < numberOfDevices; ++x) {
device dev;
dev.Readable = (bool)(RWFlags >> (x * 2) & 1);
dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1);
devices.append(dev);
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Req_Capabilities; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = x + 1;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
sendData(buf, BUF_LEN);
receiveData(buf, BUF_LEN);
devices[x].ID = buf[14];
devices[x].ID = devices[x].ID << 8 | (quint8)buf[15];
devices[x].BL_Version = buf[7];
devices[x].SizeOfDesc = buf[8];
quint32 aux;
aux = (quint8)buf[10];
aux = aux << 8 | (quint8)buf[11];
aux = aux << 8 | (quint8)buf[12];
aux = aux << 8 | (quint8)buf[13];
devices[x].FW_CRC = aux;
aux = (quint8)buf[2];
aux = aux << 8 | (quint8)buf[3];
aux = aux << 8 | (quint8)buf[4];
aux = aux << 8 | (quint8)buf[5];
devices[x].SizeOfCode = aux;
}
if (debug) {
qDebug() << "Found " << numberOfDevices << " devices";
for (int x = 0; x < numberOfDevices; ++x) {
qDebug() << "Device #" << x + 1;
qDebug() << "Device ID=" << devices[x].ID;
qDebug() << "Device Readable=" << devices[x].Readable;
qDebug() << "Device Writable=" << devices[x].Writable;
qDebug() << "Device SizeOfCode=" << devices[x].SizeOfCode;
qDebug() << "Device SizeOfDesc=" << devices[x].SizeOfDesc;
qDebug() << "BL Version=" << devices[x].BL_Version;
qDebug() << "FW CRC=" << devices[x].FW_CRC;
}
}
}
return true;
}
bool DFUObject::EndOperation()
{
char buf[BUF_LEN];
buf[0] = 0x02; // reportID
buf[1] = OP_DFU::Op_END; // DFU Command
buf[2] = 0;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
buf[8] = 0;
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
if (debug) {
qDebug() << result << " bytes sent";
}
if (result > 0) {
return true;
}
return false;
}
//
/**
Starts a firmware upload (asynchronous)
*/
bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device)
{
if (isRunning()) {
return false;
}
requestedOperation = OP_DFU::Upload;
requestFilename = sfile;
requestDevice = device;
requestVerify = verify;
start();
return true;
}
OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device)
{
OP_DFU::Status ret;
if (debug) {
qDebug() << "Starting Firmware Uploading...";
}
QFile file(sfile);
if (!file.open(QIODevice::ReadOnly)) {
if (debug) {
qDebug() << "Cant open file";
}
return OP_DFU::abort;
}
QByteArray arr = file.readAll();
if (debug) {
qDebug() << "Bytes Loaded=" << arr.length();
}
if (arr.length() % 4 != 0) {
int pad = arr.length() / 4;
++pad;
pad = pad * 4;
pad = pad - arr.length();
arr.append(QByteArray(pad, 255));
}
if (devices[device].SizeOfCode < (quint32)arr.length()) {
if (debug) {
qDebug() << "ERROR file to big for device";
}
return OP_DFU::abort;;
}
quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode);
if (debug) {
qDebug() << "NEW FIRMWARE CRC=" << crc;
}
if (!StartUpload(arr.length(), OP_DFU::FW, crc)) {
ret = StatusRequest();
if (debug) {
qDebug() << "StartUpload failed";
qDebug() << "StartUpload returned:" << StatusToString(ret);
}
return ret;
}
emit operationProgress(QString("Erasing, please wait..."));
if (debug) {
qDebug() << "Erasing memory";
}
if (StatusRequest() == OP_DFU::abort) {
return OP_DFU::abort;
}
// TODO: why is there a loop there? The "if" statement
// will cause a break or return anyway!!
for (int x = 0; x < 3; ++x) {
ret = StatusRequest();
if (debug) {
qDebug() << "Erase returned: " << StatusToString(ret);
}
if (ret == OP_DFU::uploading) {
break;
} else {
return ret;
}
}
emit operationProgress(QString("Uploading firmware"));
if (!UploadData(arr.length(), arr)) {
ret = StatusRequest();
if (debug) {
qDebug() << "Upload failed (upload data)";
qDebug() << "UploadData returned:" << StatusToString(ret);
}
return ret;
}
if (!EndOperation()) {
ret = StatusRequest();
if (debug) {
qDebug() << "Upload failed (end operation)";
qDebug() << "EndOperation returned:" << StatusToString(ret);
}
return ret;
}
ret = StatusRequest();
if (ret != OP_DFU::Last_operation_Success) {
return ret;
}
if (verify) {
emit operationProgress(QString("Verifying firmware"));
cout << "Starting code verification\n";
QByteArray arr2;
StartDownloadT(&arr2, arr.length(), OP_DFU::FW);
if (arr != arr2) {
cout << "Verify:FAILED\n";
return OP_DFU::abort;
}
}
if (debug) {
qDebug() << "Status=" << ret;
}
cout << "Firmware Uploading succeeded\n";
return ret;
}
OP_DFU::Status DFUObject::CompareFirmware(const QString &sfile, const CompareType &type, int device)
{
cout << "Starting Firmware Compare...\n";
QFile file(sfile);
if (!file.open(QIODevice::ReadOnly)) {
if (debug) {
qDebug() << "Cant open file";
}
return OP_DFU::abort;
}
QByteArray arr = file.readAll();
if (debug) {
qDebug() << "Bytes Loaded=" << arr.length();
}
if (arr.length() % 4 != 0) {
int pad = arr.length() / 4;
++pad;
pad = pad * 4;
pad = pad - arr.length();
arr.append(QByteArray(pad, 255));
}
if (type == OP_DFU::crccompare) {
quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode);
if (crc == devices[device].FW_CRC) {
cout << "Compare Successfull CRC MATCH!\n";
} else {
cout << "Compare failed CRC DONT MATCH!\n";
}
return StatusRequest();
} else {
QByteArray arr2;
StartDownloadT(&arr2, arr.length(), OP_DFU::FW);
if (arr == arr2) {
cout << "Compare Successfull ALL Bytes MATCH!\n";
} else {
cout << "Compare failed Bytes DONT MATCH!\n";
}
return StatusRequest();
}
}
void DFUObject::CopyWords(char *source, char *destination, int count)
{
for (int x = 0; x < count; x = x + 4) {
*(destination + x) = source[x + 3];
*(destination + x + 1) = source[x + 2];
*(destination + x + 2) = source[x + 1];
*(destination + x + 3) = source[x + 0];
}
}
QString DFUObject::StatusToString(OP_DFU::Status const & status)
{
switch (status) {
case DFUidle:
return "DFUidle";
case uploading:
return "";
case wrong_packet_received:
return "wrong_packet_received";
case too_many_packets:
return "too_many_packets";
case too_few_packets:
return "too_few_packets";
case Last_operation_Success:
return "Last_operation_Success";
case downloading:
return "downloading";
case idle:
return "idle";
case Last_operation_failed:
return "Last_operation_failed";
case outsideDevCapabilities:
return "outsideDevCapabilities";
case CRC_Fail:
return "CRC check FAILED";
case failed_jump:
return "Jmp to user FW failed";
case abort:
return "abort";
case uploadingStarting:
return "Uploading Starting";
default:
return "unknown";
}
}
/**
Prints a progress bar with percentage & label during an operation.
Also outputs to stdout if we are in debug mode.
*/
void DFUObject::printProgBar(int const & percent, QString const & label)
{
std::string bar;
emit(progressUpdated(percent));
if (debug) {
for (int i = 0; i < 50; i++) {
if (i < (percent / 2)) {
bar.replace(i, 1, "=");
} else if (i == (percent / 2)) {
bar.replace(i, 1, ">");
} else {
bar.replace(i, 1, " ");
}
}
std::cout << "\r" << label.toLatin1().data() << "[" << bar << "] ";
std::cout.width(3);
std::cout << percent << "% " << std::flush;
}
}
/**
Utility function
*/
quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer)
{
// Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4
while (Size--) {
static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial
0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD
};
Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits
Buffer += 1;
// Process 32-bits, 4 at a time, or 8 rounds
Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits
Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
}
return Crc;
}
/**
Utility function
*/
quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size)
{
quint32 pad = Size - array.length();
array.append(QByteArray(pad, 255));
quint32 t[Size / 4];
for (int x = 0; x < array.length() / 4; x++) {
quint32 aux = 0;
aux = (char)array[x * 4 + 3] & 0xFF;
aux = aux << 8;
aux += (char)array[x * 4 + 2] & 0xFF;
aux = aux << 8;
aux += (char)array[x * 4 + 1] & 0xFF;
aux = aux << 8;
aux += (char)array[x * 4 + 0] & 0xFF;
t[x] = aux;
}
return DFUObject::CRC32WideFast(0xFFFFFFFF, Size / 4, (quint32 *)t);
}
/**
Send data to the bootloader, either through the serial port
of through the HID handle, depending on the mode we're using
*/
int DFUObject::sendData(void *data, int size)
{
if (!use_serial) {
return hidHandle.send(0, data, size, 5000);
}
// Serial Mode:
if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) {
if (debug) {
qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0];
}
return size;
}
if (debug) {
qDebug() << "Serial send OVERRUN";
}
return -1;
}
/**
Receive data from the bootloader, either through the serial port
of through the HID handle, depending on the mode we're using
*/
int DFUObject::receiveData(void *data, int size)
{
if (!use_serial) {
return hidHandle.receive(0, data, size, 10000);
}
// Serial Mode:
int x;
QTime time;
time.start();
while (true) {
if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) {
if (time.elapsed() > 10000) {
qDebug() << "____timeout";
}
return x;
}
}
}
#define BOARD_ID_MB 1
#define BOARD_ID_INS 2
#define BOARD_ID_PIP 3
#define BOARD_ID_CC 4
#define BOARD_ID_REVO 9
/**
Gets the type of board connected
*/
OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum)
{
OP_DFU::eBoardType brdType = eBoardUnkwn;
// First of all, check what Board type we are talking to
int board = devices[boardNum].ID;
qDebug() << "Board model: " << board;
switch (board >> 8) {
case BOARD_ID_MB: // Mainboard family
brdType = eBoardMainbrd;
break;
case BOARD_ID_INS: // Inertial Nav
brdType = eBoardINS;
break;
case BOARD_ID_PIP: // PIP RF Modem
brdType = eBoardPip;
break;
case BOARD_ID_CC: // CopterControl family
brdType = eBoardCC;
break;
case BOARD_ID_REVO: // Revo board
brdType = eBoardRevo;
break;
}
return brdType;
}
Reference in New Issue View Git Blame Copy Permalink