315 lines
8.0 KiB
C
315 lines
8.0 KiB
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 Affero General Public License as published by
|
|
* the Free Software Foundation, either version 3 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 Affero General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Affero General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
*/
|
|
|
|
#include <bss_patch.h>
|
|
#include <cellmgr_debug.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <osmocom/gsm/gsm0808.h>
|
|
#include <osmocom/gsm/protocol/gsm_08_08.h>
|
|
#include <osmocom/gsm/tlv.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;
|
|
|
|
if (length == 1) {
|
|
LOGP(DMSC, LOGL_ERROR, "Hacking the Assignment Complete.\n");
|
|
msgb_v_put(msg, 0x21);
|
|
msgb_v_put(msg, 0x09);
|
|
msgb_v_put(msg, 0x2c);
|
|
msgb_v_put(msg, 0x02);
|
|
msgb_v_put(msg, 0x40);
|
|
msgb_v_put(msg, 0x25);
|
|
msg->l3h[-1] = 7;
|
|
msg->l3h[-3] = 9;
|
|
return;
|
|
}
|
|
|
|
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;
|
|
}
|