src/bss_patch.c

/* 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 <cellmgr_debug.h>

#include <string.h>

#include <osmocore/gsm0808.h>
#include <osmocore/protocol/gsm_08_08.h>

#include <osmocom/sccp/sccp.h>

#include <arpa/inet.h>

static void patch_ass_rqst(struct msgb *msg, int length)
{
	struct tlv_parsed tp;
	uint8_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 = (uint8_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;
	uint8_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 = (uint8_t *) TLVP_VAL(&tp, GSM0808_IE_CHOSEN_CHANNEL);
	data[0] = 0x09;

	data = (uint8_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 uint32_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(uint32_t offset, int pc, struct msgb *msg)
{
	struct sccp_called_party_address *party;
	uint8_t *the_pc;
	uint8_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 uint32_t called_offset =
		offsetof(struct sccp_data_unitdata, variable_called);
	static const uint32_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;
}