dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

New bust mode ftp file download

This commit is contained in:
Don Gagne 2015-05-03 19:26:54 -07:00
parent 0b5e6bdf97
commit 46da294ffb
9 changed files with 617 additions and 385 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

486
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -42,116 +42,130 @@
#include <errno.h>
#include "mavlink_ftp.h"
#include "mavlink_main.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)
int buf_size_1 = 0;
int buf_size_2 = 0;
MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
MavlinkStream(mavlink),
_session_info{},
_utRcvMsgFunc{},
_worker_data{}
{
static MavlinkFTP server;
return &server;
// initialize session
_session_info.fd = -1;
}
MavlinkFTP::MavlinkFTP() :
_request_bufs{},
_request_queue{},
_request_queue_sem{},
_utRcvMsgFunc{},
_ftp_test{}
MavlinkFTP::~MavlinkFTP()
{
// 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]);
const char*
MavlinkFTP::get_name(void) const
{
return "MAVLINK_FTP";
}
uint8_t
MavlinkFTP::get_id(void)
{
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL;
}
unsigned
MavlinkFTP::get_size(void)
{
if (_session_info.stream_download) {
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
MavlinkStream*
MavlinkFTP::new_instance(Mavlink *mavlink)
{
return new MavlinkFTP(mavlink);
}
#ifdef MAVLINK_FTP_UNIT_TEST
void
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data)
{
_utRcvMsgFunc = rcvMsgFunc;
_ftp_test = ftp_test;
_worker_data = worker_data;
}
#endif
void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
uint8_t
MavlinkFTP::_getServerSystemId(void)
{
// 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");
// We use fake ids when unit testing
return MavlinkFtpTest::serverSystemId;
#else
// Not unit testing, use the real thing
return _mavlink->get_system_id();
#endif
}
uint8_t
MavlinkFTP::_getServerComponentId(void)
{
#ifdef MAVLINK_FTP_UNIT_TEST
// We use fake ids when unit testing
return MavlinkFtpTest::serverComponentId;
#else
// Not unit testing, use the real thing
return _mavlink->get_component_id();
#endif
}
uint8_t
MavlinkFTP::_getServerChannel(void)
{
#ifdef MAVLINK_FTP_UNIT_TEST
// We use fake ids when unit testing
return MavlinkFtpTest::serverChannel;
#else
// Not unit testing, use the real thing
return _mavlink->get_channel();
#endif
}
void
MavlinkFTP::handle_message(const mavlink_message_t *msg)
{
//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
mavlink_file_transfer_protocol_t ftp_request;
mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system);
#endif
if (ftp_request.target_system == _getServerSystemId()) {
_process_request(&ftp_request, msg->sysid);
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)
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
@ -162,7 +176,7 @@ MavlinkFTP::_process_request(Request *req)
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset);
#endif
switch (payload->opcode) {
@ -197,6 +211,11 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRead(payload);
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
@ -221,7 +240,6 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
@ -232,16 +250,14 @@ MavlinkFTP::_process_request(Request *req)
}
out:
payload->seq_number++;
// 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;
@ -252,60 +268,33 @@ out:
}
}
// respond to the request
_reply(req);
_return_request(req);
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
/// @brief Sends the specified FTP reponse message out through mavlink
/// @brief Sends the specified FTP response message out through mavlink
void
MavlinkFTP::_reply(Request *req)
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_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 =
#ifdef MAVLINK_FTP_DEBUG
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#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;
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(&msg, _ftp_test);
_utRcvMsgFunc(ftp_req, _worker_data);
#else
Mavlink *mavlink = req->mavlink;
mavlink->lockMessageBufferMutex();
success = mavlink->message_buffer_write(&msg, len);
mavlink->unlockMessageBufferMutex();
_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
#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
@ -417,13 +406,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
{
int session_index = _find_unused_session();
if (session_index < 0) {
if (_session_info.fd >= 0) {
warnx("FTP: Open failed - out of sessions\n");
return kErrNoSessionsAvailable;
}
char *filename = _data_as_cstring(payload);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: open '%s'", filename);
#endif
uint32_t fileSize = 0;
struct stat st;
@ -440,9 +432,11 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
if (fd < 0) {
return kErrFailErrno;
}
_session_fds[session_index] = fd;
_session_info.fd = fd;
_session_info.file_size = fileSize;
_session_info.stream_download = false;
payload->session = session_index;
payload->session = 0;
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
@ -453,23 +447,25 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader* payload)
{
int session_index = payload->session;
if (!_valid_session(session_index)) {
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
// Seek to the specified position
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", payload->offset);
warnx("FTP: read offset:%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");
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (payload->offset >= _session_info.file_size) {
warnx("request past EOF");
return kErrEOF;
}
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
warnx("seek fail");
return kErrFailErrno;
}
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
warnx("read fail %d", bytes_read);
@ -481,27 +477,41 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
return kErrNone;
}
/// @brief Responds to a Stream command
MavlinkFTP::ErrorCode
MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: burst offset:%d", payload->offset);
#endif
// Setup for streaming sends
_session_info.stream_download = true;
_session_info.stream_offset = payload->offset;
_session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id;
return kErrNone;
}
/// @brief Responds to a Write command
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
{
int session_index = payload->session;
if (!_valid_session(session_index)) {
if (payload->session != 0 && _session_info.fd < 0) {
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) {
if (lseek(_session_info.fd, 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);
int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
if (bytes_written < 0) {
// Negative return indicates error other than eof
warnx("write fail %d", bytes_written);
@ -608,12 +618,13 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
{
if (!_valid_session(payload->session)) {
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
::close(_session_fds[payload->session]);
_session_fds[payload->session] = -1;
::close(_session_info.fd);
_session_info.fd = -1;
_session_info.stream_download = false;
payload->size = 0;
@ -624,11 +635,10 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload)
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;
}
if (_session_info.fd != -1) {
::close(_session_info.fd);
_session_info.fd = -1;
_session_info.stream_download = false;
}
payload->size = 0;
@ -726,29 +736,6 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
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 *
@ -765,40 +752,6 @@ MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
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)
@ -851,3 +804,106 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
errno = op_errno;
return (length > 0)? -1 : 0;
}
void MavlinkFTP::send(const hrt_abstime t)
{
// Anything to stream?
if (!_session_info.stream_download) {
return;
}
#ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
#ifdef MAVLINK_FTP_DEBUG
warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf());
#endif
if (max_bytes_to_send < get_size()) {
return;
}
#endif
// Send stream packets until buffer is full
bool more_data;
do {
more_data = false;
ErrorCode error_code = kErrNone;
mavlink_file_transfer_protocol_t ftp_msg;
PayloadHeader* payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]);
payload->seq_number = _session_info.stream_seq_number;
payload->session = 0;
payload->opcode = kRspAck;
payload->req_opcode = kCmdBurstReadFile;
payload->offset = _session_info.stream_offset;
_session_info.stream_seq_number++;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream send: offset %d", _session_info.stream_offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (_session_info.stream_offset >= _session_info.file_size) {
error_code = kErrEOF;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: sending Nak EOF");
#endif
}
if (error_code == kErrNone) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: seek fail");
#endif
}
}
if (error_code == kErrNone) {
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: read fail");
#endif
} else {
payload->size = bytes_read;
_session_info.stream_offset += bytes_read;
}
}
if (error_code != kErrNone) {
payload->opcode = kRspNak;
payload->size = 1;
uint8_t* pData = &payload->data[0];
*pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error
if (error_code == kErrFailErrno) {
int r_errno = errno;
payload->size = 2;
payload->data[1] = r_errno;
}
_session_info.stream_download = false;
} else {
#ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size()*2)) {
more_data = false;
payload->burst_complete = true;
_session_info.stream_download = false;
} else {
#endif
more_data = true;
payload->burst_complete = false;
#ifndef MAVLINK_FTP_UNIT_TEST
max_bytes_to_send -= get_size();
}
#endif
}
ftp_msg.target_system = _session_info.stream_target_system_id;
_reply(&ftp_msg);
} while (more_data);
}

