Merge branch 'on-waves/stp'

15 files changed, 520 insertions, 51 deletions

diff --git a/include/Makefile.am b/include/Makefile.am
index 6771b84..7693480 100644
--- a/include/Makefile.am
+++ b/include/Makefile.am
@@ -1,5 +1,6 @@
 noinst_HEADERS = mtp_level3.h mtp_data.h ipaccess.h thread.h mtp_pcap.h \
 		 mgcp_ss7.h bss_patch.h bssap_sccp.h bsc_data.h udp_input.h \
-                 snmp_mtp.h cellmgr_debug.h bsc_sccp.h bsc_ussd.h sctp_m2ua.h
+                 snmp_mtp.h cellmgr_debug.h bsc_sccp.h bsc_ussd.h sctp_m2ua.h \
+                 isup_types.h
 
 SUBDIRS = mgcp
diff --git a/include/bsc_data.h b/include/bsc_data.h
index 932b229..3d10bb3 100644
--- a/include/bsc_data.h
+++ b/include/bsc_data.h
@@ -81,6 +81,7 @@ struct bsc_data {
 	int pcap_fd;
 	int udp_reset_timeout;
 	struct mtp_link_set *link_set;
+	struct mtp_link_set *m2ua_set;
 
 	const char *token;
 
@@ -90,6 +91,7 @@ struct bsc_data {
 	int dpc;
 	int opc;
 	int sccp_opc;
+	int isup_opc;
 	int src_port;
 	int udp_port;
 	char *udp_ip;
diff --git a/include/isup_types.h b/include/isup_types.h
index 5347cb4..9ded619 100644
--- a/include/isup_types.h
+++ b/include/isup_types.h
@@ -30,15 +30,16 @@ struct mtp_link_set;
 /* This is from Table 4/Q.763 */
 #define ISUP_MSG_GRS	0x17
 #define ISUP_MSG_GRA	0x29
+#define ISUP_MSG_CGB	0x18
+#define ISUP_MSG_CGBA	0x1A
+#define ISUP_MSG_RLC	0x10
+#define ISUP_MSG_RSC	0x12
+#define ISUP_MSG_CGU	0x19
+#define ISUP_MSG_CGUA	0x1B
+
 
 struct isup_msg_hdr {
-#if __BYTE_ORDER == __LITTLE_ENDIAN
-	uint16_t cic   : 12,
-		 spare :  4;
-#elif __BYTE_ORDER == __BIG_ENDIAN
-	uint16_t spare :  4,
-		 cic   : 12;
-#endif
+	uint16_t cic;
 	uint8_t  msg_type;
 	uint8_t  data[0];
 } __attribute__((packed));
@@ -47,8 +48,8 @@ struct isup_msg_grs {
 	uint8_t  pointer_int;
 };
 
-int mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int sls);
+int mtp_link_set_isup(struct mtp_link_set *link, struct msgb *msg, int sls);
 
-int isup_parse_grs(const uint8_t *data, uint8_t length);
+int isup_parse_status(const uint8_t *data, uint8_t length);
 
 #endif
diff --git a/include/mtp_data.h b/include/mtp_data.h
index fd886a1..e00f13a 100644
--- a/include/mtp_data.h
+++ b/include/mtp_data.h
@@ -39,7 +39,7 @@ struct mtp_link;
  */
 struct mtp_link_set {
 	/* routing info.. */
-	int dpc, opc, sccp_opc;
+	int dpc, opc, sccp_opc, isup_opc;
 	int ni;
 	int spare;
 
@@ -67,6 +67,9 @@ struct mtp_link_set {
 	struct llist_head links;
 	struct mtp_link *slc[16];
 
+	/* special handling */
+	int pass_all_isup;
+
 	/* custom data */
 	struct bsc_data *bsc;
 };
@@ -98,7 +101,7 @@ struct mtp_link {
 struct mtp_link_set *mtp_link_set_alloc(void);
 void mtp_link_set_stop(struct mtp_link_set *link);
 void mtp_link_set_reset(struct mtp_link_set *link);
-int mtp_link_set_data(struct mtp_link_set *link, struct msgb *msg);
+int mtp_link_set_data(struct mtp_link *link, struct msgb *msg);
 int mtp_link_set_submit_sccp_data(struct mtp_link_set *link, int sls, const uint8_t *data, unsigned int length);
 int mtp_link_set_submit_isup_data(struct mtp_link_set *link, int sls, const uint8_t *data, unsigned int length);
 
@@ -112,6 +115,7 @@ void mtp_link_set_init(void);
 /* to be implemented for MSU sending */
 void mtp_link_set_submit(struct mtp_link *link, struct msgb *msg);
 void mtp_link_set_forward_sccp(struct mtp_link_set *link, struct msgb *msg, int sls);
+void mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int sls);
 void mtp_link_restart(struct mtp_link *link);
 void mtp_link_set_sccp_down(struct mtp_link_set *link);
 
diff --git a/include/mtp_level3.h b/include/mtp_level3.h
index 3f6d8cf..bc2d186 100644
--- a/include/mtp_level3.h
+++ b/include/mtp_level3.h
@@ -43,6 +43,7 @@
 
 #define MTP_TST_MSG_GRP		0x01
 #define MTP_PROHIBIT_MSG_GRP	0x04
+#define MTP_SROUTE_MSG_GRP	0x05
 #define MTP_TRF_RESTR_MSG_GRP	0x07
 
 /* h1 values for different groups */
@@ -51,7 +52,12 @@
 
 #define MTP_RESTR_MSG_ALLWED	0x01
 
+/* For the prohibit group */
 #define MTP_PROHIBIT_MSG_SIG	0x01
+#define MTP_PROHIBIT_MSG_TFA	0x05
+
+/* For the Signalling-route-set-test */
+#define MTP_SROUTE_MSG_TEST	0x01
 
 
 #define SCCP_SST	0x03
diff --git a/src/Makefile.am b/src/Makefile.am
index 0331406..eb6ab11 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -2,7 +2,7 @@ INCLUDES = $(all_includes) -I$(top_srcdir)/include
 AM_CFLAGS=-Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOSCCP_CFLAGS) $(LIBOSMOVTY_CFLAGS) \
 		$(NEXUSWARE_C7_CFLAGS) $(NEXUSWARE_UNIPORTE_CFLAGS) $(UNIPORTE)
 
-sbin_PROGRAMS = cellmgr_ng udt_relay mgcp_mgw
+sbin_PROGRAMS = cellmgr_ng udt_relay osmo_stp mgcp_mgw
 
 mgcp_mgw_SOURCES = mgcp_ss7.c thread.c debug.c \
 		   mgcp/mgcp_protocol.c mgcp/mgcp_network.c mgcp/mgcp_vty.c
@@ -20,3 +20,8 @@ udt_relay_SOURCES = main_udt.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \
 		     bss_patch.c isup.c links.c sctp_m2ua.c
 udt_relay_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \
 		   -lpthread -lnetsnmp -lcrypto -lm2ua -lsctp
+
+osmo_stp_SOURCES = main_stp.c mtp_layer3.c thread.c pcap.c link_udp.c snmp_mtp.c \
+		   debug.c vty_interface.c links.c isup.c sctp_m2ua.c
+osmo_stp_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \
+		   -lpthread -lnetsnmp -lcrypto -lm2ua -lsctp
diff --git a/src/isup.c b/src/isup.c
index 340af16..3f73029 100644
--- a/src/isup.c
+++ b/src/isup.c
@@ -1,6 +1,6 @@
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software: you can redistribute it and/or modify
@@ -25,15 +25,16 @@
 #include <osmocore/msgb.h>
 #include <osmocore/tlv.h>
 
-static struct msgb *isup_gra_alloc(int cic, int range)
+static struct msgb *isup_status_alloc(int cic, int msg_type, uint8_t *extra, int range, int val)
 {
 	struct isup_msg_hdr *hdr;
 	struct msgb *msg;
 	int bits, len;
+	uint8_t *data;
 
-	msg = msgb_alloc_headroom(4096, 128, "ISUP GRA");
+	msg = msgb_alloc_headroom(4096, 128, "ISUP Simple MSG");
 	if (!msg) {
-		LOGP(DISUP, LOGL_ERROR, "Allocation of GRA message failed.\n");
+		LOGP(DISUP, LOGL_ERROR, "Allocation of status message failed.\n");
 		return NULL;
 	}
 
@@ -42,7 +43,10 @@ static struct msgb *isup_gra_alloc(int cic, int range)
 	/* write the ISUP header */
 	hdr = (struct isup_msg_hdr *) msg->l2h;
 	hdr->cic = cic;
-	hdr->msg_type = ISUP_MSG_GRA;
+	hdr->msg_type = msg_type;
+
+	if (extra)
+		msgb_v_put(msg, *extra);
 
 	/*
 	 * place the pointers here.
@@ -56,18 +60,43 @@ static struct msgb *isup_gra_alloc(int cic, int range)
 	msgb_v_put(msg, len + 1);
 	msgb_v_put(msg, range);
 
-	msgb_put(msg, len);
+	data = msgb_put(msg, len);
+
+	/* set the status bits to val... FIXME this sets the extra bits too */
+	memset(data, val, len);
 
 	return msg;
 }
 
+static struct msgb *isup_simple_alloc(int cic, int msg_type)
+{
+	struct isup_msg_hdr *hdr;
+	struct msgb *msg;
+
+	msg = msgb_alloc_headroom(4096, 128, "ISUP Simple MSG");
+	if (!msg) {
+		LOGP(DISUP, LOGL_ERROR, "Allocation of Simple message failed.\n");
+		return NULL;
+	}
+
+	msg->l2h = msgb_put(msg, sizeof(*hdr));
+
+	/* write the ISUP header */
+	hdr = (struct isup_msg_hdr *) msg->l2h;
+	hdr->cic = cic;
+	hdr->msg_type = msg_type;
+
+	msgb_v_put(msg, 0);
+	return msg;
+}
+
 /* this message contains the range */
-int isup_parse_grs(const uint8_t *data, uint8_t in_length)
+int isup_parse_status(const uint8_t *data, uint8_t in_length)
 {
 	uint8_t ptr;
 	uint8_t length;
 
-	if (in_length > 3) {
+	if (in_length < 3) {
 		LOGP(DISUP, LOGL_ERROR, "This needs three bytes.\n");
 		return -1;	
 	}
@@ -96,11 +125,35 @@ static int handle_circuit_reset_grs(struct mtp_link_set *link, int sls, int cic,
 	struct msgb *resp;
 	int range;
 
-	range = isup_parse_grs(data, size);
+	range = isup_parse_status(data, size);
+	if (range < 0)
+		return -1;
+
+	resp = isup_status_alloc(cic, ISUP_MSG_GRA, NULL, range, 0);
+	if (!resp)
+		return -1;
+
+	mtp_link_set_submit_isup_data(link, sls, resp->l2h, msgb_l2len(resp));
+	msgb_free(resp);
+	return 0;
+}
+
+static int handle_circuit_reset_cgb(struct mtp_link_set *link, int sls, int cic,
+				    const uint8_t *data, int size)
+{
+	struct msgb *resp;
+	int range;
+	uint8_t val;
+
+	if (size < 1)
+		return -1;
+
+	range = isup_parse_status(data + 1, size - 1);
 	if (range < 0)
 		return -1;
 
-	resp = isup_gra_alloc(cic, range);
+	val = 0;
+	resp = isup_status_alloc(cic, ISUP_MSG_CGBA, &val, range, 0xff);
 	if (!resp)
 		return -1;
 
@@ -109,7 +162,34 @@ static int handle_circuit_reset_grs(struct mtp_link_set *link, int sls, int cic,
 	return 0;
 }
 
-int mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int sls)
+static int send_cgu(struct mtp_link_set *link, int sls, int cic, int range)
+{
+	struct msgb *resp;
+	uint8_t val;
+
+	val = 0;
+	resp = isup_status_alloc(cic, ISUP_MSG_CGU, &val, range, 0);
+	if (!resp)
+		return -1;
+
+	mtp_link_set_submit_isup_data(link, sls, resp->l2h, msgb_l2len(resp));
+	msgb_free(resp);
+	return 0;
+}
+
+static int handle_simple_resp(struct mtp_link_set *set, int sls, int cic, int msg_type)
+{
+	struct msgb *resp;
+
+	resp = isup_simple_alloc(cic, msg_type);
+	if (!resp)
+		return -1;
+	mtp_link_set_submit_isup_data(set, sls, resp->l2h, msgb_l2len(resp));
+	msgb_free(resp);
+	return 0;
+}
+
+int mtp_link_set_isup(struct mtp_link_set *link, struct msgb *msg, int sls)
 {
 	int rc = -1;
 	int payload_size;
@@ -120,6 +200,11 @@ int mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int s
 		return -1;
 	}
 
+	if (link->pass_all_isup) {
+		mtp_link_set_forward_isup(link, msg, sls);
+		return 0;
+	}
+
 	hdr = (struct isup_msg_hdr *) msg->l3h;
 	payload_size = msgb_l3len(msg) - sizeof(*hdr);
 
@@ -127,8 +212,20 @@ int mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int s
 	case ISUP_MSG_GRS:
 		rc = handle_circuit_reset_grs(link, sls, hdr->cic, hdr->data, payload_size);
 		break;
+	case ISUP_MSG_CGB:
+		rc = handle_circuit_reset_cgb(link, sls, hdr->cic, hdr->data, payload_size);
+		if (rc == 0)
+			rc = send_cgu(link, sls, hdr->cic, 28);
+		break;
+	case ISUP_MSG_CGUA:
+		LOGP(DISUP, LOGL_NOTICE, "CIC %d is now unblocked.\n", hdr->cic);
+		break;
+	case ISUP_MSG_RSC:
+		rc = handle_simple_resp(link, sls, hdr->cic, ISUP_MSG_RLC);
+		break;
 	default:
-		LOGP(DISUP, LOGL_NOTICE, "ISUP msg not handled: 0x%x\n", hdr->msg_type);
+		mtp_link_set_forward_isup(link, msg, sls);
+		rc = 0;
 		break;
 	}
 
diff --git a/src/link_udp.c b/src/link_udp.c
index 09b680f..01296dc 100644
--- a/src/link_udp.c
+++ b/src/link_udp.c
@@ -113,7 +113,7 @@ static int udp_read_cb(struct bsc_fd *fd)
 	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_set_data(link->the_link, msg);
+	mtp_link_set_data(link, msg);
 
 exit:
 	msgb_free(msg);
diff --git a/src/links.c b/src/links.c
index 1134e59..8802662 100644
--- a/src/links.c
+++ b/src/links.c
@@ -104,6 +104,7 @@ int link_init(struct bsc_data *bsc)
 	bsc->link_set->dpc = bsc->dpc;
 	bsc->link_set->opc = bsc->opc;
 	bsc->link_set->sccp_opc = bsc->sccp_opc > -1 ? bsc->sccp_opc : bsc->opc;
+	bsc->link_set->isup_opc = bsc->isup_opc > -1 ? bsc->isup_opc : bsc->opc;
 	bsc->link_set->sltm_once = bsc->once;
 	bsc->link_set->ni = bsc->ni_ni;
 	bsc->link_set->spare = bsc->ni_spare;
diff --git a/src/main.c b/src/main.c
index 3163527..6e33c9c 100644
--- a/src/main.c
+++ b/src/main.c
@@ -127,6 +127,11 @@ void mtp_link_set_forward_sccp(struct mtp_link_set *link, struct msgb *_msg, int
 	msc_send_msg(&bsc, rc, &result, _msg);
 }
 
+void mtp_link_set_forward_isup(struct mtp_link_set *set, struct msgb *msg, int sls)
+{
+	LOGP(DINP, LOGL_ERROR, "ISUP is not handled.\n");
+}
+
 /*
  * handle local message in close down mode
  */
@@ -624,6 +629,7 @@ int main(int argc, char **argv)
 	bsc.dpc = 1;
 	bsc.opc = 0;
 	bsc.sccp_opc = -1;
+	bsc.isup_opc = -1;
 	bsc.udp_port = 3456;
 	bsc.udp_ip = NULL;
 	bsc.src_port = 1313;
diff --git a/src/main_stp.c b/src/main_stp.c
new file mode 100644
index 0000000..d81b860
--- /dev/null
+++ b/src/main_stp.c
@@ -0,0 +1,275 @@
+/* Relay UDT/all SCCP messages */
+/*
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * 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 <mtp_pcap.h>
+#include <thread.h>
+#include <bsc_data.h>
+#include <snmp_mtp.h>
+#include <cellmgr_debug.h>
+#include <sctp_m2ua.h>
+
+#include <osmocore/talloc.h>
+
+#include <osmocom/vty/vty.h>
+#include <osmocom/vty/telnet_interface.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>
+
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE
+#endif
+#include <getopt.h>
+
+#undef PACKAGE_NAME
+#undef PACKAGE_VERSION
+#undef PACKAGE_BUGREPORT
+#undef PACKAGE_TARNAME
+#undef PACKAGE_STRING
+#include <cellmgr_config.h>
+
+static struct log_target *stderr_target;
+
+static char *config = "osmo_stp.cfg";
+
+struct bsc_data bsc;
+extern void cell_vty_init(void);
+
+/*
+ * methods called from the MTP Level3 part
+ */
+void mtp_link_set_forward_sccp(struct mtp_link_set *link, struct msgb *_msg, int sls)
+{
+	struct mtp_link_set *target;
+
+	target = bsc.m2ua_set == link ? bsc.link_set : bsc.m2ua_set;
+	mtp_link_set_submit_sccp_data(target, sls, _msg->l2h, msgb_l2len(_msg));
+}
+
+void mtp_link_set_forward_isup(struct mtp_link_set *set, struct msgb *msg, int sls)
+{
+	struct mtp_link_set *target;
+
+	target = bsc.m2ua_set == set ? bsc.link_set : bsc.m2ua_set;
+	mtp_link_set_submit_isup_data(target, sls, msg->l3h, msgb_l3len(msg));
+}
+
+void mtp_linkset_down(struct mtp_link_set *set)
+{
+	set->available = 0;
+	mtp_link_set_stop(set);
+}
+
+void mtp_linkset_up(struct mtp_link_set *set)
+{
+	set->available = 1;
+	mtp_link_set_reset(set);
+}
+
+static void print_usage()
+{
+	printf("Usage: osmo-stp\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)
+		link_shutdown_all(bsc.link_set);
+	exit(0);
+
+out:
+	pthread_mutex_unlock(&exit_mutex);
+}
+
+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("  -v --version. Print the version number\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'},
+			{"version", 0, 0, 0},
+			{0, 0, 0, 0},
+		};
+
+		c = getopt_long(argc, argv, "hc:p:v",
+				long_options, &option_index);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'h':
+			print_usage();
+			print_help();
+			exit(0);
+		case 'p':
+			if (bsc.pcap_fd >= 0)
+				close(bsc.pcap_fd);
+			bsc.pcap_fd = open(optarg, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP| S_IROTH);
+			if (bsc.pcap_fd < 0) {
+				fprintf(stderr, "Failed to open PCAP file.\n");
+				exit(0);
+			}
+			mtp_pcap_write_header(bsc.pcap_fd);
+			break;
+		case 'c':
+			config = optarg;
+			break;
+		case 'v':
+			printf("This is %s version %s.\n", PACKAGE, VERSION);
+			exit(0);
+			break;
+		default:
+			fprintf(stderr, "Unknown option.\n");
+			break;
+		}
+	}
+}
+
+int main(int argc, char **argv)
+{
+	int rc;
+	struct mtp_link *data;
+	struct mtp_m2ua_link *lnk;
+	INIT_LLIST_HEAD(&bsc.sccp_connections);
+
+	bsc.dpc = 1;
+	bsc.opc = 0;
+	bsc.sccp_opc = -1;
+	bsc.isup_opc = -1;
+	bsc.udp_port = 3456;
+	bsc.udp_ip = NULL;
+	bsc.src_port = 1313;
+	bsc.ni_ni = MTP_NI_NATION_NET;
+	bsc.ni_spare = 0;
+
+	mtp_link_set_init();
+	thread_init();
+
+	log_init(&log_info);
+	stderr_target = log_target_create_stderr();
+	log_add_target(stderr_target);
+
+	/* enable filters */
+	log_set_all_filter(stderr_target, 1);
+	log_set_category_filter(stderr_target, DINP, 1, LOGL_INFO);
+	log_set_category_filter(stderr_target, DSCCP, 1, LOGL_INFO);
+	log_set_category_filter(stderr_target, DMSC, 1, LOGL_INFO);
+	log_set_category_filter(stderr_target, DMGCP, 1, LOGL_INFO);
+	log_set_print_timestamp(stderr_target, 1);
+	log_set_use_color(stderr_target, 0);
+
+	sccp_set_log_area(DSCCP);
+
+	bsc.setup = 0;
+	bsc.msc_address = "127.0.0.1";
+	bsc.pcap_fd = -1;
+	bsc.udp_reset_timeout = 180;
+	bsc.ping_time = 20;
+	bsc.pong_time = 5;
+	bsc.msc_time = 20;
+	bsc.forward_only = 1;
+
+	handle_options(argc, argv);
+
+	signal(SIGPIPE, SIG_IGN);
+	signal(SIGINT, sigint);
+	srand(time(NULL));
+
+	cell_vty_init();
+	if (vty_read_config_file(config, NULL) < 0) {
+		fprintf(stderr, "Failed to read the VTY config.\n");
+		return -1;
+	}
+
+	rc = telnet_init(NULL, NULL, 4242);
+	if (rc < 0)
+		return rc;
+
+	if (link_init(&bsc) != 0)
+		return -1;
+
+	bsc.m2ua_set = mtp_link_set_alloc();
+	bsc.m2ua_set->dpc = 92;
+	bsc.m2ua_set->opc = 9;
+	bsc.m2ua_set->sccp_opc = 9;
+	bsc.m2ua_set->isup_opc = 9;
+	bsc.m2ua_set->ni = 3;
+	bsc.m2ua_set->bsc = &bsc;
+
+	/* for both links we want to have all isup messages */
+	bsc.m2ua_set->pass_all_isup = 1;
+	bsc.link_set->pass_all_isup = 1;
+
+	lnk = sctp_m2ua_transp_create("0.0.0.0", 2904);
+	lnk->base.pcap_fd = bsc.pcap_fd;
+	lnk->base.the_link = bsc.m2ua_set;
+	mtp_link_set_add_link(bsc.m2ua_set, (struct mtp_link *) lnk);
+
+	llist_for_each_entry(data, &bsc.m2ua_set->links, entry)
+		data->start(data);
+
+        while (1) {
+		bsc_select_main(0);
+        }
+
+	return 0;
+}
+
+/* dummy for links */
+int msc_init(struct bsc_data *data, int dummy)
+{
+	return 0;
+}
+
diff --git a/src/main_udt.c b/src/main_udt.c
index f13d4dd..b47ea66 100644
--- a/src/main_udt.c
+++ b/src/main_udt.c
@@ -70,6 +70,11 @@ void mtp_link_set_forward_sccp(struct mtp_link_set *link, struct msgb *_msg, int
 	msc_send_direct(&bsc, _msg);
 }
 
