From bf348cf30af24e9afc4e21ab3eece4cadaf37645 Mon Sep 17 00:00:00 2001
From: Jared Boone <jboone@earfeast.com>
Date: Sat, 27 Feb 2016 21:07:11 -0800
Subject: [PATCH] Move message handling loops to MessageQueue.

---
 firmware/application/event_m0.cpp | 5 ++---
 firmware/baseband/event_m4.cpp    | 8 +++-----
 firmware/common/message_queue.hpp | 9 +++++++++
 3 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/firmware/application/event_m0.cpp b/firmware/application/event_m0.cpp
index c4ab32a7..023f06f4 100644
--- a/firmware/application/event_m0.cpp
+++ b/firmware/application/event_m0.cpp
@@ -134,10 +134,9 @@ void EventDispatcher::dispatch(const eventmask_t events) {
 }
 
 void EventDispatcher::handle_application_queue() {
-	std::array<uint8_t, Message::MAX_SIZE> message_buffer;
-	while(Message* const message = shared_memory.application_queue.pop(message_buffer)) {
+	shared_memory.application_queue.handle([](Message* const message) {
 		message_map().send(message);
-	}
+	});
 }
 
 void EventDispatcher::handle_rtc_tick() {
diff --git a/firmware/baseband/event_m4.cpp b/firmware/baseband/event_m4.cpp
index 0e90cd15..797ef5cb 100644
--- a/firmware/baseband/event_m4.cpp
+++ b/firmware/baseband/event_m4.cpp
@@ -86,11 +86,9 @@ void EventDispatcher::dispatch(const eventmask_t events) {
 }
 
 void EventDispatcher::handle_baseband_queue() {
-	std::array<uint8_t, Message::MAX_SIZE> message_buffer;
-	while(Message* const message = shared_memory.baseband_queue.peek(message_buffer)) {
-		on_message(message);
-		shared_memory.baseband_queue.skip();
-	}
+	shared_memory.baseband_queue.handle([this](Message* const message) {
+		this->on_message(message);
+	});
 }
 
 void EventDispatcher::on_message(const Message* const message) {
diff --git a/firmware/common/message_queue.hpp b/firmware/common/message_queue.hpp
index baba13ef..f8920e1b 100644
--- a/firmware/common/message_queue.hpp
+++ b/firmware/common/message_queue.hpp
@@ -82,6 +82,15 @@ public:
 		return fifo.is_empty();
 	}
 
+	template<typename HandlerFn>
+	void handle(HandlerFn handler) {
+		std::array<uint8_t, Message::MAX_SIZE> message_buffer;
+		while(Message* const message = peek(message_buffer)) {
+			handler(message);
+			skip();
+		}
+	}
+
 private:
 	FIFO<uint8_t> fifo;
 	Mutex mutex_write;