diff --git a/container/FIFO.h b/container/FIFO.h
index 134da9b8..f70c78b0 100644
--- a/container/FIFO.h
+++ b/container/FIFO.h
@@ -3,6 +3,11 @@
 
 #include <framework/returnvalues/HasReturnvaluesIF.h>
 
+/**
+ * @brief Simple First-In-First-Out data structure
+ * @tparam T Entry Type
+ * @tparam capacity Maximum capacity
+ */
 template<typename T, uint8_t capacity>
 class FIFO {
 private:
@@ -54,6 +59,21 @@ public:
 			return HasReturnvaluesIF::RETURN_OK;
 		}
 	}
+
+	ReturnValue_t peek(T * value) {
+	    if(empty()) {
+	        return EMPTY;
+	    } else {
+	       *value = data[readIndex];
+	       return HasReturnvaluesIF::RETURN_OK;
+	    }
+	}
+
+	ReturnValue_t pop() {
+		T value;
+		return this->retrieve(&value);
+	}
+
 	static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS;
 	static const ReturnValue_t FULL = MAKE_RETURN_CODE(1);
 	static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2);
diff --git a/modes/ModeMessage.cpp b/modes/ModeMessage.cpp
index 62fea7e4..1d3baad5 100644
--- a/modes/ModeMessage.cpp
+++ b/modes/ModeMessage.cpp
@@ -16,6 +16,10 @@ ReturnValue_t ModeMessage::setModeMessage(CommandMessage* message, Command_t com
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
+ReturnValue_t ModeMessage::getCantReachModeReason(const CommandMessage* message) {
+	return message->getParameter();
+}
+
 void ModeMessage::clear(CommandMessage* message) {
 	message->setCommand(CommandMessage::CMD_NONE);
 }
diff --git a/modes/ModeMessage.h b/modes/ModeMessage.h
index fad34726..f72fdeec 100644
--- a/modes/ModeMessage.h
+++ b/modes/ModeMessage.h
@@ -23,7 +23,6 @@ public:
 	static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);//!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
 	static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03);	//!> Unrequested info about the current mode (used for composites to inform their container of a changed mode)
 	static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04); //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
-	//SHOULDDO is there a way we can transmit a returnvalue when responding that the mode is wrong, so we can give a nice failure code when commanded by PUS?
 	static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);//!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded and a transition started but was aborted; the parameters contain the mode that was reached
 	static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);//!> Command to read the current mode and reply with a REPLY_MODE_REPLY
 	static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);//!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
@@ -34,6 +33,7 @@ public:
 	static ReturnValue_t setModeMessage(CommandMessage* message,
 			Command_t command, Mode_t mode, Submode_t submode);
 	static void cantReachMode(CommandMessage* message, ReturnValue_t reason);
+	static ReturnValue_t getCantReachModeReason(const CommandMessage* message);
 	static void clear(CommandMessage* message);
 };
 
diff --git a/storagemanager/StorageManagerIF.h b/storagemanager/StorageManagerIF.h
index d85fe86f..67841770 100644
--- a/storagemanager/StorageManagerIF.h
+++ b/storagemanager/StorageManagerIF.h
@@ -48,6 +48,10 @@ union store_address_t {
 	 * Alternative access to the raw value.
 	 */
 	uint32_t raw;
+
+	bool operator==(const store_address_t& other) const {
+		return raw == other.raw;
+	}
 };
 
 /**