Merge branch 'on-waves/multiple-links'

10 files changed, 300 insertions, 190 deletions

diff --git a/include/bsc_data.h b/include/bsc_data.h
index 3d10bb3..758932f 100644
--- a/include/bsc_data.h
+++ b/include/bsc_data.h
@@ -112,8 +112,6 @@ struct bsc_data {
 
 /* bsc related functions */
 void release_bsc_resources(struct bsc_data *bsc);
-void mtp_link_down(struct mtp_link *data);
-void mtp_link_up(struct mtp_link *data);
 
 void mtp_linkset_down(struct mtp_link_set *);
 void mtp_linkset_up(struct mtp_link_set *);
diff --git a/include/mtp_data.h b/include/mtp_data.h
index e00f13a..f04c07d 100644
--- a/include/mtp_data.h
+++ b/include/mtp_data.h
@@ -26,6 +26,7 @@
 
 struct bsc_data;
 struct mtp_link;
+struct mtp_level_3_mng *mng;
 
 /* MTP Level3 timers */
 
@@ -48,24 +49,14 @@ struct mtp_link_set {
 	int available;
 	int running;
 	int sccp_up;
+	int linkset_up;
 
 	int last_sls;
 
-	/* misc data */
-	uint8_t test_ptrn[14];
-
-	int sltm_pending;
-	int sltm_once;
-	int was_up;
-
-	int slta_misses;
-	struct timer_list t1_timer;
-	struct timer_list t2_timer;
-
-	struct timer_list delay_timer;
-
 	struct llist_head links;
+	int nr_links;
 	struct mtp_link *slc[16];
+	int sltm_once;
 
 	/* special handling */
 	int pass_all_isup;
@@ -84,12 +75,24 @@ struct mtp_link {
 	struct llist_head entry;
 
 	int pcap_fd;
-	struct mtp_link_set *the_link;
+	struct mtp_link_set *set;
 
 	int available;
 
 	struct timer_list link_activate;
 
+	/* link test routine */
+	uint8_t test_ptrn[14];
+
+	int link_no;
+	int sltm_pending;
+	int was_up;
+
+	int slta_misses;
+	struct timer_list t1_timer;
+	struct timer_list t2_timer;
+
+	/* callback's to implement */
 	int (*start)(struct mtp_link *);
 	int (*write)(struct mtp_link *, struct msgb *msg);
 	int (*shutdown)(struct mtp_link *);
@@ -117,6 +120,19 @@ 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);
+
+/* link related routines */
+void mtp_link_down(struct mtp_link *data);
+void mtp_link_up(struct mtp_link *data);
+
+void mtp_link_init(struct mtp_link *link);
+void mtp_link_start_link_test(struct mtp_link *link);
+void mtp_link_stop_link_test(struct mtp_link *link);
+int mtp_link_slta(struct mtp_link *link, uint16_t l3_len, struct mtp_level_3_mng *mng);
+
+void mtp_link_failure(struct mtp_link *fail);
+
+/* internal routines */
+struct msgb *mtp_msg_alloc(struct mtp_link_set *link);
 
 #endif
diff --git a/include/mtp_level3.h b/include/mtp_level3.h
index bc2d186..f311ad5 100644
--- a/include/mtp_level3.h
+++ b/include/mtp_level3.h
@@ -65,7 +65,7 @@
 #define SCCP_SSA	0x01
 
 #define MTP_LINK_MASK	    0x0F
-#define MTP_ADDR_MASK	    0x0FFF
+#define MTP_ADDR_MASK	    0x3FFF
 #define MTP_APOC_MASK 0x3f
 
 
@@ -77,6 +77,10 @@
 	 ((link) & MTP_LINK_MASK) << 28)
 #define MTP_MAKE_APOC(apoc) \
 	(apoc & 0x3fff)
+#define MTP_ADDR_DPC(addr) \
+	(addr & MTP_ADDR_MASK)
+#define MTP_ADDR_OPC(addr) \
+	((addr >> 14) & MTP_ADDR_MASK)
 #elif __BYTE_ORDER == __BIG_ENDIAN
 static inline uint32_t c_swap_32(uint32_t in)
 {
@@ -97,6 +101,10 @@ static inline uint16_t c_swap_16(uint16_t in)
          ((link) & MTP_LINK_MASK) << 28)
 #define MTP_MAKE_APOC(apoc) \
 	c_swap_16((apoc & 0x3fff))
+#define MTP_ADDR_DPC(addr) \
+	(c_swap_32(addr) & MTP_ADDR_MASK)
+#define MTP_ADDR_OPC(addr) \
+	((c_swap_32(addr) >> 14) & MTP_ADDR_MASK)
 #endif
 
 
diff --git a/src/Makefile.am b/src/Makefile.am
index eb6ab11..defe121 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -11,17 +11,19 @@ mgcp_mgw_LDADD = $(LAFORGE_LIBS) $(NEXUSWARE_C7_LIBS) $(NEXUSWARE_UNIPORTE_LIBS)
 
 cellmgr_ng_SOURCES = main.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \
 		     bss_patch.c bssap_sccp.c bsc_sccp.c bsc_ussd.c links.c \
-		     msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c isup.c
+		     msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c isup.c \
+		     mtp_link.c
 cellmgr_ng_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \
 		   -lpthread -lnetsnmp -lcrypto
 
 udt_relay_SOURCES = main_udt.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \
 		     msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c \
-		     bss_patch.c isup.c links.c sctp_m2ua.c
+		     bss_patch.c isup.c links.c sctp_m2ua.c mtp_link.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
+		   debug.c vty_interface.c links.c isup.c sctp_m2ua.c \
+		   mtp_link.c
 osmo_stp_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \
 		   -lpthread -lnetsnmp -lcrypto -lm2ua -lsctp
diff --git a/src/link_udp.c b/src/link_udp.c
index 01296dc..7e0b94b 100644
--- a/src/link_udp.c
+++ b/src/link_udp.c
@@ -81,7 +81,7 @@ static int udp_read_cb(struct bsc_fd *fd)
 	}
 
 	/* throw away data as the link is down */
