PX4Bootloader.cc 15.8 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 28 29 30
/*=====================================================================
 
 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>

#include "PX4Bootloader.h"

#include <QFile>
dogmaphobic's avatar
dogmaphobic committed
31 32 33
#ifdef __android__
#include "qserialportinfo.h"
#else
34
#include <QSerialPortInfo>
dogmaphobic's avatar
dogmaphobic committed
35
#endif
36 37 38
#include <QDebug>
#include <QTime>

39 40
#include "QGC.h"

41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 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
static const quint32 crctab[] =
{
    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
    0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
    0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
    0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
    0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
    0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
    0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
    0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
    0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
    0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
    0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
    0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
    0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
    0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
    0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
    0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
    0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
    0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
    0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
    0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
    0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
    0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};

static quint32 crc32(const uint8_t *src, unsigned len, unsigned state)
{
    for (unsigned i = 0; i < len; i++) {
        state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
    }
    return state;
}

PX4Bootloader::PX4Bootloader(QObject *parent) :
    QObject(parent)
{

}

91
bool PX4Bootloader::write(QextSerialPort* port, const uint8_t* data, qint64 maxSize)
92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107
{
    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;
}

108
bool PX4Bootloader::write(QextSerialPort* port, const uint8_t byte)
109 110 111 112 113
{
    uint8_t buf[1] = { byte };
    return write(port, buf, 1);
}

Don Gagne's avatar
Don Gagne committed
114
bool PX4Bootloader::read(QextSerialPort* port, uint8_t* data, qint64 maxSize, int readTimeout)
115
{
116 117 118 119 120 121 122 123
    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
124 125
                return false;
            }
126
            QGC::SLEEP::usleep(100);
Don Gagne's avatar
Don Gagne committed
127 128 129
        }
        
        qint64 bytesRead;
130
        bytesRead = port->read((char*)&data[bytesAlreadyRead], maxSize);
Don Gagne's avatar
Don Gagne committed
131 132
        
        if (bytesRead == -1) {
Don Gagne's avatar
Don Gagne committed
133
            _errorString = tr("Read failed: error: %1").arg(port->errorString());
134
            return false;
Don Gagne's avatar
Don Gagne committed
135 136
        } else {
            Q_ASSERT(bytesRead != 0);
137
            bytesAlreadyRead += bytesRead;
138 139 140 141 142 143
        }
    }
    
    return true;
}

Don Gagne's avatar
Don Gagne committed
144
bool PX4Bootloader::getCommandResponse(QextSerialPort* port, int responseTimeout)
145 146 147
{
    uint8_t response[2];
    
Don Gagne's avatar
Don Gagne committed
148 149
    if (!read(port, response, 2, responseTimeout)) {
        _errorString.prepend("Get Command Response: ");
150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
        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;
}

171
bool PX4Bootloader::getBoardInfo(QextSerialPort* port, uint8_t param, uint32_t& value)
172 173 174 175
{
    uint8_t buf[3] = { PROTO_GET_DEVICE, param, PROTO_EOC };
    
    if (!write(port, buf, sizeof(buf))) {
Don Gagne's avatar
Don Gagne committed
176
        goto Error;
177
    }
178
    port->flush();
Don Gagne's avatar
Don Gagne committed
179 180
    if (!read(port, (uint8_t*)&value, sizeof(value))) {
        goto Error;
181
    }
Don Gagne's avatar
Don Gagne committed
182 183 184 185 186 187 188 189 190
    if (!getCommandResponse(port)) {
        goto Error;
    }
    
    return true;
    
Error:
    _errorString.prepend("Get Board Info: ");
    return false;
191 192
}

Don Gagne's avatar
Don Gagne committed
193
bool PX4Bootloader::sendCommand(QextSerialPort* port, const uint8_t cmd, int responseTimeout)
194 195 196 197
{
    uint8_t buf[2] = { cmd, PROTO_EOC };
    
    if (!write(port, buf, 2)) {
Don Gagne's avatar
Don Gagne committed
198
        goto Error;
199
    }
200
    port->flush();
Don Gagne's avatar
Don Gagne committed
201 202 203 204 205 206 207 208 209
    if (!getCommandResponse(port, responseTimeout)) {
        goto Error;
    }
    
    return true;

Error:
    _errorString.prepend("Send Command: ");
    return false;
210 211
}

212
bool PX4Bootloader::erase(QextSerialPort* port)
213 214
{
    // Erase is slow, need larger timeout
Don Gagne's avatar
Don Gagne committed
215
    if (!sendCommand(port, PROTO_CHIP_ERASE, _eraseTimeout)) {
216 217 218 219 220 221 222
        _errorString = tr("Board erase failed: %1").arg(_errorString);
        return false;
    }
    
    return true;
}

223
bool PX4Bootloader::program(QextSerialPort* port, const QString& firmwareFilename)
224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247
{
    QFile firmwareFile(firmwareFilename);
    if (!firmwareFile.open(QIODevice::ReadOnly)) {
        _errorString = tr("Unable to open firmware file %1: %2").arg(firmwareFilename).arg(firmwareFile.errorString());
        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
248
            _errorString = tr("Firmware file read failed: %1").arg(firmwareFile.errorString());
249 250 251 252 253
            return false;
        }
        
        Q_ASSERT(bytesToSend <= 0x8F);
        
254 255 256 257 258 259
        bool failed = true;
        if (write(port, PROTO_PROG_MULTI)) {
            if (write(port, (uint8_t)bytesToSend)) {
                if (write(port, imageBuf, bytesToSend)) {
                    if (write(port, PROTO_EOC)) {
                        port->flush();
Don Gagne's avatar
Don Gagne committed
260
                        if (getCommandResponse(port)) {
261 262 263 264 265 266 267
                            failed = false;
                        }
                    }
                }
            }
        }
        if (failed) {
268
            _errorString = tr("Flash failed: %1 at address 0x%2").arg(_errorString).arg(bytesSent, 8, 16, QLatin1Char('0'));
269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290
            return false;
        }
        
        bytesSent += bytesToSend;
        
        // Calculate the CRC now so we can test it after the board is flashed.
        _imageCRC = crc32((uint8_t *)imageBuf, bytesToSend, _imageCRC);
        
        emit updateProgramProgress(bytesSent, imageSize);
    }
    firmwareFile.close();
    
    // We calculate the CRC using the entire flash size, filling the remainder with 0xFF.
    while (bytesSent < _boardFlashSize) {
        const uint8_t fill = 0xFF;
        _imageCRC = crc32(&fill, 1, _imageCRC);
        bytesSent++;
    }
    
    return true;
}

291
bool PX4Bootloader::verify(QextSerialPort* port, const QString firmwareFilename)
292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
{
    bool ret;
    
    if (_bootloaderVersion <= 2) {
        ret = _bootloaderVerifyRev2(port, firmwareFilename);
    } else {
        ret = _bootloaderVerifyRev3(port);
    }
    
    sendBootloaderReboot(port);
    
    return ret;
}

/// @brief Verify the flash on bootloader version 2 by reading it back and comparing it against
/// the original firmware file.
308
bool PX4Bootloader::_bootloaderVerifyRev2(QextSerialPort* port, const QString firmwareFilename)
309 310 311 312 313 314 315 316
{
    QFile firmwareFile(firmwareFilename);
    if (!firmwareFile.open(QIODevice::ReadOnly)) {
        _errorString = tr("Unable to open firmware file %1: %2").arg(firmwareFilename).arg(firmwareFile.errorString());
        return false;
    }
    uint32_t imageSize = (uint32_t)firmwareFile.size();
    
Don Gagne's avatar
Don Gagne committed
317
    if (!sendCommand(port, PROTO_CHIP_VERIFY)) {
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336
        return false;
    }
    
    uint8_t fileBuf[READ_MULTI_MAX];
    uint8_t flashBuf[READ_MULTI_MAX];
    uint32_t bytesVerified = 0;
    
    Q_ASSERT(PROG_MULTI_MAX <= 0x8F);
    
    while (bytesVerified < imageSize) {
        int bytesToRead = imageSize - bytesVerified;
        if (bytesToRead > (int)sizeof(fileBuf)) {
            bytesToRead = (int)sizeof(fileBuf);
        }
        
        Q_ASSERT((bytesToRead % 4) == 0);
        
        int bytesRead = firmwareFile.read((char *)fileBuf, bytesToRead);
        if (bytesRead == -1 || bytesRead != bytesToRead) {
Don Gagne's avatar
Don Gagne committed
337
            _errorString = tr("Firmware file read failed: %1").arg(firmwareFile.errorString());
338 339 340 341 342
            return false;
        }
        
        Q_ASSERT(bytesToRead <= 0x8F);
        
343 344 345 346 347
        bool failed = true;
        if (write(port, PROTO_READ_MULTI)) {
            if (write(port, (uint8_t)bytesToRead)) {
                if (write(port, PROTO_EOC)) {
                    port->flush();
Don Gagne's avatar
Don Gagne committed
348 349
                    if (read(port, flashBuf, sizeof(flashBuf))) {
                        if (getCommandResponse(port)) {
350 351 352 353 354 355 356
                            failed = false;
                        }
                    }
                }
            }
        }
        if (failed) {
357
            _errorString = tr("Verify failed: %1 at address: 0x%2").arg(_errorString).arg(bytesVerified, 8, 16, QLatin1Char('0'));
358 359 360 361 362
            return false;
        }

        for (int i=0; i<bytesToRead; i++) {
            if (fileBuf[i] != flashBuf[i]) {
363
                _errorString = tr("Compare failed at %1: file(0x%2) flash(0x%3) at address: 0x%4").arg(bytesVerified + i).arg(fileBuf[i], 2, 16, QLatin1Char('0')).arg(flashBuf[i], 2, 16, QLatin1Char('0')).arg(bytesVerified, 8, 16, QLatin1Char('0'));
364 365 366 367 368 369 370 371 372 373 374 375
                return false;
            }
        }
        
        bytesVerified += bytesToRead;
    }
    firmwareFile.close();
    
    return true;
}

/// @Brief Verify the flash on a version 3 or higher bootloader board by comparing CRCs.
376
bool PX4Bootloader::_bootloaderVerifyRev3(QextSerialPort* port)
377 378 379 380
{
    uint8_t buf[2] = { PROTO_GET_CRC, PROTO_EOC };
    quint32 flashCRC;
    
381 382 383
    bool failed = true;
    if (write(port, buf, 2)) {
        port->flush();
Don Gagne's avatar
Don Gagne committed
384 385
        if (read(port, (uint8_t*)&flashCRC, sizeof(flashCRC), _verifyTimeout)) {
            if (getCommandResponse(port)) {
386 387 388 389 390
                failed = false;
            }
        }
    }
    if (failed) {
391 392 393 394 395 396 397 398 399 400 401
        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;
}

402
bool PX4Bootloader::open(QextSerialPort* port, const QString portName)
403 404 405 406
{
    Q_ASSERT(!port->isOpen());
    
    port->setPortName(portName);
407 408 409 410 411 412 413 414
    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());
415 416 417 418 419 420
        return false;
    }
    
    return true;
}

421
bool PX4Bootloader::sync(QextSerialPort* port)
422 423
{
    // Send sync command
Don Gagne's avatar
Don Gagne committed
424 425 426 427 428 429
    if (sendCommand(port, PROTO_GET_SYNC)) {
        return true;
    } else {
        _errorString.prepend("Sync: ");
        return false;
    }
430 431
}

432
bool PX4Bootloader::getBoardInfo(QextSerialPort* port, uint32_t& bootloaderVersion, uint32_t& boardID, uint32_t& flashSize)
433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458
{
    
    if (!getBoardInfo(port, INFO_BL_REV, _bootloaderVersion)) {
        goto Error;
    }
    if (_bootloaderVersion < BL_REV_MIN || _bootloaderVersion > BL_REV_MAX) {
        _errorString = tr("Found unsupported bootloader version: %1").arg(_bootloaderVersion);
        goto Error;
    }
    
    if (!getBoardInfo(port, INFO_BOARD_ID, _boardID)) {
        goto Error;
    }
    
    if (!getBoardInfo(port, INFO_FLASH_SIZE, _boardFlashSize)) {
        qWarning() << _errorString;
        goto Error;
    }
    
    bootloaderVersion = _bootloaderVersion;
    boardID = _boardID;
    flashSize = _boardFlashSize;
    
    return true;
    
Error:
Don Gagne's avatar
Don Gagne committed
459
    _errorString.prepend("Get Board Info: ");
460 461 462
    return false;
}

463
bool PX4Bootloader::sendBootloaderReboot(QextSerialPort* port)
464 465 466
{
    return write(port, PROTO_BOOT) && write(port, PROTO_EOC);
}