MockLinkFileServer.cc 15.6 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 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.
 *
 ****************************************************************************/

10

11 12 13 14 15 16 17 18 19
#include "MockLinkFileServer.h"
#include "MockLink.h"

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

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

32
// We only support a single fixed session
33
const uint8_t MockLinkFileServer::_sessionId = 1;
34

35
MockLinkFileServer::MockLinkFileServer(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink) :
36 37
    _errMode(errModeNone),
    _systemIdServer(systemIdServer),
38 39
    _componentIdServer(componentIdServer),
    _mockLink(mockLink)
40 41 42 43
{

}

44 45
/// @brief Handles List command requests. Only supports root folder paths.
///         File list returned is set using the setFileList method.
46
void MockLinkFileServer::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber)
47
{
48 49
    // FIXME: Does not support directories that span multiple packets
    
50
    FileManager::Request  ackResponse;
Don Gagne's avatar
Don Gagne committed
51
    QString                     path;
52
    uint16_t                    outgoingSeqNumber = _nextSeqNumber(seqNumber);
Don Gagne's avatar
Don Gagne committed
53

54 55 56
    // We only support root path
    path = (char *)&request->data[0];
    if (!path.isEmpty() && path != "/") {
57
		_sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdListDirectory);
58 59 60 61 62
        return;
    }
    
    // Offset requested is past the end of the list
    if (request->hdr.offset > (uint32_t)_fileList.size()) {
63
        _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdListDirectory);
64 65 66
        return;
    }
    
67
    ackResponse.hdr.opcode = FileManager::kRspAck;
68
    ackResponse.hdr.req_opcode = FileManager::kCmdListDirectory;
69
    ackResponse.hdr.session = 0;
70
    ackResponse.hdr.offset = request->hdr.offset;
71
    ackResponse.hdr.size = 0;
72

73 74 75 76 77
    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++) {
78
            strcpy(bufPtr, _fileList[i].toStdString().c_str());
Don Gagne's avatar
Don Gagne committed
79 80
            uint8_t cchFilename = static_cast<uint8_t>(strlen(bufPtr));
            Q_ASSERT(cchFilename);
81 82 83
            ackResponse.hdr.size += cchFilename + 1;
            bufPtr += cchFilename + 1;
        }
84

85
        _sendResponse(senderSystemId, senderComponentId, &ackResponse, outgoingSeqNumber);
86 87
    } else if (_errMode == errModeNakSecondResponse) {
        // Nak error all subsequent requests
88
        _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdListDirectory);
89 90 91 92
        return;
    } else if (_errMode == errModeNoSecondResponse) {
        // No response for all subsequent requests
        return;
93
    } else {
94
        // FIXME: Does not support directories that span multiple packets
95
        _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdListDirectory);
96 97 98
    }
}

99
/// @brief Handles Open command requests.
100
void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber)
101
{
102
    FileManager::Request  response;
103
    QString                     path;
104
    uint16_t                    outgoingSeqNumber = _nextSeqNumber(seqNumber);
105
    
106 107
    size_t cchPath = strnlen((char *)request->data, sizeof(request->data));
    Q_ASSERT(cchPath != sizeof(request->data));
108
    Q_UNUSED(cchPath); // Fix initialized-but-not-referenced warning on release builds
109 110 111
    path = (char *)request->data;
    
    // Check path against one of our known test cases
112

113 114 115 116 117 118 119 120 121
    bool found = false;
    for (size_t i=0; i<cFileTestCases; i++) {
        if (path == rgFileTestCases[i].filename) {
            found = true;
            _readFileLength = rgFileTestCases[i].length;
            break;
        }
    }
    if (!found) {
122
        _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdOpenFileRO);
123 124 125
        return;
    }
    
126 127
    response.hdr.opcode = FileManager::kRspAck;
	response.hdr.req_opcode = FileManager::kCmdOpenFileRO;
128 129
    response.hdr.session = _sessionId;
    
130 131
    // Data contains file length
    response.hdr.size = sizeof(uint32_t);
132
    response.openFileLength = _readFileLength;
133
    
134
    _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
135 136
}

137
void MockLinkFileServer::_readCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber)
138
{
139 140
    FileManager::Request	response;
    uint16_t				outgoingSeqNumber = _nextSeqNumber(seqNumber);
141 142

    if (request->hdr.session != _sessionId) {
143
		_sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdReadFile);
144 145 146
        return;
    }
    
