Merge remote-tracking branch 'upstream/master' into swissfang

6 files changed, 273 insertions, 57 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 2a85c3702..f17497aa8 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -34,6 +34,7 @@
 /// @file mavlink_ftp.cpp
 ///	@author px4dev, Don Gagne <don@thegagnes.com>
 
+#include <crc32.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
@@ -180,12 +181,16 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workList(payload);
 		break;
 
-	case kCmdOpenFile:
-		errorCode = _workOpen(payload, false);
+	case kCmdOpenFileRO:
+		errorCode = _workOpen(payload, O_RDONLY);
 		break;
 
 	case kCmdCreateFile:
-		errorCode = _workOpen(payload, true);
+		errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
+		break;
+
+	case kCmdOpenFileWO:
+		errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
 		break;
 
 	case kCmdReadFile:
@@ -200,6 +205,14 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveFile(payload);
 		break;
 
+	case kCmdRename:
+		errorCode = _workRename(payload);
+		break;
+
+	case kCmdTruncateFile:
+		errorCode = _workTruncateFile(payload);
+		break;
+
 	case kCmdCreateDirectory:
 		errorCode = _workCreateDirectory(payload);
 		break;
@@ -208,6 +221,11 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveDirectory(payload);
 		break;
 			
+
+	case kCmdCalcFileCRC32:
+		errorCode = _workCalcFileCRC32(payload);
+		break;
+
 	default:
 		errorCode = kErrUnknownCommand;
 		break;	
@@ -222,6 +240,7 @@ out:
 		warnx("FTP: ack\n");
 #endif
 	} else {
+		int r_errno = errno;
 		warnx("FTP: nak %u", errorCode);
 		payload->req_opcode = payload->opcode;
 		payload->opcode = kRspNak;
@@ -229,7 +248,7 @@ out:
 		payload->data[0] = errorCode;
 		if (errorCode == kErrFailErrno) {
 			payload->size = 2;
-			payload->data[1] = errno;
+			payload->data[1] = r_errno;
 		}
 	}
 
@@ -396,27 +415,27 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 
 /// @brief Responds to an Open command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
+MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 {
 	int session_index = _find_unused_session();
 	if (session_index < 0) {
 		warnx("FTP: Open failed - out of sessions\n");
 		return kErrNoSessionsAvailable;
 	}
-	
+
 	char *filename = _data_as_cstring(payload);
-	
+
 	uint32_t fileSize = 0;
-	if (!create) {
-		struct stat st;
-		if (stat(filename, &st) != 0) {
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		// fail only if requested open for read
+		if (oflag & O_RDONLY)
 			return kErrFailErrno;
-		}
-		fileSize = st.st_size;
+		else
+			st.st_size = 0;
 	}
+	fileSize = st.st_size;
 
-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-    
 	int fd = ::open(filename, oflag);
 	if (fd < 0) {
 		return kErrFailErrno;
@@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 	_session_fds[session_index] = fd;
 
 	payload->session = session_index;
-	if (create) {
-		payload->size = 0;
-	} else {
-		payload->size = sizeof(uint32_t);
-		*((uint32_t*)payload->data) = fileSize;
-	}
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = fileSize;
 
 	return kErrNone;
 }
@@ -470,29 +485,33 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
-#if 0
-    // NYI: Coming soon
-	auto hdr = req->header();
+	int session_index = payload->session;
 
-	// look up session
-	auto session = getSession(hdr->session);
-	if (session == nullptr) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}
 
-	// append to file
-	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+	// 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) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrFailErrno;
+	}
 
-	if (result < 0) {
-		// XXX might also be no space, I/O, etc.
-		return kErrNotAppend;
+	int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
+	if (bytes_written < 0) {
+		// Negative return indicates error other than eof
+		warnx("write fail %d", bytes_written);
+		return kErrFailErrno;
 	}
 
-	hdr->size = result;
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = bytes_written;
+
 	return kErrNone;
-#else
-	return kErrUnknownCommand;
-#endif
 }
 
 /// @brief Responds to a RemoveFile command
@@ -510,6 +529,81 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
 	}
 }
 
