Bootloader.cc 18.9 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

/// @file
///     @brief PX4 Bootloader Utility routines
///     @author Don Gagne <don@thegagnes.com>

15 16
#include "Bootloader.h"
#include "QGCLoggingCategory.h"
17 18 19 20 21 22

#include <QFile>
#include <QSerialPortInfo>
#include <QDebug>
#include <QTime>

23 24
#include "QGC.h"

25
Bootloader::Bootloader(QObject *parent) :
26 27 28 29 30
    QObject(parent)
{

}

31
bool Bootloader::_write(QextSerialPort* port, const uint8_t* data, qint64 maxSize)
32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
{
    qint64 bytesWritten = port->write((const char*)data, maxSize);
    if (bytesWritten == -1) {
        _errorString = tr("Write failed: %1").arg(port->errorString());
        qWarning() << _errorString;
        return false;
    }
    if (bytesWritten != maxSize) {
        _errorString = tr("Incorrect number of bytes returned for write: actual(%1) expected(%2)").arg(bytesWritten).arg(maxSize);
        qWarning() << _errorString;
        return false;
    }
    
    return true;
}

48
bool Bootloader::_write(QextSerialPort* port, const uint8_t byte)
49 50
{
    uint8_t buf[1] = { byte };
51
    return _write(port, buf, 1);
52 53
}

54
bool Bootloader::_read(QextSerialPort* port, uint8_t* data, qint64 maxSize, int readTimeout)
55
{
56 57 58 59 60 61 62 63
    qint64 bytesAlreadyRead = 0;
    
    while (bytesAlreadyRead < maxSize) {
        QTime timeout;
        timeout.start();
        while (port->bytesAvailable() < 1) {
            if (timeout.elapsed() > readTimeout) {
                _errorString = tr("Timeout waiting for bytes to be available");
Don Gagne's avatar
Don Gagne committed
64 65
                return false;
            }
66
            QGC::SLEEP::usleep(100);
Don Gagne's avatar
Don Gagne committed
67 68 69
        }
        
        qint64 bytesRead;
70
        bytesRead = port->read((char*)&data[bytesAlreadyRead], maxSize);
Don Gagne's avatar
Don Gagne committed
71 72
        
        if (bytesRead == -1) {
Don Gagne's avatar
Don Gagne committed
73
            _errorString = tr("Read failed: error: %1").arg(port->errorString());
74
            return false;
Don Gagne's avatar
Don Gagne committed
75 76
        } else {
            Q_ASSERT(bytesRead != 0);
77
            bytesAlreadyRead += bytesRead;
78 79 80 81 82 83
        }
    }
    
    return true;
}

84 85 86
/// Read a PROTO_SYNC command response from the bootloader
///     @param responseTimeout Msecs to wait for response bytes to become available on port
bool Bootloader::_getCommandResponse(QextSerialPort* port, int responseTimeout)
87 88 89
{
    uint8_t response[2];
    
90
    if (!_read(port, response, 2, responseTimeout)) {
Don Gagne's avatar
Don Gagne committed
91
        _errorString.prepend("Get Command Response: ");
92 93 94 95 96 97 98
        return false;
    }
    
    // Make sure we get a good sync response
    if (response[0] != PROTO_INSYNC) {
        _errorString = tr("Invalid sync response: 0x%1 0x%2").arg(response[0], 2, 16, QLatin1Char('0')).arg(response[1], 2, 16, QLatin1Char('0'));
        return false;
99 100 101
    } else if (response[0] == PROTO_INSYNC && response[1] == PROTO_BAD_SILICON_REV) {
        _errorString = tr("This board is using a microcontroller with faulty silicon and an incorrect configuration and should be put out of service.");
        return false;
102 103 104 105 106 107 108 109 110 111 112 113 114 115
    } else if (response[1] != PROTO_OK) {
        QString responseCode = tr("Unknown response code");
        if (response[1] == PROTO_FAILED) {
            responseCode = "PROTO_FAILED";
        } else if (response[1] == PROTO_INVALID) {
            responseCode = "PROTO_INVALID";
        }
        _errorString = tr("Command failed: 0x%1 (%2)").arg(response[1], 2, 16, QLatin1Char('0')).arg(responseCode);
        return false;
    }
    
    return true;
}

