PX4-Autopilot/src/modules/mavlink/mavlink_ftp.cpp

/****************************************************************************
 *
 *   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.
 *
 ****************************************************************************/

/// @file mavlink_ftp.cpp
///	@author px4dev, Don Gagne <don@thegagnes.com>

#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <errno.h>

#include "mavlink_ftp.h"
#include "mavlink_tests/mavlink_ftp_test.h"

// Uncomment the line below to get better debug output. Never commit with this left on.
//#define MAVLINK_FTP_DEBUG

MavlinkFTP *
MavlinkFTP::get_server(void)
{
	static MavlinkFTP server;
        return &server;
}

MavlinkFTP::MavlinkFTP() :
	_request_bufs{},
	_request_queue{},
	_request_queue_sem{},
	_utRcvMsgFunc{},
	_ftp_test{}
{
	// initialise the request freelist
	dq_init(&_request_queue);
	sem_init(&_request_queue_sem, 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++) {
		_return_request(&_request_bufs[i]);
	}

}

#ifdef MAVLINK_FTP_UNIT_TEST
void
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
{
	_utRcvMsgFunc = rcvMsgFunc;
	_ftp_test = ftp_test;
}
#endif

void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
{
	// get a free request
	struct Request* req = _get_request();

	// if we couldn't get a request slot, just drop it
	if (req == nullptr) {
		warnx("Dropping FTP request: queue full\n");
		return;
	}

	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
		mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
		
#ifdef MAVLINK_FTP_UNIT_TEST
		if (!_utRcvMsgFunc) {
			warnx("Incorrectly written unit test\n");
			return;
		}
		// We use fake ids when unit testing
		req->serverSystemId = MavlinkFtpTest::serverSystemId;
		req->serverComponentId = MavlinkFtpTest::serverComponentId;
		req->serverChannel = MavlinkFtpTest::serverChannel;
#else
		// Not unit testing, use the real thing
		req->serverSystemId = mavlink->get_system_id();
		req->serverComponentId = mavlink->get_component_id();
		req->serverChannel = mavlink->get_channel();
#endif
		
		// This is the system id we want to target when sending
		req->targetSystemId = msg->sysid;
			
		if (req->message.target_system == req->serverSystemId) {
			req->mavlink = mavlink;
#ifdef MAVLINK_FTP_UNIT_TEST
			// We are running in Unit Test mode. Don't queue, just call _worket directly.
			_process_request(req);
#else
			// We are running in normal mode. Queue the request to the worker
			work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
#endif
            return;
		}
	}

	_return_request(req);
}

/// @brief Queued static work queue routine to handle mavlink messages
void
MavlinkFTP::_worker_trampoline(void *arg)
{
	Request* req = reinterpret_cast<Request *>(arg);
	MavlinkFTP* server = MavlinkFTP::get_server();

	// call the server worker with the work item
	server->_process_request(req);
}

/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(Request *req)
{
	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);

	ErrorCode errorCode = kErrNone;

	// basic sanity checks; must validate length before use
	if (payload->size > kMaxDataLength) {
		errorCode = kErrInvalidDataSize;
		goto out;
	}

#ifdef MAVLINK_FTP_DEBUG
	printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
#endif

	switch (payload->opcode) {
	case kCmdNone:
		break;

	case kCmdTerminateSession:
		errorCode = _workTerminate(payload);
		break;

	case kCmdResetSessions:
		errorCode = _workReset(payload);
		break;

	case kCmdListDirectory:
		errorCode = _workList(payload);
		break;

	case kCmdOpenFile:
		errorCode = _workOpen(payload, false);
		break;

	case kCmdCreateFile:
		errorCode = _workOpen(payload, true);
		break;

	case kCmdReadFile:
		errorCode = _workRead(payload);
		break;

	case kCmdWriteFile:
		errorCode = _workWrite(payload);
		break;

	case kCmdRemoveFile:
		errorCode = _workRemoveFile(payload);
		break;

	case kCmdCreateDirectory:
		errorCode = _workCreateDirectory(payload);
		break;
			
	case kCmdRemoveDirectory:
		errorCode = _workRemoveDirectory(payload);
		break;
			
	default:
		errorCode = kErrUnknownCommand;
		break;	
	}