+/// @brief Responds to a TruncateFile command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
+{
+	char file[kMaxDataLength];
+	const char temp_file[] = "/fs/microsd/.trunc.tmp";
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	payload->size = 0;
+
+	// emulate truncate(file, payload->offset) by
+	// copying to temp and overwrite with O_TRUNC flag.
+
+	struct stat st;
+	if (stat(file, &st) != 0) {
+		return kErrFailErrno;
+	}
+
+	if (!S_ISREG(st.st_mode)) {
+		errno = EISDIR;
+		return kErrFailErrno;
+	}
+
+	// check perms allow us to write (not romfs)
+	if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
+		errno = EROFS;
+		return kErrFailErrno;
+	}
+
+	if (payload->offset == (unsigned)st.st_size) {
+		// nothing to do
+		return kErrNone;
+	}
+	else if (payload->offset == 0) {
+		// 1: truncate all data
+		int fd = ::open(file, O_TRUNC | O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		::close(fd);
+		return kErrNone;
+	}
+	else if (payload->offset > (unsigned)st.st_size) {
+		// 2: extend file
+		int fd = ::open(file, O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
+			::close(fd);
+			return kErrFailErrno;
+		}
+
+		bool ok = 1 == ::write(fd, "", 1);
+		::close(fd);
+
+		return (ok)? kErrNone : kErrFailErrno;
+	}
+	else {
+		// 3: truncate
+		if (_copy_file(file, temp_file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (_copy_file(temp_file, file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (::unlink(temp_file) != 0) {
+			return kErrFailErrno;
+		}
+
+		return kErrNone;
+	}
+}
+
 /// @brief Responds to a Terminate command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workTerminate(PayloadHeader* payload)
@@ -542,6 +636,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
 	return kErrNone;
 }
 
+/// @brief Responds to a Rename command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRename(PayloadHeader* payload)
+{
+	char oldpath[kMaxDataLength];
+	char newpath[kMaxDataLength];
+
+	char *ptr = _data_as_cstring(payload);
+	size_t oldpath_sz = strlen(ptr);
+
+	if (oldpath_sz == payload->size) {
+		// no newpath
+		errno = EINVAL;
+		return kErrFailErrno;
+	}
+
+	strncpy(oldpath, ptr, kMaxDataLength);
+	strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
+
+	if (rename(oldpath, newpath) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
 /// @brief Responds to a RemoveDirectory command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
@@ -572,6 +693,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
 	}
 }
 
+/// @brief Responds to a CalcFileCRC32 command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
+{
+	char file_buf[256];
+	uint32_t checksum = 0;
+	ssize_t bytes_read;
+	strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
+
+	int fd = ::open(file_buf, O_RDONLY);
+	if (fd < 0) {
+		return kErrFailErrno;
+	}
+
+	do {
+		bytes_read = ::read(fd, file_buf, sizeof(file_buf));
+		if (bytes_read < 0) {
+			int r_errno = errno;
+			::close(fd);
+			errno = r_errno;
+			return kErrFailErrno;
+		}
+
+		checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
+	} while (bytes_read == sizeof(file_buf));
+
+	::close(fd);
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = checksum;
+	return kErrNone;
+}
+
 /// @brief Returns true if the specified session is a valid open session
 bool
 MavlinkFTP::_valid_session(unsigned index)
@@ -645,3 +799,55 @@ MavlinkFTP::_return_request(Request *req)
 	_unlock_request_queue();
 }
 
+/// @brief Copy file (with limited space)
+int
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+{
+	char buff[512];
+	int src_fd = -1, dst_fd = -1;
+	int op_errno = 0;
+
+	src_fd = ::open(src_path, O_RDONLY);
+	if (src_fd < 0) {
+		return -1;
+	}
+
+	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
+	if (dst_fd < 0) {
+		op_errno = errno;
+		::close(src_fd);
+		errno = op_errno;
+		return -1;
+	}
+
+	while (length > 0) {
+		ssize_t bytes_read, bytes_written;
+		size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
+
+		bytes_read = ::read(src_fd, buff, blen);
+		if (bytes_read == 0) {
+			// EOF
+			break;
+		}
+		else if (bytes_read < 0) {
+			warnx("cp: read");
+			op_errno = errno;
+			break;
+		}
+
+		bytes_written = ::write(dst_fd, buff, bytes_read);
+		if (bytes_written != bytes_read) {
+			warnx("cp: short write");
+			op_errno = errno;
+			break;
+		}
+
+		length -= bytes_written;
+	}
+
+	::close(src_fd);
+	::close(dst_fd);
+
+	errno = op_errno;
+	return (length > 0)? -1 : 0;
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 657e2f855..bef6775a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -89,13 +89,17 @@ public:
 		kCmdTerminateSession,	///< Terminates open Read session
 		kCmdResetSessions,	///< Terminates all open Read sessions
 		kCmdListDirectory,	///< List files in <path> from <offset>
-		kCmdOpenFile,		///< Opens file at <path> for reading, returns <session>
+		kCmdOpenFileRO,		///< Opens file at <path> for reading, returns <session>
 		kCmdReadFile,		///< Reads <size> bytes from <offset> in <session>
 		kCmdCreateFile,		///< Creates file at <path> for writing, returns <session>
-		kCmdWriteFile,		///< Appends <size> bytes to file in <session>
+		kCmdWriteFile,		///< Writes <size> bytes to <offset> in <session>
 		kCmdRemoveFile,		///< Remove file at <path>
 		kCmdCreateDirectory,	///< Creates directory at <path>
 		kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+		kCmdOpenFileWO,		///< Opens file at <path> for writing, returns <session>
+		kCmdTruncateFile,	///< Truncate file at <path> to <offset> length
+		kCmdRename,		///< Rename <path1> to <path2>
+		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
 		
 		kRspAck = 128,		///< Ack response
 		kRspNak			///< Nak response
@@ -138,9 +142,10 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
+	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
-	ErrorCode	_workOpen(PayloadHeader *payload, bool create);
+	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
 	ErrorCode	_workRead(PayloadHeader *payload);
 	ErrorCode	_workWrite(PayloadHeader *payload);
 	ErrorCode	_workTerminate(PayloadHeader *payload);
@@ -148,6 +153,9 @@ private:
 	ErrorCode	_workRemoveDirectory(PayloadHeader *payload);
 	ErrorCode	_workCreateDirectory(PayloadHeader *payload);
 	ErrorCode	_workRemoveFile(PayloadHeader *payload);
+	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
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index 022041c74..4caa820b6 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest()
 }
 
 /// @brief Called before every test to initialize the FTP Server.
-void MavlinkFtpTest::init(void)
+void MavlinkFtpTest::_init(void)
 {
 	_ftp_server = new MavlinkFTP;;
 	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
@@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void)
 }
 
 /// @brief Called after every test to take down the FTP Server.
-void MavlinkFtpTest::cleanup(void)
+void MavlinkFtpTest::_cleanup(void)
 {
 	delete _ftp_server;
 	
@@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/foo";	// non-existent file
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 		struct stat st;
 		const ReadTestCase *test = &_rgReadTestCases[i];
 		
-		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void)
 		// 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::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void)
 }
 
 /// @brief Runs all the unit tests