116 117 118 119
/// Send a PROTO_GET_DEVICE command to retrieve a value from the PX4 bootloader
///     @param param Value to retrieve using INFO_BOARD_* enums
///     @param value Returned value
bool Bootloader::_getPX4BoardInfo(QextSerialPort* port, uint8_t param, uint32_t& value)
120 121 122
{
    uint8_t buf[3] = { PROTO_GET_DEVICE, param, PROTO_EOC };
    
123
    if (!_write(port, buf, sizeof(buf))) {
Don Gagne's avatar
Don Gagne committed
124
        goto Error;
125
    }
126
    port->flush();
127
    if (!_read(port, (uint8_t*)&value, sizeof(value))) {
Don Gagne's avatar
Don Gagne committed
128
        goto Error;
129
    }
130
    if (!_getCommandResponse(port)) {
Don Gagne's avatar
Don Gagne committed
131 132 133 134 135 136 137 138
        goto Error;
    }
    
    return true;
    
Error:
    _errorString.prepend("Get Board Info: ");
    return false;
139 140
}

141 142 143 144
/// Send a command to the bootloader
///     @param cmd Command to send using PROTO_* enums
/// @return true: Command sent and valid sync response returned
bool Bootloader::_sendCommand(QextSerialPort* port, const uint8_t cmd, int responseTimeout)
145 146 147
{
    uint8_t buf[2] = { cmd, PROTO_EOC };
    
148
    if (!_write(port, buf, 2)) {
Don Gagne's avatar
Don Gagne committed
149
        goto Error;
150
    }
151
    port->flush();
152
    if (!_getCommandResponse(port, responseTimeout)) {
Don Gagne's avatar
Don Gagne committed
153 154 155 156 157 158 159 160
        goto Error;
    }
    
    return true;

Error:
    _errorString.prepend("Send Command: ");
    return false;
161 162
}

163
bool Bootloader::erase(QextSerialPort* port)
164 165
{
    // Erase is slow, need larger timeout
166
    if (!_sendCommand(port, PROTO_CHIP_ERASE, _eraseTimeout)) {
167 168 169 170 171 172 173
        _errorString = tr("Board erase failed: %1").arg(_errorString);
        return false;
    }
    
    return true;
}

174
bool Bootloader::program(QextSerialPort* port, const FirmwareImage* image)
175
{
176 177 178 179 180 181 182 183 184 185
    if (image->imageIsBinFormat()) {
        return _binProgram(port, image);
    } else {
        return _ihxProgram(port, image);
    }
}

