diff --git a/src/TinyMqtt.cpp b/src/TinyMqtt.cpp
index 1d567b1..d28e365 100644
--- a/src/TinyMqtt.cpp
+++ b/src/TinyMqtt.cpp
@@ -418,11 +418,11 @@ if (mesg->type() != MqttMessage::Type::PingReq && mesg->type() != MqttMessage::T
   uint16_t len;
 	bool bclose=true;
 
-	switch(mesg->type() & 0XF0)
 #ifdef EPOXY_DUINO
   counters[mesg->type()]++;
 #endif
 
+	switch(mesg->type())
 	{
 		case MqttMessage::Type::Connect:
 			if (mqtt_connected)
@@ -532,7 +532,7 @@ if (mesg->type() != MqttMessage::Type::PingReq && mesg->type() != MqttMessage::T
 					Topic topic(payload, len);
 
 					payload += len;
-					if ((mesg->type() & 0XF0) == MqttMessage::Type::Subscribe)
+					if (mesg->type() == MqttMessage::Type::Subscribe)
 					{
 						uint8_t qos = *payload++;
 						if (qos != 0)
@@ -584,7 +584,7 @@ if (mesg->type() != MqttMessage::Type::PingReq && mesg->type() != MqttMessage::T
 			#endif
 			if (mqtt_connected or client == nullptr)
 			{
-				uint8_t qos = mesg->type() & 0x6;
+				uint8_t qos = mesg->flags();
 				payload = header;
 				mesg->getString(payload, len);
 				Topic published(payload, len);
diff --git a/src/TinyMqtt.h b/src/TinyMqtt.h
index 78e34d4..49fef64 100644
--- a/src/TinyMqtt.h
+++ b/src/TinyMqtt.h
@@ -114,9 +114,11 @@ class MqttMessage
 			return state == Complete ? static_cast<Type>(buffer[0]) : Unknown;
 		}
 
+		uint8_t flags() const { return static_cast<uint8_t>(buffer[0] & 0x0F); }
+
 		void create(Type type)
 		{
-			buffer=(char)type;
+			buffer=(decltype(buffer)::value_type)type;
 			buffer+='\0';		// reserved for msg length byte 1/2
 			buffer+='\0';		// reserved for msg length byte 2/2 (fixed)
 			vheader=3;      // Should never change