gsmtap: Support dissection of non CS-1 (E)GPRS data blocks

Work based on current code from epan/dissectors/packet-gsm_abis_pgsl.c,
as well as on initial patch from Holger Hans Peter Freyther [1].

Tested with one downlink MCS1 data block generated by osmo-pcu.

[1] https://osmocom.org/issues/1542

Change-Id: I01a8bd1cdb78d1c236a451fbee37854eb688fa14
Reviewed-on: https://code.wireshark.org/review/36489
Reviewed-by: Harald Welte <laforge@gnumonks.org>
Petri-Dish: Alexis La Goutte <alexis.lagoutte@gmail.com>
Tested-by: Petri Dish Buildbot
Reviewed-by: Anders Broman <a.broman58@gmail.com>

1 files changed, 217 insertions, 1 deletions

diff --git a/epan/dissectors/packet-gsmtap.c b/epan/dissectors/packet-gsmtap.c
index cc3dc86a5c..969e786ce0 100644
--- a/epan/dissectors/packet-gsmtap.c
+++ b/epan/dissectors/packet-gsmtap.c
@@ -3,6 +3,7 @@
  *
  * (C) 2008-2013 by Harald Welte <laforge@gnumonks.org>
  * (C) 2011 by Holger Hans Peter Freyther
+ * (C) 2020 by sysmocom s.f.m.c. GmbH <info@sysmocom.de>
  *
  * Wireshark - Network traffic analyzer
  * By Gerald Combs <gerald@wireshark.org>
@@ -34,6 +35,7 @@
 
 #include <epan/packet.h>
 #include <epan/conversation.h>
+#include <epan/dissectors/packet-gsm_rlcmac.h>
 
 #include "packet-gsmtap.h"
 #include "packet-lapdm.h"
@@ -555,6 +557,216 @@ handle_tetra(int channel, tvbuff_t *payload_tvb, packet_info *pinfo, proto_tree
 	tetra_dissect_pdu(tetra_chan, TETRA_DOWNLINK, payload_tvb, tree, pinfo);
 }
 