147 148
    uint32_t readOffset = request->hdr.offset;  // offset into file for reading
    uint8_t cDataBytes = 0;                     // current number of data bytes used
149
    
150 151 152 153
    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
154
            _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdReadFile);
155 156 157 158 159 160 161
            return;
        } else if (_errMode == errModeNoSecondResponse) {
            // No rsponse for all subsequent requests
            return;
        }
    }
    
162
    if (readOffset >= _readFileLength) {
163
        _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdReadFile);
164 165 166 167
        return;
    }
    
    // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF.
168
    for (; cDataBytes < sizeof(response.data) && readOffset < _readFileLength; readOffset++, cDataBytes++) {
169
        response.data[cDataBytes] = readOffset & 0xFF;
170 171
    }
    
172 173
    // We should always have written something, otherwise there is something wrong with the code above
    Q_ASSERT(cDataBytes);
174 175
    
    response.hdr.session = _sessionId;
176
    response.hdr.size = cDataBytes;
177
    response.hdr.offset = request->hdr.offset;
178 179 180
    response.hdr.opcode = FileManager::kRspAck;
	response.hdr.req_opcode = FileManager::kCmdReadFile;

181
    _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
182 183
}

184
void MockLinkFileServer::_streamCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber)
185 186 187 188 189
{
    uint16_t                outgoingSeqNumber = _nextSeqNumber(seqNumber);
    FileManager::Request    response;

    if (request->hdr.session != _sessionId) {
190
		_sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdBurstReadFile);
191 192 193 194 195 196 197 198 199 200 201 202 203 204
        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 < _readFileLength) {
        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
205
                _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdBurstReadFile);
206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226
                return;
            } else if (_errMode == errModeNoSecondResponse) {
                // No response for all subsequent requests
                return;
            }
        }
        
        // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF.
        for (; cDataAck < sizeof(response.data) && readOffset < _readFileLength; readOffset++, cDataAck++) {
            response.data[cDataAck] = readOffset & 0xFF;
        }
        
        // We should always have written something, otherwise there is something wrong with the code above
        Q_ASSERT(cDataAck);
        
        response.hdr.session = _sessionId;
        response.hdr.size = cDataAck;
        response.hdr.offset = ackOffset;
        response.hdr.opcode = FileManager::kRspAck;
        response.hdr.req_opcode = FileManager::kCmdBurstReadFile;
        
227
        _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber);
228 229 230 231 232
        
        outgoingSeqNumber = _nextSeqNumber(outgoingSeqNumber);
        ackOffset += cDataAck;
    }
	
233
    _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdBurstReadFile);
234 235
}

236
void MockLinkFileServer::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber)
237
{
238 239
    uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);

240
    if (request->hdr.session != _sessionId) {
241
		_sendNak(senderSystemId, senderComponentId, FileManager::kErrInvalidSession, outgoingSeqNumber, FileManager::kCmdTerminateSession);
242 243 244
        return;
    }
    
245
	_sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, FileManager::kCmdTerminateSession);
246
	
247 248 249
    emit terminateCommandReceived();
}

250
void MockLinkFileServer::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber)
251
{
252 253 254 255 256 257
    uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber);
    
    _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, FileManager::kCmdResetSessions);
    
    emit resetCommandReceived();
}
258

259 260 261 262 263
void MockLinkFileServer::handleFTPMessage(const mavlink_message_t& message)
{
    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
        return;
    }
264
    
265
    FileManager::Request  ackResponse;
266

267 268 269 270 271 272
    mavlink_file_transfer_protocol_t requestFTP;
    mavlink_msg_file_transfer_protocol_decode(&message, &requestFTP);
    
    if (requestFTP.target_system != _systemIdServer) {
        return;
    }
273
    
274 275
    FileManager::Request* request = (FileManager::Request*)&requestFTP.payload[0];

276
    uint16_t incomingSeqNumber = request->hdr.seqNumber;
277 278
    uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
    
279 280 281 282 283 284 285 286 287
    if (request->hdr.opcode != FileManager::kCmdResetSessions && request->hdr.opcode != FileManager::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, FileManager::kErrFail, outgoingSeqNumber, (FileManager::Opcode)request->hdr.opcode);
            return;
        }
288
    }
289

