Wireshark · Wireshark-dev: [Wireshark-dev] [RFC] btl2cap: dissect extended control field

[Emeltchenko Andrei <Andrei.Emeltchenko.news@xxxxxxxxx>]

From: Andrei Emeltchenko <andrei.emeltchenko@xxxxxxxxx>

Adds support for parsing extended control field. Extended control
field may be used for ERTM and streaming mode (if EWS specified).

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@xxxxxxxxx>
---
 epan/dissectors/packet-btl2cap.c |  173 +++++++++++++++++++++++++++++---------
 1 files changed, 132 insertions(+), 41 deletions(-)

diff --git a/epan/dissectors/packet-btl2cap.c b/epan/dissectors/packet-btl2cap.c
index ac0a909..813e47d 100644
--- a/epan/dissectors/packet-btl2cap.c
+++ b/epan/dissectors/packet-btl2cap.c
@@ -114,11 +114,16 @@ static int hf_btl2cap_option_sdu_arrival_time = -1;
 static int hf_btl2cap_option_access_latency = -1;
 static int hf_btl2cap_control = -1;
 static int hf_btl2cap_control_sar = -1;
+static int hf_btl2cap_control_sar_ext = -1;
 static int hf_btl2cap_control_reqseq = -1;
+static int hf_btl2cap_control_reqseq_ext = -1;
 static int hf_btl2cap_control_txseq = -1;
+static int hf_btl2cap_control_txseq_ext = -1;
 static int hf_btl2cap_control_retransmissiondisable = -1;
 static int hf_btl2cap_control_supervisory = -1;
+static int hf_btl2cap_control_supervisory_ext = -1;
 static int hf_btl2cap_control_type = -1;
+static int hf_btl2cap_control_type_ext = -1;
 static int hf_btl2cap_fcs = -1;
 static int hf_btl2cap_sdulength = -1;
 static int hf_btl2cap_continuation_to = -1;
@@ -152,7 +157,8 @@ typedef struct _config_data_t {
 } config_data_t;
 typedef struct _psm_data_t {
 	guint16			psm;
-	gboolean            local_service;
+	gboolean		local_service;
+	gboolean		ext_ctrl;
 	config_data_t	in;
 	config_data_t	out;
 } psm_data_t;
@@ -432,7 +438,7 @@ dissect_movechanrequest(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto
 }
 
 static int
-dissect_options(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto_tree *tree, int length, config_data_t *config_data)
+dissect_options(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto_tree *tree, int length, config_data_t *config_data, psm_data_t *psm_data)
 {
 	proto_item *ti_option=NULL;
 	proto_tree *ti_option_subtree=NULL;
@@ -549,6 +555,9 @@ dissect_options(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto_tree *t
 				break;
 
 			case 0x07: /* Extended Window Size */
+				if (psm_data)
+					psm_data->ext_ctrl = 1;
+
 				proto_tree_add_item(ti_option_subtree, hf_btl2cap_option_window, tvb, offset, 2, TRUE);
 				offset+=2;
 
@@ -593,7 +602,7 @@ dissect_configrequest(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto_t
 				config_data = &(psm_data->in);
 		else
 			config_data = NULL;
-		offset=dissect_options(tvb, offset, pinfo, tree, length - 4, config_data);
+		offset=dissect_options(tvb, offset, pinfo, tree, length - 4, config_data, psm_data);
 	}
 
 	return offset;
@@ -737,7 +746,7 @@ dissect_configresponse(tvbuff_t *tvb, int offset, packet_info *pinfo _U_, proto_
 				config_data = &(psm_data->in);
 		else
 			config_data = NULL;
-		offset=dissect_options(tvb, offset, pinfo, tree, length - 6, config_data);
+		offset=dissect_options(tvb, offset, pinfo, tree, length - 6, config_data, psm_data);
 	}
 
 	return offset;
@@ -905,15 +914,21 @@ static void
 dissect_i_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, proto_tree *btl2cap_tree, psm_data_t *psm_data, guint16 length, int offset, config_data_t *config_data)
 {
 	tvbuff_t *next_tvb = NULL;
-	guint16 control, segment;
+	guint32 control, segment;
 	guint16 sdulen;
 	proto_item* ti_control;
 	proto_tree* ti_control_subtree;
 	sdu_reassembly_t* mfp = NULL;
 	guint16 psm = (psm_data?psm_data->psm:0);
 
-	control = tvb_get_letohs(tvb, offset);
-	segment = (control & 0xC000) >> 14;
+	if (psm_data->ext_ctrl) {
+		control = tvb_get_letohl(tvb, offset);
+		segment = (control & 0x00030000) >> 16;
+	} else {
+		control = tvb_get_letohs(tvb, offset);
+		segment = (control & 0xC000) >> 14;
+	}
+
 	switch(segment)
 	{
 	case 0:
@@ -929,19 +944,36 @@ dissect_i_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, proto_tree
 		col_append_str(pinfo->cinfo, COL_INFO, "[I] Continuation SDU");
 		break;
 	}
-	ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
-		offset, 2, "Control: %s reqseq:%d r:%d txseq:%d",
-		val_to_str((control & 0xC000) >> 14, control_sar_vals, "unknown"),
-		(control & 0x3F00) >> 8,
-		(control & 0x0080) >> 7,
-		(control & 0x007E) >> 1);
-	ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_sar, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_retransmissiondisable, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_txseq, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type, tvb, offset, 2, TRUE);
-	offset += 2;
+
+	if (psm_data->ext_ctrl) {
+		ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
+				offset, 4, "Control: %s reqseq:%d F:%d txseq:%d",
+				val_to_str((control & 0x00030000) >> 16, control_sar_vals, "unknown"),
+				(control & 0x0000FFFC) >> 2,
+				(control & 0x00000002) >> 1,
+				(control & 0xFFFC0000) >> 18);
+
+		ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_sar_ext, tvb, offset, 4, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq_ext, tvb, offset, 4, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_txseq_ext, tvb, offset, 4, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type_ext, tvb, offset, 4, TRUE);
+		offset += 4;
+	} else {
+		ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
+				offset, 2, "Control: %s reqseq:%d r:%d txseq:%d",
+				val_to_str((control & 0xC000) >> 14, control_sar_vals, "unknown"),
+				(control & 0x3F00) >> 8,
+				(control & 0x0080) >> 7,
+				(control & 0x007E) >> 1);
+		ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_sar, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_retransmissiondisable, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_txseq, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type, tvb, offset, 2, TRUE);
+		offset += 2;
+	}
 
 	/*Segmented frames with SAR = start have an extra SDU length header field*/
 	if(segment == 0x01) {
@@ -1039,14 +1071,21 @@ dissect_i_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, proto_tree
 }
 
 static void
