Bootloader.cc 19.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009, 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

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

28 29
#include "Bootloader.h"
#include "QGCLoggingCategory.h"
30 31 32 33 34 35

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

36 37
#include "QGC.h"

38
Bootloader::Bootloader(QObject *parent) :
39 40 41 42 43
    QObject(parent)
{

}

44
bool Bootloader::_write(QextSerialPort* port, const uint8_t* data, qint64 maxSize)
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
{
    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;
}

61
bool Bootloader::_write(QextSerialPort* port, const uint8_t byte)
62 63
{
    uint8_t buf[1] = { byte };
64
    return _write(port, buf, 1);
65 66
}

67
bool Bootloader::_read(QextSerialPort* port, uint8_t* data, qint64 maxSize, int readTimeout)
68
{
69 70 71 72 73 74 75 76
    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
77 78
                return false;
            }
79
            QGC::SLEEP::usleep(100);
Don Gagne's avatar
Don Gagne committed
80 81 82
        }
        
        qint64 bytesRead;
83
        bytesRead = port->read((char*)&data[bytesAlreadyRead], maxSize);
Don Gagne's avatar
Don Gagne committed
84 85
        
        if (bytesRead == -1) {
Don Gagne's avatar
Don Gagne committed
86
            _errorString = tr("Read failed: error: %1").arg(port->errorString());
87
            return false;
Don Gagne's avatar
Don Gagne committed
88 89
        } else {
            Q_ASSERT(bytesRead != 0);
90
            bytesAlreadyRead += bytesRead;
91 92 93 94 95 96
        }
    }
    
    return true;
}

97 98 99
/// 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)
100 101 102
{
    uint8_t response[2];
    
103
    if (!_read(port, response, 2, responseTimeout)) {
Don Gagne's avatar
Don Gagne committed
104
        _errorString.prepend("Get Command Response: ");
105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125
        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;
    } 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;
}

126 127 128 129
/// 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)
130 131 132
{
    uint8_t buf[3] = { PROTO_GET_DEVICE, param, PROTO_EOC };
    
133
    if (!_write(port, buf, sizeof(buf))) {
Don Gagne's avatar
Don Gagne committed
134
        goto Error;
135
    }
136
    port->flush();
137
    if (!_read(port, (uint8_t*)&value, sizeof(value))) {
Don Gagne's avatar
Don Gagne committed
138
        goto Error;
139
    }
140
    if (!_getCommandResponse(port)) {
Don Gagne's avatar
Don Gagne committed
141 142 143 144 145 146 147 148
        goto Error;
    }
    
    return true;
    
Error:
    _errorString.prepend("Get Board Info: ");
    return false;
149 150
}

151 152 153 154
/// 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)
155 156 157
{
    uint8_t buf[2] = { cmd, PROTO_EOC };
    
158
    if (!_write(port, buf, 2)) {
Don Gagne's avatar
Don Gagne committed
159
        goto Error;
160
    }
161
    port->flush();
162
    if (!_getCommandResponse(port, responseTimeout)) {
Don Gagne's avatar
Don Gagne committed
163 164 165 166 167 168 169 170
        goto Error;
    }
    
    return true;

Error:
    _errorString.prepend("Send Command: ");
    return false;
171 172
}

173
bool Bootloader::erase(QextSerialPort* port)
174 175
{
    // Erase is slow, need larger timeout
176
    if (!_sendCommand(port, PROTO_CHIP_ERASE, _eraseTimeout)) {
177 178 179 180 181 182 183
        _errorString = tr("Board erase failed: %1").arg(_errorString);
        return false;
    }
    
    return true;
}

184
bool Bootloader::program(QextSerialPort* port, const FirmwareImage* image)
185
{
186 187 188 189 190 191 192 193 194 195
    if (image->imageIsBinFormat()) {
        return _binProgram(port, image);
    } else {
        return _ihxProgram(port, image);
    }
}

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

261 262 263 264 265 266 267 268 269 270 271 272 273 274 275
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
276
        qCDebug(FirmwareUpgradeVerboseLog) << QString("Bootloader::_ihxProgram - address:%1 size:%2 block:%3").arg(flashAddress).arg(bytes.count()).arg(index);
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 327 328 329 330 331 332 333 334 335 336
        
        // 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)
337 338 339
{
    bool ret;
    
340 341
    if (!image->imageIsBinFormat() || _bootloaderVersion <= 2) {
        ret = _verifyBytes(port, image);
342
    } else {
343
        ret = _verifyCRC(port);
344 345
    }
    
346
    reboot(port);
347 348 349 350
    
    return ret;
}