bool Bootloader::_binProgram(QextSerialPort* port, const FirmwareImage* image)
{
    QFile firmwareFile(image->binFilename());
186
    if (!firmwareFile.open(QIODevice::ReadOnly)) {
187
        _errorString = tr("Unable to open firmware file %1: %2").arg(image->binFilename()).arg(firmwareFile.errorString());
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
        return false;
    }
    uint32_t imageSize = (uint32_t)firmwareFile.size();
    
    uint8_t imageBuf[PROG_MULTI_MAX];
    uint32_t bytesSent = 0;
    _imageCRC = 0;
    
    Q_ASSERT(PROG_MULTI_MAX <= 0x8F);
    
    while (bytesSent < imageSize) {
        int bytesToSend = imageSize - bytesSent;
        if (bytesToSend > (int)sizeof(imageBuf)) {
            bytesToSend = (int)sizeof(imageBuf);
        }
        
        Q_ASSERT((bytesToSend % 4) == 0);
        
        int bytesRead = firmwareFile.read((char *)imageBuf, bytesToSend);
        if (bytesRead == -1 || bytesRead != bytesToSend) {
Don Gagne's avatar
Don Gagne committed
208
            _errorString = tr("Firmware file read failed: %1").arg(firmwareFile.errorString());
209 210 211 212 213
            return false;
        }
        
        Q_ASSERT(bytesToSend <= 0x8F);
        
214
        bool failed = true;
215 216 217 218
        if (_write(port, PROTO_PROG_MULTI)) {
            if (_write(port, (uint8_t)bytesToSend)) {
                if (_write(port, imageBuf, bytesToSend)) {
                    if (_write(port, PROTO_EOC)) {
219
                        port->flush();
220
                        if (_getCommandResponse(port)) {
221 222 223 224 225 226 227
                            failed = false;
                        }
                    }
                }
            }
        }
        if (failed) {
228
            _errorString = tr("Flash failed: %1 at address 0x%2").arg(_errorString).arg(bytesSent, 8, 16, QLatin1Char('0'));
229 230 231 232 233 234
            return false;
        }
        
        bytesSent += bytesToSend;
        
        // Calculate the CRC now so we can test it after the board is flashed.
235
        _imageCRC = QGC::crc32((uint8_t *)imageBuf, bytesToSend, _imageCRC);
236
        
237
        emit updateProgress(bytesSent, imageSize);
238 239 240 241 242 243
    }
    firmwareFile.close();
    
    // We calculate the CRC using the entire flash size, filling the remainder with 0xFF.
    while (bytesSent < _boardFlashSize) {
        const uint8_t fill = 0xFF;
244
        _imageCRC = QGC::crc32(&fill, 1, _imageCRC);
245 246 247 248 249 250
        bytesSent++;
    }
    
    return true;
}

251 252 253 254 255 256 257 258 259 260 261 262 263 264 265
bool Bootloader::_ihxProgram(QextSerialPort* port, const FirmwareImage* image)
{
    uint32_t imageSize = image->imageSize();
    uint32_t bytesSent = 0;

    for (uint16_t index=0; index<image->ihxBlockCount(); index++) {
        bool        failed;
        uint16_t    flashAddress;
        QByteArray  bytes;
        
        if (!image->ihxGetBlock(index, flashAddress, bytes)) {
            _errorString = QString("Unable to retrieve block from ihx: index %1").arg(index);
            return false;
        }
        
Don Gagne's avatar
Don Gagne committed
266
        qCDebug(FirmwareUpgradeVerboseLog) << QString("Bootloader::_ihxProgram - address:0x%1 size:%2 block:%3").arg(flashAddress, 8, 16, QLatin1Char('0')).arg(bytes.count()).arg(index);
267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326
        
        // Set flash address
        
        failed = true;
        if (_write(port, PROTO_LOAD_ADDRESS) &&
                _write(port, flashAddress & 0xFF) &&
                _write(port, (flashAddress >> 8) & 0xFF) &&
                _write(port, PROTO_EOC)) {
            port->flush();
            if (_getCommandResponse(port)) {
                failed = false;
            }
        }
        
        if (failed) {
            _errorString = QString("Unable to set flash start address: 0x%2").arg(flashAddress, 8, 16, QLatin1Char('0'));
            return false;
        }
        
        // Flash
        
        int bytesIndex = 0;
        uint16_t bytesLeftToWrite = bytes.count();
        
        while (bytesLeftToWrite > 0) {
            uint8_t bytesToWrite;
        
            if (bytesLeftToWrite > PROG_MULTI_MAX) {
                bytesToWrite = PROG_MULTI_MAX;
            } else {
                bytesToWrite = bytesLeftToWrite;
            }
        
            failed = true;
            if (_write(port, PROTO_PROG_MULTI) &&
                    _write(port, bytesToWrite) &&
                    _write(port, &((uint8_t *)bytes.data())[bytesIndex], bytesToWrite) &&
                    _write(port, PROTO_EOC)) {
                port->flush();
                if (_getCommandResponse(port)) {
                    failed = false;
                }
            }
            if (failed) {
                _errorString = QString("Flash failed: %1 at address 0x%2").arg(_errorString).arg(flashAddress, 8, 16, QLatin1Char('0'));
                return false;
            }
            
            bytesIndex += bytesToWrite;
            bytesLeftToWrite -= bytesToWrite;
            bytesSent += bytesToWrite;
            
            emit updateProgress(bytesSent, imageSize);
        }
    }
    
    return true;
}