-dissect_s_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree _U_, proto_tree *btl2cap_tree, guint16 psm _U_, guint16 length _U_, int offset, config_data_t *config_data _U_)
+dissect_s_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree _U_, proto_tree *btl2cap_tree, guint16 psm _U_, guint16 length _U_, int offset, config_data_t *config_data _U_, gboolean ext_ctrl)
 {
 	proto_item* ti_control;
 	proto_tree* ti_control_subtree;
-	guint16 control;
+	guint32 control;
+	guint16 supervise;
 
-	control = tvb_get_letohs(tvb, offset);
-	switch((control & 0x000C) >> 2)
+	if (ext_ctrl)
+		control = tvb_get_letohl(tvb, offset);
+	else
+		control = tvb_get_letohs(tvb, offset);
+
+	supervise = ext_ctrl ? (control & 0x00030000) >> 16 : (control & 0x000C) >> 2;
+
+	switch(supervise)
 	{
 	case 0:
 		col_append_str(pinfo->cinfo, COL_INFO, "[S] Receiver Ready");
@@ -1054,25 +1093,48 @@ dissect_s_frame(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree _U_, proto_t
 	case 1:
 		col_append_str(pinfo->cinfo, COL_INFO, "[S] Reject");
 		break;
+	case 2:
+		col_append_str(pinfo->cinfo, COL_INFO, "[S] Receiver Not Ready");
+		break;
+	case 3:
+		col_append_str(pinfo->cinfo, COL_INFO, "[S] Select Reject");
+		break;
 	default:
 		col_append_str(pinfo->cinfo, COL_INFO, "[S] Unknown supervisory frame");
 		break;
 	}
-	ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
-		offset, 2, "Control: %s reqseq:%d r:%d",
-		val_to_str((control & 0x000C) >> 2, control_supervisory_vals, "unknown"),
-		(control & 0x3F00) >> 8,
-		(control & 0x0080) >> 7);
-	ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_retransmissiondisable, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_supervisory, tvb, offset, 2, TRUE);
-	proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type, tvb, offset, 2, TRUE);
-	offset += 2;
+
+	if (ext_ctrl) {
+		ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
+				offset, 4, "ExtControl: %s reqseq:%d P:%d",
+				val_to_str((control & 0x00030000) >> 16, control_supervisory_vals, "unknown"),
+				(control & 0x0000FFFC) >> 2,
+				(control & 0x00040000) >> 18);
+
+		ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq_ext, tvb, offset, 4, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_supervisory_ext, tvb, offset, 4, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type_ext, tvb, offset, 4, TRUE);
+		offset += 4;
+	} else {
+		ti_control = proto_tree_add_none_format(btl2cap_tree, hf_btl2cap_control, tvb,
+				offset, 2, "Control: %s reqseq:%d r:%d",
+				val_to_str((control & 0x000C) >> 2, control_supervisory_vals, "unknown"),
+				(control & 0x3F00) >> 8,
+				(control & 0x0080) >> 7);
+		ti_control_subtree = proto_item_add_subtree(ti_control, ett_btl2cap_control);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_reqseq, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_retransmissiondisable, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_supervisory, tvb, offset, 2, TRUE);
+		proto_tree_add_item(ti_control_subtree, hf_btl2cap_control_type, tvb, offset, 2, TRUE);
+		offset += 2;
+	}
+
 	proto_tree_add_item(ti_control_subtree, hf_btl2cap_fcs, tvb, offset, 2, TRUE);
 	offset += 2;
 }
 
