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

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