Merge pull request #1363 from vooon/ftp_write_support

FTP: write support

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:
+	case kCmdOpenFileRO:
-		errorCode = _workOpen(payload, false);
+		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,7 +415,7 @@ 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) {
@@ -407,15 +426,15 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 	char *filename = _data_as_cstring(payload);
 
 	uint32_t fileSize = 0;
-	if (!create) {
+	struct stat st;
-		struct stat st;
+	if (stat(filename, &st) != 0) {
-		if (stat(filename, &st) != 0) {
+		// fail only if requested open for read
+		if (oflag & O_RDONLY)
 			return kErrFailErrno;
-		}
+		else
-		fileSize = st.st_size;
+			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) {
@@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 	_session_fds[session_index] = fd;
 
 	payload->session = session_index;
-	if (create) {
+	payload->size = sizeof(uint32_t);
-		payload->size = 0;
+	*((uint32_t*)payload->data) = fileSize;
-	} else {
-		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
+	int session_index = payload->session;
-    // NYI: Coming soon
-	auto hdr = req->header();
 
-	// look up session
+	if (!_valid_session(session_index)) {
-	auto session = getSession(hdr->session);
+		return kErrInvalidSession;
-	if (session == nullptr) {
-		return kErrNoSession;
 	}
 
-	// append to file
+	// Seek to the specified position
-	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+#ifdef MAVLINK_FTP_DEBUG
-
+	warnx("seek %d", payload->offset);
-	if (result < 0) {
-		// XXX might also be no space, I/O, etc.
-		return kErrNotAppend;
-	}
-
-	hdr->size = result;
-	return kErrNone;
-#else
-	return kErrUnknownCommand;
 #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);
+	if (bytes_written < 0) {
+		// Negative return indicates error other than eof
+		warnx("write fail %d", bytes_written);
+		return kErrFailErrno;
+	}
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = bytes_written;
+
+	return kErrNone;
 }
 
 /// @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;
+}

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