-void MavlinkFtpTest::runTests(void)
+bool MavlinkFtpTest::run_tests(void)
 {
 	ut_run_test(_ack_test);
 	ut_run_test(_bad_opcode_test);
@@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void)
 	ut_run_test(_removedirectory_test);
 	ut_run_test(_createdirectory_test);
 	ut_run_test(_removefile_test);
+	
+	return (_tests_failed == 0);
+
 }
 
+ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
index babd909da..2696192cc 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
@@ -46,10 +46,7 @@ public:
 	MavlinkFtpTest();
 	virtual ~MavlinkFtpTest();
 	
-	virtual void init(void);
-	virtual void cleanup(void);
-	
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 	
 	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
 	
@@ -65,6 +62,9 @@ public:
 	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
 	
 private:
+	virtual void _init(void);
+	virtual void _cleanup(void);
+	
 	bool _ack_test(void);
 	bool _bad_opcode_test(void);
 	bool _bad_datasize_test(void);
@@ -105,3 +105,4 @@ private:
 	static const char _unittest_microsd_file[];
 };
 
+bool mavlink_ftp_test(void);
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
index d9c1e413a..10cbcb0ec 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
@@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
 
 int mavlink_tests_main(int argc, char *argv[])
 {
-	MavlinkFtpTest* test = new MavlinkFtpTest;
-	
-	test->runTests();
-	test->printResults();
-
-	return 0;
+	return mavlink_ftp_test() ? 0 : -1;
 }
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
index 07cfce1dc..1cc28cce1 100644
--- a/src/modules/mavlink/mavlink_tests/module.mk
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -45,4 +45,6 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MODULE_STACKSIZE = 5000
 
+MAXOPTIMIZATION	 = -Os
+
 EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST