From 8fde4972b41529f35e85073180f94e1112330501 Mon Sep 17 00:00:00 2001
From: Jared Boone <jboone@earfeast.com>
Date: Sun, 10 Jan 2016 10:15:25 -0800
Subject: [PATCH] Methods to peek and skip messages in queue.

Allows receiver to not consume a message until after it's handled. And that enables the transmitter to block until the queue is empty, knowing that when unblocked, all messages in queue have been handled.
---
 firmware/common/fifo.hpp          | 20 ++++++++++++++++++++
 firmware/common/message_queue.hpp | 19 +++++++++++++++++++
 2 files changed, 39 insertions(+)

diff --git a/firmware/common/fifo.hpp b/firmware/common/fifo.hpp
index 66c9c811c..f217e67eb 100644
--- a/firmware/common/fifo.hpp
+++ b/firmware/common/fifo.hpp
@@ -114,6 +114,26 @@ public:
 		return len;
 	}
 
+	bool skip() {
+		if( is_empty() ) {
+			return false;
+		}
+
+		size_t len = peek_n();
+		_out += len + recsize();
+		return true;
+	}
+
+	size_t peek_r(void* const buf, size_t len) {
+		if( is_empty() ) {
+			return 0;
+		}
+
+		size_t n;
+		len = out_copy_r((T*)buf, len, &n);
+		return len;
+	}
+
 	size_t out_r(void* const buf, size_t len) {
 		if( is_empty() ) {
 			return 0;
diff --git a/firmware/common/message_queue.hpp b/firmware/common/message_queue.hpp
index 94745f3ee..c4430a237 100644
--- a/firmware/common/message_queue.hpp
+++ b/firmware/common/message_queue.hpp
@@ -47,6 +47,25 @@ public:
 		return push(&message, sizeof(message));
 	}
 
+	template<typename T>
+	bool push_and_wait(const T& message) {
+		const bool result = push(message);
+		if( result ) {
+			// TODO: More graceful method of waiting for empty? Maybe sleep for a bit?
+			while( !is_empty() );
+		}
+		return result;
+	}
+
+	Message* peek(std::array<uint8_t, Message::MAX_SIZE>& buf) {
+		Message* const p = reinterpret_cast<Message*>(buf.data());
+		return fifo.peek_r(buf.data(), buf.size()) ? p : nullptr;
+	}
+
+	bool skip() {
+		return fifo.skip();
+	}
+
 	Message* pop(std::array<uint8_t, Message::MAX_SIZE>& buf) {
 		Message* const p = reinterpret_cast<Message*>(buf.data());
 		return fifo.out_r(buf.data(), buf.size()) ? p : nullptr;