+/* length of an EGPRS RLC data block for given MCS */
+static const guint data_block_len_by_mcs[] = {
+	0,	/* MCS0 */
+	22,	/* MCS1 */
+	28,
+	37,
+	44,
+	56,
+	74,
+	56,
+	68,
+	74,	/* MCS9 */
+	0,	/* MCS_INVALID */
+};
+
+/* determine the number of rlc data blocks and their size / offsets */
+static void
+setup_rlc_mac_priv(RlcMacPrivateData_t *rm, gboolean is_uplink,
+	guint *n_calls, guint *data_block_bits, guint *data_block_offsets)
+{
+	guint nc, dbl = 0, dbo[2] = {0,0};
+
+	dbl = data_block_len_by_mcs[rm->mcs];
+
+	switch (rm->block_format) {
+	case RLCMAC_HDR_TYPE_1:
+		nc = 3;
+		dbo[0] = is_uplink ? 5*8 + 6 : 5*8 + 0;
+		dbo[1] = dbo[0] + dbl * 8 + 2;
+		break;
+	case RLCMAC_HDR_TYPE_2:
+		nc = 2;
+		dbo[0] = is_uplink ? 4*8 + 5 : 3*8 + 4;
+		break;
+	case RLCMAC_HDR_TYPE_3:
+		nc = 2;
+		dbo[0] = 3*8 + 7;
+		break;
+	default:
+		nc = 1;
+		break;
+	}
+
+	*n_calls = nc;
+	*data_block_bits = dbl * 8 + 2;
+	data_block_offsets[0] = dbo[0];
+	data_block_offsets[1] = dbo[1];
+}
+
+/* bit-shift the entire 'src' of length 'length_bytes' by 'offset_bits'
+ * and store the reuslt to caller-allocated 'buffer'.  The shifting is
+ * done lsb-first, unlike tvb_new_octet_aligned() */
+static void clone_aligned_buffer_lsbf(guint offset_bits, guint length_bytes,
+	const guint8 *src, guint8 *buffer)
+{
+	guint hdr_bytes;
+	guint extra_bits;
+	guint i;
+
+	guint8 c, last_c;
+	guint8 *dst;
+
+	hdr_bytes = offset_bits / 8;
+	extra_bits = offset_bits % 8;
+
+	if (extra_bits == 0) {
+		/* It is aligned already */
+		memmove(buffer, src + hdr_bytes, length_bytes);
+		return;
+	}
+
+	dst = buffer;
+	src = src + hdr_bytes;
+	last_c = *(src++);
+
+	for (i = 0; i < length_bytes; i++) {
+		c = src[i];
+		*(dst++) = (last_c >> extra_bits) | (c << (8 - extra_bits));
+		last_c = c;
+	}
+}
+
+/* obtain an (aligned) EGPRS data block with given bit-offset and
+ * bit-length from the parent TVB */
+static tvbuff_t *get_egprs_data_block(tvbuff_t *tvb, guint offset_bits,
+	guint length_bits, packet_info *pinfo)
+{
+	tvbuff_t *aligned_tvb;
+	const guint initial_spare_bits = 6;
+	guint8 *aligned_buf;
+	guint min_src_length_bytes = (offset_bits + length_bits + 7) / 8;
+	guint length_bytes = (initial_spare_bits + length_bits + 7) / 8;
+
+	tvb_ensure_bytes_exist(tvb, 0, min_src_length_bytes);
+
+	aligned_buf = (guint8 *) wmem_alloc(pinfo->pool, length_bytes);
+
+	/* Copy the data out of the tvb to an aligned buffer */
+	clone_aligned_buffer_lsbf(
+		offset_bits - initial_spare_bits, length_bytes,
+		tvb_get_ptr(tvb, 0, min_src_length_bytes),
+		aligned_buf);
+
+	/* clear spare bits and move block header bits to the right */
+	aligned_buf[0] = aligned_buf[0] >> initial_spare_bits;
+
+	aligned_tvb = tvb_new_child_real_data(tvb, aligned_buf,
+		length_bytes, length_bytes);
+	add_new_data_source(pinfo, aligned_tvb, "Aligned EGPRS data bits");
+
+	return aligned_tvb;
+}
+
+static void tvb_len_get_mcs_and_fmt(guint len, gboolean is_uplink, guint *frm, guint8 *mcs)
+{
+	if (len <= 5 && is_uplink) {
+		/* Assume random access burst */
+		*frm = RLCMAC_PRACH;
+		*mcs = 0;
+		return;
+	}
+
+	switch (len)
+	{
+	case 23:  *frm = RLCMAC_CS1; *mcs = 0; break;
+	case 34:  *frm = RLCMAC_CS2; *mcs = 0; break;
+	case 40:  *frm = RLCMAC_CS3; *mcs = 0; break;
+	case 54:  *frm = RLCMAC_CS4; *mcs = 0; break;
+	case 27:  *frm = RLCMAC_HDR_TYPE_3; *mcs = 1; break;
+	case 33:  *frm = RLCMAC_HDR_TYPE_3; *mcs = 2; break;
+	case 42:  *frm = RLCMAC_HDR_TYPE_3; *mcs = 3; break;
+	case 49:  *frm = RLCMAC_HDR_TYPE_3; *mcs = 4; break;
+	case 60:  /* fall through */
+	case 61:  *frm = RLCMAC_HDR_TYPE_2; *mcs = 5; break;
+	case 78:  /* fall through */
+	case 79:  *frm = RLCMAC_HDR_TYPE_2; *mcs = 6; break;
+	case 118: /* fall through */
+	case 119: *frm = RLCMAC_HDR_TYPE_1; *mcs = 7; break;
+	case 142: /* fall through */
+	case 143: *frm = RLCMAC_HDR_TYPE_1; *mcs = 8; break;
+	case 154: /* fall through */
+	case 155: *frm = RLCMAC_HDR_TYPE_1; *mcs = 9; break;
+	default:  *frm = RLCMAC_CS1; *mcs = 0; break; /* TODO: report error instead */
+	}
+}
+
+static void
+handle_rlcmac(guint32 frame_nr, tvbuff_t *payload_tvb, packet_info *pinfo, proto_tree *tree)
+{
+
+	int sub_handle;
+	RlcMacPrivateData_t rlcmac_data = {0};
+	tvbuff_t *data_tvb;
+	guint data_block_bits, data_block_offsets[2];
+	guint num_calls;
+	gboolean is_uplink;
+
+	if (pinfo->p2p_dir == P2P_DIR_SENT) {
+		is_uplink = 1;
+		sub_handle = GSMTAP_SUB_UM_RLC_MAC_UL;
+	} else {
+		is_uplink = 0;
+		sub_handle = GSMTAP_SUB_UM_RLC_MAC_DL;
+	}
+
+	rlcmac_data.magic = GSM_RLC_MAC_MAGIC_NUMBER;
+	rlcmac_data.frame_number = frame_nr;
+
+	tvb_len_get_mcs_and_fmt(tvb_reported_length(payload_tvb), is_uplink,
+				(guint *) &rlcmac_data.block_format,
+				(guint8 *) &rlcmac_data.mcs);
+
+	switch (rlcmac_data.block_format) {
+	case RLCMAC_HDR_TYPE_1:
+	case RLCMAC_HDR_TYPE_2:
+	case RLCMAC_HDR_TYPE_3:
+		/* First call of RLC/MAC dissector for header */
+		call_dissector_with_data(sub_handles[sub_handle], payload_tvb,
+					 pinfo, tree, (void *) &rlcmac_data);
+
+		/* now determine how to proceed for data */
+		setup_rlc_mac_priv(&rlcmac_data, is_uplink,
+				   &num_calls, &data_block_bits, data_block_offsets);
+
+		/* and call dissector one or two time for the data blocks */
+		if (num_calls >= 2) {
+			rlcmac_data.flags = GSM_RLC_MAC_EGPRS_BLOCK1;
+			data_tvb = get_egprs_data_block(payload_tvb, data_block_offsets[0],
+							data_block_bits, pinfo);
+			call_dissector_with_data(sub_handles[sub_handle], data_tvb, pinfo, tree,
+						 (void *) &rlcmac_data);
+		}
+		if (num_calls == 3) {
+			rlcmac_data.flags = GSM_RLC_MAC_EGPRS_BLOCK2;
+			data_tvb = get_egprs_data_block(payload_tvb, data_block_offsets[1],
+							data_block_bits, pinfo);
+			call_dissector_with_data(sub_handles[sub_handle], data_tvb, pinfo, tree,
+						 (void *) &rlcmac_data);
+		}
+		break;
+	default:
+		/* regular GPRS CS doesn't need any
+		 * shifting/re-alignment or even separate calls for
+		 * header and data blocks.  We simply call the dissector
+		 * as-is */
+		call_dissector_with_data(sub_handles[sub_handle], payload_tvb, pinfo, tree,
+					 (void *) &rlcmac_data);
+	}
+}
+
 /* dissect a GSMTAP header and hand payload off to respective dissector */
 static int
 dissect_gsmtap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _U_)
