MockLinkFTP.cc

/****************************************************************************
 *
 * (c) 2009-2020 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.
 *
 ****************************************************************************/


#include "MockLinkFTP.h"
#include "MockLink.h"

const MockLinkFTP::ErrorMode_t MockLinkFTP::rgFailureModes[] = {
    MockLinkFTP::errModeNoResponse,
    MockLinkFTP::errModeNakResponse,
    MockLinkFTP::errModeNoSecondResponse,
    MockLinkFTP::errModeNakSecondResponse,
    MockLinkFTP::errModeBadSequence,
};
const size_t MockLinkFTP::cFailureModes = sizeof(MockLinkFTP::rgFailureModes) / sizeof(MockLinkFTP::rgFailureModes[0]);

const MockLinkFTP::FileTestCase MockLinkFTP::rgFileTestCases[MockLinkFTP::cFileTestCases] = {
    // File fits one Read Ack packet, partially filling data
    { "partial.qgc",    sizeof(((MavlinkFTP::Request*)0)->data) - 1,     1,    false},
    // File fits one Read Ack packet, exactly filling all data
    { "exact.qgc",      sizeof(((MavlinkFTP::Request*)0)->data),         1,    true },
    // File is larger than a single Read Ack packets, requires multiple Reads
    { "multi.qgc",      sizeof(((MavlinkFTP::Request*)0)->data) + 1,     2,    false },
};

MockLinkFTP::MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink)
    : _systemIdServer   (systemIdServer)
    , _componentIdServer(componentIdServer)
    , _mockLink         (mockLink)
{
    srand(0); // make sure unit tests are deterministic
}

void MockLinkFTP::ensureNullTemination(MavlinkFTP::Request* request)
{
    if (request->hdr.size < sizeof(request->data)) {
        request->data[request->hdr.size] = '\0';

    } else {
        request->data[sizeof(request->data)-1] = '\0';
    }
}

/// @brief Handles List command requests. Only supports root folder paths.
///         File list returned is set using the setFileList method.
void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    // FIXME: Does not support directories that span multiple packets
    
    MavlinkFTP::Request  ackResponse;
    QString                     path;
    uint16_t                    outgoingSeqNumber = _nextSeqNumber(seqNumber);

    ensureNullTemination(request);

    // We only support root path
    path = (char *)&request->data[0];
    if (!path.isEmpty() && path != "/") {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory);
        return;
    }
    
    // Offset requested is past the end of the list
    if (request->hdr.offset > (uint32_t)_fileList.size()) {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory);
        return;
    }
    
    ackResponse.hdr.opcode = MavlinkFTP::kRspAck;
    ackResponse.hdr.req_opcode = MavlinkFTP::kCmdListDirectory;
    ackResponse.hdr.session = 0;
    ackResponse.hdr.offset = request->hdr.offset;
    ackResponse.hdr.size = 0;

    if (request->hdr.offset == 0) {
        // Requesting first batch of file names
        Q_ASSERT(_fileList.size());
        char *bufPtr = (char *)&ackResponse.data[0];
        for (int i=0; i<_fileList.size(); i++) {
            strcpy(bufPtr, _fileList[i].toStdString().c_str());
            uint8_t cchFilename = static_cast<uint8_t>(strlen(bufPtr));
            Q_ASSERT(cchFilename);
            ackResponse.hdr.size += cchFilename + 1;
            bufPtr += cchFilename + 1;
        }

        _sendResponse(senderSystemId, senderComponentId, &ackResponse, outgoingSeqNumber);
    } else if (_errMode == errModeNakSecondResponse) {
        // Nak error all subsequent requests
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory);
        return;
    } else if (_errMode == errModeNoSecondResponse) {
        // No response for all subsequent requests
        return;
    } else {
        // FIXME: Does not support directories that span multiple packets
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory);
    }
}

void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    MavlinkFTP::Request response;
    QString             path;
    uint16_t            outgoingSeqNumber = _nextSeqNumber(seqNumber);
    QString             tmpFilename;
    
    ensureNullTemination(request);

    size_t cchPath = strnlen((char *)request->data, sizeof(request->data));
    Q_ASSERT(cchPath != sizeof(request->data));
    Q_UNUSED(cchPath); // Fix initialized-but-not-referenced warning on release builds
    path = (char *)request->data;

    _currentFile.close();
    
    // Check path against one of our known test cases
    for (const FileTestCase& testCase: rgFileTestCases) {
        if (path == testCase.filename) {
            tmpFilename = _createTestCaseTempFile(testCase);
            break;
        }
    }

    if (tmpFilename.isEmpty()) {
        if (path == "/version.json") {
            tmpFilename = ":MockLink/Version.MetaData.json";
        }
    }

    if (!tmpFilename.isEmpty()) {
        _currentFile.setFileName(tmpFilename);
        if (!_currentFile.open(QIODevice::ReadOnly)) {
            _sendNakErrno(senderSystemId, senderComponentId, _currentFile.error(), outgoingSeqNumber, MavlinkFTP::kCmdOpenFileRO);
            return;
        }
    } else {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFailFileNotFound, outgoingSeqNumber, MavlinkFTP::kCmdOpenFileRO);
        return;
    }
    
    response.hdr.opcode     = MavlinkFTP::kRspAck;
    response.hdr.req_opcode = MavlinkFTP::kCmdOpenFileRO;
    response.hdr.session    = _sessionId;
    
    // Data contains file length
    response.hdr.size = sizeof(uint32_t);
    response.openFileLength = _currentFile.size();
    
    _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
}

