From 46da294ffb6839d3a368036e74ce2b71ba9d863e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 3 May 2015 19:26:54 -0700 Subject: [PATCH] New bust mode ftp file download --- src/modules/mavlink/mavlink_ftp.cpp | 486 ++++++++++-------- src/modules/mavlink/mavlink_ftp.h | 101 ++-- src/modules/mavlink/mavlink_main.cpp | 9 + src/modules/mavlink/mavlink_main.h | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 6 - src/modules/mavlink/mavlink_stream.h | 2 - .../mavlink_tests/mavlink_ftp_test.cpp | 338 ++++++++---- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 53 +- src/modules/mavlink/mavlink_tests/module.mk | 1 + 9 files changed, 617 insertions(+), 385 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4ba595a87b..10b94b6b26 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -42,116 +42,130 @@ #include #include "mavlink_ftp.h" +#include "mavlink_main.h" #include "mavlink_tests/mavlink_ftp_test.h" // Uncomment the line below to get better debug output. Never commit with this left on. //#define MAVLINK_FTP_DEBUG -MavlinkFTP * -MavlinkFTP::get_server(void) +int buf_size_1 = 0; +int buf_size_2 = 0; + +MavlinkFTP::MavlinkFTP(Mavlink* mavlink) : + MavlinkStream(mavlink), + _session_info{}, + _utRcvMsgFunc{}, + _worker_data{} { - static MavlinkFTP server; - return &server; + // initialize session + _session_info.fd = -1; } -MavlinkFTP::MavlinkFTP() : - _request_bufs{}, - _request_queue{}, - _request_queue_sem{}, - _utRcvMsgFunc{}, - _ftp_test{} +MavlinkFTP::~MavlinkFTP() { - // initialise the request freelist - dq_init(&_request_queue); - sem_init(&_request_queue_sem, 0, 1); + +} - // initialize session list - for (size_t i=0; imsgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - mavlink_msg_file_transfer_protocol_decode(msg, &req->message); - #ifdef MAVLINK_FTP_UNIT_TEST - if (!_utRcvMsgFunc) { - warnx("Incorrectly written unit test\n"); + // We use fake ids when unit testing + return MavlinkFtpTest::serverSystemId; +#else + // Not unit testing, use the real thing + return _mavlink->get_system_id(); +#endif +} + +uint8_t +MavlinkFTP::_getServerComponentId(void) +{ +#ifdef MAVLINK_FTP_UNIT_TEST + // We use fake ids when unit testing + return MavlinkFtpTest::serverComponentId; +#else + // Not unit testing, use the real thing + return _mavlink->get_component_id(); +#endif +} + +uint8_t +MavlinkFTP::_getServerChannel(void) +{ +#ifdef MAVLINK_FTP_UNIT_TEST + // We use fake ids when unit testing + return MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + return _mavlink->get_channel(); +#endif +} + +void +MavlinkFTP::handle_message(const mavlink_message_t *msg) +{ + //warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2); + + if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_file_transfer_protocol_t ftp_request; + mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request); + +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system); +#endif + + if (ftp_request.target_system == _getServerSystemId()) { + _process_request(&ftp_request, msg->sysid); return; } - // We use fake ids when unit testing - req->serverSystemId = MavlinkFtpTest::serverSystemId; - req->serverComponentId = MavlinkFtpTest::serverComponentId; - req->serverChannel = MavlinkFtpTest::serverChannel; -#else - // Not unit testing, use the real thing - req->serverSystemId = mavlink->get_system_id(); - req->serverComponentId = mavlink->get_component_id(); - req->serverChannel = mavlink->get_channel(); -#endif - - // This is the system id we want to target when sending - req->targetSystemId = msg->sysid; - - if (req->message.target_system == req->serverSystemId) { - req->mavlink = mavlink; -#ifdef MAVLINK_FTP_UNIT_TEST - // We are running in Unit Test mode. Don't queue, just call _worket directly. - _process_request(req); -#else - // We are running in normal mode. Queue the request to the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); -#endif - return; - } } - - _return_request(req); -} - -/// @brief Queued static work queue routine to handle mavlink messages -void -MavlinkFTP::_worker_trampoline(void *arg) -{ - Request* req = reinterpret_cast(arg); - MavlinkFTP* server = MavlinkFTP::get_server(); - - // call the server worker with the work item - server->_process_request(req); } /// @brief Processes an FTP message void -MavlinkFTP::_process_request(Request *req) +MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id) { - PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + bool stream_send = false; + PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); ErrorCode errorCode = kErrNone; @@ -162,7 +176,7 @@ MavlinkFTP::_process_request(Request *req) } #ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset); #endif switch (payload->opcode) { @@ -197,6 +211,11 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRead(payload); break; + case kCmdBurstReadFile: + errorCode = _workBurst(payload, target_system_id); + stream_send = true; + break; + case kCmdWriteFile: errorCode = _workWrite(payload); break; @@ -221,7 +240,6 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveDirectory(payload); break; - case kCmdCalcFileCRC32: errorCode = _workCalcFileCRC32(payload); break; @@ -232,16 +250,14 @@ MavlinkFTP::_process_request(Request *req) } out: + payload->seq_number++; + // handle success vs. error if (errorCode == kErrNone) { payload->req_opcode = payload->opcode; payload->opcode = kRspAck; -#ifdef MAVLINK_FTP_DEBUG - warnx("FTP: ack\n"); -#endif } else { int r_errno = errno; - warnx("FTP: nak %u", errorCode); payload->req_opcode = payload->opcode; payload->opcode = kRspNak; payload->size = 1; @@ -252,60 +268,33 @@ out: } } - - // respond to the request - _reply(req); - - _return_request(req); + // Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak. + if (!stream_send || errorCode != kErrNone) { + // respond to the request + ftp_req->target_system = target_system_id; + _reply(ftp_req); + } } -/// @brief Sends the specified FTP reponse message out through mavlink +/// @brief Sends the specified FTP response message out through mavlink void -MavlinkFTP::_reply(Request *req) +MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) { - PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - payload->seqNumber = payload->seqNumber + 1; - - mavlink_message_t msg; - msg.checksum = 0; -#ifndef MAVLINK_FTP_UNIT_TEST - uint16_t len = +#ifdef MAVLINK_FTP_DEBUG + PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); + warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number); #endif - mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id - req->serverComponentId, // Sender component id - req->serverChannel, // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - req->targetSystemId, // Target system id - 0, // Target component id - (const uint8_t*)payload); // Payload to pack into message - bool success = true; + ftp_req->target_network = 0; + ftp_req->target_component = 0; #ifdef MAVLINK_FTP_UNIT_TEST // Unit test hook is set, call that instead - _utRcvMsgFunc(&msg, _ftp_test); + _utRcvMsgFunc(ftp_req, _worker_data); #else - Mavlink *mavlink = req->mavlink; - - mavlink->lockMessageBufferMutex(); - success = mavlink->message_buffer_write(&msg, len); - mavlink->unlockMessageBufferMutex(); - + _mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req); #endif - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", - req->serverSystemId, - req->serverComponentId, - req->serverChannel, - msg.checksum); - } -#endif } /// @brief Responds to a List command @@ -417,13 +406,16 @@ MavlinkFTP::_workList(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) { - int session_index = _find_unused_session(); - if (session_index < 0) { + if (_session_info.fd >= 0) { warnx("FTP: Open failed - out of sessions\n"); return kErrNoSessionsAvailable; } char *filename = _data_as_cstring(payload); + +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: open '%s'", filename); +#endif uint32_t fileSize = 0; struct stat st; @@ -440,9 +432,11 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) if (fd < 0) { return kErrFailErrno; } - _session_fds[session_index] = fd; + _session_info.fd = fd; + _session_info.file_size = fileSize; + _session_info.stream_download = false; - payload->session = session_index; + payload->session = 0; payload->size = sizeof(uint32_t); *((uint32_t*)payload->data) = fileSize; @@ -453,23 +447,25 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) MavlinkFTP::ErrorCode MavlinkFTP::_workRead(PayloadHeader* payload) { - int session_index = payload->session; - - if (!_valid_session(session_index)) { + if (payload->session != 0 || _session_info.fd < 0) { return kErrInvalidSession; } - // Seek to the specified position #ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", payload->offset); + warnx("FTP: read offset:%d", payload->offset); #endif - if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { - // Unable to see to the specified location - warnx("seek fail"); + // We have to test seek past EOF ourselves, lseek will allow seek past EOF + if (payload->offset >= _session_info.file_size) { + warnx("request past EOF"); return kErrEOF; } + + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + warnx("seek fail"); + return kErrFailErrno; + } - int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); + int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof warnx("read fail %d", bytes_read); @@ -481,27 +477,41 @@ MavlinkFTP::_workRead(PayloadHeader* payload) return kErrNone; } +/// @brief Responds to a Stream command +MavlinkFTP::ErrorCode +MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id) +{ + if (payload->session != 0 && _session_info.fd < 0) { + return kErrInvalidSession; + } + +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: burst offset:%d", payload->offset); +#endif + // Setup for streaming sends + _session_info.stream_download = true; + _session_info.stream_offset = payload->offset; + _session_info.stream_seq_number = payload->seq_number + 1; + _session_info.stream_target_system_id = target_system_id; + + return kErrNone; +} + /// @brief Responds to a Write command MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(PayloadHeader* payload) { - int session_index = payload->session; - - if (!_valid_session(session_index)) { + if (payload->session != 0 && _session_info.fd < 0) { return kErrInvalidSession; } - // Seek to the specified position -#ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", payload->offset); -#endif - if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); return kErrFailErrno; } - int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size); if (bytes_written < 0) { // Negative return indicates error other than eof warnx("write fail %d", bytes_written); @@ -608,12 +618,13 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workTerminate(PayloadHeader* payload) { - if (!_valid_session(payload->session)) { + if (payload->session != 0 || _session_info.fd < 0) { return kErrInvalidSession; } - - ::close(_session_fds[payload->session]); - _session_fds[payload->session] = -1; + + ::close(_session_info.fd); + _session_info.fd = -1; + _session_info.stream_download = false; payload->size = 0; @@ -624,11 +635,10 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workReset(PayloadHeader* payload) { - for (size_t i=0; isize = 0; @@ -726,29 +736,6 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) return kErrNone; } -/// @brief Returns true if the specified session is a valid open session -bool -MavlinkFTP::_valid_session(unsigned index) -{ - if ((index >= kMaxSession) || (_session_fds[index] < 0)) { - return false; - } - return true; -} - -/// @brief Returns an unused session index -int -MavlinkFTP::_find_unused_session(void) -{ - for (size_t i=0; idata[0]); } -/// @brief Returns a unused Request entry. NULL if none available. -MavlinkFTP::Request * -MavlinkFTP::_get_request(void) -{ - _lock_request_queue(); - Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); - _unlock_request_queue(); - return req; -} - -/// @brief Locks a semaphore to provide exclusive access to the request queue -void -MavlinkFTP::_lock_request_queue(void) -{ - do {} - while (sem_wait(&_request_queue_sem) != 0); -} - -/// @brief Unlocks the semaphore providing exclusive access to the request queue -void -MavlinkFTP::_unlock_request_queue(void) -{ - sem_post(&_request_queue_sem); -} - -/// @brief Returns a no longer needed request to the queue -void -MavlinkFTP::_return_request(Request *req) -{ - _lock_request_queue(); - dq_addlast(&req->work.dq, &_request_queue); - _unlock_request_queue(); -} - /// @brief Copy file (with limited space) int MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) @@ -851,3 +804,106 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length errno = op_errno; return (length > 0)? -1 : 0; } + +void MavlinkFTP::send(const hrt_abstime t) +{ + // Anything to stream? + if (!_session_info.stream_download) { + return; + } + +#ifndef MAVLINK_FTP_UNIT_TEST + // Skip send if not enough room + unsigned max_bytes_to_send = _mavlink->get_free_tx_buf(); +#ifdef MAVLINK_FTP_DEBUG + warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf()); +#endif + if (max_bytes_to_send < get_size()) { + return; + } +#endif + + // Send stream packets until buffer is full + + bool more_data; + do { + more_data = false; + + ErrorCode error_code = kErrNone; + + mavlink_file_transfer_protocol_t ftp_msg; + PayloadHeader* payload = reinterpret_cast(&ftp_msg.payload[0]); + + payload->seq_number = _session_info.stream_seq_number; + payload->session = 0; + payload->opcode = kRspAck; + payload->req_opcode = kCmdBurstReadFile; + payload->offset = _session_info.stream_offset; + _session_info.stream_seq_number++; + +#ifdef MAVLINK_FTP_DEBUG + warnx("stream send: offset %d", _session_info.stream_offset); +#endif + // We have to test seek past EOF ourselves, lseek will allow seek past EOF + if (_session_info.stream_offset >= _session_info.file_size) { + error_code = kErrEOF; +#ifdef MAVLINK_FTP_DEBUG + warnx("stream download: sending Nak EOF"); +#endif + } + + if (error_code == kErrNone) { + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + error_code = kErrFailErrno; +#ifdef MAVLINK_FTP_DEBUG + warnx("stream download: seek fail"); +#endif + } + } + + if (error_code == kErrNone) { + int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + error_code = kErrFailErrno; +#ifdef MAVLINK_FTP_DEBUG + warnx("stream download: read fail"); +#endif + } else { + payload->size = bytes_read; + _session_info.stream_offset += bytes_read; + } + } + + if (error_code != kErrNone) { + payload->opcode = kRspNak; + payload->size = 1; + uint8_t* pData = &payload->data[0]; + *pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error + if (error_code == kErrFailErrno) { + int r_errno = errno; + payload->size = 2; + payload->data[1] = r_errno; + } + _session_info.stream_download = false; + } else { +#ifndef MAVLINK_FTP_UNIT_TEST + if (max_bytes_to_send < (get_size()*2)) { + more_data = false; + payload->burst_complete = true; + _session_info.stream_download = false; + } else { +#endif + more_data = true; + payload->burst_complete = false; +#ifndef MAVLINK_FTP_UNIT_TEST + max_bytes_to_send -= get_size(); + } +#endif + } + + ftp_msg.target_system = _session_info.stream_target_system_id; + _reply(&ftp_msg); + } while (more_data); +} + diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 9693a92a97..af8740e481 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -39,45 +39,44 @@ #include #include -#include #include -#include "mavlink_messages.h" -#include "mavlink_main.h" +#include "mavlink_stream.h" +#include "mavlink_bridge_header.h" class MavlinkFtpTest; -/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. -/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded. -class MavlinkFTP +/// MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +class MavlinkFTP : public MavlinkStream { public: - /// @brief Returns the one Mavlink FTP server in the system. - static MavlinkFTP* get_server(void); - /// @brief Contructor is only public so unit test code can new objects. - MavlinkFTP(); + MavlinkFTP(Mavlink *mavlink); + ~MavlinkFTP(); + + static MavlinkStream *new_instance(Mavlink *mavlink); - /// @brief Adds the specified message to the work queue. - void handle_message(Mavlink* mavlink, mavlink_message_t *msg); - - typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + /// Handle possible FTP message + void handle_message(const mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); /// @brief Sets up the server to run in unit test mode. /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. - /// @param ftp_test MavlinkFtpTest object which the function is associated with - void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test); + /// @param worker_data Data to pass to worker + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data); /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to /// 32 bit alignment to avoid usage of any pack pragmas. struct PayloadHeader { - uint16_t seqNumber; ///< sequence number for message + uint16_t seq_number; ///< sequence number for message uint8_t session; ///< Session id for read and write commands uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t padding[2]; ///< 32 bit aligment padding + uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming. + uint8_t padding; ///< 32 bit aligment padding uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; @@ -100,6 +99,7 @@ public: kCmdTruncateFile, ///< Truncate file at to length kCmdRename, ///< Rename to kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + kCmdBurstReadFile, ///< Burst download session file kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -118,35 +118,22 @@ public: kErrUnknownCommand ///< Unknown command opcode }; + // MavlinkStream overrides + virtual const char *get_name(void) const; + virtual uint8_t get_id(void); + virtual unsigned get_size(void); + private: - /// @brief Unit of work which is queued to work_queue - struct Request - { - work_s work; ///< work queue entry - Mavlink *mavlink; ///< Mavlink to reply to - uint8_t serverSystemId; ///< System ID to send from - uint8_t serverComponentId; ///< Component ID to send from - uint8_t serverChannel; ///< Channel to send to - uint8_t targetSystemId; ///< System ID to target reply to - - mavlink_file_transfer_protocol_t message; ///< Protocol message - }; - - Request *_get_request(void); - void _return_request(Request *req); - void _lock_request_queue(void); - void _unlock_request_queue(void); - char *_data_as_cstring(PayloadHeader* payload); - static void _worker_trampoline(void *arg); - void _process_request(Request *req); - void _reply(Request *req); + void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id); + void _reply(mavlink_file_transfer_protocol_t* ftp_req); int _copy_file(const char *src_path, const char *dst_path, size_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload); ErrorCode _workReset(PayloadHeader* payload); @@ -156,14 +143,13 @@ private: ErrorCode _workTruncateFile(PayloadHeader *payload); ErrorCode _workRename(PayloadHeader *payload); ErrorCode _workCalcFileCRC32(PayloadHeader *payload); - - static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests - Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work - dq_queue_t _request_queue; ///< Queue of available Request buffers - sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue - int _find_unused_session(void); - bool _valid_session(unsigned index); + uint8_t _getServerSystemId(void); + uint8_t _getServerComponentId(void); + uint8_t _getServerChannel(void); + + // Overrides from MavlinkStream + virtual void send(const hrt_abstime t); static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command @@ -172,9 +158,24 @@ private: /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); - static const unsigned kMaxSession = 2; ///< Max number of active sessions - int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot + struct SessionInfo { + int fd; + uint32_t file_size; + bool stream_download; + uint32_t stream_offset; + uint16_t stream_seq_number; + uint8_t stream_target_system_id; + }; + struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session - ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending - MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + void *_worker_data; ///< Additional parameter to _utRcvMsgFunc; + + /* do not allow copying this class */ + MavlinkFTP(const MavlinkFTP&); + MavlinkFTP operator=(const MavlinkFTP&); + + + // Mavlink test needs to be able to call send + friend class MavlinkFtpTest; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 22ff3edf6f..a8f07f3e21 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -131,6 +131,7 @@ Mavlink::Mavlink() : _streams(nullptr), _mission_manager(nullptr), _parameters_manager(nullptr), + _mavlink_ftp(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, @@ -868,6 +869,9 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* handle packet with parameter component */ _parameters_manager->handle_message(msg); + + /* handle packet with ftp component */ + _mavlink_ftp->handle_message(msg); if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ @@ -1364,6 +1368,11 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(120.0f)); LL_APPEND(_streams, _parameters_manager); + /* MAVLINK_FTP stream */ + _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); + _mavlink_ftp->set_interval(interval_from_rate(120.0f)); + LL_APPEND(_streams, _mavlink_ftp); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on * remote requests rate. Rate specified here controls how much bandwidth we will reserve for * mission messages. */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c285bc4052..90b84061f6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -59,6 +59,7 @@ #include "mavlink_messages.h" #include "mavlink_mission.h" #include "mavlink_parameters.h" +#include "mavlink_ftp.h" class Mavlink { @@ -296,8 +297,9 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; - MavlinkParametersManager *_parameters_manager; + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + MavlinkFTP *_mavlink_ftp; MAVLINK_MODE _mode; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4e332bf1a..4d96f389db 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -134,8 +134,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _time_offset(0) { - // make sure the FTP server is started - (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -202,10 +200,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::get_server()->handle_message(_mavlink, msg); - break; - case MAVLINK_MSG_ID_SYSTEM_TIME: handle_message_system_time(msg); break; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 5e39bbbdf9..e0a18cce70 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -73,8 +73,6 @@ public: * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(const Mavlink *mavlink); - static const char *get_name_static(); virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 7b5b9228c8..6fe0ed9c8f 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -43,19 +43,19 @@ #include "../mavlink_ftp.h" /// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py -const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { - { "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet - { "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet - { "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets +const MavlinkFtpTest::DownloadTestCase MavlinkFtpTest::_rgDownloadTestCases[] = { + { "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1, true, false }, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader), true, true }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1, false, false }, // Read take two packets }; const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file"; MavlinkFtpTest::MavlinkFtpTest() : - _ftp_server{}, - _reply_msg{}, - _lastOutgoingSeqNumber{} + _ftp_server(nullptr), + _expected_seq_number(0), + _reply_msg{} { } @@ -67,8 +67,9 @@ MavlinkFtpTest::~MavlinkFtpTest() /// @brief Called before every test to initialize the FTP Server. void MavlinkFtpTest::_init(void) { - _ftp_server = new MavlinkFTP;; - _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); + _expected_seq_number = 0; + _ftp_server = new MavlinkFTP(NULL); + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); _cleanup_microsd(); } @@ -85,15 +86,13 @@ void MavlinkFtpTest::_cleanup(void) bool MavlinkFtpTest::_ack_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; payload.opcode = MavlinkFTP::kCmdNone; bool success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -109,15 +108,13 @@ bool MavlinkFtpTest::_ack_test(void) bool MavlinkFtpTest::_bad_opcode_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; payload.opcode = 0xFF; // bogus opcode bool success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -135,8 +132,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void) { mavlink_message_t msg; MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; payload.opcode = MavlinkFTP::kCmdListDirectory; @@ -145,9 +141,9 @@ bool MavlinkFtpTest::_bad_datasize_test(void) // Set the data size to be one larger than is legal ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; - _ftp_server->handle_message(nullptr /* mavlink */, &msg); + _ftp_server->handle_message(&msg); - if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + if (!_decode_message(&_reply_msg, &reply)) { return false; } @@ -161,8 +157,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void) bool MavlinkFtpTest::_list_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; @@ -188,7 +183,6 @@ bool MavlinkFtpTest::_list_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(test->dir)+1, // size in bytes of data (uint8_t*)test->dir, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -202,16 +196,20 @@ bool MavlinkFtpTest::_list_test(void) // to a hardcoded return result string. // Convert null terminators to seperator char so we can use strok to parse returned data + char list_entry[256]; for (uint8_t j=0; jsize-1; j++) { if (reply->data[j] == 0) { - reply->data[j] = '|'; + list_entry[j] = '|'; + } else { + list_entry[j] = reply->data[j]; } } + list_entry[reply->size-1] = 0; // Loop over returned directory entries trying to find then in the response list char *dir; int response_count = 0; - dir = strtok((char *)&reply->data[0], "|"); + dir = strtok(list_entry, "|"); while (dir != nullptr) { ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); response_count++; @@ -234,8 +232,7 @@ bool MavlinkFtpTest::_list_test(void) bool MavlinkFtpTest::_list_eof_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; const char *dir = "/"; payload.opcode = MavlinkFTP::kCmdListDirectory; @@ -244,7 +241,6 @@ bool MavlinkFtpTest::_list_eof_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(dir)+1, // size in bytes of data (uint8_t*)dir, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -261,8 +257,7 @@ bool MavlinkFtpTest::_list_eof_test(void) bool MavlinkFtpTest::_open_badfile_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; const char *dir = "/foo"; // non-existent file payload.opcode = MavlinkFTP::kCmdOpenFileRO; @@ -271,7 +266,6 @@ bool MavlinkFtpTest::_open_badfile_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(dir)+1, // size in bytes of data (uint8_t*)dir, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -288,12 +282,11 @@ bool MavlinkFtpTest::_open_badfile_test(void) bool MavlinkFtpTest::_open_terminate_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; - for (size_t i=0; ifile)+1, // size in bytes of data (uint8_t*)test->file, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -321,7 +313,6 @@ bool MavlinkFtpTest::_open_terminate_test(void) success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -338,9 +329,8 @@ bool MavlinkFtpTest::_open_terminate_test(void) bool MavlinkFtpTest::_terminate_badsession_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; - const char *file = _rgReadTestCases[0].file; + const MavlinkFTP::PayloadHeader *reply; + const char *file = _rgDownloadTestCases[0].file; payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; @@ -348,7 +338,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(file)+1, // size in bytes of data (uint8_t*)file, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -363,7 +352,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -380,12 +368,11 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) bool MavlinkFtpTest::_read_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; - for (size_t i=0; ifile, &st), 0); @@ -406,7 +393,6 @@ bool MavlinkFtpTest::_read_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(test->file)+1, // size in bytes of data (uint8_t*)test->file, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -421,7 +407,6 @@ bool MavlinkFtpTest::_read_test(void) success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -429,10 +414,40 @@ bool MavlinkFtpTest::_read_test(void) ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Offset incorrect", reply->offset, 0); + + uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader); + uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0); - if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) { - ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size); - ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0); + payload.offset += expected_bytes; + + if (test->singlePacketRead) { + // Try going past EOF + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + } else { + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, payload.offset); + + expected_bytes = (uint32_t)st.st_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0); } payload.opcode = MavlinkFTP::kCmdTerminateSession; @@ -442,7 +457,91 @@ bool MavlinkFtpTest::_read_test(void) success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_burst_test(void) +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + BurstInfo burst_info; + + + + for (size_t i=0; ifile, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + // Setup for burst response handler + burst_info.burst_state = burst_state_first_ack; + burst_info.single_packet_file = test->singlePacketRead; + burst_info.file_size = st.st_size; + burst_info.file_bytes = bytes; + burst_info.ftp_test_class = this; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info); + + // Send the burst command, message response will be handled by _receive_message_handler_stream + payload.opcode = MavlinkFTP::kCmdBurstReadFile; + payload.session = reply->session; + payload.offset = 0; + + mavlink_message_t msg; + _setup_ftp_msg(&payload, 0, nullptr, &msg); + _ftp_server->handle_message(&msg); + + // First packet is sent using stream mechanism, so we need to force it out ourselves + hrt_abstime t = 0; + _ftp_server->send(t); + + ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete); + + // Put back generic message handler + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); + + // Terminate session + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload &reply); // Payload inside FTP message response if (!success) { return false; @@ -459,18 +558,16 @@ bool MavlinkFtpTest::_read_test(void) bool MavlinkFtpTest::_read_badsession_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; - const char *file = _rgReadTestCases[0].file; + const MavlinkFTP::PayloadHeader *reply; + const char *file = _rgDownloadTestCases[0].file; payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; - bool success = _send_receive_msg(&payload, // FTP payload header + bool success = _send_receive_msg(&payload, // FTP payload header strlen(file)+1, // size in bytes of data (uint8_t*)file, // Data to start into FTP message payload - &ftp_msg, // Response from server - &reply); // Payload inside FTP message response + &reply); // Payload inside FTP message response if (!success) { return false; } @@ -484,7 +581,6 @@ bool MavlinkFtpTest::_read_badsession_test(void) success = _send_receive_msg(&payload, // FTP payload header 0, // size in bytes of data nullptr, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -500,8 +596,7 @@ bool MavlinkFtpTest::_read_badsession_test(void) bool MavlinkFtpTest::_removedirectory_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; int fd; struct _testCase { @@ -534,7 +629,6 @@ bool MavlinkFtpTest::_removedirectory_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(test->dir)+1, // size in bytes of data (uint8_t*)test->dir, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -556,8 +650,7 @@ bool MavlinkFtpTest::_removedirectory_test(void) bool MavlinkFtpTest::_createdirectory_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; struct _testCase { const char *dir; @@ -579,7 +672,6 @@ bool MavlinkFtpTest::_createdirectory_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(test->dir)+1, // size in bytes of data (uint8_t*)test->dir, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -601,8 +693,7 @@ bool MavlinkFtpTest::_createdirectory_test(void) bool MavlinkFtpTest::_removefile_test(void) { MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; + const MavlinkFTP::PayloadHeader *reply; int fd; struct _testCase { @@ -611,7 +702,7 @@ bool MavlinkFtpTest::_removefile_test(void) }; static const struct _testCase rgTestCases[] = { { "/bogus", false }, - { _rgReadTestCases[0].file, false }, + { _rgDownloadTestCases[0].file, false }, { _unittest_microsd_dir, false }, { _unittest_microsd_file, true }, { _unittest_microsd_file, false }, @@ -630,7 +721,6 @@ bool MavlinkFtpTest::_removefile_test(void) bool success = _send_receive_msg(&payload, // FTP payload header strlen(test->file)+1, // size in bytes of data (uint8_t*)test->file, // Data to start into FTP message payload - &ftp_msg, // Response from server &reply); // Payload inside FTP message response if (!success) { return false; @@ -649,62 +739,119 @@ bool MavlinkFtpTest::_removefile_test(void) return true; } -/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when +/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when /// it needs to send a message out on Mavlink. -void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test) +void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data) { - ftp_test->_receive_message(msg); + ((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req); } -/// @brief Non-Static version of receive_message -void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req) { // Move the message into our own member variable - memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); + memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t)); +} + +/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data) +{ + BurstInfo* burst_info = (BurstInfo*)worker_data; + burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info); +} + +bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info) +{ + hrt_abstime t = 0; + const MavlinkFTP::PayloadHeader* reply; + uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader); + uint32_t expected_bytes; + + _decode_message(ftp_msg, &reply); + + switch (burst_info->burst_state) { + case burst_state_first_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_last_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, full_packet_bytes); + + expected_bytes = burst_info->file_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_state_nak_eof; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_nak_eof: + // Signal complete + burst_info->burst_state = burst_state_complete; + ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); + break; + + } + + return true; } /// @brief Decode and validate the incoming message -bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode - mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message - MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, ///< Incoming FTP message + const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response { - mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + //warnx("_decode_message"); // Make sure the targets are correct ut_compare("Target network non-zero", ftp_msg->target_network, 0); ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); - *payload = reinterpret_cast(ftp_msg->payload); + *payload = reinterpret_cast(ftp_msg->payload); // Make sure we have a good sequence number - ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); - - // Bump sequence number for next outgoing message - _lastOutgoingSeqNumber++; + ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number); + _expected_seq_number++; return true; } /// @brief Initializes an FTP message into a mavlink message -void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header - uint8_t size, ///< size in bytes of data - const uint8_t *data, ///< Data to start into FTP message payload - mavlink_message_t *msg) ///< Returned mavlink message +void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message { uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); - payload->seqNumber = _lastOutgoingSeqNumber; + payload->seq_number = _expected_seq_number++; payload->size = size; if (size != 0) { memcpy(payload->data, data, size); } - payload->padding[0] = 0; - payload->padding[1] = 0; + payload->burst_complete = 0; + payload->padding = 0; msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id @@ -720,14 +867,13 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, / bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header uint8_t size, ///< size in bytes of data const uint8_t *data, ///< Data to start into FTP message payload - mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server - MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response + const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response { mavlink_message_t msg; _setup_ftp_msg(payload_header, size, data, &msg); - _ftp_server->handle_message(nullptr /* mavlink */, &msg); - return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply); + _ftp_server->handle_message(&msg); + return _decode_message(&_reply_msg, payload_reply); } /// @brief Cleans up an files created on microsd during testing @@ -743,14 +889,14 @@ bool MavlinkFtpTest::run_tests(void) ut_run_test(_ack_test); ut_run_test(_bad_opcode_test); ut_run_test(_bad_datasize_test); - printf("WARNING! list test commented out, but needs proper resolution!\n"); - //ut_run_test(_list_test); + ut_run_test(_list_test); ut_run_test(_list_eof_test); ut_run_test(_open_badfile_test); ut_run_test(_open_terminate_test); ut_run_test(_terminate_badsession_test); ut_run_test(_read_test); ut_run_test(_read_badsession_test); + ut_run_test(_burst_test); ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 2696192cc6..14c9369b05 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -48,7 +48,18 @@ public: virtual bool run_tests(void); - static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); + + /// Worker data for stream handler + struct BurstInfo { + MavlinkFtpTest* ftp_test_class; + int burst_state; + bool single_packet_file; + uint32_t file_size; + uint8_t* file_bytes; + }; + + static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); static const uint8_t serverSystemId = 50; ///< System ID for server static const uint8_t serverComponentId = 1; ///< Component ID for server @@ -75,31 +86,45 @@ private: bool _terminate_badsession_test(void); bool _read_test(void); bool _read_badsession_test(void); + bool _burst_test(void); bool _removedirectory_test(void); bool _createdirectory_test(void); bool _removefile_test(void); - void _receive_message(const mavlink_message_t *msg); - void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); - bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); + void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req); + void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload); bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, - mavlink_file_transfer_protocol_t *ftp_msg_reply, - MavlinkFTP::PayloadHeader **payload_reply); + const MavlinkFTP::PayloadHeader **payload_reply); void _cleanup_microsd(void); - MavlinkFTP *_ftp_server; - - mavlink_message_t _reply_msg; - - uint16_t _lastOutgoingSeqNumber; - - struct ReadTestCase { + /// A single download test case + struct DownloadTestCase { const char *file; const uint16_t length; + bool singlePacketRead; + bool exactlyFillPacket; }; - static const ReadTestCase _rgReadTestCases[]; + + /// The set of test cases for download testing + static const DownloadTestCase _rgDownloadTestCases[]; + + /// States for stream download handler + enum { + burst_state_first_ack, + burst_state_last_ack, + burst_state_nak_eof, + burst_state_complete + }; + + bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info); + + MavlinkFTP* _ftp_server; + uint16_t _expected_seq_number; + + mavlink_file_transfer_protocol_t _reply_msg; static const char _unittest_microsd_dir[]; static const char _unittest_microsd_file[]; diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index b46d2bd355..e104860937 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = mavlink_tests SRCS = mavlink_tests.cpp \ mavlink_ftp_test.cpp \ + ../mavlink_stream.cpp \ ../mavlink_ftp.cpp \ ../mavlink.c