out:
	// handle success vs. error
	if (errorCode == kErrNone) {
		payload->req_opcode = payload->opcode;
		payload->opcode = kRspAck;
#ifdef MAVLINK_FTP_DEBUG
		warnx("FTP: ack\n");
#endif
	} else {
		warnx("FTP: nak %u", errorCode);
		payload->req_opcode = payload->opcode;
		payload->opcode = kRspNak;
		payload->size = 1;
		payload->data[0] = errorCode;
		if (errorCode == kErrFailErrno) {
			payload->size = 2;
			payload->data[1] = errno;
		}
	}

	
	// respond to the request
	_reply(req);

	_return_request(req);
}

/// @brief Sends the specified FTP reponse message out through mavlink
void
MavlinkFTP::_reply(Request *req)
{
	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
	
	payload->seqNumber = payload->seqNumber + 1;

	mavlink_message_t msg;
	msg.checksum = 0;
#ifndef MAVLINK_FTP_UNIT_TEST
	uint16_t len =
#endif
	mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId,	// Sender system id
								    req->serverComponentId,	// Sender component id
								    req->serverChannel,		// Channel to send on
								    &msg,			// Message to pack payload into
								    0,				// Target network
								    req->targetSystemId,	// Target system id
								    0,				// Target component id
								    (const uint8_t*)payload);	// Payload to pack into message
	
	bool success = true;
#ifdef MAVLINK_FTP_UNIT_TEST
	// Unit test hook is set, call that instead
	_utRcvMsgFunc(&msg, _ftp_test);
#else
	Mavlink	*mavlink = req->mavlink;
	
	mavlink->lockMessageBufferMutex();
	success = mavlink->message_buffer_write(&msg, len);
	mavlink->unlockMessageBufferMutex();
		
#endif

	if (!success) {
		warnx("FTP TX ERR");
	}
#ifdef MAVLINK_FTP_DEBUG
	else {
		warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
		      req->serverSystemId,
		      req->serverComponentId,
		      req->serverChannel,
		      msg.checksum);
	}
#endif
}

/// @brief Responds to a List command
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(PayloadHeader* payload)
{
    char dirPath[kMaxDataLength];
    strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
    
	DIR *dp = opendir(dirPath);

	if (dp == nullptr) {
		warnx("FTP: can't open path '%s'", dirPath);
		return kErrFailErrno;
	}
    
#ifdef MAVLINK_FTP_DEBUG
	warnx("FTP: list %s offset %d", dirPath, payload->offset);
#endif

	ErrorCode errorCode = kErrNone;
	struct dirent entry, *result = nullptr;
	unsigned offset = 0;

	// move to the requested offset
	seekdir(dp, payload->offset);

	for (;;) {
		// read the directory entry
		if (readdir_r(dp, &entry, &result)) {
			warnx("FTP: list %s readdir_r failure\n", dirPath);
			errorCode = kErrFailErrno;
			break;
		}

		// no more entries?
		if (result == nullptr) {
			if (payload->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;
		}

		uint32_t fileSize = 0;
		char buf[256];
		char direntType;

		// Determine the directory entry type
		switch (entry.d_type) {
		case DTYPE_FILE:
			// For files we get the file size as well
			direntType = kDirentFile;
			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
			struct stat st;
			if (stat(buf, &st) == 0) {
				fileSize = st.st_size;
			}
			break;
		case DTYPE_DIRECTORY:
			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
				// Don't bother sending these back
				direntType = kDirentSkip;
			} else {
				direntType = kDirentDir;
			}
			break;
		default:
			// We only send back file and diretory entries, skip everything else
			direntType = kDirentSkip;
		}
		
		if (direntType == kDirentSkip) {
			// Skip send only dirent identifier
			buf[0] = '\0';
		} else if (direntType == kDirentFile) {
			// Files send filename and file length
			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
		} else {
			// Everything else just sends name
			strncpy(buf, entry.d_name, sizeof(buf));
			buf[sizeof(buf)-1] = 0;
		}
		size_t nameLen = strlen(buf);

		// Do we have room for the name, the one char directory identifier and the null terminator?
		if ((offset + nameLen + 2) > kMaxDataLength) {
			break;
		}
		
		// Move the data into the buffer
		payload->data[offset++] = direntType;
		strcpy((char *)&payload->data[offset], buf);
#ifdef MAVLINK_FTP_DEBUG
		printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
#endif
		offset += nameLen + 1;
	}

	closedir(dp);
	payload->size = offset;

	return errorCode;
}

