diff --git a/firmware/application/main.cpp b/firmware/application/main.cpp
index a0c9f3c56..64eb515f8 100755
--- a/firmware/application/main.cpp
+++ b/firmware/application/main.cpp
@@ -123,15 +123,9 @@ private:
 	}
 
 	void handle_application_queue() {
-		while(true) {
-			std::array<uint8_t, Message::MAX_SIZE> message_buffer;
-			const Message* const message = reinterpret_cast<Message*>(message_buffer.data());
-			const auto message_size = shared_memory.application_queue.pop(message_buffer.data(), message_buffer.size());
-			if( message_size ) {
-				context.message_map.send(message);
-			} else {
-				break;
-			}
+		std::array<uint8_t, Message::MAX_SIZE> message_buffer;
+		while(const Message* const message = shared_memory.application_queue.pop(message_buffer)) {
+			context.message_map.send(message);
 		}
 	}
 
diff --git a/firmware/baseband/main.cpp b/firmware/baseband/main.cpp
index 37455507c..82a27049a 100755
--- a/firmware/baseband/main.cpp
+++ b/firmware/baseband/main.cpp
@@ -242,15 +242,9 @@ private:
 	}
 
 	void handle_baseband_queue() {
-		while(true) {
-			std::array<uint8_t, Message::MAX_SIZE> message_buffer;
-			const Message* const message = reinterpret_cast<Message*>(message_buffer.data());
-			const auto message_size = shared_memory.baseband_queue.pop(message_buffer.data(), message_buffer.size());
-			if( message_size ) {
-				message_map.send(message);
-			} else {
-				break;
-			}
+		std::array<uint8_t, Message::MAX_SIZE> message_buffer;
+		while(const Message* const message = shared_memory.baseband_queue.pop(message_buffer)) {
+			message_map.send(message);
 		}
 	}
 
diff --git a/firmware/common/message_queue.hpp b/firmware/common/message_queue.hpp
index 0d023f92a..dd6ff72b0 100644
--- a/firmware/common/message_queue.hpp
+++ b/firmware/common/message_queue.hpp
@@ -47,8 +47,9 @@ public:
 		return push(&message, sizeof(message));
 	}
 
-	size_t pop(void* const buf, const size_t len) {
-		return fifo.out_r(buf, len);
+	const Message* pop(std::array<uint8_t, Message::MAX_SIZE>& buf) {
+		const Message* const p = reinterpret_cast<Message*>(buf.data());
+		return fifo.out_r(buf.data(), buf.size()) ? p : nullptr;
 	}
 
 	size_t len() const {