aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/mavlink_ftp.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/mavlink_ftp.cpp')
-rw-r--r--src/modules/mavlink/mavlink_ftp.cpp415
1 files changed, 415 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..ca846a465
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ * 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 <stdio.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);
+
+ // 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++) {
+ _qFree(&_workBufs[i]);
+ }
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+ // get a free request
+ auto req = _dqFree();
+
+ // if we couldn't get a request slot, just drop it
+ if (req != nullptr) {
+
+ // decode the request
+ if (req->decode(mavlink, msg)) {
+
+ // and queue it for the worker
+ work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+ } else {
+ _qFree(req);
+ }
+ }
+}
+
+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->rawData(), req->dataSize()) != messageCRC) {
+ errorCode = kErrNoRequest;
+ goto out;
+ printf("ftp: bad crc\n");
+ }
+
+ printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+ switch (hdr->opcode) {
+ case kCmdNone:
+ break;
+
+ case kCmdTerminate:
+ errorCode = _workTerminate(req);
+ break;
+
+ case kCmdReset:
+ errorCode = _workReset();
+ 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;
+ printf("FTP: ack\n");
+ } else {
+ printf("FTP: nak %u\n", errorCode);
+ 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->rawData(), req->dataSize());
+
+ // then pack and send the reply back to the request source
+ req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+ auto hdr = req->header();
+ DIR *dp = opendir(req->dataAsCString());
+
+ if (dp == nullptr) {
+ printf("FTP: can't open path '%s'\n", req->dataAsCString());
+ return kErrNotDir;
+ }
+
+ ErrorCode errorCode = kErrNone;
+ struct dirent entry, *result = nullptr;
+ unsigned offset = 0;
+
+ // move to the requested offset
+ seekdir(dp, hdr->offset);
+
+ for (;;) {
+ // read the directory entry
+ if (readdir_r(dp, &entry, &result)) {
+ errorCode = kErrIO;
+ break;
+ }
+
+ // 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;
+ }
+
+ // name too big to fit?
+ if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+ break;
+ }
+
+ // store the type marker
+ switch (entry.d_type) {
+ case DTYPE_FILE:
+ hdr->data[offset++] = kDirentFile;
+ break;
+ case DTYPE_DIRECTORY:
+ hdr->data[offset++] = kDirentDir;
+ break;
+ default:
+ hdr->data[offset++] = kDirentUnknown;
+ break;
+ }
+
+ // copy the name, which we know will fit
+ strcpy((char *)&hdr->data[offset], entry.d_name);
+ offset += strlen(entry.d_name) + 1;
+ printf("FTP: list %s\n", entry.d_name);
+ }
+
+ closedir(dp);
+ hdr->size = offset;
+
+ return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+ auto hdr = req->header();
+
+ int session_index = _findUnusedSession();
+ if (session_index < 0) {
+ return kErrNoSession;
+ }
+
+ 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;
+
+ hdr->session = session_index;
+ hdr->size = 0;
+
+ return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+ auto hdr = req->header();
+
+ 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;
+ }
+
+ 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;
+ }
+
+ 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 = getSession(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;
+#else
+ return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+ auto hdr = req->header();
+
+ // for now, send error reply
+ return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+ auto hdr = req->header();
+
+ if (!_validSession(hdr->session)) {
+ return kErrNoSession;
+ }
+
+ ::close(_session_fds[hdr->session]);
+
+ return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+ 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::_validSession(unsigned index)
+{
+ if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+ return false;
+ }
+ return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+ for (size_t i=0; i<kMaxSession; i++) {
+ if (_session_fds[i] == -1) {
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+ // guarantee nul termination
+ if (header()->size < kMaxDataLength) {
+ requestData()[header()->size] = '\0';
+ } else {
+ requestData()[kMaxDataLength - 1] = '\0';
+ }
+
+ // and return data
+ return (char *)&(header()->data[0]);
+}