WIP: Mavlink file server

5 files changed, 599 insertions, 1 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..4cb31640e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,377 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		req->decode(channel, msg);
+
+		// and queue it for the worker
+		work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->data(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		if (!Session::terminate(hdr->session)) {
+			errorCode = kErrNoRequest;
+		}
+		break;
+
+	case kCmdReset:
+		Session::reset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+	} else {
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->data(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data());
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+
+	// open directory
+
+	// seek in directory
+
+	// read entries until buffer is full
+
+	// send reply
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+
+	// allocate session ID
+	int session = Session::allocate();
+	if (session < 0) {
+		return kErrNoSession;
+	}
+
+	// get the session to open the file
+	if (!Session::get(session)->open(req->dataAsCString(), create)) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+
+	// save the session ID in the reply
+	hdr->session = session;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	// look up session
+	auto session = Session::get(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// read from file
+	int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		return kErrIO;
+	}
+	hdr->size = result;
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+	auto hdr = req->header();
+
+	// look up session
+	auto session = Session::get(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession];
+
+int
+MavlinkFTP::Session::allocate()
+{
+	for (unsigned i = 0; i < kMaxSession; i++) {
+		if (_sessions[i]._fd < 0) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+MavlinkFTP::Session *
+MavlinkFTP::Session::get(unsigned index)
+{
+	if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) {
+		return nullptr;
+	}
+	return &_sessions[index];
+}
+
+void
+MavlinkFTP::Session::terminate()
+{
+	// clean up aborted transfers?
+	if (_fd >= 0) {
+		close(_fd);
+		_fd = -1;
+	}
+}
+
+bool
+MavlinkFTP::Session::terminate(unsigned index)
+ {
+	Session *session = get(index);
+
+	if (session == nullptr) {
+		return false;
+	}
+
+	session->terminate();
+	return true;
+}
+
+void
+MavlinkFTP::Session::reset()
+{
+	for (unsigned i = 0; i < kMaxSession; i++) {
+		terminate(i);
+	}
+}
+
+bool
+MavlinkFTP::Session::open(const char *path, bool create)
+{
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+
+	_fd = open(path, oflag);
+	if (_fd < 0) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count)
+{
+	// can we seek to the location?
+	if (lseek(_fd, offset, SEEK_SET) < 0) {
+		return -1;
+	}
+
+	return read(_fd, buf, count);
+}
+
+int
+MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count)
+{
+	// make sure that the requested offset matches our current position
+	off_t pos = lseek(_fd, 0, SEEK_CUR);
+	if (pos != offset) {
+		return -1;
+	}
+	return write(_fd, buf, count);
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		data()[header()->size] = '\0';
+	} else {
+		data()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)data();
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 000000000..a4f67793e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(mavlink_message_t *msg,
+					       mavlink_channel_t channel);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	struct FileList
+	{
+		uint32_t	fileSize;
+		uint8_t		nameLength;
+		uint8_t		name[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+	class Session
+	{
+	public:
+		Session() : _fd(-1) {}
+
+		static int	allocate();
+		static Session	*get(unsigned index);
+		static bool	terminate(unsigned index);
+		static void	reset();
+
+		void		terminate();
+		bool		open(const char *path, bool create);
+		int		read(off_t offset, uint8_t *buf, uint8_t count);
+		int		append(off_t offset, uint8_t *buf, uint8_t count);
+
+	private:
+		static const unsigned kMaxSession = 2;
+		static Session	_sessions[kMaxSession];
+
+		int		_fd;
+	};
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+		mavlink_channel_t	channel;
+
+		void		decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
+			channel = fromChannel;
+			mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+		}
+
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t		*data()    { return &_message.data[0]; }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+
+		char		*dataAsCString();
+
+	private:
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7c93c1c00..fd1abe5ee 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,6 +112,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0)
 {
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -150,6 +153,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel());
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9ab84b58a..36e6143ac 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -68,6 +68,8 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 
+#include "mavlink_ftp.h"
+
 class Mavlink;
 
 class MavlinkReceiver
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index dcca11977..c348a33db 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -43,7 +43,8 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink