Merge pull request #170 from srzamfir/dev

Mqtt dissector fix

5 files changed, 267 insertions, 3 deletions

diff --git a/src/include/ndpi_protocol_ids.h b/src/include/ndpi_protocol_ids.h
index a60cd5603..cfc0ad56d 100644
--- a/src/include/ndpi_protocol_ids.h
+++ b/src/include/ndpi_protocol_ids.h
@@ -270,9 +270,9 @@
 #define NDPI_SERVICE_OFFICE_365                         219
 #define NDPI_SERVICE_CLOUDFLARE                         220
 #define NDPI_SERVICE_MS_ONE_DRIVE                       221
-
+#define NDPI_PROTOCOL_MQTT								222
 /* UPDATE UPDATE UPDATE UPDATE UPDATE UPDATE UPDATE UPDATE UPDATE */
-#define NDPI_LAST_IMPLEMENTED_PROTOCOL			NDPI_SERVICE_MS_ONE_DRIVE
+#define NDPI_LAST_IMPLEMENTED_PROTOCOL			NDPI_PROTOCOL_MQTT
 
 #define NDPI_MAX_SUPPORTED_PROTOCOLS                    (NDPI_LAST_IMPLEMENTED_PROTOCOL + 1)
 #define NDPI_MAX_NUM_CUSTOM_PROTOCOLS                   (NDPI_NUM_BITS-NDPI_LAST_IMPLEMENTED_PROTOCOL)