-	if (link->the_link->available == 0) {
+	if (link->set->available == 0) {
 		LOGP(DINP, LOGL_ERROR, "The link is down. Not forwarding.\n");
 		rc = 0;
 		goto exit;
@@ -91,12 +91,11 @@ static int udp_read_cb(struct bsc_fd *fd)
 
 	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");
-		mtp_link_down(link);
-		mtp_link_up(link);
+		mtp_link_failure(link);
 		goto exit;
 	} else if (hdr->data_type > UDP_DATA_MSU_PRIO_3) {
 		LOGP(DINP, LOGL_ERROR, "Link failure. retrieved message.\n");
-		mtp_link_down(link);
+		mtp_link_failure(link);
 		goto exit;
 	}
 
diff --git a/src/links.c b/src/links.c
index 8802662..6374afd 100644
--- a/src/links.c
+++ b/src/links.c
@@ -46,29 +46,30 @@ void mtp_link_down(struct mtp_link *link)
 
 	was_up = link->available;
 	link->available = 0;
-	one_up = is_one_up(link->the_link);
+	link->was_up = 0;
+	one_up = is_one_up(link->set);
 
 	/* our linkset is now unsuable */
 	if (was_up && !one_up)
-		mtp_linkset_down(link->the_link);
+		mtp_linkset_down(link->set);
 	link->clear_queue(link);
-	mtp_link_set_init_slc(link->the_link);
+	mtp_link_stop_link_test(link);
+	mtp_link_set_init_slc(link->set);
 }
 
 void mtp_link_up(struct mtp_link *link)
 {
 	int one_up;
 
-	one_up = is_one_up(link->the_link);
+	one_up = is_one_up(link->set);
 	link->available = 1;
+	link->was_up = 0;
 
-	mtp_link_set_init_slc(link->the_link);
+	mtp_link_set_init_slc(link->set);
 	if (!one_up)
-		mtp_linkset_up(link->the_link);
-}
-
-void mtp_link_set_sccp_down(struct mtp_link_set *link)
-{
+		mtp_linkset_up(link->set);
+	else
+		mtp_link_start_link_test(link);
 }
 
 void mtp_link_set_submit(struct mtp_link *link, struct msgb *msg)
@@ -112,7 +113,6 @@ int link_init(struct bsc_data *bsc)
 
 	lnk = talloc_zero(bsc->link_set, struct mtp_udp_link);
 	lnk->base.pcap_fd = bsc->pcap_fd;
-	lnk->base.the_link = bsc->link_set;
 	lnk->bsc = bsc;
 	lnk->link_index = 1;
 	lnk->reset_timeout = bsc->udp_reset_timeout;
diff --git a/src/main_stp.c b/src/main_stp.c
index d81b860..36e04d6 100644
--- a/src/main_stp.c
+++ b/src/main_stp.c
@@ -254,7 +254,6 @@ int main(int argc, char **argv)
 
 	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)