void MockLinkFTP::_readCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    MavlinkFTP::Request	response;
    uint16_t			outgoingSeqNumber = _nextSeqNumber(seqNumber);

    if (request->hdr.session != _sessionId) {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdReadFile);
        return;
    }
    
    uint32_t readOffset = request->hdr.offset;  // offset into file for reading

    if (readOffset != 0) {
        // If we get here it means the client is requesting additional data past the first request
        if (_errMode == errModeNakSecondResponse) {
            // Nak error all subsequent requests
            _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdReadFile);
            return;
        } else if (_errMode == errModeNoSecondResponse) {
            // No rsponse for all subsequent requests
            return;
        }
    }
    
    if (readOffset >= _currentFile.size()) {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdReadFile);
        return;
    }
    
    uint8_t cBytesToRead = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size() - readOffset);
    _currentFile.seek(readOffset);
    QByteArray bytes = _currentFile.read(cBytesToRead);
    memcpy(response.data, bytes.constData(), cBytesToRead);
    
    // We should always have written something, otherwise there is something wrong with the code above
    Q_ASSERT(cBytesToRead);
    
    response.hdr.session    = _sessionId;
    response.hdr.size       = cBytesToRead;
    response.hdr.offset     = request->hdr.offset;
    response.hdr.opcode     = MavlinkFTP::kRspAck;
    response.hdr.req_opcode = MavlinkFTP::kCmdReadFile;

    _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
}

void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    uint16_t                outgoingSeqNumber = _nextSeqNumber(seqNumber);
    MavlinkFTP::Request    response;

    if (request->hdr.session != _sessionId) {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile);
        return;
    }
    
    uint32_t readOffset = 0;	// offset into file for reading
    uint32_t ackOffset = 0;     // offset for ack
    uint8_t cDataAck;           // number of bytes in ack
    
    while (readOffset < _currentFile.size()) {
        cDataAck = 0;
        
        if (readOffset != 0) {
            // If we get here it means the client is requesting additional data past the first request
            if (_errMode == errModeNakSecondResponse) {
                // Nak error all subsequent requests
                _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile);
                return;
            } else if (_errMode == errModeNoSecondResponse) {
                // No response for all subsequent requests
                return;
            }
        }
        
        uint8_t cBytesToRead = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size());
        _currentFile.seek(readOffset);
        QByteArray bytes = _currentFile.read(cBytesToRead);
        memcpy(response.data, bytes.constData(), cBytesToRead);

        // We should always have written something, otherwise there is something wrong with the code above
        Q_ASSERT(cBytesToRead);
        
        response.hdr.session = _sessionId;
        response.hdr.size = cDataAck;
        response.hdr.offset = cBytesToRead;
        response.hdr.opcode = MavlinkFTP::kRspAck;
        response.hdr.req_opcode = MavlinkFTP::kCmdBurstReadFile;
        
        _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
        
        outgoingSeqNumber = _nextSeqNumber(outgoingSeqNumber);
        ackOffset += cDataAck;
    }
	
    _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile);
}

void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);

    if (request->hdr.session != _sessionId) {
        _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession);
        return;
    }
    
    _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession);
	
    emit terminateCommandReceived();
}

void MockLinkFTP::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber)
{
    uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
    
    _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdResetSessions);
    
    emit resetCommandReceived();
}

