From e8906619005a09d98fe68b04f2867c537f0895a8 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 7 Jun 2014 14:46:46 -0700
Subject: [PATCH] Don't queue empty work items.

---
 src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++-----
 src/modules/mavlink/mavlink_ftp.h   | 23 ++++++++++++-----------
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 7a72835cc0..d4d659d91b 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -73,10 +73,13 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
 	if (req != nullptr) {
 
 		// decode the request
-		req->decode(channel, msg);
+		if (req->decode(msg, channel)) {
 
-		// and queue it for the worker
-		work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
 	}
 }
 
@@ -112,7 +115,7 @@ MavlinkFTP::_worker(Request *req)
 		printf("ftp: bad crc\n");
 	}
 
-	printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset);
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
 
 	switch (hdr->opcode) {
 	case kCmdNone:
@@ -186,7 +189,7 @@ MavlinkFTP::_reply(Request *req)
 	hdr->crc32 = crc32(req->rawData(), req->dataSize());
 
 	// then pack and send the reply back to the request source
-	mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData());
+	req->reply();
 }
 
 MavlinkFTP::ErrorCode
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 6a2414613f..f68dab98d5 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -144,19 +144,18 @@ private:
 			dq_entry_t	entry;
 			work_s		work;
 		};
-		mavlink_channel_t	channel;
 
-		void		decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
-			switch (fromMessage->msgid) {
-
-				case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
-					channel = fromChannel;
-					mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
-					warnx("got enc data");
-					break;
-				default:
-					warnx("unknown msg");
+		bool		decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_channel = fromChannel;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
 			}
+			return false;
+		}
+
+		void		reply() {
+			mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData());
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
@@ -164,10 +163,12 @@ private:
 		uint8_t         *requestData() { return &(header()->data[0]); }
 		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
 		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t &channel() { return _channel; }
 
 		char		*dataAsCString();
 
 	private:
+		mavlink_channel_t	_channel;
 		mavlink_encapsulated_data_t _message;
 
 	};