bool Bootloader::verify(QextSerialPort* port, const FirmwareImage* image)
327 328 329
{
    bool ret;
    
330 331
    if (!image->imageIsBinFormat() || _bootloaderVersion <= 2) {
        ret = _verifyBytes(port, image);
332
    } else {
333
        ret = _verifyCRC(port);
334 335
    }
    
336
    reboot(port);
337 338 339 340
    
    return ret;
}

Don Gagne's avatar
Don Gagne committed
341
/// @brief Verify the flash on bootloader reading it back and comparing it against the original image
342
bool Bootloader::_verifyBytes(QextSerialPort* port, const FirmwareImage* image)
343
{
344 345 346 347 348 349 350 351 352 353 354 355
    if (image->imageIsBinFormat()) {
        return _binVerifyBytes(port, image);
    } else {
        return _ihxVerifyBytes(port, image);
    }
}

bool Bootloader::_binVerifyBytes(QextSerialPort* port, const FirmwareImage* image)
{
    Q_ASSERT(image->imageIsBinFormat());
    
    QFile firmwareFile(image->binFilename());
356
    if (!firmwareFile.open(QIODevice::ReadOnly)) {
357
        _errorString = tr("Unable to open firmware file %1: %2").arg(image->binFilename()).arg(firmwareFile.errorString());
358 359 360 361
        return false;
    }
    uint32_t imageSize = (uint32_t)firmwareFile.size();
    
362
    if (!_sendCommand(port, PROTO_CHIP_VERIFY)) {
363 364 365 366
        return false;
    }
    
    uint8_t fileBuf[READ_MULTI_MAX];
367
    uint8_t readBuf[READ_MULTI_MAX];
368 369 370 371 372 373
    uint32_t bytesVerified = 0;
    
    Q_ASSERT(PROG_MULTI_MAX <= 0x8F);
    
    while (bytesVerified < imageSize) {
        int bytesToRead = imageSize - bytesVerified;
374 375
        if (bytesToRead > (int)sizeof(readBuf)) {
            bytesToRead = (int)sizeof(readBuf);
376 377 378 379 380 381
        }
        
        Q_ASSERT((bytesToRead % 4) == 0);
        
        int bytesRead = firmwareFile.read((char *)fileBuf, bytesToRead);
        if (bytesRead == -1 || bytesRead != bytesToRead) {
Don Gagne's avatar
Don Gagne committed
382
            _errorString = tr("Firmware file read failed: %1").arg(firmwareFile.errorString());
383 384 385 386 387
            return false;
        }
        
        Q_ASSERT(bytesToRead <= 0x8F);
        
388
        bool failed = true;
389 390 391 392
        if (_write(port, PROTO_READ_MULTI) &&
            _write(port, (uint8_t)bytesToRead) &&
            _write(port, PROTO_EOC)) {
            port->flush();
Don Gagne's avatar
Don Gagne committed
393
            if (_read(port, readBuf, bytesToRead)) {
394 395
                if (_getCommandResponse(port)) {
                    failed = false;
396 397 398 399
                }
            }
        }
        if (failed) {
400
            _errorString = tr("Read failed: %1 at address: 0x%2").arg(_errorString).arg(bytesVerified, 8, 16, QLatin1Char('0'));
401 402 403 404
            return false;
        }

        for (int i=0; i<bytesToRead; i++) {
405 406
            if (fileBuf[i] != readBuf[i]) {
                _errorString = tr("Compare failed: expected(0x%1) actual(0x%2) at address: 0x%3").arg(fileBuf[i], 2, 16, QLatin1Char('0')).arg(readBuf[i], 2, 16, QLatin1Char('0')).arg(bytesVerified + i, 8, 16, QLatin1Char('0'));
407 408 409 410 411
                return false;
            }
        }
        
        bytesVerified += bytesToRead;
412 413
        
        emit updateProgress(bytesVerified, imageSize);
414
    }
415
    
416 417 418 419 420
    firmwareFile.close();
    
    return true;
}

