diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-05 12:35:02 +0200 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-05 12:35:02 +0200 |
commit | 80ed01a5b4324ab13b88dca0b0c8e5ea4f8841ce (patch) | |
tree | df3d7072d1abf74c14747850ad0c32c3c91a3323 /src/modules/mavlink/mavlink_ftp.cpp | |
parent | 3239ba1570fa5357fcae5df3af6feb2ff52da05c (diff) | |
parent | 0fbdb8d326e3a764fde840e426e093bae32bcba6 (diff) | |
download | px4-firmware-80ed01a5b4324ab13b88dca0b0c8e5ea4f8841ce.tar.gz px4-firmware-80ed01a5b4324ab13b88dca0b0c8e5ea4f8841ce.tar.bz2 px4-firmware-80ed01a5b4324ab13b88dca0b0c8e5ea4f8841ce.zip |
Merge pull request #1363 from vooon/ftp_write_support
FTP: write support
Diffstat (limited to 'src/modules/mavlink/mavlink_ftp.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.cpp | 278 |
1 files changed, 242 insertions, 36 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; +} |