@@ -565,6 +777,7 @@ dissect_gsmtap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _
 	tvbuff_t *payload_tvb, *l1h_tvb = NULL;
 	guint8 hdr_len, type, sub_type, timeslot, subslot;
 	guint16 arfcn;
+	guint32 frame_nr;
 
 	len = tvb_reported_length(tvb);
 
@@ -572,6 +785,7 @@ dissect_gsmtap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _
 	type = tvb_get_guint8(tvb, offset + 2);
 	timeslot = tvb_get_guint8(tvb, offset + 3);
 	arfcn = tvb_get_ntohs(tvb, offset + 4);
+	frame_nr = tvb_get_ntohl(tvb, offset + 8);
 	sub_type = tvb_get_guint8(tvb, offset + 12);
 	subslot = tvb_get_guint8(tvb, offset + 14);
 
@@ -736,7 +950,6 @@ dissect_gsmtap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _
 			handle_lapdm(sub_type, payload_tvb, pinfo, tree);
 			return tvb_captured_length(tvb);
 		case GSMTAP_CHANNEL_PACCH:
-		case GSMTAP_CHANNEL_PDTCH:
 			if (pinfo->p2p_dir == P2P_DIR_SENT) {
 				sub_handle = GSMTAP_SUB_UM_RLC_MAC_UL;
 			}
@@ -745,6 +958,9 @@ dissect_gsmtap(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _
 				sub_handle = GSMTAP_SUB_UM_RLC_MAC_DL;
 			}
 			break;
+		case GSMTAP_CHANNEL_PDTCH:
+			handle_rlcmac(frame_nr, payload_tvb, pinfo, tree);
+			return tvb_captured_length(tvb);
 		/* See 3GPP TS 45.003, section 5.2 "Packet control channels" */
 		case GSMTAP_CHANNEL_PTCCH:
 			/* PTCCH/D carries Timing Advance updates encoded with CS-1 */