diff --git a/libraries/MySensors/core/MyGatewayTransportEthernetLinux.cpp b/libraries/MySensors/core/MyGatewayTransportEthernetLinux.cpp
index 4eadf49..c5d1091 100644
--- a/libraries/MySensors/core/MyGatewayTransportEthernetLinux.cpp
+++ b/libraries/MySensors/core/MyGatewayTransportEthernetLinux.cpp
@@ -71,7 +71,6 @@ void *connected_controller(void* thread_arg);
 
 #ifdef MY_GATEWAY_MQTT_CLIENT
 void *mqtt_thread(void *);
-void parsemqtt_and_send(char *topic, const char *payload);
 struct mosquitto *mosq;
 #endif
 
@@ -355,15 +354,35 @@ void mqtt_message_callback(struct mosquitto *mosq, void *userdata, const struct
 	
 	if(message->payloadlen){
 		printf("Got a MQTT message %s %s\n", message->topic, (char*)message->payload);
-		
-		std::string strpayload((char *)message->payload);
-		strpayload = strpayload.substr(0,(int)message->payloadlen);
-	
-		parsemqtt_and_send(message->topic, strpayload.c_str());
 	}else{
-		printf("%s (null)\n", message->topic);
+		printf("Got a MQTT message %s (null)\n", message->topic);
+	}
+	
+	MyMessage msg;
+	protocolMQTTParse(msg, message->topic, (uint8_t*)message->payload, message->payloadlen);
+	
+	if(msg.destination != 0 && msg.sensor != 0 && msg.type != 255)
+	{
+		ethernetMsg_q.push_back(msg);
+
+		// Forward the data to Ethernet
+		// Likely this is a duplicate from a C_SET that we received and published
+		// in msg_callback.  Not much we can do to avoid the duplicate as there is 
+		// no way to tell if we performed the publish that is triggering  this callback.
+		char *ethernetMsg = protocolFormat(msg);
+
+		for (uint8_t i = 0; i < MY_GATEWAY_MAX_CLIENTS; i++) {
+			if (controllers[i] == -1)
+				continue;
+			if (send(controllers[i], ethernetMsg, strlen(ethernetMsg), 0) == -1)
+				perror("send");
+		}
+	}	
+	else
+	{
+		printf("Recieved a bad MQTT message: '%s':'%s'\n destination:%i, sensor:%i, type:%i\n", 
+		message->topic, (char*)message->payload, msg.destination, msg.sensor, msg.type);
 	}
-	fflush(stdout);
 }
 
 void mqtt_connect_callback(struct mosquitto *mosq, void *userdata, int result)
@@ -432,91 +451,4 @@ void *mqtt_thread(void *)
 	return NULL;
 }
 
-void parsemqtt_and_send(char *topic, const char *payload)
-{
-	//Buffer for parsemqtt_and_send.  Do not want to re-create it every time we get a message
-	MyMessage msg;
-	char *str, *p;
-
-	char* topic_copy = strdup(topic);
-	
-
-	// TODO: Check if we should send ack or not.
-	int i = 0;
-	for (str = strtok_r(topic_copy,"/",&p) ; str && i<4 ; str = strtok_r(NULL,"/",&p))
-	{
-		// Example: /mySensors/105/4/V_LIGHT
-		// 0: mySensors - Prefix
-		// 1: 105 - node
-		// 2: 4 - sensor
-		// 3: V_LIGHT - type
-		if (i == 0) {
-			//TODO: Add warning when we receive a message from an unexpected broker prefix.
-			//if (strcmp_P(str,broker)!=0) {  //look for MQTT_BROKER_PREFIX
-			//	return;                 //Message not for us or malformatted!
-			//}
-		} else if (i==1) {
-			printf("Destination parse %s\n", str);
-			msg.destination = atoi(str);    //NodeID
-		} else if (i==2) {
-			msg.sensor = atoi(str);         //SensorID
-		} else if (i==3) {
-			unsigned char match=255;         //SensorType
-			
-			//Support for numeric types
-			if ( atoi(str)!=0 || (str[0]=='0' && str[1] =='\0') ) {
-				match=atoi(str);
-			}
-			
-			/*
-			//Check through all the data type string attary to find a match.
-			//? Not sure if there is any need to be checking for other types like sensor and internal
-			//? as those don't seem like they should be outside settable thought MQTT.
-			for (uint8_t j=0; match == 255 && j<sizeof(mysensor_data_str)/sizeof(mysensor_data_str[0]); j++) {
-				if (strcmp(str,mysensor_data_str[j])==0) { //compare sensors
-					match=j;
-				}
-			}
-			*/
-			
-			msg.type = match;
-		} else if (i==4) {
-			mSetAck(msg, atoi(str)); //Ack
-		}
-		
-		i++;
-	}
-	
-	msg.sender = GATEWAY_ADDRESS;
-	msg.last = GATEWAY_ADDRESS;
-	mSetCommand(msg, C_SET);
-	mSetRequestAck(msg, 0);
-	msg.set(payload);
-	
-	if(msg.destination != 0 && msg.sensor != 0 && msg.type != 255)
-	{
-		ethernetMsg_q.push_back(msg);
-			printf("Message not sent!\n");
-
-		// Forward the data to Ethernet
-		// Likely this is a duplicate from a C_SET that we received and published
-		// in msg_callback.  Not much we can do to avoid the duplicate as there is 
-		// no way to tell if we performed the publish that is triggering  this callback.
-		char *ethernetMsg = protocolFormat(msg);
-
-		for (uint8_t i = 0; i < MY_GATEWAY_MAX_CLIENTS; i++) {
-			if (controllers[i] == -1)
-				continue;
-			if (send(controllers[i], ethernetMsg, strlen(ethernetMsg), 0) == -1)
-				perror("send");
-		}
-	}	
-	else
-	{
-		printf("Recieved a bad MQTT message: '%s':'%s'\n destination:%i, sensor:%i, type:%i\n", 
-		topic, payload, msg.destination, msg.sensor, msg.type);
-	}
-		
-	free(topic_copy);
-}
 #endif
