FirmwareImage.cc 16.6 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

10 11 12 13 14 15 16

/// @file
///     @brief Support for Intel Hex firmware file
///     @author Don Gagne <don@thegagnes.com>

#include "FirmwareImage.h"
#include "QGCLoggingCategory.h"
17 18 19 20
#include "JsonHelper.h"
#include "QGCMAVLink.h"
#include "QGCApplication.h"
#include "FirmwarePlugin.h"
21
#include "ParameterManager.h"
22 23 24 25 26 27 28 29 30 31

#include <QDebug>
#include <QFile>
#include <QTextStream>
#include <QJsonDocument>
#include <QJsonObject>
#include <QSettings>
#include <QFileInfo>
#include <QDir>

32 33 34 35 36 37 38 39 40
const char* FirmwareImage::_jsonBoardIdKey =            "board_id";
const char* FirmwareImage::_jsonParamXmlSizeKey =       "parameter_xml_size";
const char* FirmwareImage::_jsonParamXmlKey =           "parameter_xml";
const char* FirmwareImage::_jsonAirframeXmlSizeKey =    "airframe_xml_size";
const char* FirmwareImage::_jsonAirframeXmlKey =        "airframe_xml";
const char* FirmwareImage::_jsonImageSizeKey =          "image_size";
const char* FirmwareImage::_jsonImageKey =              "image";
const char* FirmwareImage::_jsonMavAutopilotKey =       "mav_autopilot";

41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
FirmwareImage::FirmwareImage(QObject* parent) :
    QObject(parent),
    _imageSize(0)
{
    
}

bool FirmwareImage::load(const QString& imageFilename, uint32_t boardId)
{
    _imageSize = 0;
    _boardId = boardId;
    
    if (imageFilename.endsWith(".bin")) {
        return _binLoad(imageFilename);
        _binFormat = true;
        return true;
    } else if (imageFilename.endsWith(".px4")) {
        _binFormat = true;
        return _px4Load(imageFilename);
    } else if (imageFilename.endsWith(".ihx")) {
        _binFormat = false;
        return _ihxLoad(imageFilename);
    } else {
Don Gagne's avatar
Don Gagne committed
64
        emit statusMessage("Unsupported file format");
65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
        return false;
    }
}

bool FirmwareImage::_readByteFromStream(QTextStream& stream, uint8_t& byte)
{
    QString hex = stream.read(2);
    
    if (hex.count() != 2) {
        return false;
    }
    
    bool success;
    byte = (uint8_t)hex.toInt(&success, 16);
    
    return success;
}

bool FirmwareImage::_readWordFromStream(QTextStream& stream, uint16_t& word)
{
    QString hex = stream.read(4);
    
    if (hex.count() != 4) {
        return false;
    }
    
    bool success;
    word = (uint16_t)hex.toInt(&success, 16);
    
    return success;
}

bool FirmwareImage::_readBytesFromStream(QTextStream& stream, uint8_t byteCount, QByteArray& bytes)
{
    bytes.clear();
    
    while (byteCount) {
        uint8_t byte;
        
        if (!_readByteFromStream(stream, byte)) {
            return false;
        }
        bytes += byte;
        
        byteCount--;
    }
    
    return true;
}