421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437
bool Bootloader::_ihxVerifyBytes(QextSerialPort* port, const FirmwareImage* image)
{
    Q_ASSERT(!image->imageIsBinFormat());
    
    uint32_t imageSize = image->imageSize();
    uint32_t bytesVerified = 0;
    
    for (uint16_t index=0; index<image->ihxBlockCount(); index++) {
        bool        failed;
        uint16_t    readAddress;
        QByteArray  imageBytes;
        
        if (!image->ihxGetBlock(index, readAddress, imageBytes)) {
            _errorString = QString("Unable to retrieve block from ihx: index %1").arg(index);
            return false;
        }
        
Don Gagne's avatar
Don Gagne committed
438
        qCDebug(FirmwareUpgradeLog) << QString("Bootloader::_ihxVerifyBytes - address:0x%1 size:%2 block:%3").arg(readAddress, 8, 16, QLatin1Char('0')).arg(imageBytes.count()).arg(index);
439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471
        
        // Set read address
        
        failed = true;
        if (_write(port, PROTO_LOAD_ADDRESS) &&
            _write(port, readAddress & 0xFF) &&
            _write(port, (readAddress >> 8) & 0xFF) &&
            _write(port, PROTO_EOC)) {
            port->flush();
            if (_getCommandResponse(port)) {
                failed = false;
            }
        }
        
        if (failed) {
            _errorString = QString("Unable to set read start address: 0x%2").arg(readAddress, 8, 16, QLatin1Char('0'));
            return false;
        }
        
        // Read back
        
        int         bytesIndex = 0;
        uint16_t    bytesLeftToRead = imageBytes.count();
        
        while (bytesLeftToRead > 0) {
            uint8_t bytesToRead;
            uint8_t readBuf[READ_MULTI_MAX];
            
            if (bytesLeftToRead > READ_MULTI_MAX) {
                bytesToRead = READ_MULTI_MAX;
            } else {
                bytesToRead = bytesLeftToRead;
            }
Don Gagne's avatar
Don Gagne committed
472

473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510
            failed = true;
            if (_write(port, PROTO_READ_MULTI) &&
                _write(port, bytesToRead) &&
                _write(port, PROTO_EOC)) {
                port->flush();
                if (_read(port, readBuf, bytesToRead)) {
                    if (_getCommandResponse(port)) {
                        failed = false;
                    }
                }
            }
            if (failed) {
                _errorString = tr("Read failed: %1 at address: 0x%2").arg(_errorString).arg(readAddress, 8, 16, QLatin1Char('0'));
                return false;
            }
            
            // Compare
            
            for (int i=0; i<bytesToRead; i++) {
                if ((uint8_t)imageBytes[bytesIndex + i] != readBuf[i]) {
                    _errorString = QString("Compare failed: expected(0x%1) actual(0x%2) at address: 0x%3").arg(imageBytes[bytesIndex + i], 2, 16, QLatin1Char('0')).arg(readBuf[i], 2, 16, QLatin1Char('0')).arg(readAddress + i, 8, 16, QLatin1Char('0'));
                    return false;
                }
            }
            
            bytesVerified += bytesToRead;
            bytesIndex += bytesToRead;
            bytesLeftToRead -= bytesToRead;
            
            emit updateProgress(bytesVerified, imageSize);
        }
    }
    
    return true;
}

