ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       op_dfu.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup Uploader Uploader Plugin
   9  * @{
  10  * @brief The uploader plugin
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include "op_dfu.h"
  29 #include <cmath>
  30 #include <qwaitcondition.h>
  31 #include <QMetaType>
  32 #include <QtWidgets/QApplication>
  33
  34 using namespace OP_DFU;
  35
  36 DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) :
  37     debug(_debug), use_serial(_use_serial), mready(true)
  38 {
  39     info = NULL;
  40     numberOfDevices = 0;
  41
  42     qRegisterMetaType<OP_DFU::Status>("Status");
  43
  44     if (use_serial) {
  45         info = new port(portname, false);
  46         info->rxBuf      = sspRxBuf;
  47         info->rxBufSize  = MAX_PACKET_DATA_LEN;
  48         info->txBuf      = sspTxBuf;
  49         info->txBufSize  = MAX_PACKET_DATA_LEN;
  50         info->max_retry  = 3;
  51         info->timeoutLen = 100;
  52         if (info->status() != port::open) {
  53             cout << "Could not open serial port\n";
  54             mready = false;
  55             return;
  56         }
  57         serialhandle = new qsspt(info, false);
  58
  59         int count = 0;
  60         while ((serialhandle->ssp_Synchronise() == false) && (count < 10)) {
  61             if (debug) {
  62                 qDebug() << "SYNC failed, resending";
  63             }
  64             count++;
  65         }
  66         if (count == 10) {
  67             mready = false;
  68             return;
  69         }
  70         qDebug() << "SYNC Succeded";
  71         serialhandle->start();
  72 /*    } else {
  73         mready = false;
  74         QEventLoop m_eventloop;
  75         QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
  76         m_eventloop.exec();
  77         QList<USBPortInfo> devices;
  78         devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader);
  79         if (devices.length() == 1) {
  80             if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) {
  81                 mready = true;
  82                 QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
  83                 m_eventloop.exec();
  84             } else {
  85                 hidHandle.close(0);
  86             }
  87         } else {
  88             // Wait for the board to appear on the USB bus:
  89             USBSignalFilter filter(0x20a0, -1, -1, USBMonitor::Bootloader);
  90             connect(&filter, SIGNAL(deviceDiscovered()), &m_eventloop, SLOT(quit()));
  91             for (int x = 0; x < 4; ++x) {
  92                 qDebug() << "OP_DFU trying to detect bootloader:" << x;
  93
  94                 if (x == 0) {
  95                     QTimer::singleShot(10000, &m_eventloop, SLOT(quit()));
  96                 } else {
  97                     QTimer::singleShot(2000, &m_eventloop, SLOT(quit()));
  98                 }
  99                 m_eventloop.exec();
 100                 devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader);
 101                 qDebug() << "Devices length: " << devices.length();
 102                 if (devices.length() == 1) {
 103                     qDebug() << "Opening device";
 104                     if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) {
 105                         QTimer::singleShot(200, &m_eventloop, SLOT(quit()));
 106                         m_eventloop.exec();
 107                         qDebug() << "OP_DFU detected after delay";
 108                         mready = true;
 109                         qDebug() << "Detected";
 110                         break;
 111                     } else {
 112                         hidHandle.close(0);
 113                     }
 114                 } else {
 115                     qDebug() << devices.length() << " device(s) detected, don't know what to do!";
 116                     mready = false;
 117                 }
 118             }
 119         }*/
 120     }
 121 }
 122
 123 DFUObject::~DFUObject()
 124 {
 125     if (use_serial) {
 126         if (mready) {
 127             delete serialhandle;
 128             delete info;
 129         }
 130     } /*else {
 131          hidHandle.close(0);
 132          }*/
 133 }
 134
 135 void DFUObject::sendReset(void)
 136 {
 137     qDebug() << "Requesting user mode reset";
 138     uint8_t aa[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x62, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // 63
 139     uint8_t ba[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0xB9, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 140     uint8_t ca[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x10, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 141     uint8_t bb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x8D, 0x19, 0x00, 0x00, 0x13 };
 142     uint8_t ab[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x1B, 0x19, 0x00, 0x00, 0x76 };
 143     uint8_t cb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x78, 0x1C, 0x00, 0x00, 0x25 };
 144     // 125
 145     /*if (!use_serial) {
 146         hidHandle.send(0, aa, 64, 5000);
 147         hidHandle.send(0, ab, 64, 5000);
 148         delay::msleep(600);
 149         hidHandle.send(0, ba, 64, 5000);
 150         hidHandle.send(0, bb, 64, 5000);
 151         delay::msleep(600);
 152         hidHandle.send(0, ca, 64, 5000);
 153         hidHandle.send(0, cb, 64, 5000);
 154         delay::msleep(100);
 155         hidHandle.close(1);
 156        } else {*/
 157     char a[255];
 158     char b[255];
 159     char c[255];
 160     memcpy(a, aa + 2, 62);
 161     memcpy(a + 62, ab + 2, 60);
 162     memcpy(b, ba + 2, 62);
 163     memcpy(b + 62, bb + 2, 60);
 164     memcpy(c, ca + 2, 62);
 165     memcpy(c + 62, cb + 2, 60);
 166     for (int x = 0; x < 123; ++x) {
 167         info->pfSerialWrite(a[x]);
 168     }
 169     delay::msleep(600);
 170     for (int x = 0; x < 123; ++x) {
 171         info->pfSerialWrite(b[x]);
 172     }
 173     delay::msleep(600);
 174     for (int x = 0; x < 123; ++x) {
 175         info->pfSerialWrite(c[x]);
 176     }
 177     // }
 178 }
 179
 180 bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array)
 181 {
 182     QFile file(sfile);
 183
 184     if (!file.open(QIODevice::WriteOnly)) {
 185         if (debug) {
 186             qDebug() << "Can't open file";
 187         }
 188         return false;
 189     }
 190     file.write(array);
 191     file.close();
 192     return true;
 193 }
 194
 195 /**
 196    Tells the mainboard to enter DFU Mode.
 197  */
 198 bool DFUObject::enterDFU(int const &devNumber)
 199 {
 200     char buf[BUF_LEN];
 201
 202     buf[0] = 0x02; // reportID
 203     buf[1] = OP_DFU::EnterDFU; // DFU Command
 204     buf[2] = 0; // DFU Count
 205     buf[3] = 0; // DFU Count
 206     buf[4] = 0; // DFU Count
 207     buf[5] = 0; // DFU Count
 208     buf[6] = devNumber; // DFU Data0
 209     buf[7] = 1; // DFU Data1
 210     buf[8] = 1; // DFU Data2
 211     buf[9] = 1; // DFU Data3
 212
 213     int result = sendData(buf, BUF_LEN);
 214
 215     if (result < 1) {
 216         return false;
 217     }
 218     if (debug) {
 219         qDebug() << "EnterDFU: " << result << " bytes sent";
 220     }
 221     return true;
 222 }
 223
 224 /**
 225    Tells the board to get ready for an upload. It will in particular
 226    erase the memory to make room for the data. You will have to query
 227    its status to wait until erase is done before doing the actual upload.
 228  */
 229 bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc)
 230 {
 231     int lastPacketCount;
 232     qint32 numberOfPackets = numberOfBytes / 4 / 14;
 233     int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
 234
 235     if (pad == 0) {
 236         lastPacketCount = 14;
 237     } else {
 238         ++numberOfPackets;
 239         lastPacketCount = pad;
 240     }
 241     char buf[BUF_LEN];
 242     buf[0]  = 0x02; // reportID
 243     buf[1]  = setStartBit(OP_DFU::Upload); // DFU Command
 244     buf[2]  = numberOfPackets >> 24; // DFU Count
 245     buf[3]  = numberOfPackets >> 16; // DFU Count
 246     buf[4]  = numberOfPackets >> 8; // DFU Count
 247     buf[5]  = numberOfPackets; // DFU Count
 248     buf[6]  = (int)type; // DFU Data0
 249     buf[7]  = lastPacketCount; // DFU Data1
 250     buf[8]  = crc >> 24;
 251     buf[9]  = crc >> 16;
 252     buf[10] = crc >> 8;
 253     buf[11] = crc;
 254     if (debug) {
 255         qDebug() << "Number of packets:" << numberOfPackets << " Size of last packet:" << lastPacketCount;
 256     }
 257
 258     int result = sendData(buf, BUF_LEN);
 259     delay::msleep(1000);
 260
 261     if (debug) {
 262         qDebug() << result << " bytes sent";
 263     }
 264     if (result > 0) {
 265         return true;
 266     }
 267     return false;
 268 }
 269
 270
 271 /**
 272    Does the actual data upload to the board. Needs to be called once the
 273    board is ready to accept data following a StartUpload command, and it is erased.
 274  */
 275 bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data)
 276 {
 277     int lastPacketCount;
 278     qint32 numberOfPackets = numberOfBytes / 4 / 14;
 279     int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
 280
 281     if (pad == 0) {
 282         lastPacketCount = 14;
 283     } else {
 284         ++numberOfPackets;
 285         lastPacketCount = pad;
 286     }
 287     if (debug) {
 288         qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes";
 289     }
 290     char buf[BUF_LEN];
 291     buf[0] = 0x02; // reportID
 292     buf[1] = OP_DFU::Upload; // DFU Command
 293     int packetsize;
 294     float percentage;
 295     int laspercentage = 0;
 296     for (qint32 packetcount = 0; packetcount < numberOfPackets; ++packetcount) {
 297         percentage = (float)(packetcount + 1) / numberOfPackets * 100;
 298         if (laspercentage != (int)percentage) {
 299             printProgBar((int)percentage, "UPLOADING");
 300         }
 301         laspercentage = (int)percentage;
 302         if (packetcount == numberOfPackets) {
 303             packetsize = lastPacketCount;
 304         } else {
 305             packetsize = 14;
 306         }
 307         // qDebug()<<packetcount;
 308         buf[2]  = packetcount >> 24; // DFU Count
 309         buf[3]  = packetcount >> 16; // DFU Count
 310         buf[4]  = packetcount >> 8; // DFU Count
 311         buf[5]  = packetcount; // DFU Count
 312         char *pointer = data.data();
 313         pointer = pointer + 4 * 14 * packetcount;
 314         // if (debug) {
 315         // 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];
 316         // }
 317         CopyWords(pointer, buf + 6, packetsize * 4);
 318         // for (int y=0;y<packetsize*4;++y) {
 319         // qDebug()<<y<<":"<<(int)data[packetcount*14*4+y]<<"---"<<(int)buf[6+y];
 320         // }
 321         // 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];
 322         // delay::msleep(send_delay);
 323
 324         if (StatusRequest() != OP_DFU::uploading) {
 325             return false;
 326         }
 327         int result = sendData(buf, BUF_LEN);
 328         // if (debug) {
 329         // qDebug() << "sent:" << result;
 330         // }
 331         if (result < 1) {
 332             return false;
 333         }
 334         // if (debug) {
 335         // qDebug() << "UPLOAD:" << "Data=" << (int)buf[6] << (int)buf[7] << (int)buf[8] << (int)buf[9] << ";" << result << " bytes sent";
 336         // }
 337     }
 338     return true;
 339 }
 340
 341 /**
 342    Sends the firmware description to the device
 343  */
 344 OP_DFU::Status DFUObject::UploadDescription(QVariant desc)
 345 {
 346     cout << "Starting uploading description\n";
 347     QByteArray array;
 348
 349     if (desc.type() == QVariant::String) {
 350         QString description = desc.toString();
 351         if (description.length() % 4 != 0) {
 352             int pad = description.length() / 4;
 353             pad = (pad + 1) * 4;
 354             pad = pad - description.length();
 355             QString padding;
 356             padding.fill(' ', pad);
 357             description.append(padding);
 358         }
 359         array = description.toLatin1();
 360     } else if (desc.type() == QVariant::ByteArray) {
 361         array = desc.toByteArray();
 362     }
 363
 364     if (!StartUpload(array.length(), OP_DFU::Descript, 0)) {
 365         return OP_DFU::abort;
 366     }
 367     if (!UploadData(array.length(), array)) {
 368         return OP_DFU::abort;
 369     }
 370     if (!EndOperation()) {
 371         return OP_DFU::abort;
 372     }
 373     OP_DFU::Status ret = StatusRequest();
 374
 375
 376     if (debug) {
 377         qDebug() << "Upload description Status=" << StatusToString(ret);
 378     }
 379     return ret;
 380 }
 381
 382
 383 /**
 384    Downloads the description string for the current device.
 385    You have to call enterDFU before calling this function.
 386  */
 387 QString DFUObject::DownloadDescription(int const & numberOfChars)
 388 {
 389     QByteArray arr;
 390
 391     StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
 392
 393     int index = arr.indexOf(255);
 394     return QString((index == -1) ? arr : arr.left(index));
 395 }
 396
 397 QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
 398 {
 399     QByteArray arr;
 400
 401     StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
 402
 403     return arr;
 404 }
 405
 406 /**
 407    Starts a firmware download
 408    @param firmwareArray: pointer to the location where we should store the firmware
 409    @package device: the device to use for the download
 410  */
 411 bool DFUObject::DownloadFirmware(QByteArray *firmwareArray, int device)
 412 {
 413     if (isRunning()) {
 414         return false;
 415     }
 416     requestedOperation  = OP_DFU::Download;
 417     requestSize = devices[device].SizeOfCode;
 418     requestTransferType = OP_DFU::FW;
 419     requestStorage = firmwareArray;
 420     start();
 421     return true;
 422 }
 423
 424
 425 /**
 426    Runs the upload or download operations.
 427  */
 428 void DFUObject::run()
 429 {
 430     switch (requestedOperation) {
 431     case OP_DFU::Download:
 432         StartDownloadT(requestStorage, requestSize, requestTransferType);
 433         emit(downloadFinished());
 434         break;
 435     case OP_DFU::Upload:
 436     {
 437         OP_DFU::Status ret = UploadFirmwareT(requestFilename, requestVerify, requestDevice);
 438         emit(uploadFinished(ret));
 439         break;
 440     }
 441     default:
 442         break;
 443     }
 444 }
 445
 446 /**
 447    Downloads a certain number of bytes from a certain location, and stores in an array whose
 448    pointer is passed as an argument
 449  */
 450 bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type)
 451 {
 452     int lastPacketCount;
 453
 454     // First of all, work out the number of DFU packets we should ask for:
 455     qint32 numberOfPackets = numberOfBytes / 4 / 14;
 456     int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4;
 457
 458     if (pad == 0) {
 459         lastPacketCount = 14;
 460     } else {
 461         ++numberOfPackets;
 462         lastPacketCount = pad;
 463     }
 464
 465     char buf[BUF_LEN];
 466
 467     buf[0] = 0x02; // reportID
 468     buf[1] = OP_DFU::Download_Req; // DFU Command
 469     buf[2] = numberOfPackets >> 24; // DFU Count
 470     buf[3] = numberOfPackets >> 16; // DFU Count
 471     buf[4] = numberOfPackets >> 8; // DFU Count
 472     buf[5] = numberOfPackets; // DFU Count
 473     buf[6] = (int)type; // DFU Data0
 474     buf[7] = lastPacketCount; // DFU Data1
 475     buf[8] = 1; // DFU Data2
 476     buf[9] = 1; // DFU Data3
 477
 478     int result = sendData(buf, BUF_LEN);
 479     if (debug) {
 480         qDebug() << "StartDownload:" << numberOfPackets << "packets" << " Last Packet Size=" << lastPacketCount << " " << result << " bytes sent";
 481     }
 482     float percentage;
 483     int laspercentage = 0;
 484
 485     // Now get those packets:
 486     for (qint32 x = 0; x < numberOfPackets; ++x) {
 487         int size;
 488         percentage = (float)(x + 1) / numberOfPackets * 100;
 489         if (laspercentage != (int)percentage) {
 490             printProgBar((int)percentage, "DOWNLOADING");
 491         }
 492         laspercentage = (int)percentage;
 493
 494         result = receiveData(buf, BUF_LEN);
 495         if (debug) {
 496             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];
 497         }
 498         if (x == numberOfPackets - 1) {
 499             size = lastPacketCount * 4;
 500         } else {
 501             size = 14 * 4;
 502         }
 503         fw->append(buf + 6, size);
 504     }
 505
 506     StatusRequest();
 507     return true;
 508 }
 509
 510
 511 /**
 512    Resets the device
 513  */
 514 int DFUObject::ResetDevice(void)
 515 {
 516     char buf[BUF_LEN];
 517
 518     buf[0] = 0x02; // reportID
 519     buf[1] = OP_DFU::Reset; // DFU Command
 520     buf[2] = 0;
 521     buf[3] = 0;
 522     buf[4] = 0;
 523     buf[5] = 0;
 524     buf[6] = 0;
 525     buf[7] = 0;
 526     buf[8] = 0;
 527     buf[9] = 0;
 528
 529     return sendData(buf, BUF_LEN);
 530     // return hidHandle.send(0,buf, BUF_LEN, 500);
 531 }
 532
 533 int DFUObject::AbortOperation(void)
 534 {
 535     char buf[BUF_LEN];
 536
 537     buf[0] = 0x02; // reportID
 538     buf[1] = OP_DFU::Abort_Operation; // DFU Command
 539     buf[2] = 0;
 540     buf[3] = 0;
 541     buf[4] = 0;
 542     buf[5] = 0;
 543     buf[6] = 0;
 544     buf[7] = 0;
 545     buf[8] = 0;
 546     buf[9] = 0;
 547
 548     return sendData(buf, BUF_LEN);
 549 }
 550 /**
 551    Starts the firmware (leaves bootloader and boots the main software)
 552  */
 553 int DFUObject::JumpToApp(bool safeboot, bool erase)
 554 {
 555     char buf[BUF_LEN];
 556
 557     buf[0] = 0x02; // reportID
 558     buf[1] = OP_DFU::JumpFW; // DFU Command
 559     buf[2] = 0;
 560     buf[3] = 0;
 561     buf[4] = 0;
 562     buf[5] = 0;
 563     buf[6] = 0;
 564     buf[7] = 0;
 565     if (safeboot) {
 566         /* force system to safe boot mode (hwsettings == defaults) */
 567         buf[8] = 0x5A;
 568         buf[9] = 0xFE;
 569     } else {
 570         buf[8] = 0;
 571         buf[9] = 0;
 572     }
 573     if (erase) {
 574         // force data flash clear
 575         buf[10] = 0x00;
 576         buf[11] = 0x00;
 577         buf[12] = 0xFA;
 578         buf[13] = 0x5F;
 579
 580         buf[14] = 0x00;
 581         buf[15] = 0x00;
 582         buf[16] = 0x00;
 583         buf[17] = 0x01;
 584
 585         buf[18] = 0x00;
 586         buf[19] = 0x00;
 587         buf[20] = 0x00;
 588         buf[21] = 0x00;
 589     } else {
 590         buf[10] = 0x00;
 591         buf[11] = 0x00;
 592         buf[12] = 0x00;
 593         buf[13] = 0x00;
 594
 595         buf[14] = 0x00;
 596         buf[15] = 0x00;
 597         buf[16] = 0x00;
 598         buf[17] = 0x00;
 599
 600         buf[18] = 0x00;
 601         buf[19] = 0x00;
 602         buf[20] = 0x00;
 603         buf[21] = 0x00;
 604     }
 605
 606     return sendData(buf, BUF_LEN);
 607 }
 608
 609 OP_DFU::Status DFUObject::StatusRequest()
 610 {
 611     char buf[BUF_LEN];
 612
 613     buf[0] = 0x02; // reportID
 614     buf[1] = OP_DFU::Status_Request; // DFU Command
 615     buf[2] = 0;
 616     buf[3] = 0;
 617     buf[4] = 0;
 618     buf[5] = 0;
 619     buf[6] = 0;
 620     buf[7] = 0;
 621     buf[8] = 0;
 622     buf[9] = 0;
 623
 624     int result = sendData(buf, BUF_LEN);
 625     if (debug) {
 626         qDebug() << "StatusRequest: " << result << " bytes sent";
 627     }
 628     result = receiveData(buf, BUF_LEN);
 629     if (debug) {
 630         qDebug() << "StatusRequest: " << result << " bytes received";
 631     }
 632     if (buf[1] == OP_DFU::Status_Rep) {
 633         return (OP_DFU::Status)buf[6];
 634     } else {
 635         return OP_DFU::abort;
 636     }
 637 }
 638
 639 /**
 640    Ask the bootloader for the list of devices available
 641  */
 642 bool DFUObject::findDevices()
 643 {
 644     devices.clear();
 645     char buf[BUF_LEN];
 646     buf[0] = 0x02; // reportID
 647     buf[1] = OP_DFU::Req_Capabilities; // DFU Command
 648     buf[2] = 0;
 649     buf[3] = 0;
 650     buf[4] = 0;
 651     buf[5] = 0;
 652     buf[6] = 0;
 653     buf[7] = 0;
 654     buf[8] = 0;
 655     buf[9] = 0;
 656
 657     int result = sendData(buf, BUF_LEN);
 658     if (result < 1) {
 659         return false;
 660     }
 661     result = receiveData(buf, BUF_LEN);
 662     if (result < 1) {
 663         return false;
 664     }
 665
 666     numberOfDevices = buf[7];
 667     RWFlags = buf[8];
 668     RWFlags = RWFlags << 8 | buf[9];
 669
 670     if (buf[1] == OP_DFU::Rep_Capabilities) {
 671         for (int x = 0; x < numberOfDevices; ++x) {
 672             device dev;
 673             dev.Readable = (bool)(RWFlags >> (x * 2) & 1);
 674             dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1);
 675             devices.append(dev);
 676             buf[0] = 0x02; // reportID
 677             buf[1] = OP_DFU::Req_Capabilities; // DFU Command
 678             buf[2] = 0;
 679             buf[3] = 0;
 680             buf[4] = 0;
 681             buf[5] = 0;
 682             buf[6] = x + 1;
 683             buf[7] = 0;
 684             buf[8] = 0;
 685             buf[9] = 0;
 686             sendData(buf, BUF_LEN);
 687             receiveData(buf, BUF_LEN);
 688             devices[x].ID = buf[14];
 689             devices[x].ID = devices[x].ID << 8 | (quint8)buf[15];
 690             devices[x].BL_Version = buf[7];
 691             devices[x].SizeOfDesc = buf[8];
 692
 693             quint32 aux;
 694             aux = (quint8)buf[10];
 695             aux = aux << 8 | (quint8)buf[11];
 696             aux = aux << 8 | (quint8)buf[12];
 697             aux = aux << 8 | (quint8)buf[13];
 698
 699             devices[x].FW_CRC = aux;
 700
 701
 702             aux = (quint8)buf[2];
 703             aux = aux << 8 | (quint8)buf[3];
 704             aux = aux << 8 | (quint8)buf[4];
 705             aux = aux << 8 | (quint8)buf[5];
 706             devices[x].SizeOfCode = aux;
 707         }
 708         if (debug) {
 709             qDebug() << "Found " << numberOfDevices << " devices";
 710             for (int x = 0; x < numberOfDevices; ++x) {
 711                 qDebug() << "Device #" << x + 1;
 712                 qDebug() << "Device ID=" << devices[x].ID;
 713                 qDebug() << "Device Readable=" << devices[x].Readable;
 714                 qDebug() << "Device Writable=" << devices[x].Writable;
 715                 qDebug() << "Device SizeOfCode=" << devices[x].SizeOfCode;
 716                 qDebug() << "Device SizeOfDesc=" << devices[x].SizeOfDesc;
 717                 qDebug() << "BL Version=" << devices[x].BL_Version;
 718                 qDebug() << "FW CRC=" << devices[x].FW_CRC;
 719             }
 720         }
 721     }
 722     return true;
 723 }
 724
 725
 726 bool DFUObject::EndOperation()
 727 {
 728     char buf[BUF_LEN];
 729
 730     buf[0] = 0x02; // reportID
 731     buf[1] = OP_DFU::Op_END; // DFU Command
 732     buf[2] = 0;
 733     buf[3] = 0;
 734     buf[4] = 0;
 735     buf[5] = 0;
 736     buf[6] = 0;
 737     buf[7] = 0;
 738     buf[8] = 0;
 739     buf[9] = 0;
 740
 741     int result = sendData(buf, BUF_LEN);
 742     if (debug) {
 743         qDebug() << result << " bytes sent";
 744     }
 745     if (result > 0) {
 746         return true;
 747     }
 748     return false;
 749 }
 750
 751
 752 //
 753 /**
 754    Starts a firmware upload (asynchronous)
 755  */
 756 bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device)
 757 {
 758     if (isRunning()) {
 759         return false;
 760     }
 761     requestedOperation = OP_DFU::Upload;
 762     requestFilename    = sfile;
 763     requestDevice = device;
 764     requestVerify = verify;
 765     start();
 766     return true;
 767 }
 768
 769 OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device)
 770 {
 771     OP_DFU::Status ret;
 772
 773     if (debug) {
 774         qDebug() << "Starting Firmware Uploading...";
 775     }
 776
 777     QFile file(sfile);
 778
 779     if (!file.open(QIODevice::ReadOnly)) {
 780         if (debug) {
 781             qDebug() << "Cant open file";
 782         }
 783         return OP_DFU::abort;
 784     }
 785
 786     QByteArray arr = file.readAll();
 787
 788     if (debug) {
 789         qDebug() << "Bytes Loaded=" << arr.length();
 790     }
 791     if (arr.length() % 4 != 0) {
 792         int pad = arr.length() / 4;
 793         ++pad;
 794         pad = pad * 4;
 795         pad = pad - arr.length();
 796         arr.append(QByteArray(pad, 255));
 797     }
 798     if (devices[device].SizeOfCode < (quint32)arr.length()) {
 799         if (debug) {
 800             qDebug() << "ERROR file to big for device";
 801         }
 802         return OP_DFU::abort;;
 803     }
 804
 805     quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode);
 806     if (debug) {
 807         qDebug() << "NEW FIRMWARE CRC=" << crc;
 808     }
 809
 810     if (!StartUpload(arr.length(), OP_DFU::FW, crc)) {
 811         ret = StatusRequest();
 812         if (debug) {
 813             qDebug() << "StartUpload failed";
 814             qDebug() << "StartUpload returned:" << StatusToString(ret);
 815         }
 816         return ret;
 817     }
 818
 819     emit operationProgress(QString("Erasing, please wait..."));
 820
 821     if (debug) {
 822         qDebug() << "Erasing memory";
 823     }
 824     if (StatusRequest() == OP_DFU::abort) {
 825         return OP_DFU::abort;
 826     }
 827
 828     // TODO: why is there a loop there? The "if" statement
 829     // will cause a break or return anyway!!
 830     for (int x = 0; x < 3; ++x) {
 831         ret = StatusRequest();
 832         if (debug) {
 833             qDebug() << "Erase returned: " << StatusToString(ret);
 834         }
 835         if (ret == OP_DFU::uploading) {
 836             break;
 837         } else {
 838             return ret;
 839         }
 840     }
 841
 842     emit operationProgress(QString("Uploading firmware"));
 843     if (!UploadData(arr.length(), arr)) {
 844         ret = StatusRequest();
 845         if (debug) {
 846             qDebug() << "Upload failed (upload data)";
 847             qDebug() << "UploadData returned:" << StatusToString(ret);
 848         }
 849         return ret;
 850     }
 851     if (!EndOperation()) {
 852         ret = StatusRequest();
 853         if (debug) {
 854             qDebug() << "Upload failed (end operation)";
 855             qDebug() << "EndOperation returned:" << StatusToString(ret);
 856         }
 857         return ret;
 858     }
 859     ret = StatusRequest();
 860     if (ret != OP_DFU::Last_operation_Success) {
 861         return ret;
 862     }
 863
 864     if (verify) {
 865         emit operationProgress(QString("Verifying firmware"));
 866         cout << "Starting code verification\n";
 867         QByteArray arr2;
 868         StartDownloadT(&arr2, arr.length(), OP_DFU::FW);
 869         if (arr != arr2) {
 870             cout << "Verify:FAILED\n";
 871             return OP_DFU::abort;
 872         }
 873     }
 874
 875     if (debug) {
 876         qDebug() << "Status=" << ret;
 877     }
 878     cout << "Firmware Uploading succeeded\n";
 879     return ret;
 880 }
 881
 882
 883 OP_DFU::Status DFUObject::CompareFirmware(const QString &sfile, const CompareType &type, int device)
 884 {
 885     cout << "Starting Firmware Compare...\n";
 886     QFile file(sfile);
 887     if (!file.open(QIODevice::ReadOnly)) {
 888         if (debug) {
 889             qDebug() << "Cant open file";
 890         }
 891         return OP_DFU::abort;
 892     }
 893     QByteArray arr = file.readAll();
 894
 895     if (debug) {
 896         qDebug() << "Bytes Loaded=" << arr.length();
 897     }
 898     if (arr.length() % 4 != 0) {
 899         int pad = arr.length() / 4;
 900         ++pad;
 901         pad = pad * 4;
 902         pad = pad - arr.length();
 903         arr.append(QByteArray(pad, 255));
 904     }
 905     if (type == OP_DFU::crccompare) {
 906         quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode);
 907         if (crc == devices[device].FW_CRC) {
 908             cout << "Compare Successfull CRC MATCH!\n";
 909         } else {
 910             cout << "Compare failed CRC DONT MATCH!\n";
 911         }
 912         return StatusRequest();
 913     } else {
 914         QByteArray arr2;
 915         StartDownloadT(&arr2, arr.length(), OP_DFU::FW);
 916         if (arr == arr2) {
 917             cout << "Compare Successfull ALL Bytes MATCH!\n";
 918         } else {
 919             cout << "Compare failed Bytes DONT MATCH!\n";
 920         }
 921         return StatusRequest();
 922     }
 923 }
 924
 925 void DFUObject::CopyWords(char *source, char *destination, int count)
 926 {
 927     for (int x = 0; x < count; x = x + 4) {
 928         *(destination + x)     = source[x + 3];
 929         *(destination + x + 1) = source[x + 2];
 930         *(destination + x + 2) = source[x + 1];
 931         *(destination + x + 3) = source[x + 0];
 932     }
 933 }
 934 QString DFUObject::StatusToString(OP_DFU::Status const & status)
 935 {
 936     switch (status) {
 937     case DFUidle:
 938         return "DFUidle";
 939
 940     case uploading:
 941         return "";
 942
 943     case wrong_packet_received:
 944         return "wrong_packet_received";
 945
 946     case too_many_packets:
 947         return "too_many_packets";
 948
 949     case too_few_packets:
 950         return "too_few_packets";
 951
 952     case Last_operation_Success:
 953         return "Last_operation_Success";
 954
 955     case downloading:
 956         return "downloading";
 957
 958     case idle:
 959         return "idle";
 960
 961     case Last_operation_failed:
 962         return "Last_operation_failed";
 963
 964     case outsideDevCapabilities:
 965         return "outsideDevCapabilities";
 966
 967     case CRC_Fail:
 968         return "CRC check FAILED";
 969
 970     case failed_jump:
 971         return "Jmp to user FW failed";
 972
 973     case abort:
 974         return "abort";
 975
 976     case uploadingStarting:
 977         return "Uploading Starting";
 978
 979     default:
 980         return "unknown";
 981     }
 982 }
 983
 984 /**
 985    Prints a progress bar with percentage & label during an operation.
 986
 987    Also outputs to stdout if we are in debug mode.
 988  */
 989 void DFUObject::printProgBar(int const & percent, QString const & label)
 990 {
 991     std::string bar;
 992
 993     emit(progressUpdated(percent));
 994
 995     if (debug) {
 996         for (int i = 0; i < 50; i++) {
 997             if (i < (percent / 2)) {
 998                 bar.replace(i, 1, "=");
 999             } else if (i == (percent / 2)) {
1000                 bar.replace(i, 1, ">");
1001             } else {
1002                 bar.replace(i, 1, " ");
1003             }
1004         }
1005
1006         std::cout << "\r" << label.toLatin1().data() << "[" << bar << "] ";
1007         std::cout.width(3);
1008         std::cout << percent << "%     " << std::flush;
1009     }
1010 }
1011
1012 /**
1013    Utility function
1014  */
1015 quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer)
1016 {
1017     // Size = Size >> 2; // /4  Size passed in as a byte count, assumed to be a multiple of 4
1018
1019     while (Size--) {
1020         static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial
1021             0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
1022             0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD
1023         };
1024
1025         Crc     = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits
1026
1027         Buffer += 1;
1028
1029         // Process 32-bits, 4 at a time, or 8 rounds
1030
1031         Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits
1032         Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32
1033         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1034         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1035         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1036         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1037         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1038         Crc = (Crc << 4) ^ CrcTable[Crc >> 28];
1039     }
1040
1041     return Crc;
1042 }
1043
1044 /**
1045    Utility function
1046  */
1047 quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size)
1048 {
1049     quint32 pad = Size - array.length();
1050
1051     array.append(QByteArray(pad, 255));
1052     quint32 t[Size / 4];
1053     for (int x = 0; x < array.length() / 4; x++) {
1054         quint32 aux = 0;
1055         aux  = (char)array[x * 4 + 3] & 0xFF;
1056         aux  = aux << 8;
1057         aux += (char)array[x * 4 + 2] & 0xFF;
1058         aux  = aux << 8;
1059         aux += (char)array[x * 4 + 1] & 0xFF;
1060         aux  = aux << 8;
1061         aux += (char)array[x * 4 + 0] & 0xFF;
1062         t[x] = aux;
1063     }
1064     return DFUObject::CRC32WideFast(0xFFFFFFFF, Size / 4, (quint32 *)t);
1065 }
1066
1067
1068 /**
1069    Send data to the bootloader, either through the serial port
1070    of through the HID handle, depending on the mode we're using
1071  */
1072 int DFUObject::sendData(void *data, int size)
1073 {
1074     /*if (!use_serial) {
1075         return hidHandle.send(0, data, size, 5000);
1076        }*/
1077
1078     // Serial Mode:
1079     if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) {
1080         if (debug) {
1081             qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0];
1082         }
1083         return size;
1084     }
1085     if (debug) {
1086         qDebug() << "Serial send OVERRUN";
1087     }
1088     return -1;
1089 }
1090
1091
1092 /**
1093    Receive data from the bootloader, either through the serial port
1094    of through the HID handle, depending on the mode we're using
1095  */
1096 int DFUObject::receiveData(void *data, int size)
1097 {
1098     /*if (!use_serial) {
1099         return hidHandle.receive(0, data, size, 10000);
1100        }*/
1101
1102     // Serial Mode:
1103     int x;
1104     QTime time;
1105
1106     time.start();
1107     while (true) {
1108         if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) {
1109             msleep(10);
1110             if (time.elapsed() > 10000) {
1111                 qDebug() << "____timeout";
1112             }
1113             if (x > size - 1) {
1114                 qDebug() << "Error buffer overrun";
1115                 Q_ASSERT(false);
1116             }
1117             return x;
1118         }
1119     }
1120 }
1121
1122 #define BOARD_ID_MB   1
1123 #define BOARD_ID_INS  2
1124 #define BOARD_ID_PIP  3
1125 #define BOARD_ID_CC   4
1126 #define BOARD_ID_REVO 9
1127
1128 /**
1129    Gets the type of board connected
1130  */
1131 OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum)
1132 {
1133     OP_DFU::eBoardType brdType = eBoardUnkwn;
1134
1135     // First of all, check what Board type we are talking to
1136     int board = devices[boardNum].ID;
1137
1138     qDebug() << "Board model: " << board;
1139     switch (board >> 8) {
1140     case BOARD_ID_MB: // Mainboard family
1141         brdType = eBoardMainbrd;
1142         break;
1143     case BOARD_ID_INS: // Inertial Nav
1144         brdType = eBoardINS;
1145         break;
1146     case BOARD_ID_PIP: // PIP RF Modem
1147         brdType = eBoardPip;
1148         break;
1149     case BOARD_ID_CC: // CopterControl family
1150         brdType = eBoardCC;
1151         break;
1152     case BOARD_ID_REVO: // Revo board
1153         brdType = eBoardRevo;
1154         break;
1155     }
1156     return brdType;
1157 }