+void mtp_link_set_forward_isup(struct mtp_link_set *set, struct msgb *msg, int sls)
+{
+	LOGP(DINP, LOGL_ERROR, "ISUP is not handled.\n");
+}
+
 void mtp_linkset_down(struct mtp_link_set *set)
 {
 	set->available = 0;
@@ -181,6 +186,7 @@ int main(int argc, char **argv)
 	bsc.dpc = 1;
 	bsc.opc = 0;
 	bsc.sccp_opc = -1;
+	bsc.isup_opc = -1;
 	bsc.udp_port = 3456;
 	bsc.udp_ip = NULL;
 	bsc.src_port = 1313;
diff --git a/src/mtp_layer3.c b/src/mtp_layer3.c
index c3f3878..e9d01c9 100644
--- a/src/mtp_layer3.c
+++ b/src/mtp_layer3.c
@@ -101,7 +101,7 @@ static struct msgb *mtp_create_slta(struct mtp_link_set *link, struct mtp_level_
 	return out;
 }
 
-static struct msgb *mtp_tfp_alloc(struct mtp_link_set *link, int apoc)
+static struct msgb *mtp_base_alloc(struct mtp_link_set *link, int msg, int apoc)
 {
 	struct mtp_level_3_hdr *hdr;
 	struct mtp_level_3_prohib *prb;
@@ -114,12 +114,22 @@ static struct msgb *mtp_tfp_alloc(struct mtp_link_set *link, int apoc)
 	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->cmn.h1 = msg;
 	prb->apoc = MTP_MAKE_APOC(apoc);
 	return out;
 }
 
-static struct msgb *mtp_tra_alloc(struct mtp_link_set *link)
+static struct msgb *mtp_tfp_alloc(struct mtp_link_set *link, int apoc)
+{
+	return mtp_base_alloc(link, MTP_PROHIBIT_MSG_SIG, apoc);
+}
+
+static struct msgb *mtp_tfa_alloc(struct mtp_link_set *link, int apoc)
+{
+	return mtp_base_alloc(link, MTP_PROHIBIT_MSG_TFA, apoc);
+}
+
+static struct msgb *mtp_tra_alloc(struct mtp_link_set *link, int opc)
 {
 	struct mtp_level_3_hdr *hdr;
 	struct mtp_level_3_cmn *cmn;
@@ -130,6 +140,7 @@ static struct msgb *mtp_tra_alloc(struct mtp_link_set *link)
 
 	hdr = (struct mtp_level_3_hdr *) out->l2h;
 	hdr->ser_ind = MTP_SI_MNT_SNM_MSG;
+	hdr->addr = MTP_ADDR(0x0, link->dpc, opc);
 	cmn = (struct mtp_level_3_cmn *) msgb_put(out, sizeof(*cmn));
 	cmn->h0 = MTP_TRF_RESTR_MSG_GRP;
 	cmn->h1 = MTP_RESTR_MSG_ALLWED;
@@ -137,7 +148,7 @@ static struct msgb *mtp_tra_alloc(struct mtp_link_set *link)
 }
 
 static struct msgb *mtp_sccp_alloc_scmg(struct mtp_link_set *link,
-					int type, int assn, int sls)
+					int type, int assn, int apoc, int sls)
 {
 	struct sccp_data_unitdata *udt;
 	struct sccp_con_ctrl_prt_mgt *prt;
@@ -181,7 +192,7 @@ static struct msgb *mtp_sccp_alloc_scmg(struct mtp_link_set *link,
 	prt = (struct sccp_con_ctrl_prt_mgt *) msgb_put(out, sizeof(*prt));
 	prt->sst = type;
 	prt->assn = assn;
-	prt->apoc = MTP_MAKE_APOC(link->opc);
+	prt->apoc = apoc;
 	prt->mul_ind = 0;
 
 	return out;
@@ -289,9 +300,39 @@ void mtp_link_set_reset(struct mtp_link_set *link)
 	bsc_schedule_timer(&link->delay_timer, START_DELAY);
 }
 
-static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *hdr, int l3_len)
+static int send_tfp(struct mtp_link_set *link, int apoc)
+{
+	struct msgb *msg;
+	msg = mtp_tfp_alloc(link, apoc);
+	if (!msg)
+		return -1;
+
+	mtp_link_set_submit(link->slc[0], msg);
+	return 0;
+}
+
+static int send_tra(struct mtp_link_set *link, int opc)
+{
+	struct msgb *msg;
+	msg = mtp_tra_alloc(link, opc);
+	if (!msg)
+		return -1;
+	mtp_link_set_submit(link->slc[0], msg);
+	return 0;
+}
+
+static int send_tfa(struct mtp_link_set *link, int opc)
 {
 	struct msgb *msg;
+	msg = mtp_tfa_alloc(link, opc);
+	if (!msg)
+		return -1;
+	mtp_link_set_submit(link->slc[0], msg);
+	return 0;
+}
+
+static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *hdr, int l3_len)
+{
 	struct mtp_level_3_cmn *cmn;
 	uint16_t *apc;
 
@@ -313,16 +354,27 @@ static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *
 			link->sccp_up = 0;
 			mtp_link_set_sccp_down(link);
 
-			msg = mtp_tfp_alloc(link, 0);
-			if (!msg)
+			if (send_tfp(link, 0) != 0)
+				return -1;
+			if (send_tfp(link, link->opc) != 0)
+				return -1;
+			if (link->sccp_opc != link->opc &&
+			    send_tfp(link, link->sccp_opc) != 0)
+				return -1;
+			if (link->isup_opc != link->opc &&
+			    send_tfp(link, link->isup_opc) != 0)
 				return -1;
-			mtp_link_set_submit(link->slc[0], msg);
 
-			msg = mtp_tra_alloc(link);
-			if (!msg)
+			/* Send the TRA for all PCs */
+			if (send_tra(link, link->opc) != 0)
+				return -1;
+			if (link->sccp_opc != link->opc &&
+			    send_tfa(link, link->sccp_opc) != 0)
+				return -1;
+			if (link->isup_opc != link->opc &&
+			    send_tfa(link, link->isup_opc) != 0)
 				return -1;
 
-			mtp_link_set_submit(link->slc[0], msg);
 			link->sccp_up = 1;
 			link->was_up = 1;
 			LOGP(DINP, LOGL_INFO, "SCCP traffic allowed. %p\n", link);
@@ -453,7 +505,7 @@ static int mtp_link_sccp_data(struct mtp_link_set *link, struct mtp_level_3_hdr
 			type = SCCP_SSA;
 		}
 
-		out = mtp_sccp_alloc_scmg(link, type, prt->assn,
+		out = mtp_sccp_alloc_scmg(link, type, prt->assn, prt->apoc,
 					  MTP_LINK_SLS(hdr->addr));
 		if (!out)
 			return -1;
@@ -466,7 +518,7 @@ static int mtp_link_sccp_data(struct mtp_link_set *link, struct mtp_level_3_hdr
 	return 0;
 }
 
-int mtp_link_set_data(struct mtp_link_set *link, struct msgb *msg)
+int mtp_link_set_data(struct mtp_link *link, struct msgb *msg)
 {
 	int rc = -1;
 	struct mtp_level_3_hdr *hdr;
@@ -475,7 +527,7 @@ int mtp_link_set_data(struct mtp_link_set *link, struct msgb *msg)
 	if (!msg->l2h || msgb_l2len(msg) < sizeof(*hdr))
 		return -1;
 
-	if (!link->running) {
+	if (!link->the_link->running) {
 		LOGP(DINP, LOGL_ERROR, "Link is not running. Call mtp_link_reset first: %p\n", link);
 		return -1;
 	}
@@ -485,17 +537,17 @@ int mtp_link_set_data(struct mtp_link_set *link, struct msgb *msg)
 
 	switch (hdr->ser_ind) {
 	case MTP_SI_MNT_SNM_MSG:
-		rc = mtp_link_sign_msg(link, hdr, l3_len);
+		rc = mtp_link_sign_msg(link->the_link, hdr, l3_len);
 		break;
 	case MTP_SI_MNT_REG_MSG:
-		rc = mtp_link_regular_msg(link, hdr, l3_len);
+		rc = mtp_link_regular_msg(link->the_link, hdr, l3_len);
 		break;
 	case MTP_SI_MNT_SCCP:
-		rc = mtp_link_sccp_data(link, hdr, msg, l3_len);
+		rc = mtp_link_sccp_data(link->the_link, hdr, msg, l3_len);
 		break;
 	case MTP_SI_MNT_ISUP:
 		msg->l3h = &hdr->data[0];
-		rc = mtp_link_set_forward_isup(link, msg, MTP_LINK_SLS(hdr->addr));
+		rc = mtp_link_set_isup(link->the_link, msg, MTP_LINK_SLS(hdr->addr));
 		break;
 	default:
 		fprintf(stderr, "Unhandled: %u\n", hdr->ser_ind);
@@ -524,7 +576,7 @@ int mtp_link_set_submit_sccp_data(struct mtp_link_set *link, int sls, const uint
 int mtp_link_set_submit_isup_data(struct mtp_link_set *link, int sls,
 			      const uint8_t *data, unsigned int length)
 {
-	return mtp_int_submit(link, link->opc, sls, MTP_SI_MNT_ISUP, data, length);
+	return mtp_int_submit(link, link->isup_opc, sls, MTP_SI_MNT_ISUP, data, length);
 }
 
 static int mtp_int_submit(struct mtp_link_set *link, int pc, int sls, int type,
@@ -534,6 +586,9 @@ static int mtp_int_submit(struct mtp_link_set *link, int pc, int sls, int type,
 	struct mtp_level_3_hdr *hdr;
 	struct msgb *msg;
 
+	if (!link->slc[sls % 16])
+		return -1;
+
 	msg = mtp_msg_alloc(link);
 	if (!msg)
 		return -1;
diff --git a/src/sctp_m2ua.c b/src/sctp_m2ua.c
index 2ae7316..a4743fa 100644
--- a/src/sctp_m2ua.c
+++ b/src/sctp_m2ua.c
@@ -19,6 +19,7 @@
 #include <bsc_data.h>
 #include <cellmgr_debug.h>
 #include <mtp_data.h>
+#include <mtp_pcap.h>
 
 #include <osmocore/talloc.h>
 
@@ -296,6 +297,7 @@ static int m2ua_handle_data(struct sctp_m2ua_conn *conn,
 {
 	struct msgb *msg;
 	struct m2ua_msg_part *data;
+	struct mtp_link *link;
 
 	data = m2ua_msg_find_tag(m2ua, M2UA_TAG_DATA);
 	if (!data) {
@@ -316,7 +318,11 @@ static int m2ua_handle_data(struct sctp_m2ua_conn *conn,
 
 	msg->l2h = msgb_put(msg, data->len);
 	memcpy(msg->l2h, data->dat, data->len);
-	mtp_link_set_data(conn->trans->base.the_link, msg);
+
+	link = &conn->trans->base;
+	if (link->pcap_fd >= 0)
+		mtp_pcap_write_msu(link->pcap_fd, msg->l2h, msgb_l2len(msg));
+	mtp_link_set_data(link, msg);
 	msgb_free(msg);
 
 	return 0;
@@ -457,6 +463,9 @@ static int sctp_m2ua_write(struct mtp_link *link, struct msgb *msg)
 	if (!m2ua)
 		return -1;
 
+	if (link->pcap_fd >= 0)
+		mtp_pcap_write_msu(link->pcap_fd, msg->data, msg->len);
+
 	m2ua->hdr.msg_class = M2UA_CLS_MAUP;
 	m2ua->hdr.msg_type = M2UA_MAUP_DATA;
 
diff --git a/tests/isup/isup_parse_test.c b/tests/isup/isup_parse_test.c
index 5c3341c..c6dbdf4 100644
--- a/tests/isup/isup_parse_test.c
+++ b/tests/isup/isup_parse_test.c
@@ -1,6 +1,6 @@
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software: you can redistribute it and/or modify
@@ -47,7 +47,7 @@ static void test_grs_parsing()
 	int range;
 
 	hdr = (struct isup_msg_hdr *) isup_grs;
-	range = isup_parse_grs(&hdr->data[0], 3);
+	range = isup_parse_status(&hdr->data[0], 3);
 
 	ASSERT(range, 28);
 }
@@ -63,3 +63,4 @@ int main(int argc, char **argv)
 
 /* stubs */
 int mtp_link_set_submit_isup_data() {return -1;}
+int mtp_link_set_forward_isup(struct mtp_link_set *s, struct msgb *m, int l) { abort(); }