xudt: Implement address and data extraction

The cellmgr-ng unfortunately looks at the data being sent and can't
handle the presence of XUDT at all. Add the structure definition
and refactor extraction code to work on offsets. Add a unit test.

Change-Id: I45a7447cc1be432fff34849e0e35abc0410cf153

3 files changed, 142 insertions, 14 deletions

diff --git a/include/osmocom/sccp/sccp_types.h b/include/osmocom/sccp/sccp_types.h
index ab7f74f..18b54f4 100644
--- a/include/osmocom/sccp/sccp_types.h
+++ b/include/osmocom/sccp/sccp_types.h
@@ -422,6 +422,60 @@ struct sccp_data_unitdata_service {
 	uint8_t			data[0];
 } __attribute__((packed));
 
+/* Extended unitdata (XUDT) */
+struct sccp_data_ext_unitdata {
+	/* mandatory */
+	uint8_t			type;
+	uint8_t			proto_class;
+	uint8_t			hop_counter;
+
+	/* variable */
+	uint8_t			variable_called;
+	uint8_t			variable_calling;
+	uint8_t			variable_data;
+
+#if VARIABLE
+	called party address
+	calling party address
+	data
+#endif
+
+#if OPTIONAL
+	segmentation
+	importance
+#endif
+
+	uint8_t			data[0];
+
+} __attribute__((packed));
+
+
+/* Extended unitdata service (XUDTS) */
+struct  sccp_data_ext_unitdata_service {
+	/* mandantory */
+	uint8_t			type;
+	uint8_t			return_cause;
+	uint8_t			hop_counter;
+
+
+	/* variable */
+	uint8_t			variable_called;
+	uint8_t			variable_calling;
+	uint8_t			variable_data;
+
+#if VARIABLE
+	called party address
+	calling party address
+	data
+#endif
+
+#if OPTIONAL
+	segmentation
+	importancd
+#endif
+
+	uint8_t			data[0];
+} __attribute__((packed));
 
 struct sccp_data_it {
 	/* mandatory */
diff --git a/src/sccp.c b/src/sccp.c
index ec0f3b7..c14e850 100644
--- a/src/sccp.c
+++ b/src/sccp.c
@@ -406,37 +406,46 @@ int _sccp_parse_connection_dt1(struct msgb *msgb, struct sccp_parse_result *resu
 	return 0;
 }
 
-int _sccp_parse_udt(struct msgb *msgb, struct sccp_parse_result *result)
-{
-	static const uint32_t header_size = sizeof(struct sccp_data_unitdata);
-	static const uint32_t called_offset = offsetof(struct sccp_data_unitdata, variable_called);
-	static const uint32_t calling_offset = offsetof(struct sccp_data_unitdata, variable_calling);
-	static const uint32_t data_offset = offsetof(struct sccp_data_unitdata, variable_data);
+struct udt_offsets {
+	uint32_t	header_size;
+	uint32_t	called_offset;
+	uint32_t	calling_offset;
+	uint32_t	data_offset;
+};
 
-	struct sccp_data_unitdata *udt = (struct sccp_data_unitdata *)msgb->l2h;
+static int _sccp_parse_unitdata(struct msgb *msgb, struct sccp_parse_result *result,
+				const struct udt_offsets *offs)
+{
+	uint8_t		variable_called;
+	uint8_t		variable_calling;
+	uint8_t		variable_data;
 
-	if (msgb_l2len(msgb) < header_size) {
+	if (msgb_l2len(msgb) < offs->header_size) {
 		LOGP(DSCCP, LOGL_ERROR, "msgb < header_size %u %u\n",
-		        msgb_l2len(msgb), header_size);
+		        msgb_l2len(msgb), offs->header_size);
 		return -1;
 	}
 
+	variable_called = msgb->l2h[offs->called_offset];
+	variable_calling = msgb->l2h[offs->calling_offset];
+	variable_data = msgb->l2h[offs->data_offset];
+
 	/* copy out the calling and called address. Add the off */
-	if (copy_address(&result->called, called_offset + udt->variable_called, msgb) != 0)
+	if (copy_address(&result->called, offs->called_offset + variable_called, msgb) != 0)
 		return -1;
 
-	if (copy_address(&result->calling, calling_offset + udt->variable_calling, msgb) != 0)
+	if (copy_address(&result->calling, offs->calling_offset + variable_calling, msgb) != 0)
 		return -1;
 
 	/* we don't have enough size for the data */
-	if (msgb_l2len(msgb) < data_offset + udt->variable_data + 1) {
+	if (msgb_l2len(msgb) < offs->data_offset + variable_data + 1) {
 		LOGP(DSCCP, LOGL_ERROR, "msgb < header + offset %u %u %u\n",
-			msgb_l2len(msgb), header_size, udt->variable_data);
+			msgb_l2len(msgb), offs->header_size, variable_data);
 		return -1;
 	}
 
 
-	msgb->l3h = &udt->data[udt->variable_data];
+	msgb->l3h = &msgb->l2h[offs->data_offset + variable_data + 1];
 	result->data_len = msgb_l3len(msgb);
 
 	if (msgb_l3len(msgb) <  msgb->l3h[-1]) {
@@ -448,6 +457,30 @@ int _sccp_parse_udt(struct msgb *msgb, struct sccp_parse_result *result)
 	return 0;
 }
 
+int _sccp_parse_udt(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static const struct udt_offsets offsets = {
+		.header_size = sizeof(struct sccp_data_unitdata),
+		.called_offset = offsetof(struct sccp_data_unitdata, variable_called),
+		.calling_offset = offsetof(struct sccp_data_unitdata, variable_calling),
+		.data_offset = offsetof(struct sccp_data_unitdata, variable_data),
+	};
+
+	return _sccp_parse_unitdata(msgb, result, &offsets);
+}
+
+static int _sccp_parse_xudt(struct msgb *msgb, struct sccp_parse_result *result)
+{
+	static const struct udt_offsets offsets = {
+		.header_size = sizeof(struct sccp_data_ext_unitdata),
+		.called_offset = offsetof(struct sccp_data_ext_unitdata, variable_called),
+		.calling_offset = offsetof(struct sccp_data_ext_unitdata, variable_calling),
+		.data_offset = offsetof(struct sccp_data_ext_unitdata, variable_data),
+	};
+
+	return _sccp_parse_unitdata(msgb, result, &offsets);
+}
+
 static int _sccp_parse_it(struct msgb *msgb, struct sccp_parse_result *result)
 {
 	static const uint32_t header_size = sizeof(struct sccp_data_it);
@@ -1457,6 +1490,9 @@ int sccp_parse_header(struct msgb *msg, struct sccp_parse_result *result)
 	case SCCP_MSG_TYPE_UDT:
 		return _sccp_parse_udt(msg, result);
 		break;
+	case SCCP_MSG_TYPE_XUDT:
+		return _sccp_parse_xudt(msg, result);
+		break;
 	case SCCP_MSG_TYPE_IT:
 		return _sccp_parse_it(msg, result);
 		break;
diff --git a/tests/sccp/sccp_test.c b/tests/sccp/sccp_test.c
index ba9ff7a..29f343f 100644
--- a/tests/sccp/sccp_test.c
+++ b/tests/sccp/sccp_test.c
@@ -312,6 +312,32 @@ static const uint8_t proto_err[] = {
 0x0f, 0x0c, 0x04, 0x00, 0x00,
 };
 
+static const uint8_t xudt_test_src_gt[] = {
+0x00, 0x11, 0x04, 0x26, 0x18, 0x01, 0x30, 0x08,
+0x01
+};
+
+static const uint8_t xudt_test_dst_gt[] = {
+0x00, 0x61, 0x04, 0x15, 0x10, 0x80, 0x21, 0x35,
+0x98, 0x55, 0x08
+};
+
+static const uint8_t xudt_test[] = {
+0x11, 0x81, 0x02, 0x04, 0x11, 0x1C, 0x00, 0x0D,
+0x52, 0x06, 0x00, 0x61, 0x04, 0x15, 0x10, 0x80,
+0x21, 0x35, 0x98, 0x55, 0x08, 0x0B, 0x12, 0x95,
+0x00, 0x11, 0x04, 0x26, 0x18, 0x01, 0x30, 0x08,
+0x01, 0x44, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06,
+0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E,
+0x0F, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16,
+0x17, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E,
+0x1F, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26,
+0x27, 0x28, 0x29, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E,
+0x2F, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36,
+0x37, 0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E,
+0x3F, 0x40, 0x41, 0x42, 0x43, 0x44
+};
+
 static const struct sccp_parse_header_result parse_result[] = {
 	{
 		.msg_type	= SCCP_MSG_TYPE_IT,
@@ -362,6 +388,18 @@ static const struct sccp_parse_header_result parse_result[] = {
 		.src_gti_data	= tcap_global_src_gti,
 		.src_gti_len	= 9,
 	},
+	{
+		.msg_type	= SCCP_MSG_TYPE_XUDT,
+		.input		= xudt_test,
+		.input_len	= sizeof(xudt_test),
+		.wanted_len	= 68,
+		.dst_ssn	= 6,
+		.dst_gti_data	= xudt_test_dst_gt,
+		.dst_gti_len	= 11,
+		.src_ssn	= 149,
+		.src_gti_data	= xudt_test_src_gt,
+		.src_gti_len	= 9,
+	},
 };