Don Gagne's avatar
Don Gagne committed
351
/// @brief Verify the flash on bootloader reading it back and comparing it against the original image
352
bool Bootloader::_verifyBytes(QextSerialPort* port, const FirmwareImage* image)
353
{
354 355 356 357 358 359 360 361 362 363 364 365
    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());
366
    if (!firmwareFile.open(QIODevice::ReadOnly)) {
367
        _errorString = tr("Unable to open firmware file %1: %2").arg(image->binFilename()).arg(firmwareFile.errorString());
368 369 370 371
        return false;
    }
    uint32_t imageSize = (uint32_t)firmwareFile.size();
    
372
    if (!_sendCommand(port, PROTO_CHIP_VERIFY)) {
373 374 375 376
        return false;
    }
    
    uint8_t fileBuf[READ_MULTI_MAX];
377
    uint8_t readBuf[READ_MULTI_MAX];
378 379 380 381 382 383
    uint32_t bytesVerified = 0;
    
    Q_ASSERT(PROG_MULTI_MAX <= 0x8F);
    
    while (bytesVerified < imageSize) {
        int bytesToRead = imageSize - bytesVerified;
384 385
        if (bytesToRead > (int)sizeof(readBuf)) {
            bytesToRead = (int)sizeof(readBuf);
386 387 388 389 390 391
        }
        
        Q_ASSERT((bytesToRead % 4) == 0);
        
        int bytesRead = firmwareFile.read((char *)fileBuf, bytesToRead);
        if (bytesRead == -1 || bytesRead != bytesToRead) {
Don Gagne's avatar
Don Gagne committed
392
            _errorString = tr("Firmware file read failed: %1").arg(firmwareFile.errorString());
393 394 395 396 397
            return false;
        }
        
        Q_ASSERT(bytesToRead <= 0x8F);
        
398
        bool failed = true;
399 400 401 402 403 404 405
        if (_write(port, PROTO_READ_MULTI) &&
            _write(port, (uint8_t)bytesToRead) &&
            _write(port, PROTO_EOC)) {
            port->flush();
            if (_read(port, readBuf, sizeof(readBuf))) {
                if (_getCommandResponse(port)) {
                    failed = false;
406 407 408 409
                }
            }
        }
        if (failed) {
410
            _errorString = tr("Read failed: %1 at address: 0x%2").arg(_errorString).arg(bytesVerified, 8, 16, QLatin1Char('0'));
411 412 413 414
            return false;
        }

        for (int i=0; i<bytesToRead; i++) {
415 416
            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'));
417 418 419 420 421
                return false;
            }
        }
        
        bytesVerified += bytesToRead;
422 423
        
        emit updateProgress(bytesVerified, imageSize);
424
    }
425
    
426 427 428 429 430
    firmwareFile.close();
    
    return true;
}

431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447
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
448
        qCDebug(FirmwareUpgradeLog) << QString("Bootloader::_ihxVerifyBytes - address:0x%1 size:%2 block:%3").arg(readAddress, 8, 16, QLatin1Char('0')).arg(imageBytes.count()).arg(index);
449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 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 511 512 513 514 515 516 517 518 519 520
        
        // 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;
            }
            
            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)
521 522 523 524
{
    uint8_t buf[2] = { PROTO_GET_CRC, PROTO_EOC };
    quint32 flashCRC;
    
525
    bool failed = true;
526
    if (_write(port, buf, 2)) {
527
        port->flush();
528 529
        if (_read(port, (uint8_t*)&flashCRC, sizeof(flashCRC), _verifyTimeout)) {
            if (_getCommandResponse(port)) {
530 531 532 533 534
                failed = false;
            }
        }
    }
    if (failed) {
535 536 537 538 539 540 541 542 543 544 545
        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;
}

546
bool Bootloader::open(QextSerialPort* port, const QString portName)
547 548 549 550
{
    Q_ASSERT(!port->isOpen());
    
    port->setPortName(portName);
551 552 553 554 555 556 557 558
    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());
559 560 561 562 563 564
        return false;
    }
    
    return true;
}

565
bool Bootloader::sync(QextSerialPort* port)
566 567
{
    // Send sync command
568
    if (_sendCommand(port, PROTO_GET_SYNC)) {
Don Gagne's avatar
Don Gagne committed
569 570 571 572 573
        return true;
    } else {
        _errorString.prepend("Sync: ");
        return false;
    }
574 575
}

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

607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635
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)
636
{
637
    return _write(port, PROTO_BOOT) && _write(port, PROTO_EOC);
638
}