Public release of the cellmgr_ng code to convert E1 to IPA SCCP

1 files changed, 300 insertions, 0 deletions

diff --git a/src/bss_patch.c b/src/bss_patch.c
new file mode 100644
index 0000000..d8d5405
--- /dev/null
+++ b/src/bss_patch.c
@@ -0,0 +1,300 @@
+/* Patch GSM 08.08 messages for the network and BS */
+/*
+ * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010 by On-Waves
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <bss_patch.h>
+
+#include <string.h>
+
+#include <openbsc_nat/bssap.h>
+#include <openbsc_nat/tlv.h>
+#include <laf0rge1/debug.h>
+#include <sccp/sccp.h>
+
+#include <arpa/inet.h>
+
+static void patch_ass_rqst(struct msgb *msg, int length)
+{
+	struct tlv_parsed tp;
+	u_int8_t *data, audio;
+	int len;
+
+	tlv_parse(&tp, gsm0808_att_tlvdef(), msg->l3h + 1, length - 1, 0, 0);
+	len = TLVP_LEN(&tp, GSM0808_IE_CHANNEL_TYPE);
+	if (len < 3)
+		return;
+
+	data = (u_int8_t *) TLVP_VAL(&tp, GSM0808_IE_CHANNEL_TYPE);
+	/* no speech... ignore */
+	if ((data[0] & 0xf) != 0x1)
+		return;
+
+	/* blindly assign */
+	data[1] = GSM0808_SPEECH_FULL_PREF;
+	audio = GSM0808_PERM_FR2;
+	if (len > 3)
+		audio |= 0x80;
+	data[2] = audio;
+}
+
+static void patch_ass_cmpl(struct msgb *msg, int length)
+{
+	struct tlv_parsed tp;
+	u_int8_t *data;
+
+	tlv_parse(&tp, gsm0808_att_tlvdef(), msg->l3h + 1, length - 1, 0, 0);
+	if (!TLVP_PRESENT(&tp, GSM0808_IE_CHOSEN_CHANNEL)) {
+		LOGP(DMSC, LOGL_ERROR, "Chosen Channel not in the MSG.\n");
+		return;
+	}
+
+	if (!TLVP_PRESENT(&tp, GSM0808_IE_SPEECH_VERSION)) {
+		LOGP(DMSC, LOGL_ERROR, "Speech version not in the MSG.\n");
+		return;
+	}
+
+	/* claim to have a TCH/H with no mode indication */
+	data = (u_int8_t *) TLVP_VAL(&tp, GSM0808_IE_CHOSEN_CHANNEL);
+	data[0] = 0x09;
+
+	data = (u_int8_t *) TLVP_VAL(&tp, GSM0808_IE_SPEECH_VERSION);
+	data[0] = GSM0808_PERM_HR3;
+}
+
+int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp)
+{
+	int type;
+	memset(sccp, 0, sizeof(*sccp));
+	if (sccp_parse_header(msg, sccp) != 0) {
+		LOGP(DMSC, LOGL_ERROR, "Failed to parse SCCP header.\n");
+		return -1;
+	}
+
+	type = sccp_determine_msg_type(msg);
+	switch (type) {
+	case SCCP_MSG_TYPE_CR:
+		if (msg->l3h)
+			break;
+		return 0;
+		break;
+	case SCCP_MSG_TYPE_CC:
+	case SCCP_MSG_TYPE_CREF:
+		return 0;
+		break;
+	case SCCP_MSG_TYPE_RLC:
+		return BSS_FILTER_RLC;
+		break;
+	case SCCP_MSG_TYPE_RLSD:
+		return BSS_FILTER_RLSD;
+		break;
+	}
+
+	if (msgb_l3len(msg) < sccp->data_len) {
+		LOGP(DMSC, LOGL_ERROR, "Less space than there should be.\n");
+		return -1;
+	}
+
+	if (!msg->l3h || msgb_l3len(msg) < 3) {
+		return -1;
+	}
+
+	if (msg->l3h[0] != 0) {
+		return -1;
+	}
+
+	if (msgb_l3len(msg) < 2 + msg->l3h[1]) {
+		return -1;
+	}
+
+	switch (msg->l3h[2]) {
+	case BSS_MAP_MSG_ASSIGMENT_RQST:
+		msg->l3h = &msg->l3h[2];
+		patch_ass_rqst(msg, sccp->data_len - 2);
+		break;
+	case BSS_MAP_MSG_ASSIGMENT_COMPLETE:
+		msg->l3h = &msg->l3h[2];
+		patch_ass_cmpl(msg, sccp->data_len - 2);
+		break;
+	case BSS_MAP_MSG_RESET:
+		return BSS_FILTER_RESET;
+		break;
+	case BSS_MAP_MSG_RESET_ACKNOWLEDGE:
+		return BSS_FILTER_RESET_ACK;
+		break;
+	case BSS_MAP_MSG_CLEAR_COMPLETE:
+		return BSS_FILTER_CLEAR_COMPL;
+		break;
+	}
+
+	return 0;
+}
+
+static void create_cr(struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp)
+{
+	static const u_int32_t optional_offset =
+			offsetof(struct sccp_connection_request, optional_start);
+
+	unsigned int optional_length, optional_start;
+	struct sccp_connection_request *cr, *in_cr;
+
+	target->l2h = msgb_put(target, sizeof(*cr));
+	cr = (struct sccp_connection_request *) target->l2h;
+	in_cr = (struct sccp_connection_request *) inpt->l2h;
+
+	cr->type = in_cr->type;
+	cr->proto_class = in_cr->proto_class;
+	cr->source_local_reference = in_cr->source_local_reference;
+	cr->variable_called = 2;
+	cr->optional_start = 4;
+
+	/* called address */
+	target->l3h = msgb_put(target, 1 + 2);
+	target->l3h[0] = 2;
+	target->l3h[1] = 0x42;
+	target->l3h[2] = 254;
+
+	/*
+	 * We need to keep the complete optional data. The SCCP parse result
+         * is only pointing to the data payload.
+	 */
+	optional_start = in_cr->optional_start + optional_offset;
+	optional_length = msgb_l2len(inpt) - optional_start;
+	if (optional_start + optional_length <= msgb_l2len(inpt)) {
+		target->l3h = msgb_put(target, optional_length);
+		memcpy(target->l3h, inpt->l2h + optional_start, msgb_l3len(target));
+	} else {
+		LOGP(DINP, LOGL_ERROR, "Input should at least have a byte of data.\n");
+	}
+}
+
+/*
+ * Generate a simple UDT msg. FIXME: Merge it with the SCCP code
+ */
+static void create_udt(struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp)
+{
+	struct sccp_data_unitdata *udt, *in_udt;
+
+	target->l2h = msgb_put(target, sizeof(*udt));
+	udt = (struct sccp_data_unitdata *) target->l2h;
+	in_udt = (struct sccp_data_unitdata *) inpt->l2h;
+
+	udt->type = in_udt->type;
+	udt->proto_class = in_udt->proto_class;
+	udt->variable_called = 3;
+	udt->variable_calling = 5;
+	udt->variable_data = 7;
+
+	target->l3h = msgb_put(target, 1 + 2);
+	target->l3h[0] = 2;
+	target->l3h[1] = 0x42;
+	target->l3h[2] = 254;
+
+	target->l3h = msgb_put(target, 1 + 2);
+	target->l3h[0] = 2;
+	target->l3h[1] = 0x42;
+	target->l3h[2] = 254;
+
+	target->l3h = msgb_put(target, sccp->data_len + 1);
+	target->l3h[0] = sccp->data_len;
+	memcpy(&target->l3h[1], inpt->l3h, msgb_l3len(target) - 1);
+}
+
+void bss_rewrite_header_for_msc(int rc, struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp)
+{
+
+	switch (inpt->l2h[0]) {
+	case SCCP_MSG_TYPE_CR:
+		if (rc >= 0)
+			create_cr(target, inpt, sccp);
+		else
+			target->l2h = msgb_put(target, 0);
+		break;
+	case SCCP_MSG_TYPE_UDT:
+		if (rc >= 0)
+			create_udt(target, inpt, sccp);
+		else
+			target->l2h = msgb_put(target, 0);
+		break;
+	default:
+		target->l2h = msgb_put(target, msgb_l2len(inpt));
+		memcpy(target->l2h, inpt->l2h, msgb_l2len(target));
+		break;
+	}
+}
+
+/* it is asssumed that the SCCP stack checked the size */
+static int patch_address(u_int32_t offset, int pc, struct msgb *msg)
+{
+	struct sccp_called_party_address *party;
+	u_int8_t *the_pc;
+	u_int8_t pc_low, pc_high;
+
+	party = (struct sccp_called_party_address *)(msg->l2h + offset + 1);
+	the_pc = &party->data[0];
+
+	pc_low = pc & 0xff;
+	pc_high = (pc >> 8) & 0xff;
+	the_pc[0] = pc_low;
+	the_pc[1] = pc_high;
+
+	return 0;
+}
+ 
+int bss_rewrite_header_to_bsc(struct msgb *msg, int opc, int dpc)
+{
+	static const u_int32_t called_offset =
+		offsetof(struct sccp_data_unitdata, variable_called);
+	static const u_int32_t calling_offset =
+		offsetof(struct sccp_data_unitdata, variable_calling);
+
+	struct sccp_data_unitdata *udt;
+	struct sccp_parse_result sccp;
+
+	memset(&sccp, 0, sizeof(sccp));
+	if (sccp_parse_header(msg, &sccp) != 0) {
+		LOGP(DMSC, LOGL_ERROR, "Failed to parse SCCP header.\n");
+		return -1;
+	}
+
+	/* For now the MSC only sends the PC in UDT */
+	if (msg->l2h[0] != SCCP_MSG_TYPE_UDT)
+		return 0;
+
+	/* sanity checking */
+	if (sccp.called.address.point_code_indicator != 1) {
+		LOGP(DMSC, LOGL_ERROR, "MSC didn't send a PC in called address\n");
+		return -1;
+	}
+
+	if (sccp.calling.address.point_code_indicator != 1) {
+		LOGP(DMSC, LOGL_ERROR, "MSC didn't send a PC in calling address\n");
+		return -1;
+	}
+
+	/* Good thing is we can avoid most of the error checking */
+	udt = (struct sccp_data_unitdata *) msg->l2h;
+	if (patch_address(called_offset + udt->variable_called, dpc, msg) != 0)
+		return -1;
+
+	if (patch_address(calling_offset + udt->variable_calling, opc, msg) != 0)
+		return -1;
+	return 0;
+}