PX4 · LorenzMeier · Jun 28, 2014 · Jun 28, 2014
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
@@ -55,6 +55,11 @@ MavlinkFTP::MavlinkFTP()
 	// initialise the request freelist
 	dq_init(&_workFree);
 	sem_init(&_lock, 0, 1);
+
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {
@@ -122,13 +127,11 @@ MavlinkFTP::_worker(Request *req)
 		break;
 
 	case kCmdTerminate:
-		if (!Session::terminate(hdr->session)) {
-			errorCode = kErrNoRequest;
-		}
+        errorCode = _workTerminate(req);
 		break;
 
 	case kCmdReset:
-		Session::reset();
+		errorCode = _workReset();
 		break;
 
 	case kCmdList:
@@ -219,6 +222,12 @@ MavlinkFTP::_workList(Request *req)
 
 		// no more entries?
 		if (result == nullptr) {
+            if (hdr->offset != 0 && offset == 0) {
+                // User is requesting subsequent dir entries but there were none. This means the user asked
+                // to seek past EOF.
+                errorCode = kErrEOF;
+            }
+            // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
 			break;
 		}
 
@@ -256,20 +265,21 @@ MavlinkFTP::ErrorCode
 MavlinkFTP::_workOpen(Request *req, bool create)
 {
 	auto hdr = req->header();
-
-	// allocate session ID
-	int session = Session::allocate();
-	if (session < 0) {
+
+    int session_index = _findUnusedSession();
+	if (session_index < 0) {
 		return kErrNoSession;
 	}
 
-	// get the session to open the file
-	if (!Session::get(session)->open(req->dataAsCString(), create)) {
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+
+    int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
 		return create ? kErrPerm : kErrNotFile;
 	}
+    _session_fds[session_index] = fd;
 
-	// save the session ID in the reply
-	hdr->session = session;
+	hdr->session = session_index;
 	hdr->size = 0;
 
 	return kErrNone;
@@ -280,29 +290,40 @@ MavlinkFTP::_workRead(Request *req)
 {
 	auto hdr = req->header();
 
-	// look up session
-	auto session = Session::get(hdr->session);
-	if (session == nullptr) {
+    int session_index = hdr->session;
+    
+    if (!_validSession(session_index)) {
 		return kErrNoSession;
+    }
+
+    // Seek to the specified position
+    printf("Seek %d\n", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+        // Unable to see to the specified location
+		return kErrEOF;
 	}
-
-	// read from file
-	int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
-
-	if (result < 0) {
+
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+        // Negative return indicates error other than eof
 		return kErrIO;
 	}
-	hdr->size = result;
+
+    printf("Read success %d\n", bytes_read);
+	hdr->size = bytes_read;
+
 	return kErrNone;
 }
 
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(Request *req)
 {
+#if 0
+    // NYI: Coming soon
 	auto hdr = req->header();
 
 	// look up session
-	auto session = Session::get(hdr->session);
+	auto session = getSession(hdr->session);
 	if (session == nullptr) {
 		return kErrNoSession;
 	}
@@ -317,6 +338,9 @@ MavlinkFTP::_workWrite(Request *req)
 
 	hdr->size = result;
 	return kErrNone;
+#else
+	return kErrPerm;
+#endif
 }
 
 MavlinkFTP::ErrorCode
@@ -328,91 +352,52 @@ MavlinkFTP::_workRemove(Request *req)
 	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()
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
 {
-	// 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;
-	}
+	auto hdr = req->header();
+
+    if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+    }
+
+    ::close(_session_fds[hdr->session]);
 
-	session->terminate();
-	return true;
+    return kErrNone;
 }
 
-void
-MavlinkFTP::Session::reset()
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
 {
-	for (unsigned i = 0; i < kMaxSession; i++) {
-		terminate(i);
-	}
+    for (size_t i=0; i<kMaxSession; i++) {
+        if (_session_fds[i] != -1) {
+            ::close(_session_fds[i]);
+            _session_fds[i] = -1;
+        }
+    }
+
+    return kErrNone;
 }
 
 bool
-MavlinkFTP::Session::open(const char *path, bool create)
+MavlinkFTP::_validSession(unsigned index)
 {
-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-
-	_fd = open(path, oflag);
-	if (_fd < 0) {
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
 		return false;
 	}
 	return true;
 }
 
 int
-MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count)
+MavlinkFTP::_findUnusedSession(void)
 {
-	// 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);
+    for (size_t i=0; i<kMaxSession; i++) {
+        if (_session_fds[i] == -1) {
+            return i;
+        }
+    }
+
+    return -1;
 }
 
 char *

diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
@@ -115,27 +115,11 @@ class MavlinkFTP
 		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);
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
 
-	private:
-		static const unsigned kMaxSession = 2;
-		static Session	_sessions[kMaxSession];
-
-		int		_fd;
-	};
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
 
 	class Request
 	{
@@ -163,7 +147,11 @@ class MavlinkFTP
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
 				_mavlink->get_channel(), &msg, sequence(), rawData());
 
-			if (!_mavlink->message_buffer_write(&msg, len)) {
+            _mavlink->lockMessageBufferMutex();
+            bool fError = _mavlink->message_buffer_write(&msg, len);
+            _mavlink->unlockMessageBufferMutex();
+
+            if (!fError) {
 				warnx("FTP TX ERR");
 			} else {
 				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
@@ -211,6 +199,8 @@ class MavlinkFTP
 	ErrorCode		_workRead(Request *req);
 	ErrorCode		_workWrite(Request *req);
 	ErrorCode		_workRemove(Request *req);
+    ErrorCode       _workTerminate(Request *req);
+    ErrorCode       _workReset();
 
 	// work freelist
 	Request			_workBufs[kRequestQueueSize];
@@ -232,4 +222,5 @@ class MavlinkFTP
 		_qUnlock();
 		return req;
 	}
+
 };
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
@@ -2074,38 +2074,46 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_passing_on || _ftp_on) {
 
 			bool is_part;
-			void *read_ptr;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
 
-			/* guard get ptr by mutex */
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
-
-				// int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq;
-
-				const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr;
-				/* write first part of buffer */
-				_mavlink_resend_uart(_channel, msg);
-
-				// mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq;
-				// mavlink_msg_system_time_send(get_channel(), 255, 255);
-
-				message_buffer_mark_read(available);
-
+                // Reconstruct message from buffer
+
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
+
+                // Pull a single message from the buffer
+                int read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+
+                memcpy(write_ptr, read_ptr, read_count);
+
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+
+				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
-				// XXX this doesn't quite work, as the resend UART call assumes a continous block
-				if (is_part) {
-					/* guard get ptr by mutex */
-					pthread_mutex_lock(&_message_buffer_mutex);
-					available = message_buffer_get_ptr(&read_ptr, &is_part);
-					pthread_mutex_unlock(&_message_buffer_mutex);
-
-					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
+                    write_ptr += read_count;
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
+                    memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
+
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
 			}
 		}
 

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
@@ -228,6 +228,9 @@ class Mavlink
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
 	bool			message_buffer_write(void *ptr, int size);
+
+    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
 protected:
 	Mavlink			*next;