Merge pull request #2120 from DonLakeFlyer/BurstDownload

New burst mode ftp file download

src/modules/mavlink/mavlink_ftp.cpp

@@ -42,116 +42,130 @@
 #include <errno.h>
 
 #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 *
+int buf_size_1 = 0;
-MavlinkFTP::get_server(void)
+int buf_size_2 = 0;
+
+MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
+	MavlinkStream(mavlink),
+	_session_info{},
+	_utRcvMsgFunc{},
+	_worker_data{}
 {
-	static MavlinkFTP server;
+	// initialize session
-        return &server;
+	_session_info.fd = -1;
 }
 
-MavlinkFTP::MavlinkFTP() :
+MavlinkFTP::~MavlinkFTP()
-	_request_bufs{},
-	_request_queue{},
-	_request_queue_sem{},
-	_utRcvMsgFunc{},
-	_ftp_test{}
 {
-	// initialise the request freelist
+	
-	dq_init(&_request_queue);
+}
-	sem_init(&_request_queue_sem, 0, 1);
 
-	// initialize session list
+const char*
-	for (size_t i=0; i<kMaxSession; i++) {
+MavlinkFTP::get_name(void) const
-		_session_fds[i] = -1;
+{
-	}
+	return "MAVLINK_FTP";
-
+}
-	// drop work entries onto the free list
+
-	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+uint8_t
-		_return_request(&_request_bufs[i]);
+MavlinkFTP::get_id(void)
+{
+	return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL;
+}
+
+unsigned
+MavlinkFTP::get_size(void)
+{
+	if (_session_info.stream_download) {
+		return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		
+	} else {
+		return 0;
 	}
+}
 
+MavlinkStream*
+MavlinkFTP::new_instance(Mavlink *mavlink)
+{
+	return new MavlinkFTP(mavlink);
 }
 
 #ifdef MAVLINK_FTP_UNIT_TEST
 void
-MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
+MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data)
 {
 	_utRcvMsgFunc = rcvMsgFunc;
-	_ftp_test = ftp_test;
+	_worker_data = worker_data;
 }
 #endif
 
