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