void MockLinkFTP::handleFTPMessage(const mavlink_message_t& message)
{
    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
        return;
    }
    
    MavlinkFTP::Request  ackResponse;

    mavlink_file_transfer_protocol_t requestFTP;
    mavlink_msg_file_transfer_protocol_decode(&message, &requestFTP);
    
    if (requestFTP.target_system != _systemIdServer) {
        return;
    }

    MavlinkFTP::Request* request = (MavlinkFTP::Request*)&requestFTP.payload[0];

	if (_randomDropsEnabled) {
	    if (rand() % 3 == 0) {
	        qDebug() << "FileServer: Random drop of incoming packet";
	        return;
	    }
	}

	if (_lastReplyValid && request->hdr.seqNumber + 1 == _lastReplySequence) {
	    // this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS
	    // resent the request
	    qDebug() << "FileServer: resending response";
	    _mockLink->respondWithMavlinkMessage(_lastReply);
	    return;
	}

    uint16_t incomingSeqNumber = request->hdr.seqNumber;
    uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
    
    if (request->hdr.opcode != MavlinkFTP::kCmdResetSessions && request->hdr.opcode != MavlinkFTP::kCmdTerminateSession) {
        if (_errMode == errModeNoResponse) {
            // Don't respond to any requests, this shold cause the client to eventually timeout waiting for the ack
            return;
        } else if (_errMode == errModeNakResponse) {
            // Nak all requests, the actual error send back doesn't really matter as long as it's an error
            _sendNak(message.sysid, message.compid, MavlinkFTP::kErrFail, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode);
            return;
        }
    }

    switch (request->hdr.opcode) {
        case MavlinkFTP::kCmdNone:
            // ignored, always acked
            ackResponse.hdr.opcode = MavlinkFTP::kRspAck;
            ackResponse.hdr.session = 0;
            ackResponse.hdr.size = 0;
            _sendResponse(message.sysid, message.compid, &ackResponse, outgoingSeqNumber);
            break;

        case MavlinkFTP::kCmdListDirectory:
            _listCommand(message.sysid, message.compid, request, incomingSeqNumber);
            break;
            
        case MavlinkFTP::kCmdOpenFileRO:
            _openCommand(message.sysid, message.compid, request, incomingSeqNumber);
            break;

        case MavlinkFTP::kCmdReadFile:
            _readCommand(message.sysid, message.compid, request, incomingSeqNumber);
            break;

        case MavlinkFTP::kCmdBurstReadFile:
            _burstReadCommand(message.sysid, message.compid, request, incomingSeqNumber);
            break;

        case MavlinkFTP::kCmdTerminateSession:
            _terminateCommand(message.sysid, message.compid, request, incomingSeqNumber);
            break;

        case MavlinkFTP::kCmdResetSessions:
            _resetCommand(message.sysid, message.compid, incomingSeqNumber);
            break;
            
        default:
            // nack for all NYI opcodes
            _sendNak(message.sysid, message.compid, MavlinkFTP::kErrUnknownCommand, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode);
            break;
    }
}

/// @brief Sends an Ack
void MockLinkFTP::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode)
{
    MavlinkFTP::Request ackResponse;
    
    ackResponse.hdr.opcode = MavlinkFTP::kRspAck;
	ackResponse.hdr.req_opcode = reqOpcode;
    ackResponse.hdr.session = 0;
    ackResponse.hdr.size = 0;
    
    _sendResponse(targetSystemId, targetComponentId, &ackResponse, seqNumber);
}

void MockLinkFTP::_sendNak(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::ErrorCode_t error, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode)
{
    MavlinkFTP::Request nakResponse;

    nakResponse.hdr.opcode      = MavlinkFTP::kRspNak;
    nakResponse.hdr.req_opcode  = reqOpcode;
    nakResponse.hdr.session     = 0;
    nakResponse.hdr.size        = 1;
    nakResponse.data[0]         = error;
    
    _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber);
}

void MockLinkFTP::_sendNakErrno(uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode)
{
    MavlinkFTP::Request nakResponse;

    nakResponse.hdr.opcode      = MavlinkFTP::kRspNak;
    nakResponse.hdr.req_opcode  = reqOpcode;
    nakResponse.hdr.session     = 0;
    nakResponse.hdr.size        = 2;
    nakResponse.data[0]         = MavlinkFTP::kErrFailErrno;
    nakResponse.data[1]         = nakErrno;

    _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber);
}

/// @brief Emits a Request through the messageReceived signal.
void MockLinkFTP::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request* request, uint16_t seqNumber)
{
    request->hdr.seqNumber = seqNumber;
    _lastReplySequence = seqNumber;
    _lastReplyValid = true;
    
    mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer,               // System ID
                                                 _componentIdServer,            // Component ID
                                                 _mockLink->mavlinkChannel(),
                                                 &_lastReply,                   // Mavlink Message to pack into
                                                 0,                             // Target network
                                                 targetSystemId,
                                                 targetComponentId,
                                                 (uint8_t*)request);            // Payload

	if (_randomDropsEnabled) {
	    if (rand() % 3 == 0) {
	        qDebug() << "FileServer: Random drop of outgoing packet";
	        return;
	    }
	}
    
    _mockLink->respondWithMavlinkMessage(_lastReply);
}

/// @brief Generates the next sequence number given an incoming sequence number. Handles generating
/// bad sequence numbers when errModeBadSequence is set.
uint16_t MockLinkFTP::_nextSeqNumber(uint16_t seqNumber)
{
    uint16_t outgoingSeqNumber = seqNumber + 1;
    
    if (_errMode == errModeBadSequence) {
        outgoingSeqNumber++;
    }
    return outgoingSeqNumber;
}

QString MockLinkFTP::_createTestCaseTempFile(const FileTestCase& testCase)
{
    QGCTemporaryFile tmpFile("MockLinkFTPTestCase");
    tmpFile.open(QIODevice::WriteOnly | QIODevice::Truncate);
    for (int i=0; i<testCase.length; i++) {
        tmpFile.write(QByteArray(1, i % 255));
    }
    tmpFile.close();
    return tmpFile.fileName();
}