-void
+uint8_t
-MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+MavlinkFTP::_getServerSystemId(void)
 {
-	// get a free request
-	struct Request* req = _get_request();
-
-	// if we couldn't get a request slot, just drop it
-	if (req == nullptr) {
-		warnx("Dropping FTP request: queue full\n");
-		return;
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
-		mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
-		
 #ifdef MAVLINK_FTP_UNIT_TEST
-		if (!_utRcvMsgFunc) {
+	// We use fake ids when unit testing
-			warnx("Incorrectly written unit test\n");
+	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<Request *>(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<PayloadHeader *>(&req->message.payload[0]);
+	bool stream_send = false;
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&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:
 		}
 	}
 
-	
+	// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak.
-	// respond to the request
+	if (!stream_send || errorCode != kErrNone) {
-	_reply(req);
+		// respond to the request
-
+		ftp_req->target_system = target_system_id;
-	_return_request(req);
+		_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<PayloadHeader *>(&req->message.payload[0]);
 	
-	payload->seqNumber = payload->seqNumber + 1;
+#ifdef MAVLINK_FTP_DEBUG
-
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
-	mavlink_message_t msg;
+	warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
-	msg.checksum = 0;
-#ifndef MAVLINK_FTP_UNIT_TEST
-	uint16_t len =
 #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->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
-	
-	mavlink->lockMessageBufferMutex();
-	success = mavlink->message_buffer_write(&msg, len);
-	mavlink->unlockMessageBufferMutex();
-		
 #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_info.fd >= 0) {
-	if (session_index < 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 (payload->session != 0 || _session_info.fd < 0) {
-
-	if (!_valid_session(session_index)) {
 		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) {
+	// We have to test seek past EOF ourselves, lseek will allow seek past EOF
-		// Unable to see to the specified location
+	if (payload->offset >= _session_info.file_size) {
-		warnx("seek fail");
+		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 (payload->session != 0 && _session_info.fd < 0) {
-
-	if (!_valid_session(session_index)) {
 		return kErrInvalidSession;
 	}
 
-	// Seek to the specified position
+	if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
-#ifdef MAVLINK_FTP_DEBUG
-	warnx("seek %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");
 		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]);
+	::close(_session_info.fd);
-	_session_fds[payload->session] = -1;
+	_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; i<kMaxSession; i++) {
+	if (_session_info.fd != -1) {
-		if (_session_fds[i] != -1) {
+		::close(_session_info.fd);
-			::close(_session_fds[i]);
+		_session_info.fd = -1;
-			_session_fds[i] = -1;
+		_session_info.stream_download = false;
-		}
 	}
 
 	payload->size = 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; i<kMaxSession; i++) {
-		if (_session_fds[i] == -1) {
-			return i;
-		}
-	}
-
-	return -1;
-}
-
 /// @brief Guarantees that the payload data is null terminated.
 ///     @return Returns a pointer to the payload data as a char *
 char *
@@ -765,40 +752,6 @@ MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
 	return (char *)&(payload->data[0]);
 }
 
-/// @brief Returns a unused Request entry. NULL if none available.
-MavlinkFTP::Request *
-MavlinkFTP::_get_request(void)
-{
-	_lock_request_queue();
-	Request* req = reinterpret_cast<Request *>(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<PayloadHeader *>(&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);
+}
+

src/modules/mavlink/mavlink_ftp.h

@@ -39,45 +39,44 @@
 #include <dirent.h>
 #include <queue.h>
 
-#include <nuttx/wqueue.h>
 #include <systemlib/err.h>
 
-#include "mavlink_messages.h"
+#include "mavlink_stream.h"
-#include "mavlink_main.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.
+/// 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 : public MavlinkStream
-class MavlinkFTP
 {
 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.
+	/// Handle possible FTP message
-	void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
+	void handle_message(const mavlink_message_t *msg);
-    
+
-	typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	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
+	///	@param worker_data Data to pass to worker
-	void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
+	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 <path> to <offset> length
 		kCmdRename,		///< Rename <path1> to <path2>
 		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
+		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(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
-	void		_process_request(Request *req);
+	void		_reply(mavlink_file_transfer_protocol_t* ftp_req);
-	void		_reply(Request *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);
+	uint8_t _getServerSystemId(void);
-	bool _valid_session(unsigned index);
+	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
+	struct SessionInfo {
-	int	_session_fds[kMaxSession];	///< Session file descriptors, 0 for empty slot
+		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
+	ReceiveMessageFunc_t	_utRcvMsgFunc;	///< Unit test override for mavlink message sending
-	MavlinkFtpTest *_ftp_test;		///< Additional parameter to _utRcvMsgFunc;
+	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;
 };

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. */

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;
+	MavlinkMissionManager		*_mission_manager;
-	MavlinkParametersManager *_parameters_manager;
+	MavlinkParametersManager	*_parameters_manager;
+	MavlinkFTP			*_mavlink_ftp;
 
 	MAVLINK_MODE 		_mode;
 

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;

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;
 

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[] = {
+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},	// Read takes less than single packet
+	{ "/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) },		// Read completely fills 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 },	// Read take two packets
+	{ "/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{},
+	_ftp_server(nullptr),
-	_reply_msg{},
+	_expected_seq_number(0),
-	_lastOutgoingSeqNumber{}
+	_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;;
+	_expected_seq_number = 0;
-	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
+	_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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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; j<reply->size-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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	MavlinkFTP::PayloadHeader		*reply;
 	
-	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
-		const ReadTestCase *test = &_rgReadTestCases[i];
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
 		
 		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
@@ -301,7 +294,6 @@ bool MavlinkFtpTest::_open_terminate_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;
@@ -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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgDownloadTestCases[0].file;
-	const char				*file = _rgReadTestCases[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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	MavlinkFTP::PayloadHeader		*reply;
 	
-	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
-		const ReadTestCase *test = &_rgReadTestCases[i];
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
 		
 		// Read in the file so we can compare it to what we get back
 		ut_compare("stat failed", stat(test->file, &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)) {
+		payload.offset += expected_bytes;
-			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);
+		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; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
+		struct stat st;
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
+		
+		// Read in the file so we can compare it to what we get back
+		ut_compare("stat failed", stat(test->file, &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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgDownloadTestCases[0].file;
-	const char				*file = _rgReadTestCases[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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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;
+	const MavlinkFTP::PayloadHeader		*reply;
-	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_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
-void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
 {
 	// 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
+bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t	*ftp_msg,	///< Incoming FTP message
-				     mavlink_file_transfer_protocol_t	*ftp_msg,	///< Decoded FTP message
+				     const MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
-				     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<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
+	*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
 	
 	// Make sure we have a good sequence number
-	ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
+	ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
-	
+	_expected_seq_number++;
-	// Bump sequence number for next outgoing message
-	_lastOutgoingSeqNumber++;
 	
 	return true;
 }
 
 /// @brief Initializes an FTP message into a mavlink message
-void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
-				    uint8_t			size,			///< size in bytes of data
+				    uint8_t				size,			///< size in bytes of data
-				    const uint8_t		*data,			///< Data to start into FTP message payload
+				    const uint8_t			*data,			///< Data to start into FTP message payload
-				    mavlink_message_t		*msg)			///< Returned mavlink message
+				    mavlink_message_t			*msg)			///< Returned mavlink message
 {
 	uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
 	MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(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->burst_complete = 0;
-	payload->padding[1] = 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
+				      const MavlinkFTP::PayloadHeader	**payload_reply)	///< Payload inside FTP message response
-				      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);
+	_ftp_server->handle_message(&msg);
-	return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
+	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);

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 _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
-	void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
+	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_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
+	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,
+                           const MavlinkFTP::PayloadHeader	**payload_reply);
-                           MavlinkFTP::PayloadHeader		**payload_reply);
 	void _cleanup_microsd(void);
 	
-	MavlinkFTP *_ftp_server;
+	/// A single download test case
-    
+	struct DownloadTestCase {
-	mavlink_message_t _reply_msg;
-    
-	uint16_t _lastOutgoingSeqNumber;
-	
-	struct ReadTestCase {
 		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[];

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