diff --git a/src/mtp_layer3.c b/src/mtp_layer3.c
index e9d01c9..67ae70e 100644
--- a/src/mtp_layer3.c
+++ b/src/mtp_layer3.c
@@ -36,7 +36,7 @@ static void *tall_mtp_ctx = NULL;
 
 static int mtp_int_submit(struct mtp_link_set *link, int pc, int sls, int type, const uint8_t *data, unsigned int length);
 
-static struct msgb *mtp_msg_alloc(struct mtp_link_set *link)
+struct msgb *mtp_msg_alloc(struct mtp_link_set *link)
 {
 	struct mtp_level_3_hdr *hdr;
 	struct msgb *msg = msgb_alloc_headroom(4096, 128, "mtp-msg");
@@ -53,34 +53,8 @@ static struct msgb *mtp_msg_alloc(struct mtp_link_set *link)
 	return msg;
 }
 
-static struct msgb *mtp_create_sltm(struct mtp_link_set *link)
-{
-	const uint8_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);
-	uint8_t *data;
-	if (!msg)
-		return NULL;
-
-	hdr = (struct mtp_level_3_hdr *) msg->l2h;
-	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_set *link, struct mtp_level_3_mng *in_mng, int l3_len)
+static struct msgb *mtp_create_slta(struct mtp_link_set *link, int sls,
+				    struct mtp_level_3_mng *in_mng, int l3_len)
 {
 	struct mtp_level_3_hdr *hdr;
 	struct mtp_level_3_mng *mng;
@@ -91,6 +65,8 @@ static struct msgb *mtp_create_slta(struct mtp_link_set *link, struct mtp_level_
 
 	hdr = (struct mtp_level_3_hdr *) out->l2h;
 	hdr->ser_ind = MTP_SI_MNT_REG_MSG;
+	hdr->addr = MTP_ADDR(sls, link->dpc, link->opc);
+
 	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;
@@ -101,6 +77,7 @@ static struct msgb *mtp_create_slta(struct mtp_link_set *link, struct mtp_level_
 	return out;
 }
 
+
 static struct msgb *mtp_base_alloc(struct mtp_link_set *link, int msg, int apoc)
 {
 	struct mtp_level_3_hdr *hdr;
@@ -203,64 +180,6 @@ void mtp_link_set_init(void)
 	tall_mtp_ctx = talloc_named_const(NULL, 1, "mtp-link");
 }
 
-static void mtp_send_sltm(struct mtp_link_set *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_set_submit(link->slc[0], msg);
-}
-
-static void mtp_sltm_t1_timeout(void *_link)
-{
-	struct mtp_link_set *link = (struct mtp_link_set *) _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_set_sccp_down(link);
-		mtp_link_restart(link->slc[0]);
-	}
-}
-
-static void mtp_sltm_t2_timeout(void *_link)
-{
-	struct mtp_link_set *link = (struct mtp_link_set *) _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_set *mtp_link_set_alloc(void)
 {
 	struct mtp_link_set *link;
@@ -270,12 +189,6 @@ struct mtp_link_set *mtp_link_set_alloc(void)
 		return NULL;
 
 	link->ni = MTP_NI_NATION_NET;
-	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->links);
 
 	return link;
@@ -283,21 +196,23 @@ struct mtp_link_set *mtp_link_set_alloc(void)
 
 void mtp_link_set_stop(struct mtp_link_set *link)
 {
-	bsc_del_timer(&link->t1_timer);
-	bsc_del_timer(&link->t2_timer);
-	bsc_del_timer(&link->delay_timer);
+	struct mtp_link *lnk;
+	llist_for_each_entry(lnk, &link->links, entry)
+		mtp_link_stop_link_test(lnk);
+
 	link->sccp_up = 0;
 	link->running = 0;
-	link->sltm_pending = 0;
-
-	mtp_link_set_sccp_down(link);
+	link->linkset_up = 0;
 }
 
 void mtp_link_set_reset(struct mtp_link_set *link)
 {
+	struct mtp_link *lnk;
 	mtp_link_set_stop(link);
 	link->running = 1;
-	bsc_schedule_timer(&link->delay_timer, START_DELAY);
+
+	llist_for_each_entry(lnk, &link->links, entry)
+		mtp_link_start_link_test(lnk);
 }
 
 static int send_tfp(struct mtp_link_set *link, int apoc)
@@ -331,6 +246,39 @@ static int send_tfa(struct mtp_link_set *link, int opc)
 	return 0;
 }
 
+static int linkset_up(struct mtp_link_set *set)
+{
+	/* the link set is already up */
+	if (set->linkset_up)
+		return 0;
+
+	if (send_tfp(set, 0) != 0)
+		return -1;
+	if (send_tfp(set, set->opc) != 0)
+		return -1;
+	if (set->sccp_opc != set->opc &&
+	    send_tfp(set, set->sccp_opc) != 0)
+		return -1;
+	if (set->isup_opc != set->opc &&
+	    send_tfp(set, set->isup_opc) != 0)
+		return -1;
+
+	/* Send the TRA for all PCs */
+	if (send_tra(set, set->opc) != 0)
+		return -1;
+	if (set->sccp_opc != set->opc &&
+	    send_tfa(set, set->sccp_opc) != 0)
+		return -1;
+	if (set->isup_opc != set->opc &&
+	    send_tfa(set, set->isup_opc) != 0)
+		return -1;
+
+	set->linkset_up = 1;
+	LOGP(DINP, LOGL_NOTICE,
+	     "The linkset %p is considered running.\n", set);
+	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;
@@ -351,32 +299,7 @@ static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *
 		switch (cmn->h1) {
 		case MTP_RESTR_MSG_ALLWED:
 			LOGP(DINP, LOGL_INFO, "Received Restart Allowed. SST could be next: %p\n", link);
-			link->sccp_up = 0;
-			mtp_link_set_sccp_down(link);
-
-			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;
-
-			/* 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;
-
 			link->sccp_up = 1;
-			link->was_up = 1;
 			LOGP(DINP, LOGL_INFO, "SCCP traffic allowed. %p\n", link);
 			return 0;
 			break;
@@ -403,17 +326,23 @@ static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *
 	return -1;
 }
 
-static int mtp_link_regular_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *hdr, int l3_len)
+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->ni != link->ni || l3_len < 1) {
+	if (hdr->ni != link->set->ni || l3_len < 1) {
 		LOGP(DINP, LOGL_ERROR, "Unhandled data (ni: %d len: %d)\n",
 		     hdr->ni, l3_len);
 		return -1;
 	}
 
+	if (MTP_ADDR_DPC(hdr->addr) != link->set->opc) {
+		LOGP(DINP, LOGL_ERROR, "MSG for 0x%x not handled by 0x%x\n",
+			MTP_ADDR_DPC(hdr->addr), link->set->opc);
+		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);
@@ -423,32 +352,18 @@ static int mtp_link_regular_msg(struct mtp_link_set *link, struct mtp_level_3_hd
 		switch (mng->cmn.h1) {
 		case MTP_TST_MSG_SLTM:
 			/* simply respond to the request... */
-			out = mtp_create_slta(link, mng, l3_len);
+			out = mtp_create_slta(link->set,
+					      MTP_LINK_SLS(hdr->addr),
+					      mng, l3_len);
 			if (!out)
 				return -1;
-			mtp_link_set_submit(link->slc[0], out);
+			mtp_link_set_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;
-			return 0;
+			/* If this link is proven set it up */
+			if (mtp_link_slta(link, l3_len, mng) == 0)
+				linkset_up(link->set);
 			break;
 		}
 		break;
@@ -527,7 +442,7 @@ int mtp_link_set_data(struct mtp_link *link, struct msgb *msg)
 	if (!msg->l2h || msgb_l2len(msg) < sizeof(*hdr))
 		return -1;
 
-	if (!link->the_link->running) {
+	if (!link->set->running) {
 		LOGP(DINP, LOGL_ERROR, "Link is not running. Call mtp_link_reset first: %p\n", link);
 		return -1;
 	}
@@ -537,17 +452,17 @@ int mtp_link_set_data(struct mtp_link *link, struct msgb *msg)
 
 	switch (hdr->ser_ind) {
 	case MTP_SI_MNT_SNM_MSG:
-		rc = mtp_link_sign_msg(link->the_link, hdr, l3_len);
+		rc = mtp_link_sign_msg(link->set, hdr, l3_len);
 		break;
 	case MTP_SI_MNT_REG_MSG:
-		rc = mtp_link_regular_msg(link->the_link, hdr, l3_len);
+		rc = mtp_link_regular_msg(link, hdr, l3_len);
 		break;
 	case MTP_SI_MNT_SCCP:
-		rc = mtp_link_sccp_data(link->the_link, hdr, msg, l3_len);
+		rc = mtp_link_sccp_data(link->set, hdr, msg, l3_len);
 		break;
 	case MTP_SI_MNT_ISUP:
 		msg->l3h = &hdr->data[0];
-		rc = mtp_link_set_isup(link->the_link, msg, MTP_LINK_SLS(hdr->addr));
+		rc = mtp_link_set_isup(link->set, msg, MTP_LINK_SLS(hdr->addr));
 		break;
 	default:
 		fprintf(stderr, "Unhandled: %u\n", hdr->ser_ind);
@@ -648,6 +563,10 @@ void mtp_link_set_init_slc(struct mtp_link_set *set)
 
 void mtp_link_set_add_link(struct mtp_link_set *set, struct mtp_link *lnk)
 {
+	lnk->set = set;
+	lnk->link_no = set->nr_links++;
+	mtp_link_init(lnk);
+
 	llist_add_tail(&lnk->entry, &set->links);
 	mtp_link_set_init_slc(set);
 }
diff --git a/src/mtp_link.c b/src/mtp_link.c
new file mode 100644
index 0000000..5e5f418
--- /dev/null
+++ b/src/mtp_link.c
@@ -0,0 +1,157 @@
+/* MTP level3 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 Affero General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <mtp_data.h>
+#include <mtp_level3.h>
+#include <cellmgr_debug.h>
+
+#include <string.h>
+
+static struct msgb *mtp_create_sltm(struct mtp_link *link)
+{
+	const uint8_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->set);
+	uint8_t *data;
+	if (!msg)
+		return NULL;
+
+	hdr = (struct mtp_level_3_hdr *) msg->l2h;
+	hdr->ser_ind = MTP_SI_MNT_REG_MSG;
+	hdr->addr = MTP_ADDR(link->link_no % 16, link->set->dpc, link->set->opc);
+
+	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 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;
+	}
+
+	link->write(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);
+		bsc_del_timer(&link->t2_timer);
+		mtp_link_failure(link);
+	}
+}
+
+static void mtp_sltm_t2_timeout(void *_link)
+{
+	struct mtp_link *link = (struct mtp_link *) _link;
+
+	if (!link->set->running) {
+		LOGP(DINP, LOGL_INFO, "The linkset is not active. Stopping SLTM now. %p\n", link);
+		return;
+	}
+
+	link->slta_misses = 0;
+	mtp_send_sltm(link);
+
+	bsc_schedule_timer(&link->t1_timer, MTP_T1);
+
+	if (link->set->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);
+}
+
+void mtp_link_init(struct mtp_link *link)
+{
+	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;
+}
+
+void mtp_link_stop_link_test(struct mtp_link *link)
+{
+	bsc_del_timer(&link->t1_timer);
+	bsc_del_timer(&link->t2_timer);
+
+	link->sltm_pending = 0;
+}
+
+void mtp_link_start_link_test(struct mtp_link *link)
+{
+	mtp_sltm_t2_timeout(link);
+}
+
+int mtp_link_slta(struct mtp_link *link, uint16_t l3_len,
+		  struct mtp_level_3_mng *mng)
+{
+	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;
+	link->was_up = 1;
+
+	return 0;
+}
+
+void mtp_link_failure(struct mtp_link *link)
+{
+	LOGP(DINP, LOGL_ERROR, "Link has failed. Resetting it: 0x%p\n", link);
+	link->reset(link);
+}
diff --git a/tests/mtp/mtp_parse_test.c b/tests/mtp/mtp_parse_test.c
index 2ef726b..cd5d603 100644
--- a/tests/mtp/mtp_parse_test.c
+++ b/tests/mtp/mtp_parse_test.c
@@ -589,6 +589,18 @@ int main(int argc, char **argv)
 	tests[2].hdr.addr = MTP_ADDR(0x00, 136, 91);
 	tests[2].prohib.apoc = MTP_MAKE_APOC(86);
 
+	addr = MTP_ADDR(0x00, 0x2AAA, 0x1555);
+	if (MTP_ADDR_OPC(addr) != 0x1555) {
+		fprintf(stderr, "Failed to extract OPC: %d\n",
+			MTP_ADDR_OPC(addr));
+		abort();
+	}
+	if (MTP_ADDR_DPC(addr) != 0x2AAA) {
+		fprintf(stderr, "Failed to extract DPC: %d\n",
+			MTP_ADDR_DPC(addr));
+		abort();
+	}
+
 	for (i = 0; i < ARRAY_SIZE(tests); ++i) {
 		check_hdr(tests[i].input, &tests[i].hdr);
 		if (tests[i].has_mng)