101
src/modules/mavlink/mavlink_ftp.h
View File

@ -39,45 +39,44 @@
#include <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
#include "mavlink_stream.h"
#include "mavlink_bridge_header.h"
class MavlinkFtpTest;
/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
class MavlinkFTP
/// MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
class MavlinkFTP : public MavlinkStream
{
public:
/// @brief Returns the one Mavlink FTP server in the system.
static MavlinkFTP* get_server(void);
/// @brief Contructor is only public so unit test code can new objects.
MavlinkFTP();
MavlinkFTP(Mavlink *mavlink);
~MavlinkFTP();
static MavlinkStream *new_instance(Mavlink *mavlink);
/// @brief Adds the specified message to the work queue.
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
/// Handle possible FTP message
void handle_message(const mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
/// @brief Sets up the server to run in unit test mode.
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
/// @param ftp_test MavlinkFtpTest object which the function is associated with
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
/// @param worker_data Data to pass to worker
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data);
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
/// 32 bit alignment to avoid usage of any pack pragmas.
struct PayloadHeader
{
uint16_t seqNumber; ///< sequence number for message
uint16_t seq_number; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
uint8_t size; ///< Size of data
uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message
uint8_t padding[2]; ///< 32 bit aligment padding
uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming.
uint8_t padding; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode
};
@ -100,6 +99,7 @@ public:
kCmdTruncateFile, ///< Truncate file at <path> to <offset> length
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kCmdBurstReadFile, ///< Burst download session file
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
@ -118,35 +118,22 @@ public:
kErrUnknownCommand ///< Unknown command opcode
};
// MavlinkStream overrides
virtual const char *get_name(void) const;
virtual uint8_t get_id(void);
virtual unsigned get_size(void);
private:
/// @brief Unit of work which is queued to work_queue
struct Request
{
work_s work; ///< work queue entry
Mavlink *mavlink; ///< Mavlink to reply to
uint8_t serverSystemId; ///< System ID to send from
uint8_t serverComponentId; ///< Component ID to send from
uint8_t serverChannel; ///< Channel to send to
uint8_t targetSystemId; ///< System ID to target reply to
mavlink_file_transfer_protocol_t message; ///< Protocol message
};
Request *_get_request(void);
void _return_request(Request *req);
void _lock_request_queue(void);
void _unlock_request_queue(void);
char *_data_as_cstring(PayloadHeader* payload);
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t* ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload);
@ -156,14 +143,13 @@ private:
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
dq_queue_t _request_queue; ///< Queue of available Request buffers
sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue
int _find_unused_session(void);
bool _valid_session(unsigned index);
uint8_t _getServerSystemId(void);
uint8_t _getServerComponentId(void);
uint8_t _getServerChannel(void);
// Overrides from MavlinkStream
virtual void send(const hrt_abstime t);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
@ -172,9 +158,24 @@ private:
/// @brief Maximum data size in RequestHeader::data
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
static const unsigned kMaxSession = 2; ///< Max number of active sessions
int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot
struct SessionInfo {
int fd;
uint32_t file_size;
bool stream_download;
uint32_t stream_offset;
uint16_t stream_seq_number;
uint8_t stream_target_system_id;
};
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
void *_worker_data; ///< Additional parameter to _utRcvMsgFunc;
/* do not allow copying this class */
MavlinkFTP(const MavlinkFTP&);
MavlinkFTP operator=(const MavlinkFTP&);
// Mavlink test needs to be able to call send
friend class MavlinkFtpTest;
};