bool FirmwareImage::_ihxLoad(const QString& ihxFilename)
{
    _imageSize = 0;
    _ihxBlocks.clear();
    
    QFile ihxFile(ihxFilename);
    if (!ihxFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
Don Gagne's avatar
Don Gagne committed
122
        emit statusMessage(QString("Unable to open firmware file %1, error: %2").arg(ihxFilename).arg(ihxFile.errorString()));
123 124 125 126 127 128 129
        return false;
    }
    
    QTextStream stream(&ihxFile);
    
    while (true) {
        if (stream.read(1) != ":") {
Don Gagne's avatar
Don Gagne committed
130
            emit statusMessage("Incorrectly formatted .ihx file, line does not begin with :");
131 132 133 134 135 136 137 138 139 140 141 142 143 144
            return false;
        }
        
        uint8_t     blockByteCount;
        uint16_t    address;
        uint8_t     recordType;
        QByteArray  bytes;
        uint8_t     crc;
        
        if (!_readByteFromStream(stream, blockByteCount) ||
            !_readWordFromStream(stream, address) ||
            !_readByteFromStream(stream, recordType) ||
            !_readBytesFromStream(stream, blockByteCount, bytes) ||
            !_readByteFromStream(stream, crc)) {
Don Gagne's avatar
Don Gagne committed
145
            emit statusMessage("Incorrectly formatted line in .ihx file, line too short");
146 147 148 149
            return false;
        }
        
        if (!(recordType == 0 || recordType == 1)) {
Don Gagne's avatar
Don Gagne committed
150
            emit statusMessage(QString("Unsupported record type in file: %1").arg(recordType));
151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168
            return false;
        }
        
        if (recordType == 0) {
            bool appendToLastBlock = false;
            
            // Can we append this block to the last one?
            
            if (_ihxBlocks.count()) {
                int lastBlockIndex = _ihxBlocks.count() - 1;
                
                if (_ihxBlocks[lastBlockIndex].address + _ihxBlocks[lastBlockIndex].bytes.count() == address) {
                    appendToLastBlock = true;
                }
            }
            
            if (appendToLastBlock) {
                _ihxBlocks[_ihxBlocks.count() - 1].bytes += bytes;
Don Gagne's avatar
Don Gagne committed
169 170
                // Too noisy even for verbose
                //qCDebug(FirmwareUpgradeVerboseLog) << QString("_ihxLoad - append - address:%1 size:%2 block:%3").arg(address).arg(blockByteCount).arg(ihxBlockCount());
171 172 173 174 175 176 177
            } else {
                IntelHexBlock_t block;
                
                block.address = address;
                block.bytes = bytes;
                
                _ihxBlocks += block;
Don Gagne's avatar
Don Gagne committed
178
                qCDebug(FirmwareUpgradeVerboseLog) << QString("_ihxLoad - new block - address:%1 size:%2 block:%3").arg(address).arg(blockByteCount).arg(ihxBlockCount());
179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200
            }
            
            _imageSize += blockByteCount;
        } else if (recordType == 1) {
            // EOF
            qCDebug(FirmwareUpgradeLog) << QString("_ihxLoad - EOF");
            break;
        }
        
        // Move to next line
        stream.readLine();
    }
    
    ihxFile.close();
    
    return true;
}