diff --git a/libraries/MySensors/core/MyGatewayTransportMQTTClient.cpp b/libraries/MySensors/core/MyGatewayTransportMQTTClient.cpp
index 4528c8e..d622594 100644
--- a/libraries/MySensors/core/MyGatewayTransportMQTTClient.cpp
+++ b/libraries/MySensors/core/MyGatewayTransportMQTTClient.cpp
@@ -70,69 +70,8 @@ void incomingMQTT(char* topic, byte* payload,
                         unsigned int length)
 {
 	debug(PSTR("Message arrived on topic: %s\n"), topic);
-	char *str, *p;
-	uint8_t i = 0;
-	uint8_t bvalue[MAX_PAYLOAD];
-	uint8_t blen = 0;
-	uint8_t command = 0;
-	for (str = strtok_r(topic, "/", &p); str && i <= 5;
-			str = strtok_r(NULL, "/", &p)) {
-		switch (i) {
-			case 0: {
-				// Topic prefix
-				if (strcmp(str, MY_MQTT_SUBSCRIBE_TOPIC_PREFIX) != 0) {
-					// Message not for us or malformed!
-					return;
-				}
-				break;
-			}
-			case 1: {
-				// Node id
-				_mqttMsg.destination = atoi(str);
-				break;
-			}
-			case 2: {
-				// Sensor id
-				_mqttMsg.sensor = atoi(str);
-				break;
-			}
-			case 3: {
-				// Command type
-				command = atoi(str);
-				mSetCommand(_mqttMsg, command);
-				break;
-			}
-			case 4: {
-				// Ack flag
-				mSetRequestAck(_mqttMsg, atoi(str)?1:0);
-				break;
-			}
-			case 5: {
-				// Sub type
-				_mqttMsg.type = atoi(str);
-				// Add payload
-				if (command == C_STREAM) {
-					blen = 0;
-					uint8_t val;
-					while (*payload) {
-						val = protocolH2i(*payload++) << 4;
-						val += protocolH2i(*payload++);
-						bvalue[blen] = val;
-						blen++;
-					}
-					_mqttMsg.set(bvalue, blen);
-				} else {
-					char* ca;
-					ca = (char *) payload;
-					ca += length;
-					*ca = '\0';
-					_mqttMsg.set((const char*) payload);
-				}
-				_available = true;
-			}
-		}
-		i++;
-	}
+	protocolMQTTParse(_mqttMsg, topic, payload, length);
+	_available = true;
 }
 
 
diff --git a/libraries/MySensors/core/MyProtocolMySensors.cpp b/libraries/MySensors/core/MyProtocolMySensors.cpp
index 109f749..68810b3 100644
--- a/libraries/MySensors/core/MyProtocolMySensors.cpp
+++ b/libraries/MySensors/core/MyProtocolMySensors.cpp
@@ -110,6 +110,73 @@ char * protocolFormatMQTTSubscribe(const char* prefix) {
 	return _fmtBuffer;
 }
 
+void protocolMQTTParse(MyMessage &message, char* topic, uint8_t* payload, unsigned int length) {
+	//TODO: Not sure if we should copy the topic and payload into a buffer so we are not modifying original.
+	char *str, *p;
+	uint8_t i = 0;
+	uint8_t bvalue[MAX_PAYLOAD];
+	uint8_t blen = 0;
+	uint8_t command = 0;
+	for (str = strtok_r(topic, "/", &p); str && i <= 5;
+			str = strtok_r(NULL, "/", &p)) {
+		switch (i) {
+			case 0: {
+				// Topic prefix
+				//TODO: Should be checking the topic prefix
+				//if (strcmp(str, MY_MQTT_SUBSCRIBE_TOPIC_PREFIX) != 0) {
+					// Message not for us or malformed!
+				//	return;
+				//}
+				break;
+			}
+			case 1: {
+				// Node id
+				message.destination = atoi(str);
+				break;
+			}
+			case 2: {
+				// Sensor id
+				message.sensor = atoi(str);
+				break;
+			}
+			case 3: {
+				// Command type
+				command = atoi(str);
+				mSetCommand(message, command);
+				break;
+			}
+			case 4: {
+				// Ack flag
+				mSetRequestAck(message, atoi(str)?1:0);
+				break;
+			}
+			case 5: {
+				// Sub type
+				message.type = atoi(str);
+				// Add payload
+				if (command == C_STREAM) {
+					blen = 0;
+					uint8_t val;
+					while (*payload) {
+						val = protocolH2i(*payload++) << 4;
+						val += protocolH2i(*payload++);
+						bvalue[blen] = val;
+						blen++;
+					}
+					message.set(bvalue, blen);
+				} else {
+					char* ca;
+					ca = (char *) payload;
+					ca += length;
+					*ca = '\0';
+					message.set((const char*) payload);
+				}
+			}
+		}
+		i++;
+	}
+}
+
 uint8_t protocolH2i(char c) {
 	uint8_t i = 0;
 	if (c <= '9')