/// @Brief Verify the flash by comparing CRCs.
bool Bootloader::_verifyCRC(QextSerialPort* port)
511 512 513 514
{
    uint8_t buf[2] = { PROTO_GET_CRC, PROTO_EOC };
    quint32 flashCRC;
    
515
    bool failed = true;
516
    if (_write(port, buf, 2)) {
517
        port->flush();
518 519
        if (_read(port, (uint8_t*)&flashCRC, sizeof(flashCRC), _verifyTimeout)) {
            if (_getCommandResponse(port)) {
520 521 522 523 524
                failed = false;
            }
        }
    }
    if (failed) {
525 526 527 528 529 530 531 532 533 534 535
        return false;
    }

    if (_imageCRC != flashCRC) {
        _errorString = tr("CRC mismatch: board(0x%1) file(0x%2)").arg(flashCRC, 4, 16, QLatin1Char('0')).arg(_imageCRC, 4, 16, QLatin1Char('0'));
        return false;
    }
    
    return true;
}

536
bool Bootloader::open(QextSerialPort* port, const QString portName)
537 538 539 540
{
    Q_ASSERT(!port->isOpen());
    
    port->setPortName(portName);
541 542 543 544 545 546 547 548
    port->setBaudRate(BAUD115200);
    port->setDataBits(DATA_8);
    port->setParity(PAR_NONE);
    port->setStopBits(STOP_1);
    port->setFlowControl(FLOW_OFF);
    
    if (!port->open(QIODevice::ReadWrite | QIODevice::Unbuffered)) {
        _errorString = tr("Open failed on port %1: %2").arg(portName).arg(port->errorString());
549 550 551 552 553 554
        return false;
    }
    
    return true;
}

555
bool Bootloader::sync(QextSerialPort* port)
556 557
{
    // Send sync command
558
    if (_sendCommand(port, PROTO_GET_SYNC)) {
Don Gagne's avatar
Don Gagne committed
559 560 561 562 563
        return true;
    } else {
        _errorString.prepend("Sync: ");
        return false;
    }
564 565
}

566
bool Bootloader::getPX4BoardInfo(QextSerialPort* port, uint32_t& bootloaderVersion, uint32_t& boardID, uint32_t& flashSize)
567 568
{
    
569
    if (!_getPX4BoardInfo(port, INFO_BL_REV, _bootloaderVersion)) {
570 571 572 573 574 575 576
        goto Error;
    }
    if (_bootloaderVersion < BL_REV_MIN || _bootloaderVersion > BL_REV_MAX) {
        _errorString = tr("Found unsupported bootloader version: %1").arg(_bootloaderVersion);
        goto Error;
    }
    
577
    if (!_getPX4BoardInfo(port, INFO_BOARD_ID, _boardID)) {
578 579 580
        goto Error;
    }
    
581
    if (!_getPX4BoardInfo(port, INFO_FLASH_SIZE, _boardFlashSize)) {
582 583 584 585 586 587 588 589 590 591 592
        qWarning() << _errorString;
        goto Error;
    }
    
    bootloaderVersion = _bootloaderVersion;
    boardID = _boardID;
    flashSize = _boardFlashSize;
    
    return true;
    
Error:
Don Gagne's avatar
Don Gagne committed
593
    _errorString.prepend("Get Board Info: ");
594 595 596
    return false;
}

597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625
bool Bootloader::get3DRRadioBoardId(QextSerialPort* port, uint32_t& boardID)
{
    uint8_t buf[2] = { PROTO_GET_DEVICE, PROTO_EOC };
    
    if (!_write(port, buf, sizeof(buf))) {
        goto Error;
    }
    port->flush();
    
    if (!_read(port, (uint8_t*)buf, 2)) {
        goto Error;
    }
    if (!_getCommandResponse(port)) {
        goto Error;
    }
    
    boardID = buf[0];
    
    _bootloaderVersion = 0;
    _boardFlashSize = 0;
    
    return true;
    
Error:
    _errorString.prepend("Get Board Id: ");
    return false;
}

bool Bootloader::reboot(QextSerialPort* port)
626
{
627
    return _write(port, PROTO_BOOT) && _write(port, PROTO_EOC);
628
}