6#include <QtCore/QTemporaryFile>
12 , _systemIdServer(systemIdServer)
13 , _componentIdServer(componentIdServer)
24void MockLinkFTP::ensureNullTemination(MavlinkFTP::Request *request)
26 if (request->hdr.size <
sizeof(request->data)) {
27 request->data[request->hdr.size] =
'\0';
29 request->data[
sizeof(request->data) - 1] =
'\0';
33void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
35 MavlinkFTP::Request ackResponse{};
36 ensureNullTemination(request);
38 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
41 const QString path =
reinterpret_cast<char*
>(&request->data[0]);
42 if (!path.isEmpty() && path !=
"/") {
47 if (request->hdr.offset > 0) {
68 ackResponse.hdr.session = 0;
69 ackResponse.hdr.offset = request->hdr.offset;
70 ackResponse.hdr.size = 0;
73 if (request->hdr.offset <= 5) {
74 char *bufPtr =
reinterpret_cast<char*
>(&ackResponse.data[0]);
75 QString dirEntry = QStringLiteral(
"Ffile%1.txt").arg(request->hdr.offset);
76 auto cchDirEntry = dirEntry.length();
77 (void) strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry);
78 ackResponse.hdr.size += dirEntry.length() + 1;
79 bufPtr += cchDirEntry + 1;
80 dirEntry = QStringLiteral(
"Ffile%1.txt").arg(request->hdr.offset + 1);
81 cchDirEntry = dirEntry.length();
82 (void) strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry);
83 ackResponse.hdr.size += dirEntry.length() + 1;
87 ackResponse.hdr.size = 1;
90 _sendResponse(senderSystemId, senderComponentId, &ackResponse, outgoingSeqNumber);
93void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
95 MavlinkFTP::Request response{};
96 ensureNullTemination(request);
97 const QString path =
reinterpret_cast<char*
>(request->data);
99 _uploadSession.reset();
101 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
103 const size_t cchPath = strnlen(
reinterpret_cast<char*
>(request->data),
sizeof(request->data));
104 Q_ASSERT(cchPath !=
sizeof(request->data));
107 _currentFile.close();
111 if (path.startsWith(sizePrefix)) {
112 const QString sizeString = path.right(path.length() - sizePrefix.length());
113 tmpFilename = _createTestTempFile(sizeString.toInt());
114 }
else if (path ==
"/general.json") {
115 tmpFilename = QStringLiteral(
":MockLink/General.MetaData.json");
116 }
else if (path ==
"/general.json.xz") {
117 tmpFilename = QStringLiteral(
":MockLink/General.MetaData.json.xz");
118 }
else if (path ==
"/parameter.json") {
119 tmpFilename = QStringLiteral(
":MockLink/Parameter.MetaData.json");
120 }
else if (path ==
"/parameter.json.xz") {
121 tmpFilename = QStringLiteral(
":MockLink/Parameter.MetaData.json.xz");
122 }
else if (_BinParamFileEnabled && (path ==
"@PARAM/param.pck")) {
123 tmpFilename =
":MockLink/Arduplane.params.ftp.bin";
126 if (!tmpFilename.isEmpty()) {
127 _currentFile.setFileName(tmpFilename);
128 if (!_currentFile.open(QIODevice::ReadOnly)) {
139 response.hdr.session = _sessionId;
142 response.hdr.size =
sizeof(uint32_t);
145 response.openFileLength = ((path ==
"@PARAM/param.pck") ? qPow(1024, 2) : _currentFile.size());
147 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
150void MockLinkFTP::_createFileCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
152 ensureNullTemination(request);
154 const QString path =
reinterpret_cast<char*
>(request->data);
155 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
157 if (path.isEmpty()) {
162 _uploadSession.reset();
163 _uploadSession.active =
true;
164 _uploadSession.remotePath = path;
166 MavlinkFTP::Request response{};
169 response.hdr.session = _sessionId;
170 response.hdr.size = 0;
172 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
175void MockLinkFTP::_openFileWOCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
177 ensureNullTemination(request);
179 const QString path =
reinterpret_cast<char*
>(request->data);
180 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
182 if (path.isEmpty()) {
187 _uploadSession.reset();
188 _uploadSession.active =
true;
189 _uploadSession.remotePath = path;
191 MavlinkFTP::Request response{};
194 response.hdr.session = _sessionId;
195 response.hdr.size = 0;
197 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
200void MockLinkFTP::_readCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
202 MavlinkFTP::Request response{};
203 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
205 if (request->hdr.session != _sessionId) {
210 const uint32_t readOffset = request->hdr.offset;
211 if (readOffset != 0) {
225 if (readOffset >= _currentFile.size()) {
230 const uint8_t cBytesToRead =
static_cast<uint8_t
>(qMin(
static_cast<qint64
>(
sizeof(response.data)), _currentFile.size() - readOffset));
231 (void) _currentFile.seek(readOffset);
232 const QByteArray bytes = _currentFile.read(cBytesToRead);
233 (void) memcpy(response.data, bytes.constData(), cBytesToRead);
236 Q_ASSERT(cBytesToRead);
238 response.hdr.session = _sessionId;
239 response.hdr.size = cBytesToRead;
240 response.hdr.offset = request->hdr.offset;
244 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
247void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
249 MavlinkFTP::Request response{};
250 uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
252 if (request->hdr.session != _sessionId) {
257 constexpr int burstMax = 10;
259 uint32_t burstOffset = request->hdr.offset;
261 while ((burstOffset < _currentFile.size()) && (burstCount++ < burstMax)) {
262 _currentFile.seek(burstOffset);
264 const uint8_t cBytes =
static_cast<uint8_t
>(qMin(
static_cast<qint64
>(
sizeof(response.data)), _currentFile.size() - burstOffset));
265 const QByteArray bytes = _currentFile.read(cBytes);
268 (void) memcpy(response.data, bytes.constData(), cBytes);
270 response.hdr.session = _sessionId;
271 response.hdr.size = cBytes;
272 response.hdr.offset = burstOffset;
275 response.hdr.burstComplete = (burstCount == burstMax) ? 1 : 0;
277 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
279 outgoingSeqNumber = _nextSeqNumber(outgoingSeqNumber);
280 burstOffset += cBytes;
283 if (burstOffset >= _currentFile.size()) {
289void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
291 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
293 if (request->hdr.session != _sessionId) {
298 _currentFile.close();
301 _finalizeActiveUpload();
306void MockLinkFTP::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber)
308 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
310 _currentFile.close();
313 _finalizeActiveUpload();
318void MockLinkFTP::_writeCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
320 const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
322 if ((request->hdr.session != _sessionId) || !_uploadSession.active) {
327 if (request->hdr.offset >
static_cast<uint32_t
>(_uploadSession.buffer.size())) {
332 if (request->hdr.offset != 0) {
348 const uint32_t bytesToWrite = request->hdr.size;
349 if (bytesToWrite >
sizeof(request->data)) {
354 const uint32_t requiredSize = request->hdr.offset + bytesToWrite;
355 if (requiredSize >
static_cast<uint32_t
>(_uploadSession.buffer.size())) {
356 _uploadSession.buffer.resize(requiredSize);
359 if (bytesToWrite > 0) {
360 (void) memcpy(_uploadSession.buffer.data() + request->hdr.offset, request->data, bytesToWrite);
363 MavlinkFTP::Request response{};
366 response.hdr.session = _sessionId;
367 response.hdr.size = 0;
368 response.hdr.offset = request->hdr.offset;
370 _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
373void MockLinkFTP::_finalizeActiveUpload()
375 if (!_uploadSession.active) {
379 if (!_uploadSession.remotePath.isEmpty()) {
380 _uploadedFiles.insert(_uploadSession.remotePath, _uploadSession.buffer);
383 _uploadSession.reset();
388 if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
392 mavlink_file_transfer_protocol_t requestFTP{};
393 mavlink_msg_file_transfer_protocol_decode(&message, &requestFTP);
395 if (requestFTP.target_system != _systemIdServer) {
399 MavlinkFTP::Request *request =
reinterpret_cast<MavlinkFTP::Request*
>(&requestFTP.payload[0]);
403 if ((rand() % 5) == 0) {
404 qCDebug(MockLinkFTPLog) <<
"MockLinkFTP: Random drop of incoming packet";
409 if (_lastReplyValid && (request->hdr.seqNumber == (_lastReplySequence - 1))) {
412 qCDebug(MockLinkFTPLog) <<
"MockLinkFTP: resending response";
417 const uint16_t incomingSeqNumber = request->hdr.seqNumber;
418 const uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
433 MavlinkFTP::Request ackResponse{};
434 switch (request->hdr.opcode) {
438 ackResponse.hdr.session = 0;
439 ackResponse.hdr.size = 0;
440 _sendResponse(message.sysid, message.compid, &ackResponse, outgoingSeqNumber);
443 _listCommand(message.sysid, message.compid, request, incomingSeqNumber);
446 _openCommand(message.sysid, message.compid, request, incomingSeqNumber);
449 _createFileCommand(message.sysid, message.compid, request, incomingSeqNumber);
452 _openFileWOCommand(message.sysid, message.compid, request, incomingSeqNumber);
455 _readCommand(message.sysid, message.compid, request, incomingSeqNumber);
458 _burstReadCommand(message.sysid, message.compid, request, incomingSeqNumber);
461 _writeCommand(message.sysid, message.compid, request, incomingSeqNumber);
464 _terminateCommand(message.sysid, message.compid, request, incomingSeqNumber);
467 _resetCommand(message.sysid, message.compid, incomingSeqNumber);
476void MockLinkFTP::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber,
MavlinkFTP::OpCode_t reqOpcode)
478 MavlinkFTP::Request ackResponse{};
481 ackResponse.hdr.req_opcode = reqOpcode;
482 ackResponse.hdr.session = _sessionId;
483 ackResponse.hdr.size = 0;
485 _sendResponse(targetSystemId, targetComponentId, &ackResponse, seqNumber);
490 MavlinkFTP::Request nakResponse{};
493 nakResponse.hdr.req_opcode = reqOpcode;
494 nakResponse.hdr.session = _sessionId;
495 nakResponse.hdr.size = 1;
496 nakResponse.data[0] =
error;
498 _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber);
501void MockLinkFTP::_sendNakErrno(uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber,
MavlinkFTP::OpCode_t reqOpcode)
503 MavlinkFTP::Request nakResponse{};
506 nakResponse.hdr.req_opcode = reqOpcode;
507 nakResponse.hdr.session = _sessionId;
508 nakResponse.hdr.size = 2;
510 nakResponse.data[1] = nakErrno;
512 _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber);
516void MockLinkFTP::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request *request, uint16_t seqNumber)
518 request->hdr.seqNumber = seqNumber;
519 _lastReplySequence = seqNumber;
520 _lastReplyValid =
true;
522 (void) mavlink_msg_file_transfer_protocol_pack_chan(
530 reinterpret_cast<uint8_t*
>(request)
535 if ((rand() % 5) == 0) {
536 qCDebug(MockLinkFTPLog) <<
"MockLinkFTP: Random drop of outgoing packet";
545uint16_t MockLinkFTP::_nextSeqNumber(uint16_t seqNumber)
const
547 uint16_t outgoingSeqNumber = seqNumber + 1;
553 return outgoingSeqNumber;
556QString MockLinkFTP::_createTestTempFile(
int size)
558 QTemporaryFile tmpFile(QDir::tempPath() + QStringLiteral(
"/MockLinkFTPTestCaseXXXXXX"));
559 tmpFile.setAutoRemove(
false);
561 if (tmpFile.open()) {
562 for (
int i = 0; i < size; i++) {
563 (void) tmpFile.write(QByteArray(1,
static_cast<char>(i % 255)));
568 return tmpFile.fileName();
struct __mavlink_message mavlink_message_t
#define QGC_LOGGING_CATEGORY(name, categoryStr)
uint8_t mavlinkChannel() const
ErrorCode_t
Error codes returned in Nak response PayloadHeader.data[0].
@ kErrFail
Unknown failure.
@ kErrEOF
Offset past end of file for List and Read commands.
@ kErrUnknownCommand
Unknown command opcode.
@ kErrFailErrno
errno sent back in PayloadHeader.data[1]
@ kErrInvalidSession
Session is not currently open.
@ kCmdWriteFile
Writes <size> bytes to <offset> in <session>
@ kCmdCreateFile
Creates file at <path> for writing, returns <session>
@ kCmdOpenFileRO
Opens file at <path> for reading, returns <session>
@ kCmdReadFile
Reads <size> bytes from <offset> in <session>
@ kCmdOpenFileWO
Opens file at <path> for writing, returns <session>
@ kCmdResetSessions
Terminates all open Read sessions.
@ kCmdNone
ignored, always acked
@ kCmdBurstReadFile
Burst download session file.
@ kCmdTerminateSession
Terminates open Read session.
@ kCmdListDirectory
List files in <path> from <offset>
Mock implementation of Mavlink FTP server.
void resetCommandReceived()
You can connect to this signal to be notified when the server receives a Reset command.
void terminateCommandReceived()
You can connect to this signal to be notified when the server receives a Terminate command.
static constexpr const char * sizeFilenamePrefix
void mavlinkMessageReceived(const mavlink_message_t &message)
Called to handle an FTP message.
@ errModeBadSequence
Return response with bad sequence number.
@ errModeNoSecondResponseAllowRetry
No response to subsequent request to initial command, error will be cleared after this so retry will ...
@ errModeNakSecondResponse
Nak subsequent request to initial command.
@ errModeNoResponse
No response to any request, client should eventually time out with no Ack.
@ errModeNone
No error, respond correctly.
@ errModeNoSecondResponse
No response to subsequent request to initial command.
@ errModeNakResponse
Nak all requests.
void respondWithMavlinkMessage(const mavlink_message_t &msg)
Sends the specified mavlink message to QGC.