TCPROS - Protocol dissector improvements

 - Added specific dissector for ROS Clock messages
 - Added dissector heuristics
 - Improve ROS traffic detection

Problems to resolve:
 - Some topics are mistaken for Clock messages. These includes the wheel encoder topic.
 - ROS Services are not detected.
 - Ideally, the TF topic should be dissected as it is a very common topic

Bug: 12749
Change-Id: I14255cbb42ae36b7e39f64dc1a5c6efffe19c8b1
Reviewed-on: https://code.wireshark.org/review/17086/
Reviewed-on: https://code.wireshark.org/review/17086
Reviewed-by: Alexis La Goutte <alexis.lagoutte@gmail.com>
Petri-Dish: Alexis La Goutte <alexis.lagoutte@gmail.com>
Tested-by: Petri Dish Buildbot <buildbot-no-reply@wireshark.org>
Reviewed-by: Michael Mann <mmann78@netscape.net>

2 files changed, 187 insertions, 34 deletions

diff --git a/AUTHORS.src b/AUTHORS.src
index bca6dbba63..059ef43af5 100644
--- a/AUTHORS.src
+++ b/AUTHORS.src
@@ -3663,7 +3663,7 @@ Pratik Yeole		<pyeole[AT]ncsu.edu> {
          Fixed incorrect decoding of Network Layer Reachability Information (NLRI) in BGP UPDATE message with add-path support
 }
 
