/// @brief Sends the specified FTP response message out through mavlink
void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
// keep a copy of the last sent response ((n)ack), so that if it gets lost and the GCS resends the request,
// we can simply resend the response.
// we only keep small responses to reduce RAM usage and avoid large memcpy's. The larger responses are all data
// retrievals without side-effects, meaning it's ok to reexecute them if a response gets lost
if (payload->size <= sizeof(uint32_t)) {
_last_reply_valid = true;
memcpy(_last_reply, ftp_req, sizeof(_last_reply));
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data);
#else
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req);
#endif
}
/// @brief Sends the specified FTP response message out through mavlink
void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
{
#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
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data);
#else
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req);
#endif
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id)
{
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
if (!_ensure_buffers_exist()) {
PX4_ERR("Failed to allocate buffers");
errorCode = kErrFailErrno;
errno = ENOMEM;
goto out;
}
// basic sanity checks; must validate length before use
if (payload->size > kMaxDataLength) {
errorCode = kErrInvalidDataSize;
goto out;
}
// check the sequence number: if this is a resent request, resend the last response
if (_last_reply_valid) {
mavlink_file_transfer_protocol_t *last_reply = reinterpret_cast<mavlink_file_transfer_protocol_t *>(_last_reply);
PayloadHeader *last_payload = reinterpret_cast<PayloadHeader *>(&last_reply->payload[0]);
if (payload->seq_number + 1 == last_payload->seq_number) {
// this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS
// resent the request
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply);
return;
}
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), 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 kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
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:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
} else {
int r_errno = errno;
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
if (r_errno == EEXIST) {
errorCode = kErrFailFileExists;
}
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
}
}
_last_reply_valid = false;
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nack.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
