diff options
author | Holger Hans Peter Freyther <zecke@selfish.org> | 2010-07-28 03:32:52 +0800 |
---|---|---|
committer | Holger Hans Peter Freyther <zecke@selfish.org> | 2010-07-28 03:36:32 +0800 |
commit | 97f66e2b534e2a54c63360a3f8134a0189c54e25 (patch) | |
tree | 903e34443767b09ef1d11575f8a1502f6295c7fd /src |
Public release of the cellmgr_ng code to convert E1 to IPA SCCP
Diffstat (limited to 'src')
-rwxr-xr-x | src/InitHW.c | 53 | ||||
-rw-r--r-- | src/Makefile.am | 15 | ||||
-rw-r--r-- | src/bss_patch.c | 300 | ||||
-rw-r--r-- | src/bssap_sccp.c | 150 | ||||
-rw-r--r-- | src/input/ipaccess.c | 164 | ||||
-rw-r--r-- | src/link_udp.c | 235 | ||||
-rw-r--r-- | src/main.c | 1079 | ||||
-rw-r--r-- | src/mgcp/README | 3 | ||||
-rw-r--r-- | src/mgcp/mgcp_network.c | 300 | ||||
-rw-r--r-- | src/mgcp/mgcp_protocol.c | 758 | ||||
-rw-r--r-- | src/mgcp_ss7.c | 939 | ||||
-rw-r--r-- | src/msc_conn.c | 594 | ||||
-rw-r--r-- | src/mtp_layer3.c | 509 | ||||
-rw-r--r-- | src/openbsc_nat/README | 1 | ||||
-rw-r--r-- | src/openbsc_nat/bssap.c | 65 | ||||
-rw-r--r-- | src/openbsc_nat/tlv_parser.c | 160 | ||||
-rw-r--r-- | src/pcap.c | 86 | ||||
-rw-r--r-- | src/snmp_mtp.c | 112 | ||||
-rw-r--r-- | src/thread.c | 100 | ||||
-rw-r--r-- | src/write_queue.c | 91 |
20 files changed, 5714 insertions, 0 deletions
diff --git a/src/InitHW.c b/src/InitHW.c new file mode 100755 index 0000000..8d09b43 --- /dev/null +++ b/src/InitHW.c @@ -0,0 +1,53 @@ +#include <unistd.h> +#include <stdio.h> +#include <pti/NexusWare.h> + +void InitHW(void) +{ + int status,i; + + /* configure the board clock */ + status = PTI_SetHSCMStandalone(0,PTI_CLK_TRUNK + 1, PTI_CLK_NONE, + PTI_NETREF_2048K); + if (status < 0) + fprintf(stderr, "Error: PTI_SetClocking() = %d\n", status); + + sleep(1); + + /* open the port devices */ + for (i=1; i<=8; i++) + { + status = PTI_SetT1Framing(i, PTI_FRAME_E1CRC, PTI_ENCODE_HDB3); + if (status < 0) + fprintf(stderr, "Error: PTI_SetFraming(%d) = %d\n", i, status); + } + + /* configure PTMC */ + status = PTI_SetPTMCNetref(0, PTI_PTMC_NETREF_DISABLE); + if (status < 0) + fprintf(stderr, "Error: PTI_SetPTMCNetref() = %d\n", status); + status = PTI_SetPTMCClockMode(0, PTI_PTMC_CLOCKMODE_H100); + if (status < 0) + fprintf(stderr, "Error: PTI_SetPTMCClockMode() = %d\n", status); + status = PTI_SetEnetPortState(PTI_ENET_PORT_ID_PTMC+0, + PTI_ENET_PORT_STATE_ENABLE_ALL); + if (status < 0) + fprintf(stderr, "Error: PTI_SetEnetPortState() = %d\n", status); + status = PTI_AddEnetRoute(PTI_ENET_PORT_ID_PTMC+0, PTI_ENET_PORT_ID_FRONT+0, 1); + status |= PTI_AddEnetRoute(PTI_ENET_PORT_ID_PTMC+0, PTI_ENET_PORT_ID_REAR+0, 1); + status |= PTI_AddEnetRoute(PTI_ENET_PORT_ID_PTMC+0, PTI_ENET_PORT_ID_LOCAL+0, 1); + if (status < 0) + fprintf(stderr, "Error: PTI_AddEnetRoute() = %d\n", status); + + status = PTI_ConnectHSCM(PTI_HSCM_TRUNK+1,30,PTI_HSCM_DATACHAN,0,1,1); + if (status < 0) + fprintf(stderr, "Error: PTI_ConnectHSCM() = %d\n", status); + + status = PTI_ConnectHSCM(PTI_HSCM_TRUNK+1, 0, PTI_HSCM_PTMC, 0, 30, 0); + status |= PTI_ConnectHSCM(PTI_HSCM_PTMC, 128, PTI_HSCM_TRUNK+1, 0, 30, 0); + if (status < 0) + fprintf(stderr, "Error: PTI_ConnectHSCM() = %d\n", status); + + +} + diff --git a/src/Makefile.am b/src/Makefile.am new file mode 100644 index 0000000..472c6ba --- /dev/null +++ b/src/Makefile.am @@ -0,0 +1,15 @@ +INCLUDES = $(all_includes) -I$(top_srcdir)/include +AM_CFLAGS=-Wall $(LAFORGE_CFLAGS) $(SCCP_CFLAGS) \ + $(NEXUSWARE_C7_CFLAGS) $(NEXUSWARE_UNIPORTE_CFLAGS) + +sbin_PROGRAMS = cellmgr_ng mgcp_mgw + +mgcp_mgw_SOURCES = mgcp_ss7.c write_queue.c mgcp/mgcp_protocol.c mgcp/mgcp_network.c thread.c +mgcp_mgw_LDADD = $(LAFORGE_LIBS) $(NEXUSWARE_C7_LIBS) $(NEXUSWARE_UNIPORTE_LIBS) -lvty -lpthread + +cellmgr_ng_SOURCES = main.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \ + bss_patch.c \ + openbsc_nat/bssap.c openbsc_nat/tlv_parser.c write_queue.c bssap_sccp.c \ + msc_conn.c link_udp.c snmp_mtp.c +cellmgr_ng_LDADD = $(LAFORGE_LIBS) $(SCCP_LIBS) $(NEXUSWARE_C7_LIBS) \ + -lpthread -lnetsnmp -lcrypto -lvty 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; +} diff --git a/src/bssap_sccp.c b/src/bssap_sccp.c new file mode 100644 index 0000000..72e24ae --- /dev/null +++ b/src/bssap_sccp.c @@ -0,0 +1,150 @@ +/* Create GSM 08.08 messages */ +/* + * (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 <bssap_sccp.h> + +#include <laf0rge1/msgb.h> +#include <laf0rge1/debug.h> + +#include <openbsc_nat/bssap.h> + +#include <string.h> + + +struct msgb *create_clear_command(struct sccp_source_reference *dest_ref) +{ + struct sccp_data_form1 *form1; + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "clear command"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate clear command.\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*form1)); + form1 = (struct sccp_data_form1 *) msg->l2h; + form1->type = SCCP_MSG_TYPE_DT1; + form1->destination_local_reference = *dest_ref; + form1->segmenting = 0; + form1->variable_start = 1; + + /* create a Clear Command Call Control msg */ + msg->l3h = msgb_put(msg, 7); + msg->l3h[0] = msgb_l3len(msg) - 1; + msg->l3h[1] = BSSAP_MSG_BSS_MANAGEMENT; + msg->l3h[2] = msg->l3h[0] - 2; + msg->l3h[3] = BSS_MAP_MSG_CLEAR_CMD; + msg->l3h[4] = 4; + msg->l3h[5] = 1; + msg->l3h[6] = 0x09; + + return msg; +} + +struct msgb *create_sccp_rlsd(struct sccp_source_reference *src_ref, + struct sccp_source_reference *dst_ref) +{ + struct sccp_connection_released *rel; + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "rlsd"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate clear command.\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*rel)); + rel = (struct sccp_connection_released *) msg->l2h; + rel->type = SCCP_MSG_TYPE_RLSD; + rel->release_cause = SCCP_RELEASE_CAUSE_END_USER_ORIGINATED; + rel->destination_local_reference = *dst_ref; + rel->source_local_reference = *src_ref; + + return msg; +} + +struct msgb *create_sccp_rlc(struct sccp_source_reference *src_ref, + struct sccp_source_reference *dst_ref) +{ + struct sccp_connection_release_complete *rlc; + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "rlc"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate rlc.\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*rlc)); + rlc = (struct sccp_connection_release_complete *) msg->l2h; + rlc->type = SCCP_MSG_TYPE_RLC; + rlc->destination_local_reference = *dst_ref; + rlc->source_local_reference = *src_ref; + + return msg; +} + +struct msgb *create_sccp_refuse(struct sccp_source_reference *dest_ref) +{ + struct sccp_connection_refused *ref; + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "rlsd"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate connection refuse.\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*ref)); + ref = (struct sccp_connection_refused *) msg->l2h; + ref->type = SCCP_MSG_TYPE_CREF; + ref->destination_local_reference = *dest_ref; + ref->cause = SCCP_REFUSAL_END_USER_ORIGINATED; + ref->optional_start = 1; + + msg->l3h = msgb_put(msg, 1); + msg->l3h[0] = SCCP_PNC_END_OF_OPTIONAL; + + return msg; +} + +struct msgb *create_reset() +{ + static const u_int8_t reset[] = { + 0x09, 0x00, 0x03, 0x05, 0x07, 0x02, 0x42, 0xfe, + 0x02, 0x42, 0xfe, 0x06, 0x00, 0x04, 0x30, 0x04, + 0x01, 0x20 + }; + + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "reset"); + if (!msg) { + LOGP(DMSC, LOGL_ERROR, "Failed to allocate reset msg.\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(reset)); + memcpy(msg->l2h, reset, msgb_l2len(msg)); + return msg; +} diff --git a/src/input/ipaccess.c b/src/input/ipaccess.c new file mode 100644 index 0000000..b679476 --- /dev/null +++ b/src/input/ipaccess.c @@ -0,0 +1,164 @@ +/* OpenBSC Abis input driver for ip.access */ + +/* (C) 2009 by Harald Welte <laforge@gnumonks.org> + * + * 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 <stdio.h> +#include <unistd.h> +#include <stdlib.h> +#include <errno.h> +#include <string.h> +#include <time.h> +#include <sys/fcntl.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/ioctl.h> +#include <arpa/inet.h> + +#include <laf0rge1/select.h> +#include <laf0rge1/msgb.h> +#include <laf0rge1/talloc.h> +#include <ipaccess.h> + + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(x) (sizeof(x)/sizeof(x[0])) +#endif + +#define TS1_ALLOC_SIZE 4096 + +static const u_int8_t pong[] = { 0, 1, IPAC_PROTO_IPACCESS, IPAC_MSGT_PONG }; +static const u_int8_t id_ack[] = { 0, 1, IPAC_PROTO_IPACCESS, IPAC_MSGT_ID_ACK }; +static const u_int8_t id_req[] = { 0, 17, IPAC_PROTO_IPACCESS, IPAC_MSGT_ID_GET, + 0x01, IPAC_IDTAG_UNIT, + 0x01, IPAC_IDTAG_MACADDR, + 0x01, IPAC_IDTAG_LOCATION1, + 0x01, IPAC_IDTAG_LOCATION2, + 0x01, IPAC_IDTAG_EQUIPVERS, + 0x01, IPAC_IDTAG_SWVERSION, + 0x01, IPAC_IDTAG_UNITNAME, + 0x01, IPAC_IDTAG_SERNR, + }; + +static const char *idtag_names[] = { + [IPAC_IDTAG_SERNR] = "Serial_Number", + [IPAC_IDTAG_UNITNAME] = "Unit_Name", + [IPAC_IDTAG_LOCATION1] = "Location_1", + [IPAC_IDTAG_LOCATION2] = "Location_2", + [IPAC_IDTAG_EQUIPVERS] = "Equipment_Version", + [IPAC_IDTAG_SWVERSION] = "Software_Version", + [IPAC_IDTAG_IPADDR] = "IP_Address", + [IPAC_IDTAG_MACADDR] = "MAC_Address", + [IPAC_IDTAG_UNIT] = "Unit_ID", +}; + +static const char *ipac_idtag_name(int tag) +{ + if (tag >= ARRAY_SIZE(idtag_names)) + return "unknown"; + + return idtag_names[tag]; +} + +/* send the id ack */ +int ipaccess_send_id_ack(int fd) +{ + return write(fd, id_ack, sizeof(id_ack)); +} + +int ipaccess_send_id_req(int fd) +{ + return write(fd, id_req, sizeof(id_req)); +} + +/* base handling of the ip.access protocol */ +int ipaccess_rcvmsg_base(struct msgb *msg, + struct bsc_fd *bfd) +{ + u_int8_t msg_type = *(msg->l2h); + int ret = 0; + + switch (msg_type) { + case IPAC_MSGT_PING: + ret = write(bfd->fd, pong, sizeof(pong)); + break; + case IPAC_MSGT_PONG: + break; + case IPAC_MSGT_ID_ACK: + ret = ipaccess_send_id_ack(bfd->fd); + break; + } + return 0; +} + +/* + * read one ipa message from the socket + * return NULL in case of error + */ +struct msgb *ipaccess_read_msg(struct bsc_fd *bfd, int *error) +{ + struct msgb *msg = msgb_alloc(TS1_ALLOC_SIZE, "Abis/IP"); + struct ipaccess_head *hh; + int len, ret = 0; + + if (!msg) { + *error = -ENOMEM; + return NULL; + } + + /* first read our 3-byte header */ + hh = (struct ipaccess_head *) msg->data; + ret = recv(bfd->fd, msg->data, 3, 0); + if (ret < 0) { + msgb_free(msg); + *error = ret; + return NULL; + } else if (ret == 0) { + msgb_free(msg); + *error = ret; + return NULL; + } + + msgb_put(msg, ret); + + /* then read te length as specified in header */ + msg->l2h = msg->data + sizeof(*hh); + len = ntohs(hh->len); + ret = recv(bfd->fd, msg->l2h, len, 0); + if (ret < len) { + msgb_free(msg); + *error = -EIO; + return NULL; + } + msgb_put(msg, ret); + + return msg; +} + +void ipaccess_prepend_header(struct msgb *msg, int proto) +{ + struct ipaccess_head *hh; + + /* prepend the ip.access header */ + hh = (struct ipaccess_head *) msgb_push(msg, sizeof(*hh)); + hh->len = htons(msg->len - sizeof(*hh)); + hh->proto = proto; +} + diff --git a/src/link_udp.c b/src/link_udp.c new file mode 100644 index 0000000..17c90ac --- /dev/null +++ b/src/link_udp.c @@ -0,0 +1,235 @@ +/* Implementation of the C7 UDP link */ +/* + * (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 <bsc_data.h> +#include <udp_input.h> +#include <mtp_data.h> +#include <mtp_pcap.h> +#include <snmp_mtp.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/talloc.h> + +#include <sys/socket.h> +#include <netinet/in.h> +#include <arpa/inet.h> + +#include <string.h> +#include <unistd.h> + +static int udp_write_cb(struct bsc_fd *fd, struct msgb *msg) +{ + struct link_data *link; + int rc; + + link = (struct link_data *) fd->data; + + LOGP(DINP, LOGL_DEBUG, "Sending MSU: %s\n", hexdump(msg->data, msg->len)); + if (link->pcap_fd >= 0) + mtp_pcap_write_msu(link->pcap_fd, msg->l2h, msgb_l2len(msg)); + + /* the assumption is we have connected the socket to the remote */ + rc = sendto(fd->fd, msg->data, msg->len, 0, + (struct sockaddr *) &link->udp.remote, sizeof(link->udp.remote)); + if (rc != msg->len) { + LOGP(DINP, LOGL_ERROR, "Failed to write msg to socket: %d\n", rc); + return -1; + } + + return 0; +} + +static int udp_read_cb(struct bsc_fd *fd) +{ + struct link_data *link; + struct udp_data_hdr *hdr; + struct msgb *msg; + int rc; + unsigned int length; + + msg = msgb_alloc_headroom(4096, 128, "UDP datagram"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate memory.\n"); + return -1; + } + + + link = (struct link_data *) fd->data; + rc = read(fd->fd, msg->data, 2096); + if (rc < sizeof(*hdr)) { + LOGP(DINP, LOGL_ERROR, "Failed to read at least size of the header: %d\n", rc); + rc = -1; + goto exit; + } + + /* throw away data as the link is down */ + if (link->the_link->available == 0) { + LOGP(DINP, LOGL_ERROR, "The link is down. Not forwarding.\n"); + rc = 0; + goto exit; + } + + hdr = (struct udp_data_hdr *) msgb_put(msg, sizeof(*hdr)); + + if (hdr->data_type == UDP_DATA_RETR_COMPL || hdr->data_type == UDP_DATA_RETR_IMPOS) { + LOGP(DINP, LOGL_ERROR, "Link retrieval done. Restarting the link.\n"); + bsc_link_down(link); + bsc_link_up(link); + goto exit; + } else if (hdr->data_type > UDP_DATA_MSU_PRIO_3) { + LOGP(DINP, LOGL_ERROR, "Link failure. retrieved message.\n"); + bsc_link_down(link); + goto exit; + } + + length = ntohl(hdr->data_length); + if (length + sizeof(*hdr) > (unsigned int) rc) { + LOGP(DINP, LOGL_ERROR, "The MSU payload does not fit: %u + %u > %d \n", + length, sizeof(*hdr), rc); + rc = -1; + goto exit; + } + + msg->l2h = msgb_put(msg, length); + + LOGP(DINP, LOGL_DEBUG, "MSU data on: %p data %s.\n", link, hexdump(msg->data, msg->len)); + if (link->pcap_fd >= 0) + mtp_pcap_write_msu(link->pcap_fd, msg->l2h, msgb_l2len(msg)); + mtp_link_data(link->the_link, msg); + +exit: + msgb_free(msg); + return rc; +} + +static int udp_link_dummy(struct link_data *link) +{ + /* nothing todo */ + return 0; +} + +static void do_start(void *_data) +{ + struct link_data *link = (struct link_data *) _data; + + link->forced_down = 0; + snmp_mtp_activate(link->udp.session); + bsc_link_up(link); +} + +static int udp_link_reset(struct link_data *link) +{ + LOGP(DINP, LOGL_NOTICE, "Will restart SLTM transmission in %d seconds.\n", + link->udp.reset_timeout); + snmp_mtp_deactivate(link->udp.session); + bsc_link_down(link); + + /* restart the link in 90 seconds... to force a timeout on the BSC */ + link->link_activate.cb = do_start; + link->link_activate.data = link; + bsc_schedule_timer(&link->link_activate, link->udp.reset_timeout, 0); + return 0; +} + +static int udp_link_write(struct link_data *link, struct msgb *msg) +{ + struct udp_data_hdr *hdr; + + hdr = (struct udp_data_hdr *) msgb_push(msg, sizeof(*hdr)); + hdr->format_type = UDP_FORMAT_SIMPLE_UDP; + hdr->data_type = UDP_DATA_MSU_PRIO_0; + hdr->data_link_index = htons(1); + hdr->user_context = 0; + hdr->data_length = htonl(msgb_l2len(msg)); + + if (write_queue_enqueue(&link->udp.write_queue, msg) != 0) { + LOGP(DINP, LOGL_ERROR, "Failed to enqueue msg.\n"); + msgb_free(msg); + return -1; + } + + return 0; +} + +static int udp_link_start(struct link_data *link) +{ + LOGP(DINP, LOGL_NOTICE, "UDP input is ready.\n"); + do_start(link); + return 0; +} + +int link_udp_init(struct link_data *link, int src_port, const char *remote, int remote_port) +{ + struct sockaddr_in addr; + int fd; + int on; + + write_queue_init(&link->udp.write_queue, 100); + + /* function table */ + link->shutdown = udp_link_dummy; + link->clear_queue = udp_link_dummy; + + link->reset = udp_link_reset; + link->start = udp_link_start; + link->write = udp_link_write; + + /* socket creation */ + link->udp.write_queue.bfd.data = link; + link->udp.write_queue.bfd.when = BSC_FD_READ; + link->udp.write_queue.read_cb = udp_read_cb; + link->udp.write_queue.write_cb = udp_write_cb; + + link->udp.write_queue.bfd.fd = fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + LOGP(DINP, LOGL_ERROR, "Failed to create UDP socket.\n"); + return -1; + } + + on = 1; + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(src_port); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("Failed to bind UDP socket"); + close(fd); + return -1; + } + + /* now connect the socket to the remote */ + memset(&link->udp.remote, 0, sizeof(link->udp.remote)); + link->udp.remote.sin_family = AF_INET; + link->udp.remote.sin_port = htons(remote_port); + inet_aton(remote, &link->udp.remote.sin_addr); + + if (bsc_register_fd(&link->udp.write_queue.bfd) != 0) { + LOGP(DINP, LOGL_ERROR, "Failed to register BFD.\n"); + close(fd); + return -1; + } + + return 0; +} diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..7bba82e --- /dev/null +++ b/src/main.c @@ -0,0 +1,1079 @@ +/* Bloated main routine, refactor */ +/* + * (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 <mtp_data.h> +#include <mtp_pcap.h> +#include <thread.h> +#include <bss_patch.h> +#include <bssap_sccp.h> +#include <bsc_data.h> +#include <snmp_mtp.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/talloc.h> + +#include <vty/command.h> +#include <vty/vty.h> + +#include <openbsc_nat/bssap.h> + +#include <sys/stat.h> +#include <sys/types.h> + +#include <fcntl.h> +#include <signal.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include <unistd.h> + +#include <netdb.h> + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif +#include <getopt.h> + +static struct debug_target *stderr_target; +static int dpc = 1; +static int opc = 0; + +static char *config = "cellmgr_ng.cfg"; +static int udp_port = 3456; +static char *udp_ip = NULL; +static int src_port = 1313; +static int once = 0; +static int flood = 0; +static struct timer_list flood_timer; + +/* + * One SCCP connection. + * Use for connection tracking and fixups... + */ +struct active_sccp_con { + struct llist_head entry; + + struct sccp_source_reference src_ref; + struct sccp_source_reference dst_ref; + + int has_dst_ref; + + /* fixup stuff */ + + /* We get a RLSD from the MSC and need to send a RLC */ + int released_from_msc; + + /* timeout for waiting for the RLC */ + struct timer_list rlc_timeout; + + /* how often did we send a RLSD this */ + unsigned int rls_tries; + + /* sls id */ + int sls; +}; + +static struct bsc_data bsc; + +static void send_reset_ack(struct mtp_link *link, int sls); +static void bsc_resources_released(struct bsc_data *bsc); +static void handle_local_sccp(struct mtp_link *link, struct msgb *inp, struct sccp_parse_result *res, int sls); +static void clear_connections(struct bsc_data *bsc); +static void send_local_rlsd(struct mtp_link *link, struct sccp_parse_result *res); +static void start_flood(); +static void cell_vty_init(void); + +/* send a RSIP to the MGCP GW */ +static void mgcp_reset(struct bsc_data *bsc) +{ + static const char mgcp_reset[] = { + "RSIP 1 13@mgw MGCP 1.0\r\n" + }; + + mgcp_forward(bsc, (const u_int8_t *) mgcp_reset, strlen(mgcp_reset)); +} + +/* + * methods called from the MTP Level3 part + */ +void mtp_link_submit(struct mtp_link *link, struct msgb *msg) +{ + bsc.link.write(&bsc.link, msg); +} + +void mtp_link_restart(struct mtp_link *link) +{ + LOGP(DINP, LOGL_ERROR, "Need to restart the SS7 link.\n"); + bsc.link.reset(&bsc.link); +} + +void mtp_link_sccp_down(struct mtp_link *link) +{ + msc_clear_queue(&bsc); +} + +void mtp_link_forward_sccp(struct mtp_link *link, struct msgb *_msg, int sls) +{ + int rc; + struct sccp_parse_result result; + + rc = bss_patch_filter_msg(_msg, &result); + if (rc == BSS_FILTER_RESET) { + LOGP(DMSC, LOGL_NOTICE, "Filtering BSS Reset from the BSC\n"); + msc_clear_queue(&bsc); + mgcp_reset(&bsc); + send_reset_ack(link, sls); + return; + } + + /* special responder */ + if (bsc.closing) { + if (rc == BSS_FILTER_RESET_ACK && bsc.reset_count > 0) { + LOGP(DMSC, LOGL_ERROR, "Received reset ack for closing.\n"); + clear_connections(&bsc); + bsc_resources_released(&bsc); + return; + } + + if (rc != 0 && rc != BSS_FILTER_RLSD && rc != BSS_FILTER_RLC) { + LOGP(DMSC, LOGL_ERROR, "Ignoring unparsable msg during closedown.\n"); + return; + } + + return handle_local_sccp(link, _msg, &result, sls); + } + + /* update the connection state */ + update_con_state(rc, &result, _msg, 0, sls); + + if (rc == BSS_FILTER_CLEAR_COMPL) { + send_local_rlsd(link, &result); + } else if (rc == BSS_FILTER_RLC || rc == BSS_FILTER_RLSD) { + LOGP(DMSC, LOGL_DEBUG, "Not forwarding RLC/RLSD to the MSC.\n"); + return; + } + + + msc_send_msg(&bsc, rc, &result, _msg); +} + +/* + * handle local message in close down mode + */ +static void handle_local_sccp(struct mtp_link *link, struct msgb *inpt, struct sccp_parse_result *result, int sls) +{ + /* Handle msg with a reject */ + if (inpt->l2h[0] == SCCP_MSG_TYPE_CR) { + struct sccp_connection_request *cr; + struct msgb *msg; + + LOGP(DINP, LOGL_NOTICE, "Handling CR localy.\n"); + cr = (struct sccp_connection_request *) inpt->l2h; + msg = create_sccp_refuse(&cr->source_local_reference); + if (msg) { + mtp_link_submit_sccp_data(link, sls, msg->l2h, msgb_l2len(msg)); + msgb_free(msg); + } + return; + } else if (inpt->l2h[0] == SCCP_MSG_TYPE_DT1 && result->data_len >= 3) { + struct active_sccp_con *con; + struct sccp_data_form1 *form1; + struct msgb *msg; + + if (inpt->l3h[0] == 0 && inpt->l3h[2] == BSS_MAP_MSG_CLEAR_COMPLETE) { + LOGP(DINP, LOGL_DEBUG, "Received Clear Complete. Sending Release.\n"); + + form1 = (struct sccp_data_form1 *) inpt->l2h; + + llist_for_each_entry(con, &bsc.sccp_connections, entry) { + if (memcmp(&form1->destination_local_reference, + &con->dst_ref, sizeof(con->dst_ref)) == 0) { + LOGP(DINP, LOGL_DEBUG, "Sending a release request now.\n"); + msg = create_sccp_rlsd(&con->dst_ref, &con->src_ref); + if (msg) { + mtp_link_submit_sccp_data(link, con->sls, msg->l2h, msgb_l2len(msg)); + msgb_free(msg); + } + return; + } + } + + LOGP(DINP, LOGL_ERROR, "Could not find connection for the Clear Command.\n"); + } + } else if (inpt->l2h[0] == SCCP_MSG_TYPE_UDT && result->data_len >= 3) { + if (inpt->l3h[0] == 0 && inpt->l3h[2] == BSS_MAP_MSG_RESET_ACKNOWLEDGE) { + LOGP(DINP, LOGL_NOTICE, "Reset ACK. Connecting to the MSC again.\n"); + bsc_resources_released(&bsc); + return; + } + } + + + /* Update the state, maybe the connection was released? */ + update_con_state(0, result, inpt, 0, sls); + if (llist_empty(&bsc.sccp_connections)) + bsc_resources_released(&bsc); + return; +} + +/* + * remove data + */ +static void free_con(struct active_sccp_con *con) +{ + llist_del(&con->entry); + bsc_del_timer(&con->rlc_timeout); + talloc_free(con); +} + +static void clear_connections(struct bsc_data *bsc) +{ + struct active_sccp_con *tmp, *con; + + llist_for_each_entry_safe(con, tmp, &bsc->sccp_connections, entry) { + free_con(con); + } + + bsc->link.clear_queue(&bsc->link); +} + +void bsc_resources_released(struct bsc_data *bsc) +{ + bsc_del_timer(&bsc->reset_timeout); + msc_schedule_reconnect(bsc); +} + +static void bsc_reset_timeout(void *_data) +{ + struct msgb *msg; + struct bsc_data *bsc = (struct bsc_data *) _data; + + /* no reset */ + if (bsc->reset_count > 0) { + LOGP(DINP, LOGL_ERROR, "The BSC did not answer the GSM08.08 reset. Restart MTP\n"); + mtp_link_stop(bsc->link.the_link); + clear_connections(bsc); + bsc->link.reset(&bsc->link); + bsc_resources_released(bsc); + return; + } + + msg = create_reset(); + if (!msg) { + bsc_schedule_timer(&bsc->reset_timeout, 10, 0); + return; + } + + ++bsc->reset_count; + mtp_link_submit_sccp_data(bsc->link.the_link, 13, msg->l2h, msgb_l2len(msg)); + msgb_free(msg); + bsc_schedule_timer(&bsc->reset_timeout, 20, 0); +} + +/* + * We have lost the connection to the MSC. This is tough. We + * can not just bring down the MTP link as this will disable + * the BTS radio. We will have to do the following: + * + * 1.) Bring down all open SCCP connections. As this will close + * all radio resources + * 2.) Bring down all MGCP endpoints + * 3.) Clear the connection data. + * + * To make things worse we need to buffer the BSC messages... atfer + * everything has been sent we will try to connect to the MSC again. + * + * We will have to veriy that all connections are closed properly.. + * this means we need to parse response message. In the case the + * MTP link is going down while we are sending. We will simply + * reconnect to the MSC. + */ +void release_bsc_resources(struct bsc_data *bsc) +{ + struct active_sccp_con *tmp; + struct active_sccp_con *con; + + bsc->closing = 1; + bsc_del_timer(&bsc->reset_timeout); + + /* 2. clear the MGCP endpoints */ + mgcp_reset(bsc); + + /* 1. send BSSMAP Cleanup.. if we have any connection */ + llist_for_each_entry_safe(con, tmp, &bsc->sccp_connections, entry) { + if (!con->has_dst_ref) { + free_con(con); + continue; + } + + struct msgb *msg = create_clear_command(&con->src_ref); + if (!msg) + continue; + + /* wait for the clear commands */ + mtp_link_submit_sccp_data(bsc->link.the_link, con->sls, msg->l2h, msgb_l2len(msg)); + msgb_free(msg); + } + + if (llist_empty(&bsc->sccp_connections)) { + bsc_resources_released(bsc); + } else { + /* Send a reset in 20 seconds if we fail to bring everything down */ + bsc->reset_timeout.cb = bsc_reset_timeout; + bsc->reset_timeout.data = bsc; + bsc->reset_count = 0; + bsc_schedule_timer(&bsc->reset_timeout, 10, 0); + } + + /* clear pending messages from the MSC */ + while (!llist_empty(&bsc->link.the_link->pending_msgs)) { + struct msgb *msg = msgb_dequeue(&bsc->link.the_link->pending_msgs); + msgb_free(msg); + } +} + +void bsc_link_down(struct link_data *data) +{ + int was_up; + struct mtp_link *link = data->the_link; + + link->available = 0; + was_up = link->sccp_up; + mtp_link_stop(link); + clear_connections(data->bsc); + mgcp_reset(data->bsc); + + data->clear_queue(data); + + /* clear pending messages from the MSC */ + while (!llist_empty(&link->pending_msgs)) { + struct msgb *msg = msgb_dequeue(&link->pending_msgs); + msgb_free(msg); + } + + /* for the case the link is going down while we are trying to reset */ + if (data->bsc->closing) + msc_schedule_reconnect(data->bsc); + else if (was_up) + msc_send_reset(data->bsc); +} + +void bsc_link_up(struct link_data *data) +{ + data->the_link->available = 1; + + /* we have not gone through link down */ + if (data->bsc->closing) { + clear_connections(data->bsc); + bsc_resources_released(data->bsc); + } + + mtp_link_reset(data->the_link); + + if (flood) + start_flood(); +} + +/** + * update the connection state and helpers below + */ +static struct active_sccp_con *find_con_by_dest_ref(struct sccp_source_reference *ref) +{ + struct active_sccp_con *con; + + if (!ref) { + LOGP(DINP, LOGL_ERROR, "Dest Reference is NULL. No connection found.\n"); + return NULL; + } + + llist_for_each_entry(con, &bsc.sccp_connections, entry) { + if (memcmp(&con->dst_ref, ref, sizeof(*ref)) == 0) + return con; + } + + LOGP(DINP, LOGL_ERROR, "No connection fond with: 0x%x as dest\n", sccp_src_ref_to_int(ref)); + return NULL; +} + +static struct active_sccp_con *find_con_by_src_ref(struct sccp_source_reference *src_ref) +{ + struct active_sccp_con *con; + + /* it is quite normal to not find this one */ + if (!src_ref) + return NULL; + + llist_for_each_entry(con, &bsc.sccp_connections, entry) { + if (memcmp(&con->src_ref, src_ref, sizeof(*src_ref)) == 0) + return con; + } + + return NULL; +} + +static struct active_sccp_con *find_con_by_src_dest_ref(struct sccp_source_reference *src_ref, + struct sccp_source_reference *dst_ref) +{ + struct active_sccp_con *con; + + llist_for_each_entry(con, &bsc.sccp_connections, entry) { + if (memcmp(src_ref, &con->src_ref, sizeof(*src_ref)) == 0 && + memcmp(dst_ref, &con->dst_ref, sizeof(*dst_ref)) == 0) { + return con; + } + } + + return NULL; +} + +unsigned int sls_for_src_ref(struct sccp_source_reference *ref) +{ + struct active_sccp_con *con; + + con = find_con_by_src_ref(ref); + if (!con) + return 13; + return con->sls; +} + +static void send_rlc_to_bsc(unsigned int sls, struct sccp_source_reference *src, struct sccp_source_reference *dst) +{ + struct msgb *msg; + + msg = create_sccp_rlc(src, dst); + if (!msg) + return; + + mtp_link_submit_sccp_data(bsc.link.the_link, sls, msg->l2h, msgb_l2len(msg)); + msgb_free(msg); +} + +static void handle_rlsd(struct sccp_connection_released *rlsd, int from_msc) +{ + struct active_sccp_con *con; + + if (from_msc) { + /* search for a connection, reverse src/dest for MSC */ + con = find_con_by_src_dest_ref(&rlsd->destination_local_reference, + &rlsd->source_local_reference); + if (con) { + LOGP(DINP, LOGL_DEBUG, "RLSD conn still alive: local: 0x%x remote: 0x%x\n", + sccp_src_ref_to_int(&con->src_ref), + sccp_src_ref_to_int(&con->dst_ref)); + con->released_from_msc = 1; + } else { + /* send RLC */ + LOGP(DINP, LOGL_DEBUG, "Sending RLC for MSC: src: 0x%x dst: 0x%x\n", + sccp_src_ref_to_int(&rlsd->destination_local_reference), + sccp_src_ref_to_int(&rlsd->source_local_reference)); + msc_send_rlc(&bsc, &rlsd->destination_local_reference, + &rlsd->source_local_reference); + } + } else { + unsigned int sls = 13; + con = find_con_by_src_dest_ref(&rlsd->source_local_reference, + &rlsd->destination_local_reference); + if (con) { + LOGP(DINP, LOGL_DEBUG, "Timeout on BSC. Sending RLC. src: 0x%x\n", + sccp_src_ref_to_int(&rlsd->source_local_reference)); + + if (con->released_from_msc) + msc_send_rlc(&bsc, &con->src_ref, &con->dst_ref); + sls = con->sls; + free_con(con); + } else { + LOGP(DINP, LOGL_ERROR, "Timeout on BSC for unknown connection. src: 0x%x\n", + sccp_src_ref_to_int(&rlsd->source_local_reference)); + } + + /* now send a rlc back to the BSC */ + send_rlc_to_bsc(sls, &rlsd->destination_local_reference, &rlsd->source_local_reference); + } +} + +/** + * Update connection state and also send message..... + * + * RLSD from MSC: + * 1.) We don't find the entry in this case we will send a + * forged RLC to the MSC and we are done. + * 2.) We find an entry in this we will need to register that + * we need to send a RLC and we are done for now. + * RLSD from BSC: + * 1.) This is an error we are ignoring for now. + * RLC from BSC: + * 1.) We are destroying the connection, we might send a RLC to + * the MSC if we are waiting for one. + */ +void update_con_state(int rc, struct sccp_parse_result *res, struct msgb *msg, int from_msc, int sls) +{ + struct active_sccp_con *con; + struct sccp_connection_request *cr; + struct sccp_connection_confirm *cc; + struct sccp_connection_release_complete *rlc; + struct sccp_connection_refused *cref; + + /* was the header okay? */ + if (rc < 0) + return; + + /* the header was size checked */ + switch (msg->l2h[0]) { + case SCCP_MSG_TYPE_CR: + if (from_msc) { + LOGP(DMSC, LOGL_ERROR, "CR from MSC is not handled.\n"); + return; + } + + cr = (struct sccp_connection_request *) msg->l2h; + con = find_con_by_src_ref(&cr->source_local_reference); + if (con) { + LOGP(DINP, LOGL_ERROR, "Duplicate SRC reference for: 0x%x. Reusing\n", + sccp_src_ref_to_int(&con->src_ref)); + free_con(con); + } + + con = talloc_zero(NULL, struct active_sccp_con); + if (!con) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate\n"); + return; + } + + con->src_ref = cr->source_local_reference; + con->sls = sls; + llist_add_tail(&con->entry, &bsc.sccp_connections); + LOGP(DINP, LOGL_DEBUG, "Adding CR: local ref: 0x%x\n", sccp_src_ref_to_int(&con->src_ref)); + break; + case SCCP_MSG_TYPE_CC: + if (!from_msc) { + LOGP(DINP, LOGL_ERROR, "CC from BSC is not handled.\n"); + return; + } + + cc = (struct sccp_connection_confirm *) msg->l2h; + con = find_con_by_src_ref(&cc->destination_local_reference); + if (con) { + con->dst_ref = cc->source_local_reference; + con->has_dst_ref = 1; + LOGP(DINP, LOGL_DEBUG, "Updating CC: local: 0x%x remote: 0x%x\n", + sccp_src_ref_to_int(&con->src_ref), sccp_src_ref_to_int(&con->dst_ref)); + return; + } + + LOGP(DINP, LOGL_ERROR, "CCed connection can not be found: 0x%x\n", + sccp_src_ref_to_int(&cc->destination_local_reference)); + break; + case SCCP_MSG_TYPE_CREF: + if (!from_msc) { + LOGP(DINP, LOGL_ERROR, "CREF from BSC is not handled.\n"); + return; + } + + cref = (struct sccp_connection_refused *) msg->l2h; + con = find_con_by_src_ref(&cref->destination_local_reference); + if (con) { + LOGP(DINP, LOGL_DEBUG, "Releasing local: 0x%x\n", sccp_src_ref_to_int(&con->src_ref)); + free_con(con); + return; + } + + LOGP(DINP, LOGL_ERROR, "CREF from BSC is not handled.\n"); + break; + case SCCP_MSG_TYPE_RLSD: + handle_rlsd((struct sccp_connection_released *) msg->l2h, from_msc); + break; + case SCCP_MSG_TYPE_RLC: + if (from_msc) { + LOGP(DINP, LOGL_ERROR, "RLC from MSC is wrong.\n"); + return; + } + + rlc = (struct sccp_connection_release_complete *) msg->l2h; + con = find_con_by_src_dest_ref(&rlc->source_local_reference, + &rlc->destination_local_reference); + if (con) { + LOGP(DINP, LOGL_DEBUG, "Releasing local: 0x%x\n", sccp_src_ref_to_int(&con->src_ref)); + if (con->released_from_msc) + msc_send_rlc(&bsc, &con->src_ref, &con->dst_ref); + free_con(con); + return; + } + + LOGP(DINP, LOGL_ERROR, "RLC can not be found. 0x%x 0x%x\n", + sccp_src_ref_to_int(&rlc->source_local_reference), + sccp_src_ref_to_int(&rlc->destination_local_reference)); + break; + } +} + +static void send_local_rlsd_for_con(void *data) +{ + struct msgb *rlsd; + struct active_sccp_con *con = (struct active_sccp_con *) data; + + /* try again in three seconds */ + con->rlc_timeout.data = con; + con->rlc_timeout.cb = send_local_rlsd_for_con; + bsc_schedule_timer(&con->rlc_timeout, 3, 0); + + /* we send this to the BSC so we need to switch src and dest */ + rlsd = create_sccp_rlsd(&con->dst_ref, &con->src_ref); + if (!rlsd) + return; + + ++con->rls_tries; + LOGP(DINP, LOGL_DEBUG, "Sending RLSD for 0x%x the %d time.\n", + sccp_src_ref_to_int(&con->src_ref), con->rls_tries); + mtp_link_submit_sccp_data(bsc.link.the_link, con->sls, rlsd->l2h, msgb_l2len(rlsd)); + msgb_free(rlsd); +} + +static void send_local_rlsd(struct mtp_link *link, struct sccp_parse_result *res) +{ + struct active_sccp_con *con; + + LOGP(DINP, LOGL_DEBUG, "Received GSM Clear Complete. Sending RLSD locally.\n"); + + con = find_con_by_dest_ref(res->destination_local_reference); + if (!con) + return; + con->rls_tries = 0; + send_local_rlsd_for_con(con); +} + +static void send_reset_ack(struct mtp_link *link, int sls) +{ + static const u_int8_t reset_ack[] = { + 0x09, 0x00, 0x03, 0x05, 0x7, 0x02, 0x42, 0xfe, + 0x02, 0x42, 0xfe, 0x03, + 0x00, 0x01, 0x31 + }; + + mtp_link_submit_sccp_data(link, sls, reset_ack, sizeof(reset_ack)); +} + +static void start_flood() +{ + static unsigned int i = 0; + static const u_int8_t paging_cmd[] = { + 0x09, 0x00, 0x03, 0x07, 0x0b, 0x04, 0x43, 0x0a, + 0x00, 0xfe, 0x04, 0x43, 0x5c, 0x00, 0xfe, 0x10, + 0x00, 0x0e, 0x52, 0x08, 0x08, 0x29, 0x80, 0x10, + 0x76, 0x10, 0x77, 0x46, 0x05, 0x1a, 0x01, 0x06 }; + + /* change the imsi slightly */ + if (bsc.link.the_link->sltm_pending) { + LOGP(DINP, LOGL_ERROR, "Not sending due clash with SLTM.\n"); + } else { + struct msgb *msg; + msg = msgb_alloc_headroom(4096, 128, "paging"); + if (msg) { + LOGP(DINP, LOGL_NOTICE, "Flooding BSC with one paging requests.\n"); + + msg->l2h = msgb_put(msg, sizeof(paging_cmd)); + memcpy(msg->l2h, paging_cmd, msgb_l2len(msg)); + + bss_rewrite_header_to_bsc(msg, + bsc.link.the_link->opc, + bsc.link.the_link->dpc); + mtp_link_submit_sccp_data(bsc.link.the_link, i++, + msg->l2h, msgb_l2len(msg)); + msgb_free(msg); + } + } + + /* try again in five seconds */ + flood_timer.cb = start_flood; + bsc_schedule_timer(&flood_timer, 2, 0); +} + +static void print_usage() +{ + printf("Usage: cellmgr_ng\n"); +} + +static void sigint() +{ + static pthread_mutex_t exit_mutex = PTHREAD_MUTEX_INITIALIZER; + static int handled = 0; + + /* failed to lock */ + if (pthread_mutex_trylock(&exit_mutex) != 0) + return; + if (handled) + goto out; + + printf("Terminating.\n"); + handled = 1; + if (bsc.setup) + bsc.link.shutdown(&bsc.link); + exit(0); + +out: + pthread_mutex_unlock(&exit_mutex); +} + +static void sigusr2() +{ + printf("Closing the MSC connection on demand.\n"); + close(bsc.msc_connection.bfd.fd); + bsc_unregister_fd(&bsc.msc_connection.bfd); + bsc.msc_connection.bfd.fd = -1; + release_bsc_resources(&bsc); +} + +static void print_help() +{ + printf(" Some useful help...\n"); + printf(" -h --help this text\n"); + printf(" -c --config=CFG The config file to use.\n"); + printf(" -p --pcap=FILE. Write MSUs to the PCAP file.\n"); + printf(" -c --once. Send the SLTM msg only once.\n"); + printf(" -f --flood. Send flood of paging requests to the BSC.\n"); +} + +static void handle_options(int argc, char **argv) +{ + while (1) { + int option_index = 0, c; + static struct option long_options[] = { + {"help", 0, 0, 'h'}, + {"config", 1, 0, 'c'}, + {"pcap", 1, 0, 'p'}, + {"flood", 0, 0, 'f'}, + {0, 0, 0, 0}, + }; + + c = getopt_long(argc, argv, "hc:p:f", + long_options, &option_index); + if (c == -1) + break; + + switch (c) { + case 'h': + print_usage(); + print_help(); + exit(0); + case 'p': + if (bsc.link.pcap_fd >= 0) + close(bsc.link.pcap_fd); + bsc.link.pcap_fd = open(optarg, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP| S_IROTH); + if (bsc.link.pcap_fd < 0) { + fprintf(stderr, "Failed to open PCAP file.\n"); + exit(0); + } + mtp_pcap_write_header(bsc.link.pcap_fd); + break; + case 'c': + config = optarg; + break; + case 'f': + flood = 1; + break; + default: + fprintf(stderr, "Unknown option.\n"); + break; + } + } +} + +static void start_rest(void *start) +{ + bsc.setup = 1; + + if (msc_init(&bsc) != 0) { + fprintf(stderr, "Failed to init MSC part.\n"); + exit(3); + } + + bsc.link.start(&bsc.link); +} + + +int main(int argc, char **argv) +{ + INIT_LLIST_HEAD(&bsc.sccp_connections); + + mtp_link_init(); + thread_init(); + debug_init(); + + stderr_target = debug_target_create_stderr(); + debug_add_target(stderr_target); + + /* enable filters */ + debug_set_all_filter(stderr_target, 1); + debug_set_category_filter(stderr_target, DINP, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DSCCP, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DMSC, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DMGCP, 1, LOGL_INFO); + debug_set_print_timestamp(stderr_target, 1); + debug_set_use_color(stderr_target, 0); + + bsc.setup = 0; + bsc.msc_address = "127.0.0.1"; + bsc.link.pcap_fd = -1; + bsc.link.udp.reset_timeout = 180; + bsc.ping_time = 20; + bsc.pong_time = 5; + bsc.msc_time = 20; + + handle_options(argc, argv); + + signal(SIGPIPE, SIG_IGN); + signal(SIGINT, sigint); + signal(SIGUSR2, sigusr2); + srand(time(NULL)); + + cell_vty_init(); + if (vty_read_config_file(config) < 0) { + fprintf(stderr, "Failed to read the VTY config.\n"); + return -1; + } + + bsc.link.the_link = mtp_link_alloc(); + bsc.link.the_link->dpc = dpc; + bsc.link.the_link->opc = opc; + bsc.link.the_link->link = 0; + bsc.link.the_link->sltm_once = once; + bsc.link.bsc = &bsc; + + if (udp_ip) { + LOGP(DINP, LOGL_NOTICE, "Using UDP MTP mode.\n"); + + /* setup SNMP first, it is blocking */ + bsc.link.udp.session = snmp_mtp_session_create(udp_ip); + if (!bsc.link.udp.session) + return -1; + + /* now connect to the transport */ + if (link_udp_init(&bsc.link, src_port, udp_ip, udp_port) != 0) + return -1; + + /* + * We will ask the MTP link to be taken down for two + * timeouts of the BSC to make sure we are missing the + * SLTM and it begins a reset. Then we will take it up + * again and do the usual business. + */ + snmp_mtp_deactivate(bsc.link.udp.session); + bsc.start_timer.cb = start_rest; + bsc.start_timer.data = &bsc; + bsc_schedule_timer(&bsc.start_timer, bsc.link.udp.reset_timeout, 0); + LOGP(DMSC, LOGL_NOTICE, "Making sure SLTM will timeout.\n"); + } else { + LOGP(DINP, LOGL_NOTICE, "Using NexusWare C7 input.\n"); + if (link_c7_init(&bsc.link) != 0) + return -1; + + /* give time to things to start*/ + bsc.start_timer.cb = start_rest; + bsc.start_timer.data = &bsc; + bsc_schedule_timer(&bsc.start_timer, 30, 0); + LOGP(DMSC, LOGL_NOTICE, "Waiting to continue to startup.\n"); + } + + + while (1) { + bsc_select_main(0); + } + + return 0; +} + +/* vty code */ +static struct cmd_node cell_node = { + GSMNET_NODE, + "%s(cellmgr)#", + 1, +}; + +static int config_write_cell() +{ + return CMD_SUCCESS; +} + +DEFUN(cfg_cell, cfg_cell_cmd, + "cellmgr", "Configure the Cellmgr") +{ + vty->node = GSMNET_NODE; + return CMD_SUCCESS; +} + +DEFUN(cfg_net_dpc, cfg_net_dpc_cmd, + "mtp dpc DPC_NR", + "Set the DPC to be used.") +{ + dpc = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_net_opc, cfg_net_opc_cmd, + "mtp opc OPC_NR", + "Set the OPC to be used.") +{ + opc = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_udp_dst_ip, cfg_udp_dst_ip_cmd, + "udp dest ip IP", + "Set the IP when UDP mode is supposed to be used.") +{ + struct hostent *hosts; + struct in_addr *addr; + + hosts = gethostbyname(argv[0]); + if (!hosts || hosts->h_length < 1 || hosts->h_addrtype != AF_INET) { + vty_out(vty, "Failed to resolve '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + addr = (struct in_addr *) hosts->h_addr_list[0]; + udp_ip = talloc_strdup(NULL, inet_ntoa(*addr)); + return CMD_SUCCESS; +} + +DEFUN(cfg_udp_dst_port, cfg_udp_dst_port_cmd, + "udp dest port PORT_NR", + "If UDP mode is used specify the UDP dest port") +{ + udp_port = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_udp_src_port, cfg_udp_src_port_cmd, + "udp src port PORT_NR", + "Set the UDP source port to be used.") +{ + src_port = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_udp_reset, cfg_udp_reset_cmd, + "udp reset TIMEOUT", + "Set the timeout to take the link down") +{ + bsc.link.udp.reset_timeout = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_sltm_once, cfg_sltm_once_cmd, + "mtp sltm once (0|1)", + "Send SLTMs until the link is established.") +{ + once = !!atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_msc_ip, cfg_msc_ip_cmd, + "msc ip IP", + "Set the MSC IP") +{ + struct hostent *hosts; + struct in_addr *addr; + + hosts = gethostbyname(argv[0]); + if (!hosts || hosts->h_length < 1 || hosts->h_addrtype != AF_INET) { + vty_out(vty, "Failed to resolve '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + addr = (struct in_addr *) hosts->h_addr_list[0]; + + bsc.msc_address = talloc_strdup(NULL, inet_ntoa(*addr)); + return CMD_SUCCESS; +} + +DEFUN(cfg_msc_ip_dscp, cfg_msc_ip_dscp_cmd, + "msc ip-dscp <0-255>", + "Set the IP DSCP on the A-link\n" + "Set the DSCP in IP packets to the MSC") +{ + bsc.msc_ip_dscp = atoi(argv[0]); + return CMD_SUCCESS; +} + +ALIAS_DEPRECATED(cfg_msc_ip_dscp, cfg_msc_ip_tos_cmd, + "msc ip-tos <0-255>", + "Set the IP DSCP on the A-link\n" + "Set the DSCP in IP packets to the MSC") + +DEFUN(cfg_msc_token, cfg_msc_token_cmd, + "msc token TOKEN", + "Set the Token to be used for the MSC") +{ + bsc.token = talloc_strdup(NULL, argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_ping_time, cfg_ping_time_cmd, + "timeout ping NR", + "Set the PING interval. Negative to disable it") +{ + bsc.ping_time = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_pong_time, cfg_pong_time_cmd, + "timeout pong NR", + "Set the PING interval. Negative to disable it") +{ + bsc.pong_time = atoi(argv[0]); + return CMD_SUCCESS; +} + +DEFUN(cfg_msc_time, cfg_msc_time_cmd, + "timeout msc NR", + "Set the MSC connect timeout") +{ + bsc.msc_time = atoi(argv[0]); + return CMD_SUCCESS; +} + +static void cell_vty_init(void) +{ + cmd_init(1); + vty_init(); + + install_element(CONFIG_NODE, &cfg_cell_cmd); + install_node(&cell_node, config_write_cell); + + install_element(GSMNET_NODE, &cfg_net_dpc_cmd); + install_element(GSMNET_NODE, &cfg_net_opc_cmd); + install_element(GSMNET_NODE, &cfg_udp_dst_ip_cmd); + install_element(GSMNET_NODE, &cfg_udp_dst_port_cmd); + install_element(GSMNET_NODE, &cfg_udp_src_port_cmd); + install_element(GSMNET_NODE, &cfg_udp_reset_cmd); + install_element(GSMNET_NODE, &cfg_sltm_once_cmd); + install_element(GSMNET_NODE, &cfg_msc_ip_cmd); + install_element(GSMNET_NODE, &cfg_msc_token_cmd); + install_element(GSMNET_NODE, &cfg_msc_ip_dscp_cmd); + install_element(GSMNET_NODE, &cfg_msc_ip_tos_cmd); + install_element(GSMNET_NODE, &cfg_ping_time_cmd); + install_element(GSMNET_NODE, &cfg_pong_time_cmd); + install_element(GSMNET_NODE, &cfg_msc_time_cmd); +} + +void subscr_put() {} +void vty_event() {} diff --git a/src/mgcp/README b/src/mgcp/README new file mode 100644 index 0000000..5ba7f48 --- /dev/null +++ b/src/mgcp/README @@ -0,0 +1,3 @@ +This is OpenBSCs MGCP implementation and is made to fit on top of the +NexusWare UniPorte library. All changes to the MGCP must be merged back +to the OpenBSC implementation in one way or another. diff --git a/src/mgcp/mgcp_network.c b/src/mgcp/mgcp_network.c new file mode 100644 index 0000000..fbbfbf2 --- /dev/null +++ b/src/mgcp/mgcp_network.c @@ -0,0 +1,300 @@ +/* A Media Gateway Control Protocol Media Gateway: RFC 3435 */ +/* The protocol implementation */ + +/* + * (C) 2009-2010 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2009-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 <string.h> +#include <stdlib.h> +#include <unistd.h> +#include <endian.h> +#include <errno.h> + +#include <sys/socket.h> +#include <arpa/inet.h> + +#include <mgcp/mgcp.h> +#include <mgcp/mgcp_internal.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/msgb.h> +#include <laf0rge1/talloc.h> +#include <laf0rge1/select.h> + +#warning "Make use of the rtp proxy code" + +/* according to rtp_proxy.c RFC 3550 */ +struct rtp_hdr { +#if __BYTE_ORDER == __LITTLE_ENDIAN + u_int8_t csrc_count:4, + extension:1, + padding:1, + version:2; + u_int8_t payload_type:7, + marker:1; +#elif __BYTE_ORDER == __BIG_ENDIAN + u_int8_t version:2, + padding:1, + extension:1, + csrc_count:4; + u_int8_t marker:1, + payload_type:7; +#endif + u_int16_t sequence; + u_int32_t timestamp; + u_int32_t ssrc; +} __attribute__((packed)); + + +enum { + DEST_NETWORK = 0, + DEST_BTS = 1, +}; + +enum { + PROTO_RTP, + PROTO_RTCP, +}; + +#define DUMMY_LOAD 0x23 + + +static int udp_send(int fd, struct in_addr *addr, int port, char *buf, int len) +{ + struct sockaddr_in out; + out.sin_family = AF_INET; + out.sin_port = port; + memcpy(&out.sin_addr, addr, sizeof(*addr)); + + return sendto(fd, buf, len, 0, (struct sockaddr *)&out, sizeof(out)); +} + +int mgcp_send_dummy(struct mgcp_endpoint *endp) +{ + static char buf[] = { DUMMY_LOAD }; + + return udp_send(endp->local_rtp.fd, &endp->remote, + endp->net_rtp, buf, 1); +} + +static void patch_payload(int payload, char *data, int len) +{ + struct rtp_hdr *rtp_hdr; + + if (len < sizeof(*rtp_hdr)) + return; + + if (payload < 0) + return; + + rtp_hdr = (struct rtp_hdr *) data; + rtp_hdr->payload_type = payload; +} + +/* + * There is data coming. We will have to figure out if it + * came from the BTS or the MediaGateway of the MSC. On top + * of that we need to figure out if it was RTP or RTCP. + * + * Currently we do not communicate with the BSC so we have + * no idea where the BTS is listening for RTP and need to + * do the classic routing trick. Wait for the first packet + * from the BTS and then go ahead. + */ +static int rtp_data_cb(struct bsc_fd *fd, unsigned int what) +{ + char buf[4096]; + struct sockaddr_in addr; + socklen_t slen = sizeof(addr); + struct mgcp_endpoint *endp; + struct mgcp_config *cfg; + int rc, dest, proto; + + endp = (struct mgcp_endpoint *) fd->data; + cfg = endp->cfg; + + rc = recvfrom(fd->fd, &buf, sizeof(buf), 0, + (struct sockaddr *) &addr, &slen); + if (rc < 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to receive message on: 0x%x errno: %d/%s\n", + ENDPOINT_NUMBER(endp), errno, strerror(errno)); + return -1; + } + + /* do not forward aynthing... maybe there is a packet from the bts */ + if (endp->ci == CI_UNUSED) { + LOGP(DMGCP, LOGL_DEBUG, "Unknown message on endpoint: 0x%x\n", ENDPOINT_NUMBER(endp)); + return -1; + } + + /* + * Figure out where to forward it to. This code assumes that we + * have received the Connection Modify and know who is a legitimate + * partner. According to the spec we could attempt to forward even + * after the Create Connection but we will not as we are not really + * able to tell if this is legitimate. + */ + #warning "Slight spec violation. With connection mode recvonly we should attempt to forward." + dest = memcmp(&addr.sin_addr, &endp->remote, sizeof(addr.sin_addr)) == 0 && + (endp->net_rtp == addr.sin_port || endp->net_rtcp == addr.sin_port) + ? DEST_BTS : DEST_NETWORK; + proto = fd == &endp->local_rtp ? PROTO_RTP : PROTO_RTCP; + + /* We have no idea who called us, maybe it is the BTS. */ + if (dest == DEST_NETWORK && (endp->bts_rtp == 0 || cfg->forward_ip)) { + /* it was the BTS... */ + if (!cfg->bts_ip + || memcmp(&addr.sin_addr, &cfg->bts_in, sizeof(cfg->bts_in)) == 0 + || memcmp(&addr.sin_addr, &endp->bts, sizeof(endp->bts)) == 0) { + if (fd == &endp->local_rtp) { + endp->bts_rtp = addr.sin_port; + } else { + endp->bts_rtcp = addr.sin_port; + } + + endp->bts = addr.sin_addr; + LOGP(DMGCP, LOGL_NOTICE, "Found BTS for endpoint: 0x%x on port: %d/%d of %s\n", + ENDPOINT_NUMBER(endp), ntohs(endp->bts_rtp), ntohs(endp->bts_rtcp), + inet_ntoa(addr.sin_addr)); + + } + } + + /* throw away the dummy message */ + if (rc == 1 && buf[0] == DUMMY_LOAD) { + LOGP(DMGCP, LOGL_NOTICE, "Filtered dummy on 0x%x\n", + ENDPOINT_NUMBER(endp)); + return 0; + } + + /* do this before the loop handling */ + if (dest == DEST_NETWORK) + ++endp->in_bts; + else + ++endp->in_remote; + + /* For loop toggle the destination and then dispatch. */ + if (cfg->audio_loop) + dest = !dest; + + if (dest == DEST_NETWORK) { + if (proto == PROTO_RTP) + patch_payload(endp->net_payload_type, buf, rc); + return udp_send(fd->fd, &endp->remote, + proto == PROTO_RTP ? endp->net_rtp : endp->net_rtcp, + buf, rc); + } else { + if (proto == PROTO_RTP) + patch_payload(endp->bts_payload_type, buf, rc); + return udp_send(fd->fd, &endp->bts, + proto == PROTO_RTP ? endp->bts_rtp : endp->bts_rtcp, + buf, rc); + } +} + +static int create_bind(const char *source_addr, struct bsc_fd *fd, int port) +{ + struct sockaddr_in addr; + int on = 1; + + fd->fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd->fd < 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to create UDP port.\n"); + return -1; + } + + setsockopt(fd->fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + inet_aton(source_addr, &addr.sin_addr); + + if (bind(fd->fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + return -1; + } + + return 0; +} + +static int set_ip_tos(int fd, int tos) +{ + int ret; + ret = setsockopt(fd, IPPROTO_IP, IP_TOS, + &tos, sizeof(tos)); + return ret != 0; +} + +static int bind_rtp(struct mgcp_endpoint *endp) +{ + struct mgcp_config *cfg = endp->cfg; + + if (create_bind(cfg->source_addr, &endp->local_rtp, endp->rtp_port) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to create RTP port: %s:%d on 0x%x\n", + cfg->source_addr, endp->rtp_port, ENDPOINT_NUMBER(endp)); + goto cleanup0; + } + + if (create_bind(cfg->source_addr, &endp->local_rtcp, endp->rtp_port + 1) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to create RTCP port: %s:%d on 0x%x\n", + cfg->source_addr, endp->rtp_port + 1, ENDPOINT_NUMBER(endp)); + goto cleanup1; + } + + set_ip_tos(endp->local_rtp.fd, cfg->endp_dscp); + set_ip_tos(endp->local_rtcp.fd, cfg->endp_dscp); + + endp->local_rtp.cb = rtp_data_cb; + endp->local_rtp.data = endp; + endp->local_rtp.when = BSC_FD_READ; + if (bsc_register_fd(&endp->local_rtp) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to register RTP port %d on 0x%x\n", + endp->rtp_port, ENDPOINT_NUMBER(endp)); + goto cleanup2; + } + + endp->local_rtcp.cb = rtp_data_cb; + endp->local_rtcp.data = endp; + endp->local_rtcp.when = BSC_FD_READ; + if (bsc_register_fd(&endp->local_rtcp) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to register RTCP port %d on 0x%x\n", + endp->rtp_port + 1, ENDPOINT_NUMBER(endp)); + goto cleanup3; + } + + return 0; + +cleanup3: + bsc_unregister_fd(&endp->local_rtp); +cleanup2: + close(endp->local_rtcp.fd); + endp->local_rtcp.fd = -1; +cleanup1: + close(endp->local_rtp.fd); + endp->local_rtp.fd = -1; +cleanup0: + return -1; +} + +int mgcp_bind_rtp_port(struct mgcp_endpoint *endp, int rtp_port) +{ + endp->rtp_port = rtp_port; + return bind_rtp(endp); +} diff --git a/src/mgcp/mgcp_protocol.c b/src/mgcp/mgcp_protocol.c new file mode 100644 index 0000000..ea99543 --- /dev/null +++ b/src/mgcp/mgcp_protocol.c @@ -0,0 +1,758 @@ +/* A Media Gateway Control Protocol Media Gateway: RFC 3435 */ +/* The protocol implementation */ + +/* + * (C) 2009-2010 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2009-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 <ctype.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <limits.h> +#include <unistd.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/msgb.h> +#include <laf0rge1/talloc.h> +#include <laf0rge1/select.h> +#include <mgcp/mgcp.h> +#include <mgcp/mgcp_internal.h> + +/** + * Macro for tokenizing MGCP messages and SDP in one go. + * + */ +#define MSG_TOKENIZE_START \ + line_start = 0; \ + for (i = 0; i < msgb_l3len(msg); ++i) { \ + /* we have a line end */ \ + if (msg->l3h[i] == '\n') { \ + /* skip the first line */ \ + if (line_start == 0) { \ + line_start = i + 1; \ + continue; \ + } \ + \ + /* check if we have a proper param */ \ + if (i - line_start == 1 && msg->l3h[line_start] == '\r') { \ + } else if (i - line_start > 2 \ + && islower(msg->l3h[line_start]) \ + && msg->l3h[line_start + 1] == '=') { \ + } else if (i - line_start < 3 \ + || msg->l3h[line_start + 1] != ':' \ + || msg->l3h[line_start + 2] != ' ') \ + goto error; \ + \ + msg->l3h[i] = '\0'; \ + if (msg->l3h[i-1] == '\r') \ + msg->l3h[i-1] = '\0'; + +#define MSG_TOKENIZE_END \ + line_start = i + 1; \ + } \ + } + + +struct mgcp_request { + char *name; + struct msgb *(*handle_request) (struct mgcp_config *cfg, struct msgb *msg); + char *debug_name; +}; + +#define MGCP_REQUEST(NAME, REQ, DEBUG_NAME) \ + { .name = NAME, .handle_request = REQ, .debug_name = DEBUG_NAME }, + +static struct msgb *handle_audit_endpoint(struct mgcp_config *cfg, struct msgb *msg); +static struct msgb *handle_create_con(struct mgcp_config *cfg, struct msgb *msg); +static struct msgb *handle_delete_con(struct mgcp_config *cfg, struct msgb *msg); +static struct msgb *handle_modify_con(struct mgcp_config *cfg, struct msgb *msg); +static struct msgb *handle_rsip(struct mgcp_config *cfg, struct msgb *msg); + +static int generate_call_id(struct mgcp_config *cfg) +{ + int i; + + /* use the call id */ + ++cfg->last_call_id; + + /* handle wrap around */ + if (cfg->last_call_id == CI_UNUSED) + ++cfg->last_call_id; + + /* callstack can only be of size number_of_endpoints */ + /* verify that the call id is free, e.g. in case of overrun */ + for (i = 1; i < cfg->number_endpoints; ++i) + if (cfg->endpoints[i].ci == cfg->last_call_id) + return generate_call_id(cfg); + + return cfg->last_call_id; +} + +/* + * array of function pointers for handling various + * messages. In the future this might be binary sorted + * for performance reasons. + */ +static const struct mgcp_request mgcp_requests [] = { + MGCP_REQUEST("AUEP", handle_audit_endpoint, "AuditEndpoint") + MGCP_REQUEST("CRCX", handle_create_con, "CreateConnection") + MGCP_REQUEST("DLCX", handle_delete_con, "DeleteConnection") + MGCP_REQUEST("MDCX", handle_modify_con, "ModifiyConnection") + + /* SPEC extension */ + MGCP_REQUEST("RSIP", handle_rsip, "ReSetInProgress") +}; + +static struct msgb *mgcp_msgb_alloc(void) +{ + struct msgb *msg; + msg = msgb_alloc_headroom(4096, 128, "MGCP msg"); + if (!msg) + LOGP(DMGCP, LOGL_ERROR, "Failed to msgb for MGCP data.\n"); + + return msg; +} + +struct msgb *mgcp_create_response_with_data(int code, const char *msg, const char *trans, + const char *data) +{ + int len; + struct msgb *res; + + res = mgcp_msgb_alloc(); + if (!res) + return NULL; + + if (data) { + len = snprintf((char *) res->data, 2048, "%d %s\n%s", code, trans, data); + } else { + len = snprintf((char *) res->data, 2048, "%d %s\n", code, trans); + } + + res->l2h = msgb_put(res, len); + LOGP(DMGCP, LOGL_DEBUG, "Sending response: code: %d for '%s'\n", code, res->l2h); + return res; +} + +static struct msgb *create_response(int code, const char *msg, const char *trans) +{ + return mgcp_create_response_with_data(code, msg, trans, NULL); +} + +static struct msgb *create_response_with_sdp(struct mgcp_endpoint *endp, + const char *msg, const char *trans_id) +{ + const char *addr = endp->cfg->local_ip; + char sdp_record[4096]; + + if (!addr) + addr = endp->cfg->source_addr; + + snprintf(sdp_record, sizeof(sdp_record) - 1, + "I: %d\n\n" + "v=0\r\n" + "c=IN IP4 %s\r\n" + "m=audio %d RTP/AVP %d\r\n" + "a=rtpmap:%d %s\r\n", + endp->ci, addr, endp->rtp_port, + endp->bts_payload_type, endp->bts_payload_type, + endp->cfg->audio_name); + return mgcp_create_response_with_data(200, msg, trans_id, sdp_record); +} + +/* + * handle incoming messages: + * - this can be a command (four letters, space, transaction id) + * - or a response (three numbers, space, transaction id) + */ +struct msgb *mgcp_handle_message(struct mgcp_config *cfg, struct msgb *msg) +{ + int code; + struct msgb *resp = NULL; + + if (msgb_l2len(msg) < 4) { + LOGP(DMGCP, LOGL_ERROR, "mgs too short: %d\n", msg->len); + return NULL; + } + + /* attempt to treat it as a response */ + if (sscanf((const char *)&msg->l2h[0], "%3d %*s", &code) == 1) { + LOGP(DMGCP, LOGL_DEBUG, "Response: Code: %d\n", code); + } else { + int i, handled = 0; + msg->l3h = &msg->l2h[4]; + for (i = 0; i < ARRAY_SIZE(mgcp_requests); ++i) + if (strncmp(mgcp_requests[i].name, (const char *) &msg->l2h[0], 4) == 0) { + handled = 1; + resp = mgcp_requests[i].handle_request(cfg, msg); + break; + } + if (!handled) { + LOGP(DMGCP, LOGL_NOTICE, "MSG with type: '%.4s' not handled\n", &msg->l2h[0]); + } + } + + return resp; +} + +/* string tokenizer for the poor */ +static int find_msg_pointers(struct msgb *msg, struct mgcp_msg_ptr *ptrs, int ptrs_length) +{ + int i, found = 0; + + int whitespace = 1; + for (i = 0; i < msgb_l3len(msg) && ptrs_length > 0; ++i) { + /* if we have a space we found an end */ + if (msg->l3h[i] == ' ' || msg->l3h[i] == '\r' || msg->l3h[i] == '\n') { + if (!whitespace) { + ++found; + whitespace = 1; + ptrs->length = i - ptrs->start - 1; + ++ptrs; + --ptrs_length; + } else { + /* skip any number of whitespace */ + } + + /* line end... stop */ + if (msg->l3h[i] == '\r' || msg->l3h[i] == '\n') + break; + } else if (msg->l3h[i] == '\r' || msg->l3h[i] == '\n') { + /* line end, be done */ + break; + } else if (whitespace) { + whitespace = 0; + ptrs->start = i; + } + } + + if (ptrs_length == 0) + return -1; + return found; +} + +static struct mgcp_endpoint *find_endpoint(struct mgcp_config *cfg, const char *mgcp) +{ + char *endptr = NULL; + unsigned int gw = INT_MAX; + + gw = strtoul(mgcp, &endptr, 16); + if (gw == 0 || gw >= cfg->number_endpoints || strcmp(endptr, "@mgw") != 0) { + LOGP(DMGCP, LOGL_ERROR, "Not able to find endpoint: '%s'\n", mgcp); + return NULL; + } + + return &cfg->endpoints[gw]; +} + +int mgcp_analyze_header(struct mgcp_config *cfg, struct msgb *msg, + struct mgcp_msg_ptr *ptr, int size, + const char **transaction_id, struct mgcp_endpoint **endp) +{ + int found; + + *transaction_id = "000000"; + + if (size < 3) { + LOGP(DMGCP, LOGL_ERROR, "Not enough space in ptr\n"); + return -1; + } + + found = find_msg_pointers(msg, ptr, size); + + if (found <= 3) { + LOGP(DMGCP, LOGL_ERROR, "Gateway: Not enough params. Found: %d\n", found); + return -1; + } + + /* + * replace the space with \0. the main method gurantess that + * we still have + 1 for null termination + */ + msg->l3h[ptr[3].start + ptr[3].length + 1] = '\0'; + msg->l3h[ptr[2].start + ptr[2].length + 1] = '\0'; + msg->l3h[ptr[1].start + ptr[1].length + 1] = '\0'; + msg->l3h[ptr[0].start + ptr[0].length + 1] = '\0'; + + if (strncmp("1.0", (const char *)&msg->l3h[ptr[3].start], 3) != 0 + || strncmp("MGCP", (const char *)&msg->l3h[ptr[2].start], 4) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Wrong MGCP version. Not handling: '%s' '%s'\n", + (const char *)&msg->l3h[ptr[3].start], + (const char *)&msg->l3h[ptr[2].start]); + return -1; + } + + *transaction_id = (const char *)&msg->l3h[ptr[0].start]; + if (endp) { + *endp = find_endpoint(cfg, (const char *)&msg->l3h[ptr[1].start]); + return *endp == NULL; + } + return 0; +} + +static int verify_call_id(const struct mgcp_endpoint *endp, + const char *callid) +{ + if (strcmp(endp->callid, callid) != 0) { + LOGP(DMGCP, LOGL_ERROR, "CallIDs does not match on 0x%x. '%s' != '%s'\n", + ENDPOINT_NUMBER(endp), endp->callid, callid); + return -1; + } + + return 0; +} + +static int verify_ci(const struct mgcp_endpoint *endp, + const char *ci) +{ + if (atoi(ci) != endp->ci) { + LOGP(DMGCP, LOGL_ERROR, "ConnectionIdentifiers do not match on 0x%x. %d != %s\n", + ENDPOINT_NUMBER(endp), endp->ci, ci); + return -1; + } + + return 0; +} + +static struct msgb *handle_audit_endpoint(struct mgcp_config *cfg, struct msgb *msg) +{ + struct mgcp_msg_ptr data_ptrs[6]; + int found, response; + const char *trans_id; + struct mgcp_endpoint *endp; + + found = mgcp_analyze_header(cfg, msg, data_ptrs, ARRAY_SIZE(data_ptrs), &trans_id, &endp); + if (found != 0) + response = 500; + else + response = 200; + + return create_response(response, "AUEP", trans_id); +} + +static int parse_conn_mode(const char* msg, int *conn_mode) +{ + int ret = 0; + if (strcmp(msg, "recvonly") == 0) + *conn_mode = MGCP_CONN_RECV_ONLY; + else if (strcmp(msg, "sendrecv") == 0) + *conn_mode = MGCP_CONN_RECV_SEND; + else { + LOGP(DMGCP, LOGL_ERROR, "Unknown connection mode: '%s'\n", msg); + ret = -1; + } + + return ret; +} + +static struct msgb *handle_create_con(struct mgcp_config *cfg, struct msgb *msg) +{ + struct mgcp_msg_ptr data_ptrs[6]; + int found, i, line_start; + const char *trans_id; + struct mgcp_endpoint *endp; + int error_code = 500; + int port; + + found = mgcp_analyze_header(cfg, msg, data_ptrs, ARRAY_SIZE(data_ptrs), &trans_id, &endp); + if (found != 0) + return create_response(500, "CRCX", trans_id); + + if (endp->ci != CI_UNUSED) { + if (cfg->force_realloc) { + LOGP(DMGCP, LOGL_NOTICE, "Endpoint 0x%x already allocated. Forcing realloc.\n", + ENDPOINT_NUMBER(endp)); + } else { + LOGP(DMGCP, LOGL_ERROR, "Endpoint is already used. 0x%x\n", + ENDPOINT_NUMBER(endp)); + return create_response(500, "CRCX", trans_id); + } + } + + /* parse CallID C: and LocalParameters L: */ + MSG_TOKENIZE_START + switch (msg->l3h[line_start]) { + case 'L': + endp->local_options = talloc_strdup(cfg->endpoints, + (const char *)&msg->l3h[line_start + 3]); + break; + case 'C': + endp->callid = talloc_strdup(cfg->endpoints, + (const char *)&msg->l3h[line_start + 3]); + break; + case 'M': + if (parse_conn_mode((const char *)&msg->l3h[line_start + 3], + &endp->conn_mode) != 0) { + error_code = 517; + goto error2; + } + break; + default: + LOGP(DMGCP, LOGL_NOTICE, "Unhandled option: '%c'/%d on 0x%x\n", + msg->l3h[line_start], msg->l3h[line_start], + ENDPOINT_NUMBER(endp)); + break; + } + MSG_TOKENIZE_END + + /* initialize */ + endp->net_rtp = endp->net_rtcp = endp->bts_rtp = endp->bts_rtcp = 0; + + /* set to zero until we get the info */ + memset(&endp->remote, 0, sizeof(endp->remote)); + + /* bind to the port now */ + port = rtp_calculate_port(ENDPOINT_NUMBER(endp), cfg->rtp_base_port); + if (cfg->early_bind) + endp->rtp_port = port; + else if (mgcp_bind_rtp_port(endp, port) != 0) + goto error2; + + /* assign a local call identifier or fail */ + endp->ci = generate_call_id(cfg); + if (endp->ci == CI_UNUSED) + goto error2; + + endp->bts_payload_type = cfg->audio_payload; + + /* policy CB */ + if (cfg->policy_cb) { + switch (cfg->policy_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_CRCX, trans_id)) { + case MGCP_POLICY_REJECT: + LOGP(DMGCP, LOGL_NOTICE, "CRCX rejected by policy on 0x%x\n", + ENDPOINT_NUMBER(endp)); + mgcp_free_endp(endp); + return create_response(500, "CRCX", trans_id); + break; + case MGCP_POLICY_DEFER: + /* stop processing */ + return NULL; + break; + case MGCP_POLICY_CONT: + /* just continue */ + break; + } + } + + LOGP(DMGCP, LOGL_NOTICE, "Creating endpoint on: 0x%x CI: %u port: %u\n", + ENDPOINT_NUMBER(endp), endp->ci, endp->rtp_port); + if (cfg->change_cb) + cfg->change_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_CRCX, endp->rtp_port); + + return create_response_with_sdp(endp, "CRCX", trans_id); +error: + LOGP(DMGCP, LOGL_ERROR, "Malformed line: %s on 0x%x with: line_start: %d %d\n", + hexdump(msg->l3h, msgb_l3len(msg)), + ENDPOINT_NUMBER(endp), line_start, i); + return create_response(error_code, "CRCX", trans_id); + +error2: + LOGP(DMGCP, LOGL_NOTICE, "Resource error on 0x%x\n", ENDPOINT_NUMBER(endp)); + return create_response(error_code, "CRCX", trans_id); +} + +static struct msgb *handle_modify_con(struct mgcp_config *cfg, struct msgb *msg) +{ + struct mgcp_msg_ptr data_ptrs[6]; + int found, i, line_start; + const char *trans_id; + struct mgcp_endpoint *endp; + int error_code = 500; + int silent = 0; + + found = mgcp_analyze_header(cfg, msg, data_ptrs, ARRAY_SIZE(data_ptrs), &trans_id, &endp); + if (found != 0) + return create_response(error_code, "MDCX", trans_id); + + if (endp->ci == CI_UNUSED) { + LOGP(DMGCP, LOGL_ERROR, "Endpoint is not holding a connection. 0x%x\n", ENDPOINT_NUMBER(endp)); + return create_response(error_code, "MDCX", trans_id); + } + + MSG_TOKENIZE_START + switch (msg->l3h[line_start]) { + case 'C': { + if (verify_call_id(endp, (const char *)&msg->l3h[line_start + 3]) != 0) + goto error3; + break; + } + case 'I': { + if (verify_ci(endp, (const char *)&msg->l3h[line_start + 3]) != 0) + goto error3; + break; + } + case 'L': + /* skip */ + break; + case 'M': + if (parse_conn_mode((const char *)&msg->l3h[line_start + 3], + &endp->conn_mode) != 0) { + error_code = 517; + goto error3; + } + break; + case 'Z': + silent = strcmp("noanswer", (const char *)&msg->l3h[line_start + 3]) == 0; + break; + case '\0': + /* SDP file begins */ + break; + case 'a': + case 'o': + case 's': + case 't': + case 'v': + /* skip these SDP attributes */ + break; + case 'm': { + int port; + int payload; + const char *param = (const char *)&msg->l3h[line_start]; + + if (sscanf(param, "m=audio %d RTP/AVP %d", &port, &payload) == 2) { + endp->net_rtp = htons(port); + endp->net_rtcp = htons(port + 1); + endp->net_payload_type = payload; + } + break; + } + case 'c': { + char ipv4[16]; + const char *param = (const char *)&msg->l3h[line_start]; + + if (sscanf(param, "c=IN IP4 %15s", ipv4) == 1) { + inet_aton(ipv4, &endp->remote); + } + break; + } + default: + LOGP(DMGCP, LOGL_NOTICE, "Unhandled option: '%c'/%d on 0x%x\n", + msg->l3h[line_start], msg->l3h[line_start], + ENDPOINT_NUMBER(endp)); + break; + } + MSG_TOKENIZE_END + + /* policy CB */ + if (cfg->policy_cb) { + switch (cfg->policy_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_MDCX, trans_id)) { + case MGCP_POLICY_REJECT: + LOGP(DMGCP, LOGL_NOTICE, "MDCX rejected by policy on 0x%x\n", + ENDPOINT_NUMBER(endp)); + if (silent) + goto out_silent; + return create_response(500, "MDCX", trans_id); + break; + case MGCP_POLICY_DEFER: + /* stop processing */ + return NULL; + break; + case MGCP_POLICY_CONT: + /* just continue */ + break; + } + } + + /* modify */ + LOGP(DMGCP, LOGL_NOTICE, "Modified endpoint on: 0x%x Server: %s:%u\n", + ENDPOINT_NUMBER(endp), inet_ntoa(endp->remote), ntohs(endp->net_rtp)); + if (cfg->change_cb) + cfg->change_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_MDCX, endp->rtp_port); + if (silent) + goto out_silent; + + return create_response_with_sdp(endp, "MDCX", trans_id); + +error: + LOGP(DMGCP, LOGL_ERROR, "Malformed line: %s on 0x%x with: line_start: %d %d %d\n", + hexdump(msg->l3h, msgb_l3len(msg)), + ENDPOINT_NUMBER(endp), line_start, i, msg->l3h[line_start]); + return create_response(error_code, "MDCX", trans_id); + +error3: + return create_response(error_code, "MDCX", trans_id); + + +out_silent: + return NULL; +} + +static struct msgb *handle_delete_con(struct mgcp_config *cfg, struct msgb *msg) +{ + struct mgcp_msg_ptr data_ptrs[6]; + int found, i, line_start; + const char *trans_id; + struct mgcp_endpoint *endp; + int error_code = 500; + int silent = 0; + + found = mgcp_analyze_header(cfg, msg, data_ptrs, ARRAY_SIZE(data_ptrs), &trans_id, &endp); + if (found != 0) + return create_response(error_code, "DLCX", trans_id); + + if (endp->ci == CI_UNUSED) { + LOGP(DMGCP, LOGL_ERROR, "Endpoint is not used. 0x%x\n", ENDPOINT_NUMBER(endp)); + return create_response(error_code, "DLCX", trans_id); + } + + MSG_TOKENIZE_START + switch (msg->l3h[line_start]) { + case 'C': { + if (verify_call_id(endp, (const char *)&msg->l3h[line_start + 3]) != 0) + goto error3; + break; + } + case 'I': { + if (verify_ci(endp, (const char *)&msg->l3h[line_start + 3]) != 0) + goto error3; + break; + case 'Z': + silent = strcmp("noanswer", (const char *)&msg->l3h[line_start + 3]) == 0; + break; + } + default: + LOGP(DMGCP, LOGL_NOTICE, "Unhandled option: '%c'/%d on 0x%x\n", + msg->l3h[line_start], msg->l3h[line_start], + ENDPOINT_NUMBER(endp)); + break; + } + MSG_TOKENIZE_END + + /* policy CB */ + if (cfg->policy_cb) { + switch (cfg->policy_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_DLCX, trans_id)) { + case MGCP_POLICY_REJECT: + LOGP(DMGCP, LOGL_NOTICE, "DLCX rejected by policy on 0x%x\n", + ENDPOINT_NUMBER(endp)); + if (silent) + goto out_silent; + return create_response(500, "DLCX", trans_id); + break; + case MGCP_POLICY_DEFER: + /* stop processing */ + return NULL; + break; + case MGCP_POLICY_CONT: + /* just continue */ + break; + } + } + + /* free the connection */ + LOGP(DMGCP, LOGL_NOTICE, "Deleted endpoint on: 0x%x Server: %s:%u\n", + ENDPOINT_NUMBER(endp), inet_ntoa(endp->remote), ntohs(endp->net_rtp)); + mgcp_free_endp(endp); + if (cfg->change_cb) + cfg->change_cb(cfg, ENDPOINT_NUMBER(endp), MGCP_ENDP_DLCX, endp->rtp_port); + + if (silent) + goto out_silent; + return create_response(250, "DLCX", trans_id); + +error: + LOGP(DMGCP, LOGL_ERROR, "Malformed line: %s on 0x%x with: line_start: %d %d\n", + hexdump(msg->l3h, msgb_l3len(msg)), + ENDPOINT_NUMBER(endp), line_start, i); + return create_response(error_code, "DLCX", trans_id); + +error3: + return create_response(error_code, "DLCX", trans_id); + +out_silent: + return NULL; +} + +static struct msgb *handle_rsip(struct mgcp_config *cfg, struct msgb *msg) +{ + if (cfg->reset_cb) + cfg->reset_cb(cfg); + return NULL; +} + +struct mgcp_config *mgcp_config_alloc(void) +{ + struct mgcp_config *cfg; + + cfg = talloc_zero(NULL, struct mgcp_config); + if (!cfg) { + LOGP(DMGCP, LOGL_FATAL, "Failed to allocate config.\n"); + return NULL; + } + + cfg->source_port = 2427; + cfg->source_addr = talloc_strdup(cfg, "0.0.0.0"); + cfg->audio_name = talloc_strdup(cfg, "GSM-EFR/8000"); + cfg->audio_payload = 97; + cfg->rtp_base_port = RTP_PORT_DEFAULT; + + return cfg; +} + +int mgcp_endpoints_allocate(struct mgcp_config *cfg) +{ + int i; + + /* Initialize all endpoints */ + cfg->endpoints = _talloc_zero_array(cfg, + sizeof(struct mgcp_endpoint), + cfg->number_endpoints, "endpoints"); + if (!cfg->endpoints) + return -1; + + for (i = 0; i < cfg->number_endpoints; ++i) { + cfg->endpoints[i].local_rtp.fd = -1; + cfg->endpoints[i].local_rtcp.fd = -1; + cfg->endpoints[i].ci = CI_UNUSED; + cfg->endpoints[i].cfg = cfg; + cfg->endpoints[i].net_payload_type = -1; + cfg->endpoints[i].bts_payload_type = -1; + } + + return 0; +} + +void mgcp_free_endp(struct mgcp_endpoint *endp) +{ + LOGP(DMGCP, LOGL_DEBUG, "Deleting endpoint on: 0x%x\n", ENDPOINT_NUMBER(endp)); + endp->ci= CI_UNUSED; + + if (endp->callid) { + talloc_free(endp->callid); + endp->callid = NULL; + } + + if (endp->local_options) { + talloc_free(endp->local_options); + endp->local_options = NULL; + } + + if (!endp->cfg->early_bind) { + bsc_unregister_fd(&endp->local_rtp); + bsc_unregister_fd(&endp->local_rtcp); + } + + endp->net_rtp = endp->net_rtcp = endp->bts_rtp = endp->bts_rtcp = 0; + endp->net_payload_type = endp->bts_payload_type = -1; + endp->in_bts = endp->in_remote = 0; + memset(&endp->remote, 0, sizeof(endp->remote)); + memset(&endp->bts, 0, sizeof(endp->bts)); +} diff --git a/src/mgcp_ss7.c b/src/mgcp_ss7.c new file mode 100644 index 0000000..b273c31 --- /dev/null +++ b/src/mgcp_ss7.c @@ -0,0 +1,939 @@ +/* Use the UniPorte library to allocate endpoints */ +/* + * (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 <mgcp_ss7.h> +#include <mgcp/mgcp.h> +#include <mgcp/mgcp_internal.h> + +#include <write_queue.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/select.h> +#include <laf0rge1/talloc.h> +#include <laf0rge1/timer.h> + +#include <vty/command.h> +#include <vty/vty.h> + +/* uniporte includes */ +#ifndef NO_UNIPORTE +#include <UniPorte.h> +#include <BusMastHostApi.h> +#include <MtnSa.h> +#include <SystemLayer.h> +#include <PredefMobs.h> +#endif + +#include <errno.h> +#include <limits.h> +#include <string.h> +#include <unistd.h> +#include <signal.h> +#include <netdb.h> + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif +#include <getopt.h> + +static struct debug_target *stderr_target; +static int payload = 126; +static int number_endpoints = 32; +static char *mgw_ip = "172.18.0.30"; +static int base_port = RTP_PORT_DEFAULT; +static char *local_ip = "172.18.0.20"; +static char *config_file = "mgcp_mgw.cfg"; +static int exit_on_failure = 0; +static int endp_dscp = 0; + +#define TO_MGW_PORT(no) (no-1) +#define FROM_MGW_PORT(no) (no+1) + +static struct mgcp_ss7 *s_ss7; + +struct mgcp_ss7_endpoint { + unsigned int port; + int block; +}; + +static void mgcp_ss7_endp_free(struct mgcp_ss7* ss7, int endp); +static void mgcp_ss7_do_exec(struct mgcp_ss7 *mgcp, u_int8_t type, u_int32_t port, u_int32_t param); +static void mgcp_mgw_vty_init(); + +static void check_exit(int status) +{ + if (exit_on_failure && status == 21) { + LOGP(DMGCP, LOGL_ERROR, "Failure detected with the MGW. Exiting.\n"); + exit(-1); + } +} + +#ifndef NO_UNIPORTE +static void Force_Poll( int milliseconds ) +{ + int timeout = 0; + unsigned long startTime; + + startTime = SysLyrGetTime(); + + /* Loop until the specified number of milliseconds + * have elapsed. + */ + do { + MtnSaPoll(); + SysLyrSleep( 20 ); + } while ((SysLyrGetTime()-startTime)<(unsigned long)milliseconds); + return; +} + +static char eventName[Event_TELEMETRY_DATA + 1][128] = { + { "Event_NOT_READY" }, + { "Event_READY" }, + { "Event_ANSWER" }, + { "Event_OUTGOING_CALL" }, + { "Event_ABORT" }, + { "Event_CONNECT" }, + { "Event_DISCONNECT" }, + { "Event_MANAGED_OBJECT_GET_COMPLETE" }, + { "Event_MANAGED_OBJECT_GET_AND_CLEAR_COMPLETE" }, + { "Event_MANAGED_OBJECT_SET_COMPLETE" }, + { "Event_MANAGED_OBJECT_TRAP" }, + { "Event_PREDEF_MOB_SET_COMPLETE" }, + { "Event_PREDEF_MOB_GET_COMPLETE" }, + { "Event_USER_MOB_DEFINE_COMPLETE" }, + { "Event_USER_MOB_SET_COMPLETE" }, + { "Event_USER_MOB_GET_COMPLETE" }, + { "Event_RECEIVE_DATA" }, + { "Event_SEND_COMPLETE" }, + { "Event_TDM_CONNECT_COMPLETE" }, + { "Event_LOG" }, + { "Event_DEVICE_IN_CONTACT" }, + { "Event_DEVICE_MANAGED" }, + { "Event_DEVICE_OUT_OF_CONTACT" }, + { "Event_TELEMETRY_DATA" } }; + +static char stateName[PortState_END_OF_ENUM][128] = { + { "PortState_IDLE" }, + { "PortState_SIGNALING" }, + { "PortState_INITIATING" }, + { "PortState_LINK" }, + { "PortState_TRAINING" }, + { "PortState_EC_NEGOTIATING" }, + { "PortState_DATA" }, + { "PortState_RESYNCING" }, + { "PortState_FAX" }, + { "PortState_COMMAND_ESCAPE" }, + { "PortState_TERMINATING" }, + { "PortState_VOICE" }, + { "PortState_PORT_RESET" }, + { "PortState_DSP_RESET" }, + { "PortState_ALLOCATED" }, + { "PortState_OUT_OF_SERVICE" }, + { "PortState_RECONFIGURE" }, + { "PortState_ON_HOLD" } }; +static int uniporte_events(unsigned long port, EventTypeT event, + void *event_data, unsigned long event_data_length ) { + char text[128]; + ManObjectInfoPtr info; + DataReceiveInfoPtr dataInfo; + int i; + ToneDetectionPtr tones; + + + /* Don't print output when we receive data or complete + * sending data. That would be too verbose. + */ + if (event==Event_DEVICE_MANAGED) { + MtnSaSetManObject(0, ChannelType_ETHERNET, ManObj_C_MOE_COMM_LOSS_RESET_DELAY , + 10, 0); + } + else if (event==Event_MANAGED_OBJECT_TRAP ) { + info = (ManObjectInfoPtr)event_data; + if (info->trapId == Trap_PORT_STATE_CHANGE) { + sprintf(text, "Port #%ld, Change to state %s", port, stateName[info->value]); + puts(text); + + /* update the mgcp state */ + int mgcp_endp = FROM_MGW_PORT(port); + if (s_ss7->mgw_end[mgcp_endp].block != 1) + fprintf(stderr, "State change on a non blocked port. ERROR.\n"); + s_ss7->mgw_end[mgcp_endp].block = 0; + } + } + else if ( event == Event_MANAGED_OBJECT_SET_COMPLETE ) { + info = (ManObjectInfoPtr)event_data; + + sprintf(text, "Object %d value %d status %d", info->object, info->value, + info->status ); + puts(text); + check_exit(info->status); + } + else if ( ( event == Event_USER_MOB_SET_COMPLETE ) || + ( event == Event_USER_MOB_DEFINE_COMPLETE ) ) + { + info = (ManObjectInfoPtr)event_data; + + sprintf( text, "Mob ID %d status %d", info->MOBId, info->status ); + puts(text); + check_exit(info->status); + } + else if ( event == Event_USER_MOB_GET_COMPLETE ) + { + info = (ManObjectInfoPtr)event_data; + + sprintf( text, "Mob ID %d status %d", info->MOBId, info->status ); + puts(text); + check_exit(info->status); + } + else if (event == Event_CONNECT) + { + sprintf(text, "Port %d connected",port ); + } + else if (event == Event_PREDEF_MOB_GET_COMPLETE) + { + info = (ManObjectInfoPtr)event_data; + + sprintf(text, "Mob ID %d status %d", info->MOBId, info->status ); + puts(text); + check_exit(info->status); + } + + return( 0 ); +} + +static int initialize_uniporte(struct mgcp_ss7 *mgcp) +{ + ProfileT profile; + unsigned long mgw_address; + int rc; + + LOGP(DMGCP, LOGL_NOTICE, "Initializing MGW on %s\n", mgcp->cfg->bts_ip); + + MtnSaSetEthernetOnly(); + rc = MtnSaStartup(uniporte_events); + if (rc != 0) + LOGP(DMGCP, LOGL_ERROR, "Failed to startup the MGW.\n"); + SysEthGetHostAddress(mgcp->cfg->bts_ip, &mgw_address); + rc = MtnSaRegisterEthernetDevice(mgw_address, 0); + if (rc != 0) + LOGP(DMGCP, LOGL_ERROR, "Failed to register ethernet.\n"); + Force_Poll(2000); + MtnSaTakeOverDevice(0); + Force_Poll(2000); + MtnSaSetReceiveTraps(1); + MtnSaSetTransparent(); + + /* change the voice profile to AMR */ + MtnSaGetProfile(ProfileType_VOICE, 0, &profile); + profile.countryCode = CountryCode_INTERNAT_ALAW; + MtnSaSetProfile(ProfileType_VOICE, 0, &profile); + + if (MtnSaGetPortCount() == 0) + return -1; + + return 0; +} + + +static void* start_uniporte(void *_ss7) { + struct llist_head blocked; + struct mgcp_ss7_cmd *cmd, *tmp; + struct mgcp_ss7 *ss7 = _ss7; + + s_ss7 = ss7; + + if (initialize_uniporte(ss7) != 0) { + fprintf(stderr, "Failed to create Uniporte.\n"); + exit(-1); + return 0; + } + + fprintf(stderr, "Created the MGCP processing thread.\n"); + INIT_LLIST_HEAD(&blocked); + for (;;) { + thread_swap(ss7->cmd_queue); +start_over: + /* handle items that are currently blocked */ + llist_for_each_entry_safe(cmd, tmp, &blocked, entry) { + if (ss7->mgw_end[cmd->port].block) + continue; + + mgcp_ss7_do_exec(ss7, cmd->type, cmd->port, cmd->param); + llist_del(&cmd->entry); + free(cmd); + + /* We might have unblocked something, make sure we operate in order */ + MtnSaPoll(); + goto start_over; + } + + llist_for_each_entry_safe(cmd, tmp, ss7->cmd_queue->main_head, entry) { + if (ss7->mgw_end[cmd->port].block) { + llist_del(&cmd->entry); + llist_add_tail(&cmd->entry, &blocked); + continue; + } + + mgcp_ss7_do_exec(ss7, cmd->type, cmd->port, cmd->param); + llist_del(&cmd->entry); + free(cmd); + + /* We might have unblocked something, make sure we operate in order */ + MtnSaPoll(); + goto start_over; + } + + Force_Poll(20); + } + + return 0; +} +#endif + +static void update_mute_status(int mgw_port, int conn_mode) +{ +#ifndef NO_UNIPORTE + if (conn_mode == MGCP_CONN_NONE) { + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_UPSTREAM_MUTE, 1, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_DOWNSTREAM_MUTE, 1, 0); + } else if (conn_mode == MGCP_CONN_RECV_ONLY) { + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_UPSTREAM_MUTE, 1, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_DOWNSTREAM_MUTE, 0, 0); + } else if (conn_mode == MGCP_CONN_SEND_ONLY) { + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_UPSTREAM_MUTE, 0, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_DOWNSTREAM_MUTE, 1, 0); + } else if (conn_mode == MGCP_CONN_RECV_SEND) { + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_UPSTREAM_MUTE, 0, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, ManObj_C_VOICE_DOWNSTREAM_MUTE, 0, 0); + } else { + LOGP(DMGCP, LOGL_ERROR, "Unhandled conn mode: %d\n", conn_mode); + } +#endif +} + +#ifndef NO_UNIPORTE +static void allocate_endp(struct mgcp_ss7 *ss7, int endp_no) +{ + int mgw_port; + unsigned long mgw_address, loc_address; + struct mgcp_ss7_endpoint *mgw_endp = &ss7->mgw_end[endp_no]; + struct mgcp_endpoint *mg_endp = &ss7->cfg->endpoints[endp_no]; + + mgw_port = TO_MGW_PORT(endp_no); + mgw_endp->port = MtnSaAllocate(mgw_port); + if (mgw_endp->port == UINT_MAX) { + fprintf(stderr, "Failed to allocate the port: %d\n", endp_no); + return; + } + + /* Select AMR 5.9, Payload 98, no CRC, hardcoded */ + MtnSaApplyProfile(mgw_port, ProfileType_VOICE, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, + ManObj_G_DATA_PATH, DataPathT_ETHERNET, 0 ); + MtnSaSetManObject(mgw_port, ChannelType_PORT, + ManObj_C_VOICE_RTP_TELEPHONE_EVENT_PT_TX, ss7->cfg->audio_payload, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, + ManObj_G_RTP_AMR_PAYLOAD_TYPE, ss7->cfg->audio_payload, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, + ManObj_G_RTP_AMR_PAYLOAD_FORMAT, RtpAmrPayloadFormat_OCTET_ALIGNED, 0); + MtnSaSetManObject(mgw_port, ChannelType_PORT, + ManObj_G_VOICE_ENCODING, Voice_Encoding_AMR_5_90, 0); + + update_mute_status(mgw_port, mg_endp->conn_mode); + + /* set the addresses */ + SysEthGetHostAddress(ss7->cfg->bts_ip, &mgw_address); + SysEthGetHostAddress(ss7->cfg->local_ip, &loc_address); + MtnSaSetVoIpAddresses(mgw_port, + mgw_address, mg_endp->rtp_port, + loc_address, mg_endp->rtp_port); + MtnSaConnect(mgw_port, mgw_port); + mgw_endp->block = 1; +} +#endif + +static void mgcp_ss7_do_exec(struct mgcp_ss7 *mgcp, u_int8_t type, u_int32_t port, u_int32_t param) +{ +#ifndef NO_UNIPORTE + struct mgcp_ss7_endpoint *mgw_endp = &mgcp->mgw_end[port]; + int rc; + + switch (type) { + case MGCP_SS7_MUTE_STATUS: + if (mgw_endp->port != UINT_MAX) + update_mute_status(TO_MGW_PORT(port), param); + break; + case MGCP_SS7_DELETE: + if (mgw_endp->port != UINT_MAX) { + rc = MtnSaDisconnect(mgw_endp->port); + if (rc != 0) + fprintf(stderr, "Failed to disconnect port: %u\n", mgw_endp->port); + rc = MtnSaDeallocate(mgw_endp->port); + if (rc != 0) + fprintf(stderr, "Failed to deallocate port: %u\n", mgw_endp->port); + mgw_endp->port = UINT_MAX; + mgw_endp->block = 1; + } + break; + case MGCP_SS7_ALLOCATE: + allocate_endp(mgcp, port); + break; + case MGCP_SS7_SHUTDOWN: + MtnSaShutdown(); + break; + } +#endif +} + +void mgcp_ss7_exec(struct mgcp_ss7 *mgcp, u_int8_t type, u_int32_t port, u_int32_t param) +{ + struct mgcp_ss7_cmd *cmd = malloc(sizeof(*cmd)); + memset(cmd, 0, sizeof(*cmd)); + cmd->type = type; + cmd->port = port; + cmd->param = param; + + thread_safe_add(mgcp->cmd_queue, &cmd->entry); +} + +static int ss7_allocate_endpoint(struct mgcp_ss7 *ss7, int endp_no, struct mgcp_ss7_endpoint *endp) +{ + struct mgcp_endpoint *mg_endp; + + mg_endp = &ss7->cfg->endpoints[endp_no]; + mg_endp->bts_rtp = htons(mg_endp->rtp_port); + mg_endp->bts_rtcp = htons(mg_endp->rtp_port + 1); + mg_endp->bts = ss7->cfg->bts_in; + + mgcp_ss7_exec(ss7, MGCP_SS7_ALLOCATE, endp_no, 0); + return MGCP_POLICY_CONT; +} + +static int ss7_modify_endpoint(struct mgcp_ss7 *ss7, int endp_no, struct mgcp_ss7_endpoint *endp) +{ + struct mgcp_endpoint *mg_endp; + + mg_endp = &ss7->cfg->endpoints[endp_no]; + mgcp_ss7_exec(ss7, MGCP_SS7_MUTE_STATUS, endp_no, mg_endp->conn_mode); + + /* + * this is a bad assumption of the network. We assume + * to have the remote addr now. + */ + mgcp_send_dummy(mg_endp); + + /* update the remote end */ + return MGCP_POLICY_CONT; +} + +static int ss7_delete_endpoint(struct mgcp_ss7 *ss7, int endp_no, struct mgcp_ss7_endpoint *endp) +{ + mgcp_ss7_endp_free(ss7, endp_no); + return MGCP_POLICY_CONT; +} + +static int mgcp_ss7_policy(struct mgcp_config *cfg, int endp_no, int state, const char *trans) +{ + int rc; + struct mgcp_ss7 *ss7; + struct mgcp_ss7_endpoint *endp; + + ss7 = (struct mgcp_ss7 *) cfg->data; + endp = &ss7->mgw_end[endp_no]; + + /* TODO: Make it async and wait for the port to be connected */ + rc = MGCP_POLICY_REJECT; + switch (state) { + case MGCP_ENDP_CRCX: + rc = ss7_allocate_endpoint(ss7, endp_no, endp); + break; + case MGCP_ENDP_MDCX: + rc = ss7_modify_endpoint(ss7, endp_no, endp); + break; + case MGCP_ENDP_DLCX: + rc = ss7_delete_endpoint(ss7, endp_no, endp); + break; + } + + return rc; +} + +static void enqueue_msg(struct write_queue *queue, struct sockaddr_in *addr, struct msgb *msg) +{ + struct sockaddr_in *data; + + data = (struct sockaddr_in *) msgb_push(msg, sizeof(*data)); + *data = *addr; + if (write_queue_enqueue(queue, msg) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to queue the message.\n"); + msgb_free(msg); + } +} + +static int write_call_agent(struct bsc_fd *bfd, struct msgb *msg) +{ + int rc; + struct sockaddr_in *addr; + + addr = (struct sockaddr_in *) msg->data; + rc = sendto(bfd->fd, msg->l2h, msgb_l2len(msg), 0, + (struct sockaddr *) addr, sizeof(*addr)); + + if (rc != msgb_l2len(msg)) + LOGP(DMGCP, LOGL_ERROR, "Failed to write MGCP message: rc: %d errno: %d\n", rc, errno); + + return rc; +} + + +static int read_call_agent(struct bsc_fd *fd) +{ + struct sockaddr_in addr; + socklen_t slen = sizeof(addr); + struct msgb *resp; + struct mgcp_ss7 *cfg; + struct write_queue *queue; + + cfg = (struct mgcp_ss7 *) fd->data; + queue = container_of(fd, struct write_queue, bfd); + + /* read one less so we can use it as a \0 */ + int rc = recvfrom(fd->fd, cfg->mgcp_msg->data, cfg->mgcp_msg->data_len - 1, 0, + (struct sockaddr *) &addr, &slen); + + if (rc < 0) { + perror("Gateway failed to read"); + return -1; + } else if (slen > sizeof(addr)) { + fprintf(stderr, "Gateway received message from outerspace: %d %d\n", + slen, sizeof(addr)); + return -1; + } + + /* handle message now */ + cfg->mgcp_msg->l2h = msgb_put(cfg->mgcp_msg, rc); + resp = mgcp_handle_message(cfg->cfg, cfg->mgcp_msg); + msgb_reset(cfg->mgcp_msg); + + if (resp) + enqueue_msg(queue, &addr, resp); + return 0; +} + +static int create_socket(struct mgcp_ss7 *cfg) +{ + int on; + struct sockaddr_in addr; + struct bsc_fd *bfd; + + bfd = &cfg->mgcp_fd.bfd; + + cfg->mgcp_fd.read_cb = read_call_agent; + cfg->mgcp_fd.write_cb = write_call_agent; + bfd->when = BSC_FD_READ; + bfd->fd = socket(AF_INET, SOCK_DGRAM, 0); + if (bfd->fd < 0) { + perror("Gateway failed to listen"); + return -1; + } + + on = 1; + setsockopt(bfd->fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(cfg->cfg->source_port); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(bfd->fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("Gateway failed to bind"); + close(bfd->fd); + return -1; + } + + bfd->data = cfg; + cfg->mgcp_msg = msgb_alloc(4096, "mgcp-msg"); + if (!cfg->mgcp_msg) { + fprintf(stderr, "Gateway memory error.\n"); + close(bfd->fd); + return -1; + } + talloc_steal(cfg, cfg->mgcp_msg); + + + if (bsc_register_fd(bfd) != 0) { + DEBUGP(DMGCP, "Failed to register the fd\n"); + close(bfd->fd); + return -1; + } + + return 0; +} + +static void mgcp_ss7_endp_free(struct mgcp_ss7 *ss7, int endp) +{ + mgcp_ss7_exec(ss7, MGCP_SS7_DELETE, endp, 0); +} + +static int reset_cb(struct mgcp_config *cfg) +{ + mgcp_ss7_reset((struct mgcp_ss7 *) cfg->data); + return 0; +} + +struct mgcp_ss7 *mgcp_ss7_init(int endpoints, const char *local_ip, const char *mgw_ip, int base_port, int payload) +{ + int i; + struct mgcp_ss7 *conf = talloc_zero(NULL, struct mgcp_ss7); + if (!conf) + return NULL; + + write_queue_init(&conf->mgcp_fd, 30); + conf->cfg = mgcp_config_alloc(); + if (!conf->cfg) { + LOGP(DMGCP, LOGL_ERROR, "Failed to allocate memory.\n"); + talloc_free(conf); + return NULL; + } + + /* take over the ownership */ + talloc_steal(conf, conf->cfg); + conf->cfg->number_endpoints = endpoints; + conf->cfg->local_ip = talloc_strdup(conf->cfg, local_ip); + conf->cfg->bts_ip = talloc_strdup(conf->cfg, mgw_ip); + inet_aton(conf->cfg->bts_ip, &conf->cfg->bts_in); + talloc_free(conf->cfg->audio_name); + conf->cfg->audio_name = talloc_strdup(conf->cfg, "AMR/8000"); + conf->cfg->audio_payload = payload; + conf->cfg->rtp_base_port = base_port; + conf->cfg->policy_cb = mgcp_ss7_policy; + conf->cfg->reset_cb = reset_cb; + conf->cfg->data = conf; + conf->cfg->endp_dscp = endp_dscp; + + /* do not attempt to allocate call ids */ + conf->cfg->early_bind = 1; + + if (mgcp_endpoints_allocate(conf->cfg) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to allocate endpoints: %d\n", endpoints); + talloc_free(conf); + return NULL; + } + + if (create_socket(conf) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to create socket.\n"); + talloc_free(conf); + return NULL; + } + + conf->mgw_end = _talloc_zero_array(conf, sizeof(struct mgcp_ss7_endpoint), + conf->cfg->number_endpoints, "mgw endpoints"); + if (!conf->mgw_end) { + LOGP(DMGCP, LOGL_ERROR, "Failed to allocate MGW endpoint array.\n"); + talloc_free(conf); + return NULL; + } + + for (i = 0; i < conf->cfg->number_endpoints; ++i) { + struct mgcp_endpoint *endp; + int rtp_port; + + /* initialize the MGW part */ + conf->mgw_end[i].port = UINT_MAX; + + /* allocate the ports */ + endp = &conf->cfg->endpoints[i]; + rtp_port = rtp_calculate_port(ENDPOINT_NUMBER(endp), conf->cfg->rtp_base_port); + if (mgcp_bind_rtp_port(endp, rtp_port) != 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to bind: %d\n", rtp_port); + mgcp_ss7_free(conf); + return NULL; + } + } + + conf->cmd_queue = thread_notifier_alloc(); + if (!conf->cmd_queue) { + LOGP(DMGCP, LOGL_ERROR, "Failed to allocate the command queue.\n"); + talloc_free(conf); + return NULL; + } + +#ifndef NO_UNIPORTE + conf->cmd_queue->no_write = 1; + pthread_create(&conf->thread, NULL, start_uniporte, conf); +#endif + + return conf; +} + +void mgcp_ss7_free(struct mgcp_ss7 *mgcp) +{ + /* close everything */ + mgcp_ss7_reset(mgcp); + + mgcp_ss7_exec(mgcp, MGCP_SS7_SHUTDOWN, 0, 0); + + close(mgcp->mgcp_fd.bfd.fd); + bsc_unregister_fd(&mgcp->mgcp_fd.bfd); + bsc_del_timer(&mgcp->poll_timer); + talloc_free(mgcp); +} + +void mgcp_ss7_reset(struct mgcp_ss7 *mgcp) +{ + int i; + + if (!mgcp) + return; + + LOGP(DMGCP, LOGL_INFO, "Resetting all endpoints.\n"); + + /* free UniPorted and MGCP data */ + for (i = 0; i < mgcp->cfg->number_endpoints; ++i) { + mgcp_ss7_endp_free(mgcp, i); + mgcp_free_endp(&mgcp->cfg->endpoints[i]); + } +} + +static void print_help() +{ + printf(" Some useful help...\n"); + printf(" -h This help text.\n"); + printf(" -c --config=CFG. The configuration file.\n"); + printf(" -e --exit-on-failure. Exit the app on MGW failure.\n"); +} + +static void print_usage() +{ + printf("Usage: mgcp_mgw\n"); +} + + +static void handle_options(int argc, char **argv) +{ + while (1) { + int option_index = 0, c; + static struct option long_options[] = { + {"help", 0, 0, 'h'}, + {"config", 1, 0, 'c'}, + {"exit", 0, 0, 'e'}, + {0, 0, 0, 0}, + }; + + c = getopt_long(argc, argv, "hc:e", + long_options, &option_index); + if (c == -1) + break; + + switch (c) { + case 'h': + print_usage(); + print_help(); + exit(0); + case 'c': + config_file = optarg; + break; + case 'e': + exit_on_failure = 1; + break; + default: + fprintf(stderr, "Unknown option.\n"); + break; + } + } +} + + +int main(int argc, char **argv) +{ + struct mgcp_ss7 *mgcp; + debug_init(); + + stderr_target = debug_target_create_stderr(); + debug_add_target(stderr_target); + + /* enable filters */ + debug_set_all_filter(stderr_target, 1); + debug_set_category_filter(stderr_target, DINP, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DSCCP, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DMSC, 1, LOGL_INFO); + debug_set_category_filter(stderr_target, DMGCP, 1, LOGL_INFO); + debug_set_print_timestamp(stderr_target, 1); + debug_set_use_color(stderr_target, 0); + + handle_options(argc, argv); + + signal(SIGPIPE, SIG_IGN); + + mgcp_mgw_vty_init(); + if (vty_read_config_file(config_file) < 0) { + fprintf(stderr, "Failed to parse the config file: '%s'\n", config_file); + return -1; + } + + printf("Creating MGCP MGW with endpoints: %d ip: %s mgw: %s rtp-base: %d payload: %d\n", + number_endpoints, local_ip, mgw_ip, base_port, payload); + + mgcp = mgcp_ss7_init(number_endpoints, local_ip, mgw_ip, base_port, payload); + if (!mgcp) { + fprintf(stderr, "Failed to create MGCP\n"); + exit(-1); + } + while (1) { + bsc_select_main(0); + } + return 0; +} + +/* VTY code */ +struct cmd_node mgcp_node = { + MGCP_NODE, + "%s(mgcp)#", + 1, +}; + +DEFUN(cfg_mgcp, + cfg_mgcp_cmd, + "mgcp", + "Configure the MGCP") +{ + vty->node = MGCP_NODE; + return CMD_SUCCESS; +} + +DEFUN(cfg_mgcp_local_ip, + cfg_mgcp_local_ip_cmd, + "local ip IP", + "Set the IP to be used in SDP records") +{ + struct hostent *hosts; + struct in_addr *addr; + + hosts = gethostbyname(argv[0]); + if (!hosts || hosts->h_length < 1 || hosts->h_addrtype != AF_INET) { + vty_out(vty, "Failed to resolve '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + addr = (struct in_addr *) hosts->h_addr_list[0]; + local_ip = talloc_strdup(NULL, inet_ntoa(*addr)); + return CMD_SUCCESS; +} + +DEFUN(cfg_mgcp_mgw_ip, + cfg_mgcp_mgw_ip_cmd, + "mgw ip IP", + "Set the IP of the MGW for RTP forwarding") +{ + struct hostent *hosts; + struct in_addr *addr; + + hosts = gethostbyname(argv[0]); + if (!hosts || hosts->h_length < 1 || hosts->h_addrtype != AF_INET) { + vty_out(vty, "Failed to resolve '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + addr = (struct in_addr *) hosts->h_addr_list[0]; + mgw_ip = talloc_strdup(NULL, inet_ntoa(*addr)); + return CMD_SUCCESS; +} + +DEFUN(cfg_mgcp_rtp_base_port, + cfg_mgcp_rtp_base_port_cmd, + "rtp base <0-65534>", + "Base port to use") +{ + unsigned int port = atoi(argv[0]); + if (port > 65534) { + vty_out(vty, "%% wrong base port '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + base_port = port; + return CMD_SUCCESS; +} + +DEFUN(cfg_mgcp_rtp_ip_dscp, + cfg_mgcp_rtp_ip_dscp_cmd, + "rtp ip-dscp <0-255>", + "Set the IP_TOS socket attribute on the RTP/RTCP sockets.\n" "The TOS value.") +{ + int dscp = atoi(argv[0]); + endp_dscp = dscp; + return CMD_SUCCESS; +} + +ALIAS_DEPRECATED(cfg_mgcp_rtp_ip_dscp, cfg_mgcp_rtp_ip_tos_cmd, + "rtp ip-tos <0-255>", + "Set the IP_TOS socket attribute on the RTP/RTCP sockets.\n" "The TOS value.") + + +DEFUN(cfg_mgcp_sdp_payload_number, + cfg_mgcp_sdp_payload_number_cmd, + "sdp audio payload number <1-255>", + "Set the audio codec to use") +{ + unsigned int new_payload = atoi(argv[0]); + if (new_payload > 255) { + vty_out(vty, "%% wrong payload number '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + payload = new_payload; + return CMD_SUCCESS; +} + +DEFUN(cfg_mgcp_number_endp, + cfg_mgcp_number_endp_cmd, + "number endpoints <0-65534>", + "The number of endpoints to allocate. This is not dynamic.") +{ + /* + 1 as we start counting at one */ + number_endpoints = atoi(argv[0]) + 1; + return CMD_SUCCESS; +} + +static int config_write_mgcp() +{ + return CMD_SUCCESS; +} + +static void mgcp_mgw_vty_init(void) +{ + cmd_init(1); + vty_init(); + + install_element(CONFIG_NODE, &cfg_mgcp_cmd); + install_node(&mgcp_node, config_write_mgcp); + install_default(MGCP_NODE); + install_element(MGCP_NODE, &cfg_mgcp_local_ip_cmd); + install_element(MGCP_NODE, &cfg_mgcp_mgw_ip_cmd); + install_element(MGCP_NODE, &cfg_mgcp_rtp_base_port_cmd); + install_element(MGCP_NODE, &cfg_mgcp_rtp_ip_tos_cmd); + install_element(MGCP_NODE, &cfg_mgcp_rtp_ip_dscp_cmd); + install_element(MGCP_NODE, &cfg_mgcp_sdp_payload_number_cmd); + install_element(MGCP_NODE, &cfg_mgcp_number_endp_cmd); +} + +void subscr_put() {} +void vty_event() {} diff --git a/src/msc_conn.c b/src/msc_conn.c new file mode 100644 index 0000000..1e3357d --- /dev/null +++ b/src/msc_conn.c @@ -0,0 +1,594 @@ +/* MSC related stuff... */ +/* + * (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 <bsc_data.h> +#include <bss_patch.h> +#include <bssap_sccp.h> +#include <ipaccess.h> +#include <mtp_data.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/tlv.h> + +#include <arpa/inet.h> +#include <sys/socket.h> +#include <netinet/tcp.h> + +#include <fcntl.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> + +#define RECONNECT_TIME 10, 0 +#define NAT_MUX 0xfc + +static void msc_send_id_response(struct bsc_data *bsc); +static void msc_send(struct bsc_data *bsc, struct msgb *msg, int proto); + +void mtp_link_slta_recv(struct mtp_link *link) +{ + struct msgb *msg; + unsigned int sls; + + while (!llist_empty(&link->pending_msgs)) { + msg = msgb_dequeue(&link->pending_msgs); + sls = (unsigned int) msg->l3h; + + if (mtp_link_submit_sccp_data(link, sls, msg->l2h, msgb_l2len(msg)) != 0) + LOGP(DMSC, LOGL_ERROR, "Could not forward SCCP message.\n"); + + msgb_free(msg); + } +} + +void msc_clear_queue(struct bsc_data *data) +{ + struct msgb *msg; + + LOGP(DMSC, LOGL_NOTICE, "Clearing the MSC to BSC queue.\n"); + while (!llist_empty(&data->link.the_link->pending_msgs)) { + msg = msgb_dequeue(&data->link.the_link->pending_msgs); + msgb_free(msg); + } +} + +static void close_msc(struct bsc_data *bsc) +{ + struct bsc_fd *bfd = &bsc->msc_connection.bfd; + + close(bfd->fd); + bsc_unregister_fd(bfd); + bfd->fd = -1; + release_bsc_resources(bsc); + bsc_del_timer(&bsc->ping_timeout); + bsc_del_timer(&bsc->pong_timeout); +} + +static void msc_connect_timeout(void *_bsc_data) +{ + struct bsc_data *bsc_data = _bsc_data; + + LOGP(DMSC, LOGL_ERROR, "Timeout on the MSC connection.\n"); + close_msc(bsc_data); +} + +static void msc_pong_timeout(void *_bsc_data) +{ + struct bsc_data *bsc_data = _bsc_data; + LOGP(DMSC, LOGL_ERROR, "MSC didn't respond to ping. Closing.\n"); + close_msc(bsc_data); +} + +static void send_ping(struct bsc_data *bsc) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "ping"); + if (!msg) { + LOGP(DMSC, LOGL_ERROR, "Failed to create PING.\n"); + return; + } + + msg->l2h = msgb_put(msg, 1); + msg->l2h[0] = IPAC_MSGT_PING; + + msc_send(bsc, msg, IPAC_PROTO_IPACCESS); +} + +static void msc_ping_timeout(void *_bsc_data) +{ + struct bsc_data *bsc_data = _bsc_data; + + if (bsc_data->ping_time < 0) + return; + + send_ping(bsc_data); + + /* send another ping in 20 seconds */ + bsc_schedule_timer(&bsc_data->ping_timeout, bsc_data->ping_time, 0); + + /* also start a pong timer */ + bsc_schedule_timer(&bsc_data->pong_timeout, bsc_data->pong_time, 0); +} + +/* + * callback with IP access data + */ +static int ipaccess_a_fd_cb(struct bsc_fd *bfd) +{ + int error; + struct ipaccess_head *hh; + struct mtp_link *link; + struct bsc_data *bsc; + struct msgb *msg; + + msg = ipaccess_read_msg(bfd, &error); + + bsc = (struct bsc_data *) bfd->data; + + if (!msg) { + if (error == 0) + fprintf(stderr, "The connection to the MSC was lost, exiting\n"); + else + fprintf(stderr, "Error in the IPA stream.\n"); + + close_msc(bsc); + return -1; + } + + LOGP(DMSC, LOGL_DEBUG, "From MSC: %s proto: %d\n", hexdump(msg->data, msg->len), msg->l2h[0]); + + /* handle base message handling */ + hh = (struct ipaccess_head *) msg->data; + ipaccess_rcvmsg_base(msg, bfd); + + link = bsc->link.the_link; + + /* initialize the networking. This includes sending a GSM08.08 message */ + if (hh->proto == IPAC_PROTO_IPACCESS) { + if (bsc->first_contact) { + LOGP(DMSC, LOGL_NOTICE, "Connected to MSC. Sending reset.\n"); + bsc_del_timer(&bsc->msc_timeout); + bsc->first_contact = 0; + bsc->closing = 0; + msc_send_reset(bsc); + } + if (msg->l2h[0] == IPAC_MSGT_ID_GET && bsc->token) { + msc_send_id_response(bsc); + } else if (msg->l2h[0] == IPAC_MSGT_PONG) { + bsc_del_timer(&bsc->pong_timeout); + } + } else if (hh->proto == IPAC_PROTO_SCCP) { + struct sccp_parse_result result; + int rc; + rc = bss_patch_filter_msg(msg, &result); + + if (rc == BSS_FILTER_RESET_ACK) { + LOGP(DMSC, LOGL_NOTICE, "Filtering reset ack from the MSC\n"); + } else if (rc == BSS_FILTER_RLSD) { + LOGP(DMSC, LOGL_DEBUG, "Filtering RLSD from the MSC\n"); + update_con_state(rc, &result, msg, 1, 0); + } else if (rc == BSS_FILTER_RLC) { + /* if we receive this we have forwarded a RLSD to the network */ + LOGP(DMSC, LOGL_ERROR, "RLC from the network. BAD!\n"); + } else if (rc == BSS_FILTER_CLEAR_COMPL) { + LOGP(DMSC, LOGL_ERROR, "Clear Complete from the network.\n"); + } else if (link->sccp_up) { + unsigned int sls; + + update_con_state(rc, &result, msg, 1, 0); + sls = sls_for_src_ref(result.destination_local_reference); + + /* patch a possible PC */ + bss_rewrite_header_to_bsc(msg, link->opc, link->dpc); + + /* we can not forward it right now */ + if (link->sltm_pending) { + LOGP(DMSC, LOGL_NOTICE, "Queueing msg for pending SLTM.\n"); + msg->l3h = (u_int8_t *) sls; + msgb_enqueue(&link->pending_msgs, msg); + return 0; + } + + if (mtp_link_submit_sccp_data(link, sls, msg->l2h, msgb_l2len(msg)) != 0) + LOGP(DMSC, LOGL_ERROR, "Could not forward SCCP message.\n"); + } + } else if (hh->proto == NAT_MUX) { + mgcp_forward(bsc, msg->l2h, msgb_l2len(msg)); + } else { + LOGP(DMSC, LOGL_ERROR, "Unknown IPA proto 0x%x\n", hh->proto); + } + + msgb_free(msg); + return 0; +} + +static int ipaccess_write_cb(struct bsc_fd *fd, struct msgb *msg) +{ + int rc; + + LOGP(DMSC, LOGL_DEBUG, "Sending to MSC: %s\n", hexdump(msg->data, msg->len)); + rc = write(fd->fd, msg->data, msg->len); + if (rc != msg->len) + LOGP(DMSC, LOGL_ERROR, "Could not write to MSC.\n"); + + return rc; +} + +/* called in the case of a non blocking connect */ +static int msc_connection_connect(struct bsc_fd *fd, unsigned int what) +{ + int rc; + int val; + socklen_t len = sizeof(val); + struct bsc_data *bsc; + + bsc = (struct bsc_data *) fd->data; + + if (fd != &bsc->msc_connection.bfd) { + LOGP(DMSC, LOGL_ERROR, "This is only working with the MSC connection.\n"); + return -1; + } + + if ((what & BSC_FD_WRITE) == 0) + return -1; + + /* check the socket state */ + rc = getsockopt(fd->fd, SOL_SOCKET, SO_ERROR, &val, &len); + if (rc != 0) { + LOGP(DMSC, LOGL_ERROR, "getsockopt for the MSC socket failed.\n"); + goto error; + } + if (val != 0) { + LOGP(DMSC, LOGL_ERROR, "Not connected to the MSC.\n"); + goto error; + } + + + /* go to full operation */ + fd->cb = write_queue_bfd_cb; + fd->when = BSC_FD_READ; + if (!llist_empty(&bsc->msc_connection.msg_queue)) + fd->when |= BSC_FD_WRITE; + return 0; + +error: + bsc_unregister_fd(fd); + close(fd->fd); + fd->fd = -1; + fd->cb = write_queue_bfd_cb; + fd->when = 0; + release_bsc_resources(bsc); + bsc_del_timer(&bsc->ping_timeout); + bsc_del_timer(&bsc->pong_timeout); + return -1; +} + +static void setnonblocking(struct bsc_fd *fd) +{ + int flags; + + flags = fcntl(fd->fd, F_GETFL); + if (flags < 0) { + perror("fcntl get failed"); + close(fd->fd); + fd->fd = -1; + return; + } + + flags |= O_NONBLOCK; + flags = fcntl(fd->fd, F_SETFL, flags); + if (flags < 0) { + perror("fcntl get failed"); + close(fd->fd); + fd->fd = -1; + return; + } +} + +static int connect_to_msc(struct bsc_fd *fd, const char *ip, int port, int tos) +{ + struct sockaddr_in sin; + int on = 1, ret; + + LOGP(DMSC, LOGL_NOTICE, "Attempting to connect MSC at %s:%d\n", ip, port); + + fd->fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + + if (fd->fd < 0) { + perror("Creating TCP socket failed"); + return fd->fd; + } + + /* make it non blocking */ + setnonblocking(fd); + + memset(&sin, 0, sizeof(sin)); + sin.sin_family = AF_INET; + sin.sin_port = htons(port); + inet_aton(ip, &sin.sin_addr); + + setsockopt(fd->fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + ret = setsockopt(fd->fd, IPPROTO_TCP, TCP_NODELAY, &on, sizeof(on)); + if (ret != 0) + LOGP(DMSC, LOGL_ERROR, "Failed to set TCP_NODELAY: %s\n", strerror(errno)); + ret = setsockopt(fd->fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)); + if (ret != 0) + LOGP(DMSC, LOGL_ERROR, "Failed to set IP_TOS: %s\n", strerror(errno)); + + ret = connect(fd->fd, (struct sockaddr *) &sin, sizeof(sin)); + + if (ret == -1 && errno == EINPROGRESS) { + LOGP(DMSC, LOGL_ERROR, "MSC Connection in progress\n"); + fd->when = BSC_FD_WRITE; + fd->cb = msc_connection_connect; + } else if (ret < 0) { + perror("Connection failed"); + close(fd->fd); + fd->fd = -1; + return ret; + } else { + fd->when = BSC_FD_READ; + fd->cb = write_queue_bfd_cb; + } + + ret = bsc_register_fd(fd); + if (ret < 0) { + perror("Registering the fd failed"); + close(fd->fd); + fd->fd = -1; + return ret; + } + + return ret; +} + +static void msc_reconnect(void *_data) +{ + int rc; + struct bsc_data *bsc = (struct bsc_data *) _data; + + bsc_del_timer(&bsc->reconnect_timer); + bsc->first_contact = 1; + + rc = connect_to_msc(&bsc->msc_connection.bfd, bsc->msc_address, 5000, bsc->msc_ip_dscp); + if (rc < 0) { + fprintf(stderr, "Opening the MSC connection failed. Trying again\n"); + bsc_schedule_timer(&bsc->reconnect_timer, RECONNECT_TIME); + return; + } + + bsc->msc_timeout.cb = msc_connect_timeout; + bsc->msc_timeout.data = bsc; + bsc_schedule_timer(&bsc->msc_timeout, bsc->msc_time, 0); +} + +void msc_schedule_reconnect(struct bsc_data *bsc) +{ + bsc_schedule_timer(&bsc->reconnect_timer, RECONNECT_TIME); +} + +/* + * mgcp forwarding is below + */ +static int mgcp_do_write(struct bsc_fd *fd, struct msgb *msg) +{ + int ret; + + LOGP(DMGCP, LOGL_DEBUG, "Sending msg to MGCP GW size: %u\n", msg->len); + + ret = write(fd->fd, msg->data, msg->len); + if (ret != msg->len) + LOGP(DMGCP, LOGL_ERROR, "Failed to forward message to MGCP GW (%s).\n", strerror(errno)); + + return ret; +} + +static int mgcp_do_read(struct bsc_fd *fd) +{ + struct msgb *mgcp; + int ret; + + mgcp = msgb_alloc_headroom(4096, 128, "mgcp_from_gw"); + if (!mgcp) { + LOGP(DMGCP, LOGL_ERROR, "Failed to allocate MGCP message.\n"); + return -1; + } + + ret = read(fd->fd, mgcp->data, 4096 - 128); + if (ret <= 0) { + LOGP(DMGCP, LOGL_ERROR, "Failed to read: %d/%s\n", errno, strerror(errno)); + msgb_free(mgcp); + return -1; + } else if (ret > 4096 - 128) { + LOGP(DMGCP, LOGL_ERROR, "Too much data: %d\n", ret); + msgb_free(mgcp); + return -1; + } + + mgcp->l2h = msgb_put(mgcp, ret); + msc_send(fd->data, mgcp, NAT_MUX); + return 0; +} + +void mgcp_forward(struct bsc_data *bsc, const u_int8_t *data, unsigned int length) +{ + struct msgb *mgcp; + + if (length > 4096) { + LOGP(DMGCP, LOGL_ERROR, "Can not forward too big message.\n"); + return; + } + + mgcp = msgb_alloc(4096, "mgcp_to_gw"); + if (!mgcp) { + LOGP(DMGCP, LOGL_ERROR, "Failed to send message.\n"); + return; + } + + msgb_put(mgcp, length); + memcpy(mgcp->data, data, mgcp->len); + if (write_queue_enqueue(&bsc->mgcp_agent, mgcp) != 0) { + LOGP(DMGCP, LOGL_FATAL, "Could not queue message to MGCP GW.\n"); + msgb_free(mgcp); + } +} + +static int mgcp_create_port(struct bsc_data *bsc) +{ + int on; + struct sockaddr_in addr; + + bsc->mgcp_agent.bfd.fd = socket(AF_INET, SOCK_DGRAM, 0); + if (bsc->mgcp_agent.bfd.fd < 0) { + LOGP(DMGCP, LOGL_FATAL, "Failed to create UDP socket errno: %d\n", errno); + return -1; + } + + on = 1; + setsockopt(bsc->mgcp_agent.bfd.fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + + /* try to bind the socket */ + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + addr.sin_port = 0; + + if (bind(bsc->mgcp_agent.bfd.fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + LOGP(DMGCP, LOGL_FATAL, "Failed to bind to any port.\n"); + close(bsc->mgcp_agent.bfd.fd); + bsc->mgcp_agent.bfd.fd = -1; + return -1; + } + + /* connect to the remote */ + addr.sin_port = htons(2427); + if (connect(bsc->mgcp_agent.bfd.fd, (struct sockaddr *) & addr, sizeof(addr)) < 0) { + LOGP(DMGCP, LOGL_FATAL, "Failed to connect to local MGCP GW. %s\n", strerror(errno)); + close(bsc->mgcp_agent.bfd.fd); + bsc->mgcp_agent.bfd.fd = -1; + return -1; + } + + write_queue_init(&bsc->mgcp_agent, 10); + bsc->mgcp_agent.bfd.data = bsc; + bsc->mgcp_agent.bfd.when = BSC_FD_READ; + bsc->mgcp_agent.read_cb = mgcp_do_read; + bsc->mgcp_agent.write_cb = mgcp_do_write; + + if (bsc_register_fd(&bsc->mgcp_agent.bfd) != 0) { + LOGP(DMGCP, LOGL_FATAL, "Failed to register BFD\n"); + close(bsc->mgcp_agent.bfd.fd); + bsc->mgcp_agent.bfd.fd = -1; + return -1; + } + + return 0; +} + +int msc_init(struct bsc_data *bsc) +{ + write_queue_init(&bsc->msc_connection, 100); + bsc->reconnect_timer.cb = msc_reconnect; + bsc->reconnect_timer.data = bsc; + bsc->msc_connection.read_cb = ipaccess_a_fd_cb; + bsc->msc_connection.write_cb = ipaccess_write_cb; + bsc->msc_connection.bfd.data = bsc; + bsc->closing = 1; + + /* handle the timeout */ + bsc->ping_timeout.cb = msc_ping_timeout; + bsc->ping_timeout.data = bsc; + bsc->pong_timeout.cb = msc_pong_timeout; + bsc->pong_timeout.data = bsc; + + /* create MGCP port */ + if (mgcp_create_port(bsc) != 0) + return -1; + return 0; +} + +static void msc_send(struct bsc_data *bsc, struct msgb *msg, int proto) +{ + ipaccess_prepend_header(msg, proto); + + if (write_queue_enqueue(&bsc->msc_connection, msg) != 0) { + LOGP(DMSC, LOGL_FATAL, "Failed to queue MSG for the MSC.\n"); + msgb_free(msg); + return; + } +} + +void msc_send_rlc(struct bsc_data *bsc, + struct sccp_source_reference *src, struct sccp_source_reference *dst) +{ + struct msgb *msg; + + msg = create_sccp_rlc(src, dst); + if (!msg) + return; + + msc_send(bsc, msg, IPAC_PROTO_SCCP); +} + +void msc_send_reset(struct bsc_data *bsc) +{ + struct msgb *msg; + + msg = create_reset(); + if (!msg) + return; + + msc_send(bsc, msg, IPAC_PROTO_SCCP); + msc_ping_timeout(bsc); +} + +static void msc_send_id_response(struct bsc_data *bsc) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(4096, 128, "id resp"); + msg->l2h = msgb_v_put(msg, IPAC_MSGT_ID_RESP); + msgb_l16tv_put(msg, strlen(bsc->token) + 1, + IPAC_IDTAG_UNITNAME, (u_int8_t *) bsc->token); + + msc_send(bsc, msg, IPAC_PROTO_IPACCESS); +} + +void msc_send_msg(struct bsc_data *bsc, int rc, struct sccp_parse_result *result, struct msgb *_msg) +{ + struct msgb *msg; + + if (bsc->msc_connection.bfd.fd < 0) { + LOGP(DMSC, LOGL_ERROR, "No connection to the MSC. dropping\n"); + return; + } + + msg = msgb_alloc_headroom(4096, 128, "SCCP to MSC"); + if (!msg) { + LOGP(DMSC, LOGL_ERROR, "Failed to alloc MSC msg.\n"); + return; + } + + bss_rewrite_header_for_msc(rc, msg, _msg, result); + msc_send(bsc, msg, IPAC_PROTO_SCCP); +} diff --git a/src/mtp_layer3.c b/src/mtp_layer3.c new file mode 100644 index 0000000..ef067e5 --- /dev/null +++ b/src/mtp_layer3.c @@ -0,0 +1,509 @@ +/* MTP layer3 main handling code */ +/* + * (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 <mtp_data.h> +#include <mtp_level3.h> + +#include <laf0rge1/debug.h> +#include <laf0rge1/talloc.h> + +#include <sccp/sccp.h> + +#include <arpa/inet.h> + +#include <string.h> + +static void *tall_mtp_ctx = NULL; + +static struct msgb *mtp_msg_alloc(struct mtp_link *link) +{ + struct mtp_level_3_hdr *hdr; + struct msgb *msg = msgb_alloc_headroom(4096, 128, "mtp-msg"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate mtp msg\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*hdr)); + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->addr = MTP_ADDR(0x0, link->dpc, link->opc); + return msg; +} + +static struct msgb *mtp_create_sltm(struct mtp_link *link) +{ + const u_int8_t test_ptrn[14] = { 'G', 'S', 'M', 'M', 'M', 'S', }; + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_mng *mng; + struct msgb *msg = mtp_msg_alloc(link); + u_int8_t *data; + if (!msg) + return NULL; + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_REG_MSG; + + mng = (struct mtp_level_3_mng *) msgb_put(msg, sizeof(*mng)); + mng->cmn.h0 = MTP_TST_MSG_GRP; + mng->cmn.h1 = MTP_TST_MSG_SLTM; + mng->length = ARRAY_SIZE(test_ptrn); + + data = msgb_put(msg, ARRAY_SIZE(test_ptrn)); + memcpy(data, test_ptrn, ARRAY_SIZE(test_ptrn)); + + /* remember the last tst ptrn... once we have some */ + memcpy(link->test_ptrn, test_ptrn, ARRAY_SIZE(test_ptrn)); + + return msg; +} + +static struct msgb *mtp_create_slta(struct mtp_link *link, struct mtp_level_3_mng *in_mng, int l3_len) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_mng *mng; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_REG_MSG; + mng = (struct mtp_level_3_mng *) msgb_put(out, sizeof(*mng)); + mng->cmn.h0 = MTP_TST_MSG_GRP; + mng->cmn.h1 = MTP_TST_MSG_SLTA; + mng->length = l3_len - 2; + msgb_put(out, mng->length); + memcpy(mng->data, in_mng->data, mng->length); + + return out; +} + +static struct msgb *mtp_tfp_alloc(struct mtp_link *link, int apoc) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_prohib *prb; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SNM_MSG; + prb = (struct mtp_level_3_prohib *) msgb_put(out, sizeof(*prb)); + prb->cmn.h0 = MTP_PROHIBIT_MSG_GRP; + prb->cmn.h1 = MTP_PROHIBIT_MSG_SIG; + prb->apoc = MTP_MAKE_APOC(apoc); + return out; +} + +static struct msgb *mtp_tra_alloc(struct mtp_link *link) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_cmn *cmn; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SNM_MSG; + cmn = (struct mtp_level_3_cmn *) msgb_put(out, sizeof(*cmn)); + cmn->h0 = MTP_TRF_RESTR_MSG_GRP; + cmn->h1 = MTP_RESTR_MSG_ALLWED; + return out; +} + +static struct msgb *mtp_sccp_alloc_ssa(struct mtp_link *link, int sls) +{ + struct sccp_data_unitdata *udt; + struct sccp_con_ctrl_prt_mgt *prt; + struct mtp_level_3_hdr *hdr; + u_int8_t *data; + + + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SCCP; + + /* this appears to be round robin or such.. */ + hdr->addr = MTP_ADDR(sls % 16, link->dpc, link->opc); + + /* generate the UDT message... libsccp does not offer formating yet */ + udt = (struct sccp_data_unitdata *) msgb_put(out, sizeof(*udt)); + udt->type = SCCP_MSG_TYPE_UDT; + udt->proto_class = SCCP_PROTOCOL_CLASS_0; + udt->variable_called = 3; + udt->variable_calling = 5; + udt->variable_data = 7; + + /* put the called and calling address. It is LV */ + data = msgb_put(out, 2 + 1); + data[0] = 2; + data[1] = 0x42; + data[2] = 0x1; + + data = msgb_put(out, 2 + 1); + data[0] = 2; + data[1] = 0x42; + data[2] = 0x1; + + data = msgb_put(out, 1); + data[0] = sizeof(*prt); + + prt = (struct sccp_con_ctrl_prt_mgt *) msgb_put(out, sizeof(*prt)); + prt->sst = SCCP_SSA; + prt->assn = 254; + prt->apoc = MTP_MAKE_APOC(link->opc); + prt->mul_ind = 0; + + return out; +} + +void mtp_link_init(void) +{ + tall_mtp_ctx = talloc_named_const(NULL, 1, "mtp-link"); +} + +static void mtp_send_sltm(struct mtp_link *link) +{ + struct msgb *msg; + + link->sltm_pending = 1; + msg = mtp_create_sltm(link); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate SLTM.\n"); + return; + } + + mtp_link_submit(link, msg); +} + +static void mtp_sltm_t1_timeout(void *_link) +{ + struct mtp_link *link = (struct mtp_link *) _link; + + if (link->slta_misses == 0) { + LOGP(DINP, LOGL_ERROR, "No SLTM response. Retrying. Link: %p\n", link); + ++link->slta_misses; + mtp_send_sltm(link); + bsc_schedule_timer(&link->t1_timer, MTP_T1); + } else { + LOGP(DINP, LOGL_ERROR, "Two missing SLTAs. Restart link: %p\n", link); + link->sccp_up = 0; + link->running = 0; + bsc_del_timer(&link->t2_timer); + mtp_link_sccp_down(link); + mtp_link_restart(link); + } +} + +static void mtp_sltm_t2_timeout(void *_link) +{ + struct mtp_link *link = (struct mtp_link *) _link; + + if (!link->running) { + LOGP(DINP, LOGL_INFO, "Not restarting SLTM timer on link: %p\n", link); + return; + } + + link->slta_misses = 0; + mtp_send_sltm(link); + + bsc_schedule_timer(&link->t1_timer, MTP_T1); + + if (link->sltm_once && link->was_up) + LOGP(DINP, LOGL_INFO, "Not sending SLTM again as configured.\n"); + else + bsc_schedule_timer(&link->t2_timer, MTP_T2); +} + +static void mtp_delayed_start(void *link) +{ + mtp_sltm_t2_timeout(link); +} + +struct mtp_link *mtp_link_alloc(void) +{ + struct mtp_link *link; + + link = talloc_zero(tall_mtp_ctx, struct mtp_link); + if (!link) + return NULL; + + link->t1_timer.data = link; + link->t1_timer.cb = mtp_sltm_t1_timeout; + link->t2_timer.data = link; + link->t2_timer.cb = mtp_sltm_t2_timeout; + link->delay_timer.data = link; + link->delay_timer.cb = mtp_delayed_start; + INIT_LLIST_HEAD(&link->pending_msgs); + return link; +} + +void mtp_link_stop(struct mtp_link *link) +{ + bsc_del_timer(&link->t1_timer); + bsc_del_timer(&link->t2_timer); + bsc_del_timer(&link->delay_timer); + link->sccp_up = 0; + link->running = 0; + link->sltm_pending = 0; + + mtp_link_sccp_down(link); +} + +void mtp_link_reset(struct mtp_link *link) +{ + mtp_link_stop(link); + link->running = 1; + bsc_schedule_timer(&link->delay_timer, START_DELAY); +} + +static int mtp_link_sign_msg(struct mtp_link *link, struct mtp_level_3_hdr *hdr, int l3_len) +{ + struct msgb *msg; + struct mtp_level_3_cmn *cmn; + + if (hdr->spare != 0 || hdr->ni != MTP_NI_NATION_NET || l3_len < 1) { + LOGP(DINP, LOGL_ERROR, "Unhandled data (%d, %d, %d)\n", + hdr->spare, hdr->ni, l3_len); + return -1; + } + + cmn = (struct mtp_level_3_cmn *) &hdr->data[0]; + LOGP(DINP, LOGL_DEBUG, "reg msg: h0: 0x%x h1: 0x%x\n", + cmn->h0, cmn->h1); + + switch (cmn->h0) { + case MTP_TRF_RESTR_MSG_GRP: + switch (cmn->h1) { + case MTP_RESTR_MSG_ALLWED: + LOGP(DINP, LOGL_INFO, "Received Restart Allowed. SST should be next: %p\n", link); + link->sccp_up = 0; + mtp_link_sccp_down(link); + + msg = mtp_tfp_alloc(link, 0); + if (!msg) + return -1; + mtp_link_submit(link, msg); + + msg = mtp_tra_alloc(link); + if (!msg) + return -1; + + mtp_link_submit(link, msg); + return 0; + break; + } + break; + } + + abort(); + return -1; +} + +static int mtp_link_regular_msg(struct mtp_link *link, struct mtp_level_3_hdr *hdr, int l3_len) +{ + struct msgb *out; + struct mtp_level_3_mng *mng; + + if (hdr->spare != 0 || hdr->ni != MTP_NI_NATION_NET || l3_len < 2) { + LOGP(DINP, LOGL_ERROR, "Unhandled data (%d, %d, %d)\n", + hdr->spare, hdr->ni, l3_len); + return -1; + } + + mng = (struct mtp_level_3_mng *) &hdr->data[0]; + LOGP(DINP, LOGL_DEBUG, "reg msg: h0: 0x%x h1: 0x%x\n", + mng->cmn.h0, mng->cmn.h1); + + switch (mng->cmn.h0) { + case MTP_TST_MSG_GRP: + switch (mng->cmn.h1) { + case MTP_TST_MSG_SLTM: + /* simply respond to the request... */ + out = mtp_create_slta(link, mng, l3_len); + if (!out) + return -1; + mtp_link_submit(link, out); + return 0; + break; + case MTP_TST_MSG_SLTA: + if (mng->length != 14) { + LOGP(DINP, LOGL_ERROR, "Wrongly sized SLTA: %u\n", mng->length); + return -1; + } + + if (l3_len != 16) { + LOGP(DINP, LOGL_ERROR, "Wrongly sized SLTA: %u\n", mng->length); + return -1; + } + + if (memcmp(mng->data, link->test_ptrn, sizeof(link->test_ptrn)) != 0) { + LOGP(DINP, LOGL_ERROR, "Wrong test pattern SLTA\n"); + return -1; + } + + /* we had a matching slta */ + bsc_del_timer(&link->t1_timer); + link->sltm_pending = 0; + mtp_link_slta_recv(link); + return 0; + break; + } + break; + } + + return -1; +} + +static int mtp_link_sccp_data(struct mtp_link *link, struct mtp_level_3_hdr *hdr, struct msgb *msg, int l3_len) +{ + struct msgb *out; + struct sccp_con_ctrl_prt_mgt *prt; + + msg->l2h = &hdr->data[0]; + if (msgb_l2len(msg) != l3_len) { + LOGP(DINP, LOGL_ERROR, "Size is wrong after playing with the l2h header.\n"); + return -1; + } + + + if (link->sccp_up) { + mtp_link_forward_sccp(link, msg, MTP_LINK_SLS(hdr->addr)); + return 0; + } else { + struct sccp_parse_result sccp; + memset(&sccp, 0, sizeof(sccp)); + if (sccp_parse_header(msg, &sccp) != 0) { + LOGP(DINP, LOGL_ERROR, "Failed to parsed SCCP header.\n"); + return -1; + } + + if (sccp_determine_msg_type(msg) != SCCP_MSG_TYPE_UDT) { + LOGP(DINP, LOGL_ERROR, "Dropping sccp data: 0x%x\n", + sccp_determine_msg_type(msg)); + return -1; + } + + if (msgb_l3len(msg) != 5) { + LOGP(DINP, LOGL_ERROR, "SCCP UDT msg of unexpected size: %u\n", + msgb_l3len(msg)); + return -1; + } + + if (msg->l3h[0] != SCCP_SST) { + LOGP(DINP, LOGL_ERROR, "Expected SCCP SST but got 0x%x\n", + msg->l3h[0]); + return -1; + } + + prt = (struct sccp_con_ctrl_prt_mgt *) &msg->l3h[0]; + if (prt->assn != 254 || prt->apoc != MTP_MAKE_APOC(link->opc)) { + LOGP(DINP, LOGL_ERROR, "Unknown SSN/APOC assn: %u, apoc: %u/%u\n", + prt->assn, ntohs(prt->apoc), prt->apoc); + return -1; + } + + out = mtp_sccp_alloc_ssa(link, MTP_LINK_SLS(hdr->addr)); + if (!out) + return -1; + + link->sccp_up = 1; + link->was_up = 1; + LOGP(DINP, LOGL_INFO, "SCCP is established. %p\n", link); + mtp_link_submit(link, out); + } + return 0; +} + +int mtp_link_data(struct mtp_link *link, struct msgb *msg) +{ + int rc = -1; + struct mtp_level_3_hdr *hdr; + int l3_len; + + if (!msg->l2h || msgb_l2len(msg) < sizeof(*hdr)) + return -1; + + if (!link->running) { + LOGP(DINP, LOGL_ERROR, "Link is not running. Call mtp_link_reset first: %p\n", link); + return -1; + } + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + l3_len = msgb_l2len(msg) - sizeof(*hdr); + + switch (hdr->ser_ind) { + case MTP_SI_MNT_SNM_MSG: + rc = mtp_link_sign_msg(link, hdr, l3_len); + break; + case MTP_SI_MNT_REG_MSG: + rc = mtp_link_regular_msg(link, hdr, l3_len); + break; + case MTP_SI_MNT_SCCP: + rc = mtp_link_sccp_data(link, hdr, msg, l3_len); + break; + default: + fprintf(stderr, "Unhandled: %u\n", hdr->ser_ind); + break; + } + + return rc; +} + +int mtp_link_submit_sccp_data(struct mtp_link *link, int sls, const u_int8_t *data, unsigned int length) +{ + u_int8_t *put_ptr; + struct mtp_level_3_hdr *hdr; + struct msgb *msg; + + if (!link->sccp_up) { + LOGP(DINP, LOGL_ERROR, "SCCP msg after TRA and before SSA. Dropping it.\n"); + return -1; + } + + msg = mtp_msg_alloc(link); + if (!msg) + return -1; + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SCCP; + + hdr->addr = MTP_ADDR(sls % 16, link->dpc, link->opc); + + /* copy the raw sccp data */ + put_ptr = msgb_put(msg, length); + memcpy(put_ptr, data, length); + + mtp_link_submit(link, msg); + return 0; +} diff --git a/src/openbsc_nat/README b/src/openbsc_nat/README new file mode 100644 index 0000000..9393d56 --- /dev/null +++ b/src/openbsc_nat/README @@ -0,0 +1 @@ +This is OpenBSC code. Do not patch. Fix it upstream. diff --git a/src/openbsc_nat/bssap.c b/src/openbsc_nat/bssap.c new file mode 100644 index 0000000..8ffdf04 --- /dev/null +++ b/src/openbsc_nat/bssap.c @@ -0,0 +1,65 @@ +/* GSM 08.08 BSSMAP handling */ +/* (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2009 by on-waves.com + * 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 <openbsc_nat/bssap.h> +#include <openbsc_nat/tlv.h> + +#include <sccp/sccp.h> + +#include <arpa/inet.h> +#include <assert.h> + + +#define BSSMAP_MSG_SIZE 512 +#define BSSMAP_MSG_HEADROOM 128 + + +static const struct tlv_definition bss_att_tlvdef = { + .def = { + [GSM0808_IE_IMSI] = { TLV_TYPE_TLV }, + [GSM0808_IE_TMSI] = { TLV_TYPE_TLV }, + [GSM0808_IE_CELL_IDENTIFIER_LIST] = { TLV_TYPE_TLV }, + [GSM0808_IE_CHANNEL_NEEDED] = { TLV_TYPE_TV }, + [GSM0808_IE_EMLPP_PRIORITY] = { TLV_TYPE_TV }, + [GSM0808_IE_CHANNEL_TYPE] = { TLV_TYPE_TLV }, + [GSM0808_IE_PRIORITY] = { TLV_TYPE_TLV }, + [GSM0808_IE_CIRCUIT_IDENTITY_CODE] = { TLV_TYPE_TV }, + [GSM0808_IE_DOWNLINK_DTX_FLAG] = { TLV_TYPE_TV }, + [GSM0808_IE_INTERFERENCE_BAND_TO_USE] = { TLV_TYPE_TV }, + [GSM0808_IE_CLASSMARK_INFORMATION_T2] = { TLV_TYPE_TLV }, + [GSM0808_IE_GROUP_CALL_REFERENCE] = { TLV_TYPE_TLV }, + [GSM0808_IE_TALKER_FLAG] = { TLV_TYPE_T }, + [GSM0808_IE_CONFIG_EVO_INDI] = { TLV_TYPE_TV }, + [GSM0808_IE_LSA_ACCESS_CTRL_SUPPR] = { TLV_TYPE_TV }, + [GSM0808_IE_SERVICE_HANDOVER] = { TLV_TYPE_TV}, + [GSM0808_IE_ENCRYPTION_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_IE_CIPHER_RESPONSE_MODE] = { TLV_TYPE_TV }, + [GSM0808_IE_SPEECH_VERSION] = { TLV_TYPE_TV }, + [GSM0808_IE_CHOSEN_ENCR_ALG] = { TLV_TYPE_TV }, + [GSM0808_IE_CHOSEN_CHANNEL] = { TLV_TYPE_TV }, + }, +}; + +const struct tlv_definition *gsm0808_att_tlvdef() +{ + return &bss_att_tlvdef; +} + diff --git a/src/openbsc_nat/tlv_parser.c b/src/openbsc_nat/tlv_parser.c new file mode 100644 index 0000000..e44b3a1 --- /dev/null +++ b/src/openbsc_nat/tlv_parser.c @@ -0,0 +1,160 @@ +#include <stdio.h> +#include <openbsc_nat/tlv.h> + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(x) (sizeof(x)/sizeof(x[0])) +#endif + +struct tlv_definition tvlv_att_def; + +int tlv_dump(struct tlv_parsed *dec) +{ + int i; + + for (i = 0; i <= 0xff; i++) { + if (!dec->lv[i].val) + continue; + printf("T=%02x L=%d\n", i, dec->lv[i].len); + } + return 0; +} + +/* o_tag: output: tag found + * o_len: output: length of the data + * o_val: output: pointer to the data + * def: input: a structure defining the valid TLV tags / configurations + * buf: input: the input data buffer to be parsed + * buf_len: input: the length of the input data buffer + * + * Also, returns the number of bytes consumed by the TLV entry + */ +int tlv_parse_one(u_int8_t *o_tag, u_int16_t *o_len, const u_int8_t **o_val, + const struct tlv_definition *def, + const u_int8_t *buf, int buf_len) +{ + u_int8_t tag; + int len; + + tag = *buf; + *o_tag = tag; + + /* FIXME: use tables for knwon IEI */ + switch (def->def[tag].type) { + case TLV_TYPE_T: + /* GSM TS 04.07 11.2.4: Type 1 TV or Type 2 T */ + *o_val = buf; + *o_len = 0; + len = 1; + break; + case TLV_TYPE_TV: + *o_val = buf+1; + *o_len = 1; + len = 2; + break; + case TLV_TYPE_FIXED: + *o_val = buf+1; + *o_len = def->def[tag].fixed_len; + len = def->def[tag].fixed_len + 1; + break; + case TLV_TYPE_TLV: + /* GSM TS 04.07 11.2.4: Type 4 TLV */ + if (buf + 1 > buf + buf_len) + return -1; + *o_val = buf+2; + *o_len = *(buf+1); + len = *o_len + 2; + if (len > buf_len) + return -2; + break; + case TLV_TYPE_TvLV: + if (*(buf+1) & 0x80) { + /* like TLV, but without highest bit of len */ + if (buf + 1 > buf + buf_len) + return -1; + *o_val = buf+2; + *o_len = *(buf+1) & 0x7f; + len = *o_len + 2; + if (len > buf_len) + return -2; + break; + } + /* like TL16V, fallthrough */ + case TLV_TYPE_TL16V: + if (2 > buf_len) + return -1; + *o_val = buf+3; + *o_len = *(buf+1) << 8 | *(buf+2); + len = *o_len + 3; + if (len > buf_len) + return -2; + break; + default: + return -3; + } + + return len; +} + +/* dec: output: a caller-allocated pointer to a struct tlv_parsed, + * def: input: a structure defining the valid TLV tags / configurations + * buf: input: the input data buffer to be parsed + * buf_len: input: the length of the input data buffer + * lv_tag: input: an initial LV tag at the start of the buffer + * lv_tag2: input: a second initial LV tag following lv_tag + */ +int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def, + const u_int8_t *buf, int buf_len, u_int8_t lv_tag, + u_int8_t lv_tag2) +{ + int ofs = 0, num_parsed = 0; + u_int16_t len; + + memset(dec, 0, sizeof(*dec)); + + if (lv_tag) { + if (ofs > buf_len) + return -1; + dec->lv[lv_tag].val = &buf[ofs+1]; + dec->lv[lv_tag].len = buf[ofs]; + len = dec->lv[lv_tag].len + 1; + if (ofs + len > buf_len) + return -2; + num_parsed++; + ofs += len; + } + if (lv_tag2) { + if (ofs > buf_len) + return -1; + dec->lv[lv_tag2].val = &buf[ofs+1]; + dec->lv[lv_tag2].len = buf[ofs]; + len = dec->lv[lv_tag2].len + 1; + if (ofs + len > buf_len) + return -2; + num_parsed++; + ofs += len; + } + + while (ofs < buf_len) { + int rv; + u_int8_t tag; + const u_int8_t *val; + + rv = tlv_parse_one(&tag, &len, &val, def, + &buf[ofs], buf_len-ofs); + if (rv < 0) + return rv; + dec->lv[tag].val = val; + dec->lv[tag].len = len; + ofs += rv; + num_parsed++; + } + //tlv_dump(dec); + return num_parsed; +} + +static __attribute__((constructor)) void on_dso_load_tlv(void) +{ + int i; + for (i = 0; i < ARRAY_SIZE(tvlv_att_def.def); i++) + tvlv_att_def.def[i].type = TLV_TYPE_TvLV; +} diff --git a/src/pcap.c b/src/pcap.c new file mode 100644 index 0000000..549afa9 --- /dev/null +++ b/src/pcap.c @@ -0,0 +1,86 @@ +/* PCAP code from OpenBSC done by Holger Freyther */ +/* + * (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 <mtp_pcap.h> + +#include <sys/time.h> + +#include <unistd.h> + +#define static_assert(exp, name) typedef int dummy##name [(exp) ? 1 : -1]; + +/* + * pcap writing of the misdn load + * pcap format is from http://wiki.wireshark.org/Development/LibpcapFileFormat + */ +struct pcap_hdr { + u_int32_t magic_number; + u_int16_t version_major; + u_int16_t version_minor; + int32_t thiszone; + u_int32_t sigfigs; + u_int32_t snaplen; + u_int32_t network; +} __attribute__((packed)); + +struct pcaprec_hdr { + u_int32_t ts_sec; + u_int32_t ts_usec; + u_int32_t incl_len; + u_int32_t orig_len; +} __attribute__((packed)); + +int mtp_pcap_write_header(int fd) +{ + static struct pcap_hdr hdr = { + .magic_number = 0xa1b2c3d4, + .version_major = 2, + .version_minor = 4, + .thiszone = 0, + .sigfigs = 0, + .snaplen = 65535, + .network = 141, + }; + + return write(fd, &hdr, sizeof(hdr)); +} + +int mtp_pcap_write_msu(int fd, const u_int8_t *data, int length) +{ + int rc_h, rc_d; + struct timeval tv; + struct pcaprec_hdr payload_header = { + .ts_sec = 0, + .ts_usec = 0, + .incl_len = length, + .orig_len = length, + }; + + gettimeofday(&tv, NULL); + payload_header.ts_sec = tv.tv_sec; + payload_header.ts_usec = tv.tv_usec; + + rc_h = write(fd, &payload_header, sizeof(payload_header)); + rc_d = write(fd, data, length); + + return rc_h == sizeof(payload_header) && rc_d == length; +} diff --git a/src/snmp_mtp.c b/src/snmp_mtp.c new file mode 100644 index 0000000..d2ff495 --- /dev/null +++ b/src/snmp_mtp.c @@ -0,0 +1,112 @@ +/* + * (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 <snmp_mtp.h> +#include <laf0rge1/talloc.h> + +static void add_pdu_var(netsnmp_pdu *pdu, const char *mib_name, int id, const char *value) +{ + oid oid_name[MAX_OID_LEN]; + size_t name_length; + + char buf[4096]; + buf[4095] = '\0'; + snprintf(buf, sizeof(buf)-1, "%s.%d", mib_name, id); + + name_length = MAX_OID_LEN; + if (snmp_parse_oid(buf, oid_name, &name_length) == NULL) { + snmp_perror(buf); + return; + } + + if (snmp_add_var(pdu, oid_name, name_length, '=', value)) { + snmp_perror(buf); + return; + } +} + +static void send_pdu(netsnmp_session *ss, netsnmp_pdu *pdu) +{ + int status; + netsnmp_pdu *response; + + status = snmp_synch_response(ss, pdu, &response); + if (status == STAT_ERROR) { + snmp_sess_perror("set failed", ss); + } else if (status == STAT_TIMEOUT) { + fprintf(stderr, "Timeout for SNMP.\n"); + } + + if (response) + snmp_free_pdu(response); +} + +void snmp_mtp_start_c7_datalink(struct snmp_mtp_session *session, int link_id) +{ + netsnmp_pdu *pdu; + pdu = snmp_pdu_create(SNMP_MSG_SET); + + add_pdu_var(pdu, "PTI-NexusWareC7-MIB::nwc7DatalinkCommand", link_id, "nwc7DatalinkCmdPowerOn"); + add_pdu_var(pdu, "PTI-NexusWareC7-MIB::nwc7Mtp2Active", link_id, "true"); + send_pdu(session->ss, pdu); +} + +void snmp_mtp_stop_c7_datalink(struct snmp_mtp_session *session, int link_id) +{ + netsnmp_pdu *pdu; + pdu = snmp_pdu_create(SNMP_MSG_SET); + + add_pdu_var(pdu, "PTI-NexusWareC7-MIB::nwc7Mtp2Active", link_id, "false"); + send_pdu(session->ss, pdu); +} + +struct snmp_mtp_session *snmp_mtp_session_create(char *host) +{ + struct snmp_mtp_session *session = talloc_zero(NULL, struct snmp_mtp_session); + if (!session) + return NULL; + + init_snmp("cellmgr_ng"); + snmp_sess_init(&session->session); + session->session.peername = host; + session->session.version = SNMP_VERSION_1; + session->session.community = (unsigned char *) "private"; + session->session.community_len = strlen((const char *) session->session.community); + + session->ss = snmp_open(&session->session); + if (!session->ss) { + snmp_perror("create failure"); + snmp_log(LOG_ERR, "Could not connect to the remote.\n"); + talloc_free(session); + return NULL; + } + + return session; +} + +void snmp_mtp_deactivate(struct snmp_mtp_session *session) +{ + snmp_mtp_stop_c7_datalink(session, 1); +} + +void snmp_mtp_activate(struct snmp_mtp_session *session) +{ + snmp_mtp_start_c7_datalink(session, 1); +} diff --git a/src/thread.c b/src/thread.c new file mode 100644 index 0000000..8175a49 --- /dev/null +++ b/src/thread.c @@ -0,0 +1,100 @@ +/* + * (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 <thread.h> + +#include <laf0rge1/talloc.h> + +#include <sys/types.h> +#include <sys/socket.h> + +#include <string.h> +#include <unistd.h> + +static void *tall_ctx_thr; + +static int thread_finish(struct thread_notifier *not) +{ + pthread_mutex_destroy(¬->guard); + close(not->fd[0]); + close(not->fd[1]); + + return 0; +} + +void thread_init(void) +{ + tall_ctx_thr = talloc_named_const(NULL, 1, "threads"); +} + +struct thread_notifier *thread_notifier_alloc() +{ + struct thread_notifier *not = talloc_zero(tall_ctx_thr, struct thread_notifier); + if (!not) + return NULL; + + if (socketpair(AF_UNIX, SOCK_STREAM, 0, not->fd) == -1) { + talloc_free(not); + return NULL; + } + + if (pthread_mutex_init(¬->guard, NULL) != 0) { + close(not->fd[0]); + close(not->fd[1]); + talloc_free(not); + return NULL; + } + + not->bfd.fd = not->fd[1]; + INIT_LLIST_HEAD(¬->__head1); + INIT_LLIST_HEAD(¬->__head2); + not->main_head = ¬->__head1; + not->thread_head = ¬->__head2; + talloc_set_destructor(not, thread_finish); + return not; +} + +void thread_safe_add(struct thread_notifier *not, struct llist_head *_new) +{ + char c; + pthread_mutex_lock(¬->guard); + + llist_add_tail(_new, not->thread_head); + if (!not->no_write && write(not->fd[0], &c, sizeof(c)) != 1) { + fprintf(stderr, "BAD NEWS. Socket write failed.\n"); + } + + pthread_mutex_unlock(¬->guard); +} + +void thread_swap(struct thread_notifier *not) +{ + pthread_mutex_lock(¬->guard); + + if (not->main_head == ¬->__head1) { + not->main_head = ¬->__head2; + not->thread_head = ¬->__head1; + } else { + not->main_head = ¬->__head1; + not->thread_head = ¬->__head2; + } + + pthread_mutex_unlock(¬->guard); +} diff --git a/src/write_queue.c b/src/write_queue.c new file mode 100644 index 0000000..ac529a3 --- /dev/null +++ b/src/write_queue.c @@ -0,0 +1,91 @@ +/* Generic write queue implementation */ +/* + * (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 <write_queue.h> + +#include <laf0rge1/debug.h> + +int write_queue_bfd_cb(struct bsc_fd *fd, unsigned int what) +{ + struct write_queue *queue; + + queue = container_of(fd, struct write_queue, bfd); + + if (what & BSC_FD_READ) + queue->read_cb(fd); + + if (what & BSC_FD_WRITE) { + struct msgb *msg; + + fd->when &= ~BSC_FD_WRITE; + msg = msgb_dequeue(&queue->msg_queue); + if (!msg) + return -1; + + --queue->current_length; + queue->write_cb(fd, msg); + msgb_free(msg); + + if (!llist_empty(&queue->msg_queue)) + fd->when |= BSC_FD_WRITE; + } + + return 0; +} + +void write_queue_init(struct write_queue *queue, int max_length) +{ + queue->max_length = max_length; + queue->current_length = 0; + queue->read_cb = NULL; + queue->write_cb = NULL; + queue->bfd.cb = write_queue_bfd_cb; + queue->paused = 0; + INIT_LLIST_HEAD(&queue->msg_queue); +} + +int write_queue_enqueue(struct write_queue *queue, struct msgb *data) +{ + if (queue->current_length + 1 >= queue->max_length) + LOGP(DMSC, LOGL_ERROR, "The queue is full. Dropping not yet implemented.\n"); + + ++queue->current_length; + msgb_enqueue(&queue->msg_queue, data); + + if (!queue->paused) + queue->bfd.when |= BSC_FD_WRITE; + + return 0; +} + +void write_queue_pause(struct write_queue *queue) +{ + queue->paused = 1; + queue->bfd.when &= ~BSC_FD_WRITE; +} + +void write_queue_unpause(struct write_queue *queue) +{ + queue->paused = 0; + if (!llist_empty(&queue->msg_queue)) + queue->bfd.when |= BSC_FD_WRITE; +} |