Don Gagne's avatar
Don Gagne committed
290
    switch (request->hdr.opcode) {
291
        case FileManager::kCmdTestNoAck:
Don Gagne's avatar
Don Gagne committed
292 293 294
            // ignored, ack not sent back, for testing only
            break;
            
295
        case FileManager::kCmdNone:
296
            // ignored, always acked
297
            ackResponse.hdr.opcode = FileManager::kRspAck;
Don Gagne's avatar
Don Gagne committed
298 299
            ackResponse.hdr.session = 0;
            ackResponse.hdr.size = 0;
300
            _sendResponse(message.sysid, message.compid, &ackResponse, outgoingSeqNumber);
Don Gagne's avatar
Don Gagne committed
301 302
            break;

303
        case FileManager::kCmdListDirectory:
304
            _listCommand(message.sysid, message.compid, request, incomingSeqNumber);
305
            break;
306
            
307
        case FileManager::kCmdOpenFileRO:
308
            _openCommand(message.sysid, message.compid, request, incomingSeqNumber);
309
            break;
Don Gagne's avatar
Don Gagne committed
310

311
        case FileManager::kCmdReadFile:
312
            _readCommand(message.sysid, message.compid, request, incomingSeqNumber);
313
            break;
314

315
        case FileManager::kCmdBurstReadFile:
316
            _streamCommand(message.sysid, message.compid, request, incomingSeqNumber);
317 318 319
            break;

        case FileManager::kCmdTerminateSession:
320
            _terminateCommand(message.sysid, message.compid, request, incomingSeqNumber);
321 322
            break;

323 324 325 326
        case FileManager::kCmdResetSessions:
            _resetCommand(message.sysid, message.compid, incomingSeqNumber);
            break;
            
327 328
        default:
            // nack for all NYI opcodes
329
            _sendNak(message.sysid, message.compid, FileManager::kErrUnknownCommand, outgoingSeqNumber, (FileManager::Opcode)request->hdr.opcode);
330 331 332
            break;
    }
}
Don Gagne's avatar
Don Gagne committed
333

334
/// @brief Sends an Ack
335
void MockLinkFileServer::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, FileManager::Opcode reqOpcode)
336
{
337
    FileManager::Request ackResponse;
338
    
339 340
    ackResponse.hdr.opcode = FileManager::kRspAck;
	ackResponse.hdr.req_opcode = reqOpcode;
341 342 343
    ackResponse.hdr.session = 0;
    ackResponse.hdr.size = 0;
    
344
    _sendResponse(targetSystemId, targetComponentId, &ackResponse, seqNumber);
345 346
}

347
/// @brief Sends a Nak with the specified error code.
348
void MockLinkFileServer::_sendNak(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::ErrorCode error, uint16_t seqNumber, FileManager::Opcode reqOpcode)
Don Gagne's avatar
Don Gagne committed
349
{
350
    FileManager::Request nakResponse;
Don Gagne's avatar
Don Gagne committed
351

352 353
    nakResponse.hdr.opcode = FileManager::kRspNak;
	nakResponse.hdr.req_opcode = reqOpcode;
Don Gagne's avatar
Don Gagne committed
354
    nakResponse.hdr.session = 0;
355 356
    nakResponse.hdr.size = 1;
    nakResponse.data[0] = error;
Don Gagne's avatar
Don Gagne committed
357
    
358
    _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber);
Don Gagne's avatar
Don Gagne committed
359 360
}

361
/// @brief Emits a Request through the messageReceived signal.
362
void MockLinkFileServer::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::Request* request, uint16_t seqNumber)
Don Gagne's avatar
Don Gagne committed
363 364 365
{
    mavlink_message_t   mavlinkMessage;
    
366 367 368 369 370 371
    request->hdr.seqNumber = seqNumber;
    
    mavlink_msg_file_transfer_protocol_pack(_systemIdServer,    // System ID
                                            0,                  // Component ID
                                            &mavlinkMessage,    // Mavlink Message to pack into
                                            0,                  // Target network
372 373
                                            targetSystemId,
                                            targetComponentId,
374
                                            (uint8_t*)request); // Payload
Don Gagne's avatar
Don Gagne committed
375
    
376
    _mockLink->respondWithMavlinkMessage(mavlinkMessage);
Lorenz Meier's avatar
Lorenz Meier committed
377
}
378 379 380

/// @brief Generates the next sequence number given an incoming sequence number. Handles generating
/// bad sequence numbers when errModeBadSequence is set.
381
uint16_t MockLinkFileServer::_nextSeqNumber(uint16_t seqNumber)
382 383 384 385 386 387 388 389
{
    uint16_t outgoingSeqNumber = seqNumber + 1;
    
    if (_errMode == errModeBadSequence) {
        outgoingSeqNumber++;
    }
    return outgoingSeqNumber;
}