9
src/modules/mavlink/mavlink_main.cpp
View File

@ -131,6 +131,7 @@ Mavlink::Mavlink() :
_streams(nullptr),
_mission_manager(nullptr),
_parameters_manager(nullptr),
_mavlink_ftp(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
@ -868,6 +869,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
/* handle packet with parameter component */
_parameters_manager->handle_message(msg);
/* handle packet with ftp component */
_mavlink_ftp->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
@ -1364,6 +1368,11 @@ Mavlink::task_main(int argc, char *argv[])
_parameters_manager->set_interval(interval_from_rate(120.0f));
LL_APPEND(_streams, _parameters_manager);
/* MAVLINK_FTP stream */
_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this);
_mavlink_ftp->set_interval(interval_from_rate(120.0f));
LL_APPEND(_streams, _mavlink_ftp);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
* remote requests rate. Rate specified here controls how much bandwidth we will reserve for
* mission messages. */

6
src/modules/mavlink/mavlink_main.h
View File

@ -59,6 +59,7 @@
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
#include "mavlink_ftp.h"
class Mavlink
{
@ -296,8 +297,9 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MavlinkFTP *_mavlink_ftp;
MAVLINK_MODE _mode;

6
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -134,8 +134,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_time_offset(0)
{
// make sure the FTP server is started
(void)MavlinkFTP::get_server();
}
MavlinkReceiver::~MavlinkReceiver()
@ -202,10 +200,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_message_system_time(msg);
break;

2
src/modules/mavlink/mavlink_stream.h
View File

@ -73,8 +73,6 @@ public:
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
static MavlinkStream *new_instance(const Mavlink *mavlink);
static const char *get_name_static();
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;

338
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
View File

@ -43,19 +43,19 @@
#include "../mavlink_ftp.h"
/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets
const MavlinkFtpTest::DownloadTestCase MavlinkFtpTest::_rgDownloadTestCases[] = {
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1, true, false }, // Read takes less than single packet
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader), true, true }, // Read completely fills single packet
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1, false, false }, // Read take two packets
};
const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
MavlinkFtpTest::MavlinkFtpTest() :
_ftp_server{},
_reply_msg{},
_lastOutgoingSeqNumber{}
_ftp_server(nullptr),
_expected_seq_number(0),
_reply_msg{}
{
}
@ -67,8 +67,9 @@ MavlinkFtpTest::~MavlinkFtpTest()
/// @brief Called before every test to initialize the FTP Server.
void MavlinkFtpTest::_init(void)
{
_ftp_server = new MavlinkFTP;;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
_expected_seq_number = 0;
_ftp_server = new MavlinkFTP(NULL);
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
_cleanup_microsd();
}
@ -85,15 +86,13 @@ void MavlinkFtpTest::_cleanup(void)
bool MavlinkFtpTest::_ack_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = MavlinkFTP::kCmdNone;
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -109,15 +108,13 @@ bool MavlinkFtpTest::_ack_test(void)
bool MavlinkFtpTest::_bad_opcode_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = 0xFF; // bogus opcode
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -135,8 +132,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
{
mavlink_message_t msg;
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = MavlinkFTP::kCmdListDirectory;
@ -145,9 +141,9 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
// Set the data size to be one larger than is legal
((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
_ftp_server->handle_message(&msg);
if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
if (!_decode_message(&_reply_msg, &reply)) {
return false;
}
@ -161,8 +157,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
bool MavlinkFtpTest::_list_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
char response2[] = "Ddev|Detc|Dfs|Dobj";
@ -188,7 +183,6 @@ bool MavlinkFtpTest::_list_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -202,16 +196,20 @@ bool MavlinkFtpTest::_list_test(void)
// to a hardcoded return result string.
// Convert null terminators to seperator char so we can use strok to parse returned data
char list_entry[256];
for (uint8_t j=0; j<reply->size-1; j++) {
if (reply->data[j] == 0) {
reply->data[j] = '|';
list_entry[j] = '|';
} else {
list_entry[j] = reply->data[j];
}
}
list_entry[reply->size-1] = 0;
// Loop over returned directory entries trying to find then in the response list
char *dir;
int response_count = 0;
dir = strtok((char *)&reply->data[0], "|");
dir = strtok(list_entry, "|");
while (dir != nullptr) {
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
response_count++;
@ -234,8 +232,7 @@ bool MavlinkFtpTest::_list_test(void)
bool MavlinkFtpTest::_list_eof_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
const char *dir = "/";
payload.opcode = MavlinkFTP::kCmdListDirectory;
@ -244,7 +241,6 @@ bool MavlinkFtpTest::_list_eof_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(dir)+1, // size in bytes of data
(uint8_t*)dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -261,8 +257,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
bool MavlinkFtpTest::_open_badfile_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
const char *dir = "/foo"; // non-existent file
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
@ -271,7 +266,6 @@ bool MavlinkFtpTest::_open_badfile_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(dir)+1, // size in bytes of data
(uint8_t*)dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -288,12 +282,11 @@ bool MavlinkFtpTest::_open_badfile_test(void)
bool MavlinkFtpTest::_open_terminate_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
const DownloadTestCase *test = &_rgDownloadTestCases[i];
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
@ -301,7 +294,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -321,7 +313,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -338,9 +329,8 @@ bool MavlinkFtpTest::_open_terminate_test(void)
bool MavlinkFtpTest::_terminate_badsession_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
const MavlinkFTP::PayloadHeader *reply;
const char *file = _rgDownloadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
@ -348,7 +338,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(file)+1, // size in bytes of data
(uint8_t*)file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -363,7 +352,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -380,12 +368,11 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
bool MavlinkFtpTest::_read_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
const DownloadTestCase *test = &_rgDownloadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
@ -406,7 +393,6 @@ bool MavlinkFtpTest::_read_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -421,7 +407,6 @@ bool MavlinkFtpTest::_read_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -429,10 +414,40 @@ bool MavlinkFtpTest::_read_test(void)
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, 0);
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0);
if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
payload.offset += expected_bytes;
if (test->singlePacketRead) {
// Try going past EOF
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
} else {
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, payload.offset);
expected_bytes = (uint32_t)st.st_size - full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0);
}
payload.opcode = MavlinkFTP::kCmdTerminateSession;
@ -442,7 +457,91 @@ bool MavlinkFtpTest::_read_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Incorrect payload size", reply->size, 0);
}
return true;
}
/// @brief Tests for correct reponse to a Read command on an open session.
bool MavlinkFtpTest::_burst_test(void)
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
BurstInfo burst_info;
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const DownloadTestCase *test = &_rgDownloadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
uint8_t *bytes = new uint8_t[st.st_size];
ut_assert("new failed", bytes != nullptr);
int fd = ::open(test->file, O_RDONLY);
ut_assert("open failed", fd != -1);
int bytes_read = ::read(fd, bytes, st.st_size);
ut_compare("read failed", bytes_read, st.st_size);
::close(fd);
// Test case data files are created for specific boundary conditions
ut_compare("Test case data files are out of date", test->length, st.st_size);
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
// Setup for burst response handler
burst_info.burst_state = burst_state_first_ack;
burst_info.single_packet_file = test->singlePacketRead;
burst_info.file_size = st.st_size;
burst_info.file_bytes = bytes;
burst_info.ftp_test_class = this;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info);
// Send the burst command, message response will be handled by _receive_message_handler_stream
payload.opcode = MavlinkFTP::kCmdBurstReadFile;
payload.session = reply->session;
payload.offset = 0;
mavlink_message_t msg;
_setup_ftp_msg(&payload, 0, nullptr, &msg);
_ftp_server->handle_message(&msg);
// First packet is sent using stream mechanism, so we need to force it out ourselves
hrt_abstime t = 0;
_ftp_server->send(t);
ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete);
// Put back generic message handler
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
// Terminate session
payload.opcode = MavlinkFTP::kCmdTerminateSession;
payload.session = reply->session;
payload.size = 0;
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -459,18 +558,16 @@ bool MavlinkFtpTest::_read_test(void)
bool MavlinkFtpTest::_read_badsession_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
const MavlinkFTP::PayloadHeader *reply;
const char *file = _rgDownloadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(file)+1, // size in bytes of data
(uint8_t*)file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
@ -484,7 +581,6 @@ bool MavlinkFtpTest::_read_badsession_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -500,8 +596,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
bool MavlinkFtpTest::_removedirectory_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
int fd;
struct _testCase {
@ -534,7 +629,6 @@ bool MavlinkFtpTest::_removedirectory_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -556,8 +650,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
bool MavlinkFtpTest::_createdirectory_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
struct _testCase {
const char *dir;
@ -579,7 +672,6 @@ bool MavlinkFtpTest::_createdirectory_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -601,8 +693,7 @@ bool MavlinkFtpTest::_createdirectory_test(void)
bool MavlinkFtpTest::_removefile_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
int fd;
struct _testCase {
@ -611,7 +702,7 @@ bool MavlinkFtpTest::_removefile_test(void)
};
static const struct _testCase rgTestCases[] = {
{ "/bogus", false },
{ _rgReadTestCases[0].file, false },
{ _rgDownloadTestCases[0].file, false },
{ _unittest_microsd_dir, false },
{ _unittest_microsd_file, true },
{ _unittest_microsd_file, false },
@ -630,7 +721,6 @@ bool MavlinkFtpTest::_removefile_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@ -649,62 +739,119 @@ bool MavlinkFtpTest::_removefile_test(void)
return true;
}
/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when
/// it needs to send a message out on Mavlink.
void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
{
ftp_test->_receive_message(msg);
((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req);
}
/// @brief Non-Static version of receive_message
void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
{
// Move the message into our own member variable
memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t));
}
/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when
/// it needs to send a message out on Mavlink.
void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
{
BurstInfo* burst_info = (BurstInfo*)worker_data;
burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info);
}
bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info)
{
hrt_abstime t = 0;
const MavlinkFTP::PayloadHeader* reply;
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
uint32_t expected_bytes;
_decode_message(ftp_msg, &reply);
switch (burst_info->burst_state) {
case burst_state_first_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, 0);
expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_last_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
expected_bytes = burst_info->file_size - full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_state_nak_eof;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_nak_eof:
// Signal complete
burst_info->burst_state = burst_state_complete;
ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
break;
}
return true;
}
/// @brief Decode and validate the incoming message
bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode
mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message
MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, ///< Incoming FTP message
const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
{
mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
//warnx("_decode_message");
// Make sure the targets are correct
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
// Make sure we have a good sequence number
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
// Bump sequence number for next outgoing message
_lastOutgoingSeqNumber++;
ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
_expected_seq_number++;
return true;
}
/// @brief Initializes an FTP message into a mavlink message
void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_message_t *msg) ///< Returned mavlink message
void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_message_t *msg) ///< Returned mavlink message
{
uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
payload->seqNumber = _lastOutgoingSeqNumber;
payload->seq_number = _expected_seq_number++;
payload->size = size;
if (size != 0) {
memcpy(payload->data, data, size);
}
payload->padding[0] = 0;
payload->padding[1] = 0;
payload->burst_complete = 0;
payload->padding = 0;
msg->checksum = 0;
mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id
@ -720,14 +867,13 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, /
bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server
MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
{
mavlink_message_t msg;
_setup_ftp_msg(payload_header, size, data, &msg);
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
_ftp_server->handle_message(&msg);
return _decode_message(&_reply_msg, payload_reply);
}
/// @brief Cleans up an files created on microsd during testing
@ -743,14 +889,14 @@ bool MavlinkFtpTest::run_tests(void)
ut_run_test(_ack_test);
ut_run_test(_bad_opcode_test);
ut_run_test(_bad_datasize_test);
printf("WARNING! list test commented out, but needs proper resolution!\n");
//ut_run_test(_list_test);
ut_run_test(_list_test);
ut_run_test(_list_eof_test);
ut_run_test(_open_badfile_test);
ut_run_test(_open_terminate_test);
ut_run_test(_terminate_badsession_test);
ut_run_test(_read_test);
ut_run_test(_read_badsession_test);
ut_run_test(_burst_test);
ut_run_test(_removedirectory_test);
ut_run_test(_createdirectory_test);
ut_run_test(_removefile_test);

53
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
View File

@ -48,7 +48,18 @@ public:
virtual bool run_tests(void);
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
/// Worker data for stream handler
struct BurstInfo {
MavlinkFtpTest* ftp_test_class;
int burst_state;
bool single_packet_file;
uint32_t file_size;
uint8_t* file_bytes;
};
static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
static const uint8_t serverSystemId = 50; ///< System ID for server
static const uint8_t serverComponentId = 1; ///< Component ID for server
@ -75,31 +86,45 @@ private:
bool _terminate_badsession_test(void);
bool _read_test(void);
bool _read_badsession_test(void);
bool _burst_test(void);
bool _removedirectory_test(void);
bool _createdirectory_test(void);
bool _removefile_test(void);
void _receive_message(const mavlink_message_t *msg);
void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload);
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
uint8_t size,
const uint8_t *data,
mavlink_file_transfer_protocol_t *ftp_msg_reply,
MavlinkFTP::PayloadHeader **payload_reply);
const MavlinkFTP::PayloadHeader **payload_reply);
void _cleanup_microsd(void);
MavlinkFTP *_ftp_server;
mavlink_message_t _reply_msg;
uint16_t _lastOutgoingSeqNumber;
struct ReadTestCase {
/// A single download test case
struct DownloadTestCase {
const char *file;
const uint16_t length;
bool singlePacketRead;
bool exactlyFillPacket;
};
static const ReadTestCase _rgReadTestCases[];
/// The set of test cases for download testing
static const DownloadTestCase _rgDownloadTestCases[];
/// States for stream download handler
enum {
burst_state_first_ack,
burst_state_last_ack,
burst_state_nak_eof,
burst_state_complete
};
bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info);
MavlinkFTP* _ftp_server;
uint16_t _expected_seq_number;
mavlink_file_transfer_protocol_t _reply_msg;
static const char _unittest_microsd_dir[];
static const char _unittest_microsd_file[];

1
src/modules/mavlink/mavlink_tests/module.mk
View File

@ -38,6 +38,7 @@
MODULE_COMMAND = mavlink_tests
SRCS = mavlink_tests.cpp \
mavlink_ftp_test.cpp \
../mavlink_stream.cpp \
../mavlink_ftp.cpp \
../mavlink.c