예제 #1
0
/// @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);
}
예제 #2
0
/// @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);
	}
}
예제 #3
0
void
MavlinkFTP::_worker(Request *req)
{
	auto hdr = req->header();
	ErrorCode errorCode = kErrNone;
	uint32_t messageCRC;

	// basic sanity checks; must validate length before use
	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
		errorCode = kErrNoRequest;
		goto out;
	}

	// check request CRC to make sure this is one of ours
	messageCRC = hdr->crc32;
	hdr->crc32 = 0;
	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
		errorCode = kErrNoRequest;
		goto out;
		printf("ftp: bad crc\n");
	}

	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);

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

	case kCmdTerminate:
		errorCode = _workTerminate(req);
		break;

	case kCmdReset:
		errorCode = _workReset();
		break;

	case kCmdList:
		errorCode = _workList(req);
		break;

	case kCmdOpen:
		errorCode = _workOpen(req, false);
		break;

	case kCmdCreate:
		errorCode = _workOpen(req, true);
		break;

	case kCmdRead:
		errorCode = _workRead(req);
		break;

	case kCmdWrite:
		errorCode = _workWrite(req);
		break;

	case kCmdRemove:
		errorCode = _workRemove(req);
		break;

	default:
		errorCode = kErrNoRequest;
		break;	
	}

out:
	// handle success vs. error
	if (errorCode == kErrNone) {
		hdr->opcode = kRspAck;
		printf("FTP: ack\n");
	} else {
		printf("FTP: nak %u\n", errorCode);
		hdr->opcode = kRspNak;
		hdr->size = 1;
		hdr->data[0] = errorCode;
	}

	// respond to the request
	_reply(req);

	// free the request buffer back to the freelist
	_qFree(req);
}
예제 #4
0
/// @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;

    // 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", _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;
        payload->data[0] = errorCode;
        if (errorCode == kErrFailErrno) {
            payload->size = 2;
            payload->data[1] = r_errno;
        }
    }

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