bool FirmwareImage::_px4Load(const QString& imageFilename)
{
    _imageSize = 0;
    
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
201
    // We need to collect information from the .px4 file as well as pull the binary image out to a separate file.
202 203 204
    
    QFile px4File(imageFilename);
    if (!px4File.open(QIODevice::ReadOnly | QIODevice::Text)) {
Don Gagne's avatar
Don Gagne committed
205
        emit statusMessage(QString("Unable to open firmware file %1, error: %2").arg(imageFilename).arg(px4File.errorString()));
206 207 208 209 210 211 212 213
        return false;
    }
    
    QByteArray bytes = px4File.readAll();
    px4File.close();
    QJsonDocument doc = QJsonDocument::fromJson(bytes);
    
    if (doc.isNull()) {
Don Gagne's avatar
Don Gagne committed
214
        emit statusMessage("Supplied file is not a valid JSON document");
215 216 217 218 219 220
        return false;
    }
    
    QJsonObject px4Json = doc.object();
    
    // Make sure the keys we need are available
221 222 223 224
    QString errorString;
    QStringList requiredKeys;
    requiredKeys << _jsonBoardIdKey << _jsonImageKey << _jsonImageSizeKey;
    if (!JsonHelper::validateRequiredKeys(px4Json, requiredKeys, errorString)) {
Don Gagne's avatar
Don Gagne committed
225
        emit statusMessage(QString("Firmware file mission required key: %1").arg(errorString));
226
        return false;
227
    }
228 229 230 231 232 233 234

    // Make sure the keys are the correct type
    QStringList keys;
    QList<QJsonValue::Type> types;
    keys << _jsonBoardIdKey << _jsonParamXmlSizeKey << _jsonParamXmlKey << _jsonAirframeXmlSizeKey << _jsonAirframeXmlKey << _jsonImageSizeKey << _jsonImageKey << _jsonMavAutopilotKey;
    types << QJsonValue::Double << QJsonValue::Double << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::Double;
    if (!JsonHelper::validateKeyTypes(px4Json, keys, types, errorString)) {
Don Gagne's avatar
Don Gagne committed
235
        emit statusMessage(QString("Firmware file has invalid key: %1").arg(errorString));
236 237 238 239
        return false;
    }

    uint32_t firmwareBoardId = (uint32_t)px4Json.value(_jsonBoardIdKey).toInt();
240
    if (firmwareBoardId != _boardId) {
Don Gagne's avatar
Don Gagne committed
241
        emit statusMessage(QString("Downloaded firmware board id does not match hardware board id: %1 != %2").arg(firmwareBoardId).arg(_boardId));
242 243
        return false;
    }
244 245 246 247

    // What firmware type is this?
    MAV_AUTOPILOT firmwareType = (MAV_AUTOPILOT)px4Json[_jsonMavAutopilotKey].toInt(MAV_AUTOPILOT_PX4);
    emit statusMessage(QString("MAV_AUTOPILOT = %1").arg(firmwareType));
248 249 250 251 252
    
    // Decompress the parameter xml and save to file
    QByteArray decompressedBytes;
    bool success = _decompressJsonValue(px4Json,               // JSON object
                                        bytes,                 // Raw bytes of JSON document
253 254
                                        _jsonParamXmlSizeKey,  // key which holds byte size
                                        _jsonParamXmlKey,      // key which holds compressed bytes
255 256
                                        decompressedBytes);    // Returned decompressed bytes
    if (success) {
257 258
        // Use settings location as our work directory, this way is something goes wrong the file is still there
        // sitting next to the cache files.
259 260
        QSettings settings;
        QDir parameterDir = QFileInfo(settings.fileName()).dir();
261
        QString parameterFilename = parameterDir.filePath("ParameterFactMetaData.xml");
262
        QFile parameterFile(parameterFilename);
263

264 265 266 267 268 269 270 271 272 273 274 275
        if (parameterFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
            qint64 bytesWritten = parameterFile.write(decompressedBytes);
            if (bytesWritten != decompressedBytes.count()) {
                emit statusMessage(QString("Write failed for parameter meta data file, error: %1").arg(parameterFile.errorString()));
                parameterFile.close();
                QFile::remove(parameterFilename);
            } else {
                parameterFile.close();
            }
        } else {
            emit statusMessage(QString("Unable to open parameter meta data file %1 for writing, error: %2").arg(parameterFilename).arg(parameterFile.errorString()));
        }
276 277

        // Cache this file with the system
278
        ParameterManager::cacheMetaDataFile(parameterFilename, firmwareType);
279
    }
280 281

    // Decompress the airframe xml and save to file
282 283 284 285 286
    success = _decompressJsonValue(px4Json,                         // JSON object
                                        bytes,                      // Raw bytes of JSON document
                                        _jsonAirframeXmlSizeKey,    // key which holds byte size
                                        _jsonAirframeXmlKey,        // key which holds compressed bytes
                                        decompressedBytes);         // Returned decompressed bytes
287 288 289 290 291
    if (success) {
        // We cache the airframe xml in the same location as settings and parameters
        QSettings settings;
        QDir airframeDir = QFileInfo(settings.fileName()).dir();
        QString airframeFilename = airframeDir.filePath("PX4AirframeFactMetaData.xml");
dogmaphobic's avatar
dogmaphobic committed
292
        //qDebug() << airframeFilename;
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308
        QFile airframeFile(airframeFilename);

        if (airframeFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
            qint64 bytesWritten = airframeFile.write(decompressedBytes);
            if (bytesWritten != decompressedBytes.count()) {
                // FIXME: What about these warnings?
                emit statusMessage(QString("Write failed for airframe meta data file, error: %1").arg(airframeFile.errorString()));
                airframeFile.close();
                QFile::remove(airframeFilename);
            } else {
                airframeFile.close();
            }
        } else {
            emit statusMessage(QString("Unable to open airframe meta data file %1 for writing, error: %2").arg(airframeFilename).arg(airframeFile.errorString()));
        }
    }
309 310 311 312 313
    
    // Decompress the image and save to file
    _imageSize = px4Json.value(QString("image_size")).toInt();
    success = _decompressJsonValue(px4Json,               // JSON object
                                   bytes,                 // Raw bytes of JSON document
314 315
                                   _jsonImageSizeKey,     // key which holds byte size
                                   _jsonImageKey,         // key which holds compressed bytes
316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331
                                   decompressedBytes);    // Returned decompressed bytes
    if (!success) {
        return false;
    }
    
    // Pad image to 4-byte boundary
    while ((decompressedBytes.count() % 4) != 0) {
        decompressedBytes.append(static_cast<char>(static_cast<unsigned char>(0xFF)));
    }
    
    // Store decompressed image file in same location as original download file
    QDir imageDir = QFileInfo(imageFilename).dir();
    QString decompressFilename = imageDir.filePath("PX4FlashUpgrade.bin");
    
    QFile decompressFile(decompressFilename);
    if (!decompressFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
Don Gagne's avatar
Don Gagne committed
332
        emit statusMessage(QString("Unable to open decompressed file %1 for writing, error: %2").arg(decompressFilename).arg(decompressFile.errorString()));
333 334 335 336 337
        return false;
    }
    
    qint64 bytesWritten = decompressFile.write(decompressedBytes);
    if (bytesWritten != decompressedBytes.count()) {
Don Gagne's avatar
Don Gagne committed
338
        emit statusMessage(QString("Write failed for decompressed image file, error: %1").arg(decompressFile.errorString()));
339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361
        return false;
    }
    decompressFile.close();
    
    _binFilename = decompressFilename;
    
    return true;
}