-Guillaume Autran	<gautran[AT]clearpathrobotics.com> {
+Guillaume Autran	<gautran[AT]clearpath.ai> {
 	TCPROS support
 }
 
diff --git a/epan/dissectors/packet-tcpros.c b/epan/dissectors/packet-tcpros.c
index ed66fc40fe..429c69d68c 100644
--- a/epan/dissectors/packet-tcpros.c
+++ b/epan/dissectors/packet-tcpros.c
@@ -38,6 +38,7 @@ void proto_reg_handoff_tcpros(void);
 
 
 static int proto_tcpros = -1;
+static dissector_handle_t tcpros_handle;
 
 /** desegmentation of TCPROS over TCP */
 static gboolean tcpros_desegment = TRUE;
@@ -51,6 +52,8 @@ static int hf_tcpros_connection_header_field_length = -1;
 static int hf_tcpros_connection_header_field_data = -1;
 static int hf_tcpros_connection_header_field_name = -1;
 static int hf_tcpros_connection_header_field_value = -1;
+static int hf_tcpros_clock = -1;
+static int hf_tcpros_clock_length = -1;
 static int hf_tcpros_message = -1;
 static int hf_tcpros_message_length = -1;
 static int hf_tcpros_message_body = -1;
@@ -160,7 +163,6 @@ dissect_ros_message_header_stamp(tvbuff_t *tvb, proto_tree *root_tree, packet_in
 	gint consumed_len = 0;
 	guint32 sec, nsec;
 
-
 	ti = proto_tree_add_item(root_tree, hf_tcpros_message_header_stamp, tvb, offset + consumed_len, SIZE_OF_LENGTH_STAMP, ENC_LITTLE_ENDIAN);
 	sub_tree = proto_item_add_subtree(ti, ett_tcpros);
 
@@ -168,13 +170,36 @@ dissect_ros_message_header_stamp(tvbuff_t *tvb, proto_tree *root_tree, packet_in
 	proto_tree_add_item(sub_tree, hf_tcpros_message_header_stamp_sec, tvb, offset + consumed_len, SIZE_OF_LENGTH_FIELD, ENC_LITTLE_ENDIAN);
 	sec = tvb_get_letohl(tvb, offset + consumed_len);
 	consumed_len += SIZE_OF_LENGTH_FIELD;
-	col_append_fstr(pinfo->cinfo, COL_INFO, "Sec: %d ", sec);
 
 	/** Nano seconds */
 	proto_tree_add_item(sub_tree, hf_tcpros_message_header_stamp_nsec, tvb, offset + consumed_len, SIZE_OF_LENGTH_FIELD, ENC_LITTLE_ENDIAN);
 	nsec = tvb_get_letohl(tvb, offset + consumed_len);
 	consumed_len += SIZE_OF_LENGTH_FIELD;
-	col_append_fstr(pinfo->cinfo, COL_INFO, "NSec: %d ", nsec);
+
+	/** Info */
+	col_append_fstr(pinfo->cinfo, COL_INFO, "Timestamp: %d.%d ", sec, nsec);
+
+	return consumed_len;
+}
+
+static gint
+dissect_ros_clock(tvbuff_t *tvb, proto_tree *root_tree, packet_info *pinfo, gint offset)
+{
+	proto_item *ti;
+	proto_tree *sub_tree;
+
+	gint consumed_len = 0;
+
+	col_append_str(pinfo->cinfo, COL_INFO, "[ROS Clock] ");
+
+	/** We got a ROS Clock msg */
+	ti = proto_tree_add_item(root_tree, hf_tcpros_clock, tvb, offset, SIZE_OF_LENGTH_FIELD, ENC_LITTLE_ENDIAN);
+	sub_tree = proto_item_add_subtree(ti, ett_tcpros);
+
+	proto_tree_add_item(sub_tree, hf_tcpros_clock_length, tvb, offset, SIZE_OF_LENGTH_FIELD, ENC_LITTLE_ENDIAN);
+	consumed_len += SIZE_OF_LENGTH_FIELD;
+
+	consumed_len += dissect_ros_message_header_stamp(tvb, sub_tree, pinfo, offset + consumed_len);
 
 	return consumed_len;
 }
@@ -216,7 +241,7 @@ dissect_ros_message_header(tvbuff_t *tvb, proto_tree *root_tree, packet_info *pi
 	consumed_len += SIZE_OF_LENGTH_FIELD;
 
 	proto_tree_add_item_ret_string(sub_tree, hf_tcpros_message_header_frame_value, tvb, offset + consumed_len, frame_id_len, ENC_UTF_8|ENC_NA, wmem_packet_scope(), &frame_str);
-	col_append_fstr(pinfo->cinfo, COL_INFO, "Frame: '%s' ",	frame_str);
+	col_append_fstr(pinfo->cinfo, COL_INFO, "Frame ID: '%s' ", frame_str);
 	consumed_len += frame_id_len;
 
 	return consumed_len;
@@ -258,7 +283,7 @@ dissect_ros_message(tvbuff_t *tvb, proto_tree *root_tree, packet_info *pinfo, gi
 	/** Body.Payload */
 	payload_len = (total_len + SIZE_OF_LENGTH_FIELD) - consumed_len;
 	proto_tree_add_item(sub_tree, hf_tcpros_message_payload, tvb, offset + consumed_len, payload_len, ENC_NA);
-        consumed_len += payload_len;
+	consumed_len += payload_len;
 
 
 	return consumed_len;
@@ -284,6 +309,103 @@ is_connection_header(tvbuff_t *tvb, packet_info *pinfo _U_ , guint offset)
 	return is_con_header;
 }
 
+static gboolean
+is_rosheaderfield(tvbuff_t *tvb, packet_info *pinfo _U_ , guint offset)
+{
+	/** ROS Header Field:
+	    4-byte len + string */
+	gint available = tvb_reported_length_remaining(tvb, offset);
+	gint string_len = 0;
+	gint i;
+
+	if( available < 4 )
+		return FALSE;
+
+	string_len = tvb_get_letohl(tvb, offset);
+	if( string_len < 0 )
+		return FALSE;
+
+	/** If we don't have enough data for the whole string, assume its not */
+	if( available < (string_len + 4) )
+		return FALSE;
+	/** Check for a valid ascii character and not nil */
+	for( i = 0; i < string_len; i++ ) {
+		gint8 ch = tvb_get_guint8(tvb, offset + 4 + i);
+		if( !g_ascii_isalnum(ch) || 0x00 == ch )
+			return FALSE;
+	}
+
+	/** Assume it is */
+	return TRUE;
+}
+
+static gboolean
+is_rosconnection_header(tvbuff_t *tvb, packet_info *pinfo _U_ , guint offset)
+{
+	/** ROS Connection Headers: http://wiki.ros.org/ROS/Connection%20Header
+	    4-byte length + [4-byte length + string] */
+	gint available = tvb_reported_length_remaining(tvb, offset);
+	gint msg_len = 0;
+
+	if( available < 8+1 )
+		return FALSE;
+
+	msg_len = tvb_get_letohl(tvb, offset);
+	if( msg_len < 4+1 )
+		return FALSE;
+
+	/** Check first header field */
+	if( !is_rosheaderfield(tvb, pinfo, offset + 4) )
+		return FALSE;
+
+
+	return TRUE;
+}
+
+static gboolean
+is_rosclock(tvbuff_t *tvb, packet_info *pinfo _U_ , guint offset)
+{
+	/** ROS Clock message: http://docs.ros.org/api/rosgraph_msgs/html/msg/Clock.html
+	    4-byte length + 8-byte timestamp == 12 bytes exactly */
+	gint available = tvb_reported_length_remaining(tvb, offset);
+	if( available < 12 )
+		return FALSE;
+
+	if( tvb_get_letohl(tvb, offset) != 8 )
+		return FALSE;
+
+	/** This is highly likely a clock message. */
+	if( available != 12 )
+		return FALSE;
+
+	return TRUE;
+}
+
+static gboolean
+is_rosmsg(tvbuff_t *tvb, packet_info *pinfo _U_ , guint offset)
+{
+	/** Most ROS messages start with a header: http://docs.ros.org/jade/api/std_msgs/html/msg/Header.html
+	    4-byte size + 4-byte sequence id + 8-byte timestamp + 4-byte frame id length + frame id */
+	gint available = tvb_reported_length_remaining(tvb, offset);
+	gint string_len = 0;
+	gint msg_len = 0;
+
+	if( available < 20 )
+		return FALSE;
+
+	msg_len = tvb_get_letohl(tvb, offset);
+	if( msg_len < 16 )
+		return FALSE;
+
+	/** Check to see if the frame id length is reasonable */
+	string_len = tvb_get_letohl(tvb, offset + 4 + 4 + 8);
+	if( string_len > (msg_len - (4 + 8 + 4)) )
+		return FALSE;
+
+	/** This is highly likely a ROS message. */
+	return TRUE;
+}
+
 static void
 dissect_ros_common(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, gboolean is_tcp _U_ )
 {
@@ -311,7 +433,16 @@ dissect_ros_common(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, gboolean
 			return;
 		}
 		/** There are two types of packet: Connection Headers and ROS Message. Which one is it? */
-		if( is_connection_header(tvb, pinfo, offset) ) {
+		if( is_rosclock(tvb, pinfo, offset) ) {
+			/** This is a ROS Clock message. */
+			offset += dissect_ros_clock(tvb, root_tree, pinfo, offset);
+		} else if( is_rosmsg(tvb, pinfo, offset) ) {
+			/** We have a ROS message. */
+			offset += dissect_ros_message(tvb, root_tree, pinfo, offset);
+		} else if( is_rosconnection_header(tvb, pinfo, offset) ) {
+			/** Check for a connection header */
+			offset += dissect_ros_connection_header(tvb, root_tree, pinfo, offset);
+		} else if( is_connection_header(tvb, pinfo, offset) ) {
 			/** We have a ROS connection header. */
 			offset += dissect_ros_connection_header(tvb, root_tree, pinfo, offset);
 		} else {
@@ -365,92 +496,76 @@ proto_register_tcpros(void)
 		{ &hf_tcpros_connection_header,             { "ROS Connection", "tcpros.header",
 							      FT_UINT_BYTES, BASE_NONE, NULL, 0,
 							      "Message Header", HFILL } },
-
 		{ &hf_tcpros_connection_header_length,      { "Header Length", "tcpros.header_length",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "Message Header Length", HFILL } },
-
 		{ &hf_tcpros_connection_header_content,     { "Header Content", "tcpros.header_content",
 							      FT_BYTES, BASE_NONE, NULL, 0,
 							      "Message Header Content", HFILL } },
 
-
 		{ &hf_tcpros_connection_header_field,       { "Field", "tcpros.header_field",
 							      FT_UINT_STRING, BASE_NONE, NULL, 0,
 							      "Message Header Field", HFILL } },
-
 		{ &hf_tcpros_connection_header_field_length, { "Field Length", "tcpros.header_field_length",
 							       FT_UINT32, BASE_DEC, NULL, 0,
 							       "Message Header Field Length", HFILL } },
-
 		{ &hf_tcpros_connection_header_field_data, { "Field Content", "tcpros.header_field_data",
 							     FT_STRING, BASE_NONE, NULL, 0,
 							     "Message Header Field Content", HFILL } },
-
 		{ &hf_tcpros_connection_header_field_name,  { "Name", "tcpros.header_field_name",
 							      FT_STRING, BASE_NONE, NULL, 0,
 							      "Message Header Field Name", HFILL } },
-
 		{ &hf_tcpros_connection_header_field_value, { "Value", "tcpros.header_field_value",
 							      FT_STRING, BASE_NONE, NULL, 0,
 							      "Message Header Field Value", HFILL } },
 
+		{ &hf_tcpros_clock,                         { "ROS Clock", "tcpros.clock",
+							      FT_UINT_BYTES, BASE_NONE, NULL, 0,
+							      "ROS Clock Packet", HFILL } },
+		{ &hf_tcpros_clock_length,                  { "Clock Length", "tcpros.clock.length",
+							      FT_UINT32, BASE_DEC, NULL, 0,
+							      "ROS Clock Packet length", HFILL } },
 
 		{ &hf_tcpros_message,                       { "ROS Message", "tcpros.message",
 							      FT_UINT_BYTES, BASE_NONE, NULL, 0,
 							      "ROS Message Packet", HFILL } },
-
 		{ &hf_tcpros_message_length,                { "Message Length", "tcpros.message.length",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "ROS Message Packet length", HFILL } },
-
 		{ &hf_tcpros_message_body,                   { "Message Content", "tcpros.message.body",
-							      FT_BYTES, BASE_NONE, NULL, 0,
-							      "ROS Message Packet Body", HFILL } },
-
-
-
+							       FT_BYTES, BASE_NONE, NULL, 0,
+							       "ROS Message Packet Body", HFILL } },
 
 		{ &hf_tcpros_message_header,                { "Header", "tcpros.message.header",
 							      FT_BYTES, BASE_NONE, NULL, 0,
 							      "ROS Message Header", HFILL } },
-
 		{ &hf_tcpros_message_header_seq,            { "Sequence ID", "tcpros.message.header.seq",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "ROS Message Header Sequence", HFILL } },
 		{ &hf_tcpros_message_header_stamp,          { "Timestamp", "tcpros.message.header.stamp",
 							      FT_ABSOLUTE_TIME, ABSOLUTE_TIME_LOCAL, NULL, 0,
 							      "ROS Message Header Stamp", HFILL } },
-
 		{ &hf_tcpros_message_header_stamp_sec,      { "Seconds", "tcpros.message.header.stamp.sec",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "ROS Message Header Stamp Sec", HFILL } },
-
 		{ &hf_tcpros_message_header_stamp_nsec,     { "Nanoseconds", "tcpros.message.header.stamp.nsec",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "ROS Message Header Stamp NSec", HFILL } },
 
-
 		{ &hf_tcpros_message_header_frame,          { "Frame ID", "tcpros.message.header.frame",
 							      FT_UINT_STRING, BASE_NONE, NULL, 0,
 							      "ROS Message Header Frame ID", HFILL } },
-
 		{ &hf_tcpros_message_header_frame_length,   { "Length", "tcpros.message.header.frame.len",
 							      FT_UINT32, BASE_DEC, NULL, 0,
 							      "ROS Message Header Frame ID Length", HFILL } },
-
 		{ &hf_tcpros_message_header_frame_value,     { "Value", "tcpros.message.header.frame.value",
-							     FT_STRING, BASE_NONE, NULL, 0,
-							     "ROS Message Header Frame ID Value", HFILL } },
-
+							       FT_STRING, BASE_NONE, NULL, 0,
+							       "ROS Message Header Frame ID Value", HFILL } },
 
 		{ &hf_tcpros_message_payload,               { "Payload", "tcpros.message.payload",
 							      FT_BYTES, BASE_NONE, NULL, 0,
 							      "ROS Message Packet Payload", HFILL } },
 
-
-
-
 	};
 
 	static gint *ett[] = {
@@ -476,16 +591,54 @@ proto_register_tcpros(void)
 
 }
 
+/* Heuristics test */
+static gboolean
+test_tcpros(packet_info *pinfo, tvbuff_t *tvb, int offset, void *data _U_)
+{
+	if (tvb_captured_length(tvb) < 8)
+		return FALSE;
+
+	if( is_rosclock(tvb, pinfo, offset) )
+		return TRUE;
+	if( is_rosmsg(tvb, pinfo, offset) )
+		return TRUE;
+	if( is_rosconnection_header(tvb, pinfo, offset) )
+		return TRUE;
+
+	return FALSE;
+}
+
+static gboolean
+dissect_tcpros_heur_tcp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data)
+{
+	conversation_t *conversation;
+
+	if (!test_tcpros(pinfo, tvb, 0, data))
+		return FALSE;
+
+	conversation = find_or_create_conversation(pinfo);
+	conversation_set_dissector(conversation, tcpros_handle);
+
+	dissect_tcpros(tvb, pinfo, tree, data);
+
+	return (TRUE);
+}
+
+
 
 void
 proto_reg_handoff_tcpros(void)
 {
-	static dissector_handle_t tcpros_handle;
 	static gboolean Initialized = FALSE;
 
 	if (!Initialized) {
 		tcpros_handle = create_dissector_handle(dissect_tcpros, proto_tcpros);
 		dissector_add_for_decode_as("tcp.port", tcpros_handle);   /* for "decode-as" */
+
+		/* register as heuristic dissector */
+		heur_dissector_add("tcp", dissect_tcpros_heur_tcp, "TCPROS over TCP",
+				   "TCPROS_tcp", proto_tcpros, HEURISTIC_DISABLE);
+
 		Initialized    = TRUE;
 	}
 }