forked from Archive/PX4-Autopilot
mavlink: consolidated nuttx and posix changes
Removed nuttx and posix specific files for mavlink_ftp and mavlink_receiver. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
parent
d63d2f7a61
commit
e764c68d0a
|
|
@ -820,7 +820,12 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
|
|||
return -1;
|
||||
}
|
||||
|
||||
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY, 0x0777);
|
||||
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY
|
||||
// POSIX requires the permissions to be supplied if O_CREAT passed
|
||||
#ifdef __PX4_POSIX
|
||||
, 0x0777
|
||||
#endif
|
||||
);
|
||||
if (dst_fd < 0) {
|
||||
op_errno = errno;
|
||||
::close(src_fd);
|
||||
|
|
@ -1,853 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <crc32.h>
|
||||
#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 kCmdOpenFileRO:
|
||||
errorCode = _workOpen(payload, O_RDONLY);
|
||||
break;
|
||||
|
||||
case kCmdCreateFile:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdOpenFileWO:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdReadFile:
|
||||
errorCode = _workRead(payload);
|
||||
break;
|
||||
|
||||
case kCmdWriteFile:
|
||||
errorCode = _workWrite(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveFile:
|
||||
errorCode = _workRemoveFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdRename:
|
||||
errorCode = _workRename(payload);
|
||||
break;
|
||||
|
||||
case kCmdTruncateFile:
|
||||
errorCode = _workTruncateFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdCreateDirectory:
|
||||
errorCode = _workCreateDirectory(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveDirectory:
|
||||
errorCode = _workRemoveDirectory(payload);
|
||||
break;
|
||||
|
||||
|
||||
case kCmdCalcFileCRC32:
|
||||
errorCode = _workCalcFileCRC32(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 {
|
||||
int r_errno = errno;
|
||||
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] = r_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, 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;
|
||||
struct stat st;
|
||||
if (stat(filename, &st) != 0) {
|
||||
// fail only if requested open for read
|
||||
if (oflag & O_RDONLY)
|
||||
return kErrFailErrno;
|
||||
else
|
||||
st.st_size = 0;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
|
||||
int fd = ::open(filename, oflag);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
_session_fds[session_index] = fd;
|
||||
|
||||
payload->session = session_index;
|
||||
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)
|
||||
{
|
||||
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 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
|
||||
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 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)
|
||||
{
|
||||
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 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)
|
||||
{
|
||||
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 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)
|
||||
{
|
||||
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();
|
||||
}
|
||||
|
||||
/// @brief Copy file (with limited space)
|
||||
int
|
||||
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_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;
|
||||
}
|
||||
|
|
@ -41,7 +41,12 @@
|
|||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/sched.h>
|
||||
#else
|
||||
#include <px4_config.h>
|
||||
#endif
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
|
|
@ -1495,14 +1500,14 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
|
||||
prctl(PR_SET_NAME, thread_name, getpid());
|
||||
|
||||
px4_pollfd_struct_t fds[1];
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = uart_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
ssize_t nread = 0;
|
||||
|
||||
while (!_mavlink->_task_should_exit) {
|
||||
if (px4_poll(fds, 1, timeout) > 0) {
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
|
||||
/* non-blocking read. read may return negative values */
|
||||
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -38,12 +38,8 @@
|
|||
MODULE_COMMAND = mavlink_tests
|
||||
SRCS = mavlink_tests.cpp \
|
||||
mavlink_ftp_test.cpp \
|
||||
SRCS += ../mavlink_ftp.cpp \
|
||||
../mavlink.c
|
||||
ifeq ($(PX4_TARGET_NUTTX),nuttx)
|
||||
SRCS += ../mavlink_ftp_nuttx.cpp
|
||||
else
|
||||
SRCS += ../mavlink_ftp_posix.cpp
|
||||
endif
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
|
|
|
|||
|
|
@ -37,13 +37,9 @@
|
|||
|
||||
MODULE_COMMAND = mavlink
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
SRCS += mavlink_main_nuttx.cpp \
|
||||
mavlink_ftp_nuttx.cpp \
|
||||
mavlink_receiver_nuttx.cpp
|
||||
SRCS += mavlink_main_nuttx.cpp
|
||||
else
|
||||
SRCS += mavlink_main_posix.cpp \
|
||||
mavlink_ftp_posix.cpp \
|
||||
mavlink_receiver_posix.cpp
|
||||
SRCS += mavlink_main_posix.cpp
|
||||
endif
|
||||
|
||||
SRCS += mavlink.c \
|
||||
|
|
@ -52,7 +48,9 @@ SRCS += mavlink.c \
|
|||
mavlink_orb_subscription.cpp \
|
||||
mavlink_messages.cpp \
|
||||
mavlink_stream.cpp \
|
||||
mavlink_rate_limiter.cpp
|
||||
mavlink_rate_limiter.cpp \
|
||||
mavlink_receiver.cpp \
|
||||
mavlink_ftp.cpp
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue