bss: Patch CC messages with bearer capabilities to contain AMR 3

* This changes bss_patch_filter_msg to return -1 or BSS_FILTER_DTAP
  for DTAP messages. This way app_forward_sccp should continue to behave
  the same besides now looking into DTAP messages.
* Introduce a direction in case we want to advertize FR into the BSS
  side and HR into the other direction.
* Patch AMR HR3 and Fullrate/Halfrate capabilities in the Bearer
  Capabilities. Add a test case that is patching the bearer capabilities

4 files changed, 152 insertions, 13 deletions

diff --git a/include/bss_patch.h b/include/bss_patch.h
index 6529619..d9a055b 100644
--- a/include/bss_patch.h
+++ b/include/bss_patch.h
@@ -1,7 +1,7 @@
 /* Patch Messages to and from the MSC */
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software: you can redistribute it and/or modify
@@ -30,13 +30,20 @@
 #define BSS_FILTER_RLSD		3
 #define BSS_FILTER_RLC		4
 #define BSS_FILTER_CLEAR_COMPL	5
+#define BSS_FILTER_DTAP		6
+
+#define BSS_DIR_BSC		1
+#define BSS_DIR_MSC		2
+#define BSS_DIR_ANY		(BSS_DIR_MSC | BSS_DIR_BSC)
 
 /**
  * Error is < 0
  * Success is == 0
  * Filter is > 0
+ *
+ * Direction...
  */
-int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *result);
+int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *result, int dir);
 
 /*
  * Copy inpt->l2h to target->l2h but rewrite the SCCP header on the way
diff --git a/src/bss_patch.c b/src/bss_patch.c
index 242b72d..be5aaf2 100644
--- a/src/bss_patch.c
+++ b/src/bss_patch.c
@@ -24,7 +24,9 @@
 
 #include <string.h>
 
+#include <osmocom/gsm/gsm48.h>
 #include <osmocom/gsm/gsm0808.h>
+#include <osmocom/gsm/protocol/gsm_04_08.h>
 #include <osmocom/gsm/protocol/gsm_08_08.h>
 #include <osmocom/gsm/tlv.h>
 
@@ -32,6 +34,9 @@
 
 #include <arpa/inet.h>
 
+static int handle_bss_mgmt(struct msgb *msg, struct sccp_parse_result *sccp);
+static int handle_bss_dtap(struct msgb *msg, struct sccp_parse_result *sccp, int dir);
+
 static void patch_ass_rqst(struct msgb *msg, int length)
 {
 	struct tlv_parsed tp;
@@ -93,7 +98,7 @@ static void patch_ass_cmpl(struct msgb *msg, int length)
 	data[0] = GSM0808_PERM_HR3;
 }
 
-int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp)
+int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp, int dir)
 {
 	int type;
 	memset(sccp, 0, sizeof(*sccp));
@@ -130,14 +135,21 @@ int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp)
 		return -1;
 	}
 
-	if (msg->l3h[0] != BSSAP_MSG_BSS_MANAGEMENT) {
-		return -1;
-	}
-
 	if (msgb_l3len(msg) < 2 + msg->l3h[1]) {
 		return -1;
 	}
 
+	if (msg->l3h[0] == BSSAP_MSG_BSS_MANAGEMENT)
+		return handle_bss_mgmt(msg, sccp);
+	if (msg->l3h[0] == BSSAP_MSG_DTAP)
+		return handle_bss_dtap(msg, sccp, dir);
+
+	/* Not handled... */
+	return -1;
+}
+
+static int handle_bss_mgmt(struct msgb *msg, struct sccp_parse_result *sccp)
+{
 	switch (msg->l3h[2]) {
 	case BSS_MAP_MSG_ASSIGMENT_RQST:
 		msg->l3h = &msg->l3h[2];
@@ -161,6 +173,109 @@ int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp)
 	return 0;
 }
 
+/* patch bearer capabilities towards the BSC */
+static int handle_bss_dtap(struct msgb *msg, struct sccp_parse_result *sccp, int dir)
+{
+	struct gsm48_hdr *hdr48;
+	struct tlv_parsed tp;
+	uint8_t proto, msg_type;
+	uint8_t *data;
+	int rc, len, i, has_amr;
+
+	/* early exit for messages not going to the MSC */
+	if ((dir & BSS_DIR_MSC) == 0)
+		return BSS_FILTER_DTAP;
+
+	/* check if the plain gsm header fits */
+	if (msg->l3h[2] < sizeof(*hdr48)) {
+		LOGP(DMSC, LOGL_ERROR,
+		     "DTAP GSM48 hdr does not fit: %d\n", msgb_l3len(msg));
+		return -1;
+	}
+
+	/* check if the whole message fits */
+	if (msgb_l3len(msg) - 3 < msg->l3h[2]) {
+		LOGP(DMSC, LOGL_ERROR,
+		     "DTAG GSM48 msg does not fit: %d\n", msgb_l3len(msg) - 3);
+		return -1;
+	}
+
+	/* right now we only need to patch call control messages */
+	msg->l3h = &msg->l3h[3];
+	hdr48 = (struct gsm48_hdr *) &msg->l3h[0];
+	proto = hdr48->proto_discr & 0x0f;
+        msg_type = hdr48->msg_type & 0xbf;
+
+	if (proto != GSM48_PDISC_CC)
+		return BSS_FILTER_DTAP;
+
+	switch (msg_type) {
+	case GSM48_MT_CC_CALL_CONF:
+	case GSM48_MT_CC_SETUP:
+		rc = tlv_parse(&tp, &gsm48_att_tlvdef, &hdr48->data[0],
+			       msgb_l3len(msg) - sizeof(*hdr48), 0, 0);
+		if (rc <= 0) {
+			LOGP(DMSC, LOGL_ERROR,
+			     "Failed to parse CC message: %d\n", rc);
+			return BSS_FILTER_DTAP;
+		}
+
+		/* not judging if this is optional here or not */
+		if (!TLVP_PRESENT(&tp, GSM48_IE_BEARER_CAP))
+			return BSS_FILTER_DTAP;
+
+		if (TLVP_LEN(&tp, GSM48_IE_BEARER_CAP) < 2){
+			LOGP(DMSC, LOGL_ERROR,
+			     "Octet3/Octet3a do not fit: %d\n",
+			     TLVP_LEN(&tp, GSM48_IE_BEARER_CAP));
+			return BSS_FILTER_DTAP;
+		}
+
+		data = (uint8_t *) TLVP_VAL(&tp, GSM48_IE_BEARER_CAP);
+		if ((data[0] & 0x80) != 0) {
+			LOGP(DMSC, LOGL_DEBUG, "Octet3a not present.\n");
+			return BSS_FILTER_DTAP;
+		}
+
+		/*
+		 * Some lazy bit checks that work because the defines are
+		 * are 0. If this would not be the case we will need additional
+		 * shifts
+		 */
+		if ((data[0] & 0x07) != GSM48_BCAP_ITCAP_SPEECH)
+			return BSS_FILTER_DTAP;
+		if ((data[0] & 0x08) != GSM48_BCAP_TMOD_CIRCUIT)
+			return BSS_FILTER_DTAP;
+		if ((data[0] & 0x10) != GSM48_BCAP_CODING_GSM_STD)
+			return BSS_FILTER_DTAP;
+
+		/* Check if we have fr only */
+		if ((data[0] & 0x60) >> 5 == GSM48_BCAP_RRQ_FR_ONLY) {
+			data[0] &= ~0x60;
+			data[0] |= GSM48_BCAP_RRQ_DUAL_HR << 5;
+		}
+
+		/* Now check if HR AMR 3 shows up */
+		has_amr = 0;
+		len = TLVP_LEN(&tp, GSM48_IE_BEARER_CAP);
+		for (i = 1; i < len && !has_amr; ++i) {
+			/* ended the octet3a */
+			if ((data[i] & 0x80) > 0)
+				break;
+			if ((data[i] & 0x0f) == 0x5)
+				has_amr = 1;
+		}
+
+		/* patch HR AMR 3 as first used audio codec */
+		if (!has_amr)
+			data[1] = (data[1] & 0x80) | 0x5;
+
+		break;
+	}
+
+	return BSS_FILTER_DTAP;
+}
+
 static void create_cr(struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp)
 {
 	static const uint32_t optional_offset =
diff --git a/src/sccp_state.c b/src/sccp_state.c
index 1d12ce7..c2b3834 100644
--- a/src/sccp_state.c
+++ b/src/sccp_state.c
@@ -71,7 +71,7 @@ void app_forward_sccp(struct ss7_application *app, struct msgb *_msg, int sls)
 		return;
 	}
 