/// @brief Responds to an Open command
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
{
	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) {
			return kErrFailErrno;
		}
		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;
	}
	_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;
	}

	return kErrNone;
}

/// @brief Responds to a Read command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader* payload)
{
	int session_index = payload->session;

	if (!_valid_session(session_index)) {
		return kErrInvalidSession;
	}

	// 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 kErrEOF;
	}

	int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
	if (bytes_read < 0) {
		// Negative return indicates error other than eof
		warnx("read fail %d", bytes_read);
		return kErrFailErrno;
	}

	payload->size = bytes_read;

	return kErrNone;
}

/// @brief Responds to a Write command
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
{
#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 kErrUnknownCommand;
#endif
}

/// @brief Responds to a RemoveFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
{
	char file[kMaxDataLength];
	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
	
	if (unlink(file) == 0) {
		payload->size = 0;
		return kErrNone;
	} else {
		return kErrFailErrno;
	}
}

/// @brief Responds to a Terminate command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
{
	if (!_valid_session(payload->session)) {
		return kErrInvalidSession;
	}
    
	::close(_session_fds[payload->session]);
	_session_fds[payload->session] = -1;
	
	payload->size = 0;

	return kErrNone;
}

/// @brief Responds to a Reset command
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(PayloadHeader* payload)
{
	for (size_t i=0; i<kMaxSession; i++) {
		if (_session_fds[i] != -1) {
			::close(_session_fds[i]);
			_session_fds[i] = -1;
		}
	}

	payload->size = 0;
	
	return kErrNone;
}

/// @brief Responds to a RemoveDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
{
	char dir[kMaxDataLength];
	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
	
	if (rmdir(dir) == 0) {
		payload->size = 0;
		return kErrNone;
	} else {
		return kErrFailErrno;
	}
}

/// @brief Responds to a CreateDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
{
	char dir[kMaxDataLength];
	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
	
	if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
		payload->size = 0;
		return kErrNone;
	} else {
		return kErrFailErrno;
	}
}

/// @brief Returns true if the specified session is a valid open session
bool
MavlinkFTP::_valid_session(unsigned index)
{
	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
		return false;
	}
	return true;
}

/// @brief Returns an unused session index
int
MavlinkFTP::_find_unused_session(void)
{
	for (size_t i=0; i<kMaxSession; i++) {
		if (_session_fds[i] == -1) {
			return i;
		}
	}

	return -1;
}

/// @brief Guarantees that the payload data is null terminated.
///     @return Returns a pointer to the payload data as a char *
char *
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
{
	// guarantee nul termination
	if (payload->size < kMaxDataLength) {
		payload->data[payload->size] = '\0';
	} else {
		payload->data[kMaxDataLength - 1] = '\0';
	}

	// and return data
	return (char *)&(payload->data[0]);
}

/// @brief Returns a unused Request entry. NULL if none available.
MavlinkFTP::Request *
MavlinkFTP::_get_request(void)
{
	_lock_request_queue();
	Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
	_unlock_request_queue();
	return req;
}

/// @brief Locks a semaphore to provide exclusive access to the request queue
void
MavlinkFTP::_lock_request_queue(void)
{
	do {}
	while (sem_wait(&_request_queue_sem) != 0);
}

/// @brief Unlocks the semaphore providing exclusive access to the request queue
void
MavlinkFTP::_unlock_request_queue(void)
{
	sem_post(&_request_queue_sem);
}

/// @brief Returns a no longer needed request to the queue
void
MavlinkFTP::_return_request(Request *req)
{
	_lock_request_queue();
	dq_addlast(&req->work.dq, &_request_queue);
	_unlock_request_queue();
}