+
 /* Code to actually dissect the packets
  * This dissector will only be called ontop of BTHCI ACL
  * and this dissector _REQUIRES_ that
@@ -1267,7 +1329,7 @@ dissect_btl2cap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree)
 		if (cid == BTL2CAP_FIXED_CID_AMP_MAN) {
 			control = tvb_get_letohs(tvb, offset);
 			if(control & 0x1) {
-				dissect_s_frame(tvb, pinfo, tree, btl2cap_tree, 0 /* unused */, length, offset, NULL /* unused */);
+				dissect_s_frame(tvb, pinfo, tree, btl2cap_tree, 0 /* unused */, length, offset, NULL /* unused */, 0);
 			} else {
 				proto_item* ti_control;
 				proto_tree* ti_control_subtree;
@@ -1310,12 +1372,15 @@ dissect_btl2cap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree)
 			if(config_data->mode == 0) {
 				dissect_b_frame(tvb, pinfo, tree, btl2cap_tree, psm, psm_data->local_service, length, offset);
 			} else {
-				control = tvb_get_letohs(tvb, offset);
-				if(control & 0x1) {
-					dissect_s_frame(tvb, pinfo, tree, btl2cap_tree, psm, length, offset, config_data);
-				} else {
+				if (psm_data->ext_ctrl)
+					control = tvb_get_letohl(tvb, offset);
+				else
+					control = tvb_get_letohs(tvb, offset);
+
+				if (control & 0x01)
+					dissect_s_frame(tvb, pinfo, tree, btl2cap_tree, psm, length, offset, config_data, psm_data->ext_ctrl);
+				else
 					dissect_i_frame(tvb, pinfo, tree, btl2cap_tree, psm_data, length, offset, config_data);
-				}
 			}
 		} else {
 			psm=0;
@@ -1699,16 +1764,31 @@ proto_register_btl2cap(void)
 				FT_UINT16, BASE_HEX, VALS(control_sar_vals), 0xC000,
 				NULL, HFILL }
 		},
+		{ &hf_btl2cap_control_sar_ext,
+			{ "Segmentation and reassembly",           "btl2cap.control_sar",
+				FT_UINT32, BASE_HEX, VALS(control_sar_vals), 0x00030000,
+				NULL, HFILL }
+		},
 		{ &hf_btl2cap_control_reqseq,
 			{ "ReqSeq",           "btl2cap.control_reqseq",
 				FT_UINT16, BASE_DEC, NULL, 0x3F00,
 				"Request Sequence Number", HFILL }
 		},
+		{ &hf_btl2cap_control_reqseq_ext,
+			{ "ReqSeq",           "btl2cap.control_reqseq_ext",
+				FT_UINT32, BASE_DEC, NULL, 0x0000FFFC,
+				"Request Sequence Number", HFILL }
+		},
 		{ &hf_btl2cap_control_txseq,
 			{ "TxSeq",           "btl2cap.control_txseq",
 				FT_UINT16, BASE_DEC, NULL, 0x007E,
 				"Transmitted Sequence Number", HFILL }
 		},
+		{ &hf_btl2cap_control_txseq_ext,
+			{ "TxSeq",           "btl2cap.control_txseq_ext",
+				FT_UINT32, BASE_DEC, NULL, 0xFFFC0000,
+				"Transmitted Sequence Number", HFILL }
+		},
 		{ &hf_btl2cap_control_retransmissiondisable,
 			{ "R",           "btl2cap.control_retransmissiondisable",
 				FT_UINT16, BASE_HEX, NULL, 0x0080,
@@ -1719,11 +1799,22 @@ proto_register_btl2cap(void)
 				FT_UINT16, BASE_HEX, VALS(control_supervisory_vals), 0x000C,
 				"Supervisory Function", HFILL }
 		},
+		{ &hf_btl2cap_control_supervisory_ext,
+			{ "S",           "btl2cap.control_supervisory",
+				FT_UINT32, BASE_HEX, VALS(control_supervisory_vals), 0x00030000,
+				"Supervisory Function", HFILL }
+		},
 		{ &hf_btl2cap_control_type,
 			{ "Frame Type",           "btl2cap.control_type",
 				FT_UINT16, BASE_HEX, VALS(control_type_vals), 0x0001,
 				NULL, HFILL }
 		},
+		{ &hf_btl2cap_control_type_ext,
+			{ "Frame Type",           "btl2cap.control_type",
+				FT_UINT32, BASE_HEX, VALS(control_type_vals), 0x00000001,
+				NULL, HFILL }
+		},
+
 		{ &hf_btl2cap_control,
 			{ "Control field",           "btl2cap.control",
 				FT_NONE, BASE_NONE, NULL, 0x0,
-- 
1.7.4.1