/// Decompress a set of bytes stored in a Json document.
bool FirmwareImage::_decompressJsonValue(const QJsonObject&	jsonObject,			///< JSON object
                                         const QByteArray&	jsonDocBytes,		///< Raw bytes of JSON document
                                         const QString&		sizeKey,			///< key which holds byte size
                                         const QString&		bytesKey,			///< key which holds compress bytes
                                         QByteArray&		decompressedBytes)	///< Returned decompressed bytes
{
    // Validate decompressed size key
    if (!jsonObject.contains(sizeKey)) {
        emit statusMessage(QString("Firmware file missing %1 key").arg(sizeKey));
        return false;
    }
    int decompressedSize = jsonObject.value(QString(sizeKey)).toInt();
    if (decompressedSize == 0) {
362
        emit statusMessage(QString("Firmware file has invalid decompressed size for %1").arg(sizeKey));
363 364 365 366 367 368 369 370 371 372 373
        return false;
    }
    
    // XXX Qt's JSON string handling is terribly broken, strings
    // with some length (18K / 25K) are just weirdly cut.
    // The code below works around this by manually 'parsing'
    // for the image string. Since its compressed / checksummed
    // this should be fine.
    
    QStringList parts = QString(jsonDocBytes).split(QString("\"%1\": \"").arg(bytesKey));
    if (parts.count() == 1) {
374
        emit statusMessage(QString("Could not find compressed bytes for %1 in Firmware file").arg(bytesKey));
375 376 377 378
        return false;
    }
    parts = parts.last().split("\"");
    if (parts.count() == 1) {
379
        emit statusMessage(QString("Incorrectly formed compressed bytes section for %1 in Firmware file").arg(bytesKey));
380 381 382 383 384 385 386 387 388 389 390 391 392 393 394
        return false;
    }
    
    // Store decompressed size as first four bytes. This is required by qUncompress routine.
    QByteArray raw;
    raw.append((unsigned char)((decompressedSize >> 24) & 0xFF));
    raw.append((unsigned char)((decompressedSize >> 16) & 0xFF));
    raw.append((unsigned char)((decompressedSize >> 8) & 0xFF));
    raw.append((unsigned char)((decompressedSize >> 0) & 0xFF));
    
    QByteArray raw64 = parts.first().toUtf8();
    raw.append(QByteArray::fromBase64(raw64));
    decompressedBytes = qUncompress(raw);
    
    if (decompressedBytes.count() == 0) {
395
        emit statusMessage(QString("Firmware file has 0 length %1").arg(bytesKey));
396 397 398
        return false;
    }
    if (decompressedBytes.count() != decompressedSize) {
399
        emit statusMessage(QString("Size for decompressed %1 does not match stored size: Expected(%1) Actual(%2)").arg(decompressedSize).arg(decompressedBytes.count()));
400 401 402
        return false;
    }
    
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
403
    emit statusMessage(QString("Successfully decompressed %1").arg(bytesKey));
404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430
    
    return true;
}

uint16_t FirmwareImage::ihxBlockCount(void) const
{
    return _ihxBlocks.count();
}

bool FirmwareImage::ihxGetBlock(uint16_t index, uint16_t& address, QByteArray& bytes) const
{
    address = 0;
    bytes.clear();
    
    if (index < ihxBlockCount()) {
        address = _ihxBlocks[index].address;
        bytes = _ihxBlocks[index].bytes;
        return true;
    } else {
        return false;
    }
}

bool FirmwareImage::_binLoad(const QString& imageFilename)
{
    QFile binFile(imageFilename);
    if (!binFile.open(QIODevice::ReadOnly)) {
Don Gagne's avatar
Don Gagne committed
431
        emit statusMessage(QString("Unabled to open firmware file %1, %2").arg(imageFilename).arg(binFile.errorString()));
432 433 434 435 436 437 438
        return false;
    }
    
    _imageSize = (uint32_t)binFile.size();
    
    binFile.close();
    
439 440
    _binFilename = imageFilename;
    
441 442
    return true;
}