diff --git a/src/include/ndpi_protocols.h b/src/include/ndpi_protocols.h
index cb83633bd..b5df1c937 100644
--- a/src/include/ndpi_protocols.h
+++ b/src/include/ndpi_protocols.h
@@ -197,7 +197,7 @@ void ndpi_search_mpegts(struct ndpi_detection_module_struct *ndpi_struct, struct
 void ndpi_search_starcraft(struct ndpi_detection_module_struct *ndpi_struct, struct ndpi_flow_struct *flow);
 void ndpi_search_ubntac2(struct ndpi_detection_module_struct *ndpi_struct, struct ndpi_flow_struct *flow);
 void ndpi_search_coap(struct ndpi_detection_module_struct *ndpi_struct, struct ndpi_flow_struct *flow);
-
+void ndpi_search_mqtt (struct ndpi_detection_module_struct *ndpi_struct, struct ndpi_flow_struct *flow);
 /* --- INIT FUNCTIONS --- */
 void init_afp_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
 void init_aimini_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
@@ -336,4 +336,5 @@ void init_zmq_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int3
 void init_stracraft_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
 void init_ubntac2_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
 void init_coap_dissector(struct ndpi_detection_module_struct *ndpi_struct, u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
+void init_mqtt_dissector (struct ndpi_detection_module_struct *ndpi_struct,u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask);
 #endif /* __NDPI_PROTOCOLS_H__ */
diff --git a/src/lib/Makefile.am b/src/lib/Makefile.am
index 3b0a2943f..abda7ede1 100644
--- a/src/lib/Makefile.am
+++ b/src/lib/Makefile.am
@@ -153,6 +153,7 @@ libndpi_la_SOURCES = ndpi_content_match.c.inc \
 		     protocols/zattoo.c \
 		     protocols/zeromq.c \
 		     protocols/coap.c \
+		     protocols/mqtt.c \
 		     third_party/include/actypes.h \
 		     third_party/include/ahocorasick.h \
 		     third_party/include/node.h \
diff --git a/src/lib/ndpi_main.c b/src/lib/ndpi_main.c
index af5bbdbe4..845c56c63 100644
--- a/src/lib/ndpi_main.c
+++ b/src/lib/ndpi_main.c
@@ -1523,6 +1523,11 @@ static void ndpi_init_protocol_defaults(struct ndpi_detection_module_struct *ndp
 			    no_master, "COAP",
 			    ndpi_build_default_ports(ports_a, 0, 0, 0, 0, 0),         /* TCP */
 			    ndpi_build_default_ports(ports_b, 5683, 5684, 0, 0, 0));  /* UDP */
+    ndpi_set_proto_defaults(ndpi_mod,NDPI_PROTOCOL_ACCEPTABLE,NDPI_PROTOCOL_MQTT,
+    		    no_master,
+				no_master, "MQTT",
+				ndpi_build_default_ports(ports_a, 1883, 8883, 0, 0, 0),  /* TCP */
+				ndpi_build_default_ports(ports_b, 0, 0, 0, 0, 0));  /* UDP */
 
     /* calling function for host and content matched protocols */
     init_string_based_protocols(ndpi_mod);
@@ -2487,6 +2492,9 @@ void ndpi_set_protocol_detection_bitmask2(struct ndpi_detection_module_struct *n
   /* COAP */
   init_coap_dissector(ndpi_struct, &a, detection_bitmask);
 
+  /* MQTT */
+  init_mqtt_dissector(ndpi_struct, &a, detection_bitmask);
+
   /* Put false-positive sensitive protocols at the end */
 
   /* SKYPE */
diff --git a/src/lib/protocols/mqtt.c b/src/lib/protocols/mqtt.c
new file mode 100644
index 000000000..024fad8a7
--- /dev/null
+++ b/src/lib/protocols/mqtt.c
@@ -0,0 +1,254 @@
+/*
+ * mqtt.c
+ *
+ * Copyright (C) 2016 Sorin Zamfir <sorin.zamfir@yahoo.com>
+ *
+ * This file is part of nDPI, an open source deep packet inspection
+ * library based on the OpenDPI and PACE technology by ipoque GmbH
+ *
+ * nDPI is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * nDPI is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with nDPI.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "ndpi_protocols.h"
+#ifdef NDPI_PROTOCOL_MQTT
+
+/**
+ * The type of control messages in mqtt version 3.1.1
+ * see http://docs.oasis-open.org/mqtt/mqtt/v3.1.1
+ */
+enum MQTT_PACKET_TYPES {
+	CONNECT = 1,
+	CONNACK = 2,
+	PUBLISH = 3,
+	PUBACK = 4,
+	PUBREC = 5,
+	PUBREL = 6,
+	PUBCOMP = 7,
+	SUBSCRIBE = 8,
+	SUBACK = 9,
+	UNSUBSCRIBE = 10,
+	UNSUBACK = 11,
+	PINGREQ = 12,
+	PINGRESP = 13,
+	DISCONNECT = 14
+};
+
+/**
+ * Entry point when protocol is identified.
+ */
+static void ndpi_int_mqtt_add_connection (struct ndpi_detection_module_struct *ndpi_struct,
+		struct ndpi_flow_struct *flow)
+{
+	ndpi_set_detected_protocol(ndpi_struct,flow,NDPI_PROTOCOL_MQTT,NDPI_PROTOCOL_UNKNOWN);
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found.\n");
+}
+
+/**
+ * Dissector function that searches Mqtt headers
+ */
+void ndpi_search_mqtt (struct ndpi_detection_module_struct *ndpi_struct,
+		struct ndpi_flow_struct *flow)
+{
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt search called...\n");
+	struct ndpi_packet_struct *packet = &flow->packet;
+	if (packet->detected_protocol_stack[0] != NDPI_PROTOCOL_UNKNOWN) {
+		return;
+	}
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt detection...\n");
+	if (flow->packet_counter > 10) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt .. mandatory header not found!\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "====>>>> Mqtt header: %4x%4x%4x%4x [len: %u]\n",
+			packet->payload[0], packet->payload[1], packet->payload[2], packet->payload[3], packet->payload_packet_len);
+	if (packet->payload_packet_len < 2) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt .. mandatory header not found!\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	if (packet->payload_packet_len > 258) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt .. maximum packet size exceeded!\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	// we extract the remaining length
+	u_int8_t rl = (u_int8_t) (packet->payload[1]);
+	if (packet->payload_packet_len != (rl + 2)) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt .. packet size exceeded!\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	// we extract the packet type
+	u_int8_t pt = (u_int8_t) ((packet->payload[0] & 0xF0) >> 4);
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG,"====>>>> Mqtt packet type: [%d]\n",pt);
+	if ((pt == 0) || (pt == 15)) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt .. invalid packet type!\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	// we extract the flags
+	u_int8_t flags = (u_int8_t) (packet->payload[0] & 0x0F);
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG,"====>>>> Mqtt flags type: [%d]\n",flags);
+	// first stage verification
+	if (((pt == CONNECT) || (pt == CONNACK) || (pt == PUBACK) || (pt == PUBREC) ||
+					(pt == PUBCOMP) || (pt == SUBACK) || (pt == UNSUBACK) || (pt == PINGREQ) ||
+					(pt == PINGRESP) || (pt == DISCONNECT)) && (flags > 0)) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid Packet-Flag combination flag!=0\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	if (((pt == PUBREL) || (pt == SUBSCRIBE) || (pt == UNSUBSCRIBE)) && (flags != 2)) {
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid Packet-Flag combination flag!=2\n");
+		NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+		return;
+	}
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG,"====>>>> Passed first stage of identification\n");
+	// second stage verification (no payload, just variable headers)
+	if ((pt == CONNACK) || (pt == PUBACK) || (pt == PUBREL) ||
+			(pt == PUBREC) || (pt == PUBCOMP) || (pt == UNSUBACK)) {
+		if (packet->payload_packet_len != 4) { // these packets are always 4 bytes long
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid Packet-Length < 4 \n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found CONNACK/PUBACK/PUBREL/PUBREC/PUBCOMP/UNSUBACK\n");
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		}
+	}
+	if ((pt == PINGREQ) || (pt == PINGRESP) || (pt == DISCONNECT)) {
+		if (packet->payload_packet_len != 2) { // these packets are always 2 bytes long
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid Packet-Length <2 \n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found PING/PINGRESP/DISCONNECT\n");
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		}
+	}
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG,"====>>>> Passed second stage of identification\n");
+	// third stage verification (payload)
+	if (pt == CONNECT) {
+		if (memcmp(&(packet->payload[4]),"MQTT",4) == 0) {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found CONNECT\n");
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid CONNECT\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		}
+	}
+	if (pt == PUBLISH) {
+		// payload CAN be zero bytes length (section 3.3.3 of MQTT standard)
+		u_int8_t qos = (u_int8_t) (flags & 0x06);
+		u_int8_t retain = (u_int8_t) (flags & 0x01);
+		u_int8_t dup = (u_int8_t) (flags & 0x04);
+		if (qos > 2) { // qos values possible are 0,1,2
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH qos\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		}
+		if (retain > 1) { // retain flag possible 0,1
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH retain\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		}
+		if (dup > 1) { // dup flag possible 0,1
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH dup\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		}
+		if (qos == 0) {
+			if (dup != 0) {
+				NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH qos0 and dup combination\n");
+				NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+				return;
+			}
+			if (packet->payload_packet_len < 5) { // at least topic (3Bytes + 2Bytes fixed header)
+				NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH qos0 size\n");
+				NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+				return;
+			}
+		}
+		if ((qos == 1) || (qos == 2)) {
+			if (packet->payload_packet_len < 7 ) { // at least topic + pkt identifier (3Bytes + 2Bytes + 2Bytes fixed header)
+				NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid PUBLISH qos1&2\n");
+				NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+				return;
+			}
+		}
+		NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found PUBLISH\n");
+		ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+		return;
+	}
+	if (pt == SUBSCRIBE) {
+		if (packet->payload_packet_len < 8) { // at least one topic+filter is required in the payload
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid SUBSCRIBE\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found SUBSCRIBE\n");
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		}
+	}
+	if (pt == SUBACK ) {
+		if (packet->payload_packet_len <5 ) { // must have at least a response code
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid SUBACK\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found SUBACK\n");
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		}
+	}
+	if (pt == UNSUBSCRIBE) {
+		if (packet->payload_packet_len < 7) { // at least a topic
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Excluding Mqtt invalid UNSUBSCRIBE\n");
+			NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+			return;
+		} else {
+			NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt found UNSUBSCRIBE\n",pt);
+			ndpi_int_mqtt_add_connection(ndpi_struct,flow);
+			return;
+		}
+	}
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG,"====>>>> Passed third stage of identification");
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Reached the end excluding Mqtt ...\n");
+	NDPI_ADD_PROTOCOL_TO_BITMASK(flow->excluded_protocol_bitmask, NDPI_PROTOCOL_MQTT);
+	return;
+}
+/**
+ * Entry point for the ndpi library
+ */
+void init_mqtt_dissector (struct ndpi_detection_module_struct *ndpi_struct,
+		u_int32_t *id, NDPI_PROTOCOL_BITMASK *detection_bitmask)
+{
+	NDPI_LOG(NDPI_PROTOCOL_MQTT, ndpi_struct, NDPI_LOG_DEBUG, "Mqtt dissector init...\n");
+	ndpi_set_bitmask_protocol_detection ("MQTT", ndpi_struct, detection_bitmask, *id,
+			NDPI_PROTOCOL_MQTT,
+			ndpi_search_mqtt,
+			NDPI_SELECTION_BITMASK_PROTOCOL_V4_V6_TCP_WITH_PAYLOAD,
+			SAVE_DETECTION_BITMASK_AS_UNKNOWN, ADD_TO_DETECTION_BITMASK);
+	*id +=1;
+}
+
+#endif // NDPI_PROTOCOL_MQTT
+