-	rc = bss_patch_filter_msg(_msg, &result);
+	rc = bss_patch_filter_msg(_msg, &result, BSS_DIR_MSC);
 	if (rc == BSS_FILTER_RESET) {
 		LOGP(DMSC, LOGL_NOTICE, "Filtering BSS Reset from the BSC\n");
 		msc_mgcp_reset(msc);
@@ -549,7 +549,7 @@ void msc_dispatch_sccp(struct msc_connection *msc, struct msgb *msg)
 		struct sccp_parse_result result;
 		int rc;
 
-		rc = bss_patch_filter_msg(msg, &result);
+		rc = bss_patch_filter_msg(msg, &result, BSS_DIR_BSC);
 
 		if (rc == BSS_FILTER_RESET_ACK) {
 			LOGP(DMSC, LOGL_NOTICE, "Filtering reset ack from the MSC\n");
diff --git a/tests/patching/patching_test.c b/tests/patching/patching_test.c
index 0456f0f..6a02e83 100644
--- a/tests/patching/patching_test.c
+++ b/tests/patching/patching_test.c
@@ -118,6 +118,16 @@ static uint8_t cr2_without_poi[] = {
 0x18, 0x93, 0x08, 0x29, 0x47, 0x80, 0x00, 0x00,
 0x00, 0x00, 0x80, 0x00};
 
+static uint8_t dt1_cc_confirmed[] = {
+0x06, 0x01, 0x0b, 0xef, 0x00, 0x01, 0x0e, 0x01,
+0x00, 0x0b, 0x83, 0x48, 0x04, 0x04, 0x20, 0x02,
+0x00, 0x81, 0x15, 0x01, 0x01};
+
+static uint8_t dt1_cc_confirmed_hr3[] = {
+0x06, 0x01, 0x0b, 0xef, 0x00, 0x01, 0x0e, 0x01,
+0x00, 0x0b, 0x83, 0x48, 0x04, 0x04, 0x40, 0x05,
+0x00, 0x81, 0x15, 0x01, 0x01};
+
 static struct result rewrite_results_to_msc[] = {
 	{
 		.input = udt_with_poi,
@@ -139,6 +149,13 @@ static struct result rewrite_results_to_msc[] = {
 		.expected = cr2_without_poi,
 		.exp_len = sizeof(cr2_without_poi),
 	},
+
+	{
+		.input = dt1_cc_confirmed,
+		.inp_len = sizeof(dt1_cc_confirmed),
+		.expected = dt1_cc_confirmed_hr3,
+		.exp_len = sizeof(dt1_cc_confirmed_hr3),
+	},
 };
 
 
@@ -177,7 +194,7 @@ static void test_patch_filter(void)
 		msgb_put(msg, 1);
 		msg->l2h = msgb_put(msg, results[i].inp_len);
 		memcpy(msg->l2h, results[i].input, msgb_l2len(msg));
-		rc = bss_patch_filter_msg(msg, &result);
+		rc = bss_patch_filter_msg(msg, &result, BSS_DIR_ANY);
 
 		if (memcmp(msg->l2h, results[i].expected, results[i].exp_len) != 0) {
 			printf("Failed to patch the packet.\n");
@@ -209,7 +226,7 @@ static void test_rewrite_msc(void)
 		inp->l2h = msgb_put(inp, rewrite_results_to_msc[i].inp_len);
 		memcpy(inp->l2h, rewrite_results_to_msc[i].input, msgb_l2len(inp));
 
-		rc = bss_patch_filter_msg(inp, &result);
+		rc = bss_patch_filter_msg(inp, &result, BSS_DIR_MSC);
 		if (rc < 0) {
 			printf("Failed to parse header msg: %d\n", i);
 			abort();
@@ -218,7 +235,7 @@ static void test_rewrite_msc(void)
 		bss_rewrite_header_for_msc(rc, outp, inp, &result);
 
 		memset(&result, 0, sizeof(result));
-		rc = bss_patch_filter_msg(outp, &result);
+		rc = bss_patch_filter_msg(outp, &result, BSS_DIR_MSC);
 		if (rc < 0) {
 			printf("Patched message doesn't work: %d\n", i);
 			printf("hex: %s\n", osmo_hexdump(outp->l2h, msgb_l2len(outp)));