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