udp: Allow to run multiple links through the same socket

The UDP socket will be shared between multiple links, the
snmp session will be also shared between multiple links on
the same hardware.

2 files changed, 71 insertions, 29 deletions

diff --git a/src/link_udp.c b/src/link_udp.c
index 7e0b94b..04dcec8 100644
--- a/src/link_udp.c
+++ b/src/link_udp.c
@@ -35,12 +35,30 @@
 #include <string.h>
 #include <unistd.h>
 
+static struct mtp_udp_link *find_link(struct mtp_udp_data *data, uint16_t link_index)
+{
+	struct mtp_udp_link *lnk;
+
+	llist_for_each_entry(lnk, &data->links, entry)
+		if (lnk->link_index == link_index)
+			return lnk;
+
+	return NULL;
+}
+
+
 static int udp_write_cb(struct bsc_fd *fd, struct msgb *msg)
 {
+	struct mtp_udp_data *data;
 	struct mtp_udp_link *link;
 	int rc;
 
-	link = fd->data;
+	data = fd->data;
+	link = find_link(data, msg->cb[0]);
+	if (!link) {
+		LOGP(DINP, LOGL_ERROR, "Failed to find link with %lu\n", msg->cb[0]);
+		return -1;
+	}
 
 	LOGP(DINP, LOGL_DEBUG, "Sending MSU: %s\n", hexdump(msg->data, msg->len));
 	if (link->base.pcap_fd >= 0)
@@ -59,6 +77,7 @@ static int udp_write_cb(struct bsc_fd *fd, struct msgb *msg)
 
 static int udp_read_cb(struct bsc_fd *fd)
 {
+	struct mtp_udp_data *data;
 	struct mtp_link *link;
 	struct udp_data_hdr *hdr;
 	struct msgb *msg;
@@ -72,7 +91,7 @@ static int udp_read_cb(struct bsc_fd *fd)
 	}
 	    
 
-	link = (struct mtp_link *) fd->data;
+	data = (struct mtp_udp_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);
@@ -80,6 +99,16 @@ static int udp_read_cb(struct bsc_fd *fd)
 		goto exit;
 	}
 
+	hdr = (struct udp_data_hdr *) msgb_put(msg, sizeof(*hdr));
+	link = (struct mtp_link *) find_link(data, ntohs(hdr->data_link_index));
+
+	if (!link) {
+		LOGP(DINP, LOGL_ERROR, "No link registered for %d\n",
+		     ntohs(hdr->data_link_index));
+		goto exit;
+	}
+
+
 	/* throw away data as the link is down */
 	if (link->set->available == 0) {
 		LOGP(DINP, LOGL_ERROR, "The link is down. Not forwarding.\n");
@@ -87,8 +116,6 @@ static int udp_read_cb(struct bsc_fd *fd)
 		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");
 		mtp_link_failure(link);
@@ -129,7 +156,7 @@ static void do_start(void *_data)
 {
 	struct mtp_udp_link *link = (struct mtp_udp_link *) _data;
 
-	snmp_mtp_activate(link->session, link->link_index);
+	snmp_mtp_activate(link->data->session, link->link_index);
 	mtp_link_up(&link->base);
 }
 
@@ -142,7 +169,7 @@ static int udp_link_reset(struct mtp_link *link)
 	LOGP(DINP, LOGL_NOTICE, "Will restart SLTM transmission in %d seconds.\n",
 	     ulnk->reset_timeout);
 
-	snmp_mtp_deactivate(ulnk->session, ulnk->link_index);
+	snmp_mtp_deactivate(ulnk->data->session, ulnk->link_index);
 	mtp_link_down(link);
 
 	/* restart the link in 90 seconds... to force a timeout on the BSC */
@@ -166,7 +193,9 @@ static int udp_link_write(struct mtp_link *link, struct msgb *msg)
 	hdr->user_context = 0;
 	hdr->data_length = htonl(msgb_l2len(msg));
 
-	if (write_queue_enqueue(&ulnk->write_queue, msg) != 0) {
+	msg->cb[0] = ulnk->link_index;
+
+	if (write_queue_enqueue(&ulnk->data->write_queue, msg) != 0) {
 		LOGP(DINP, LOGL_ERROR, "Failed to enqueue msg.\n");
 		msgb_free(msg);
 		return -1;
@@ -182,14 +211,8 @@ static int udp_link_start(struct mtp_link *link)
 	return 0;
 }
 
-int link_udp_init(struct mtp_udp_link *link, int src_port, const char *remote, int remote_port)
+int link_udp_init(struct mtp_udp_link *link, const char *remote, int port)
 {
-	struct sockaddr_in addr;
-	int fd;
-	int on;
-
-	write_queue_init(&link->write_queue, 100);
-
 	/* function table */
 	link->base.shutdown = udp_link_dummy;
 	link->base.clear_queue = udp_link_dummy;
@@ -198,13 +221,33 @@ int link_udp_init(struct mtp_udp_link *link, int src_port, const char *remote, i
 	link->base.start = udp_link_start;
 	link->base.write = udp_link_write;
 
+	/* prepare the remote */
+	memset(&link->remote, 0, sizeof(link->remote));
+	link->remote.sin_family = AF_INET;
+	link->remote.sin_port = htons(port);
+	inet_aton(remote, &link->remote.sin_addr);
+
+	/* add it to the list of udp connections */
+	llist_add(&link->entry, &link->data->links);
+	return 0;
+}
+
+int link_global_init(struct mtp_udp_data *data, int src_port)
+{
+	struct sockaddr_in addr;
+	int fd;
+	int on;
+
+	INIT_LLIST_HEAD(&data->links);
+	write_queue_init(&data->write_queue, 100);
+
 	/* socket creation */
-	link->write_queue.bfd.data = link;
-	link->write_queue.bfd.when = BSC_FD_READ;
-	link->write_queue.read_cb = udp_read_cb;
-	link->write_queue.write_cb = udp_write_cb;
+	data->write_queue.bfd.data = data;
+	data->write_queue.bfd.when = BSC_FD_READ;
+	data->write_queue.read_cb = udp_read_cb;
+	data->write_queue.write_cb = udp_write_cb;
 
-	link->write_queue.bfd.fd = fd = socket(AF_INET, SOCK_DGRAM, 0);
+	data->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;
@@ -225,12 +268,7 @@ int link_udp_init(struct mtp_udp_link *link, int src_port, const char *remote, i
 	}
 
 	/* now connect the socket to the remote */
-	memset(&link->remote, 0, sizeof(link->remote));
-	link->remote.sin_family = AF_INET;
-	link->remote.sin_port = htons(remote_port);
-	inet_aton(remote, &link->remote.sin_addr);
-
-	if (bsc_register_fd(&link->write_queue.bfd) != 0) {
+	if (bsc_register_fd(&data->write_queue.bfd) != 0) {
 		LOGP(DINP, LOGL_ERROR, "Failed to register BFD.\n");
 		close(fd);
 		return -1;
diff --git a/src/links.c b/src/links.c
index 6374afd..bf282b6 100644
--- a/src/links.c
+++ b/src/links.c
@@ -114,6 +114,7 @@ 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->bsc = bsc;
+	lnk->data = &bsc->udp_data;
 	lnk->link_index = 1;
 	lnk->reset_timeout = bsc->udp_reset_timeout;
 	mtp_link_set_add_link(bsc->link_set, (struct mtp_link *) lnk);
@@ -126,12 +127,15 @@ int link_init(struct bsc_data *bsc)
 	LOGP(DINP, LOGL_NOTICE, "Using UDP MTP mode.\n");
 
 	/* setup SNMP first, it is blocking */
-	lnk->session = snmp_mtp_session_create(bsc->udp_ip);
-	if (!lnk->session)
+	bsc->udp_data.session = snmp_mtp_session_create(bsc->udp_ip);
+	if (!bsc->udp_data.session)
+		return -1;
+
+	if (link_global_init(&bsc->udp_data, bsc->src_port) != 0)
 		return -1;
 
 	/* now connect to the transport */
-	if (link_udp_init(lnk, bsc->src_port, bsc->udp_ip, bsc->udp_port) != 0)
+	if (link_udp_init(lnk, bsc->udp_ip, bsc->udp_port) != 0)
 		return -1;
 
 	/*
@@ -140,7 +144,7 @@ int link_init(struct bsc_data *bsc)
 	 * SLTM and it begins a reset. Then we will take it up
 	 * again and do the usual business.
 	 */
-	snmp_mtp_deactivate(lnk->session,
+	snmp_mtp_deactivate(lnk->data->session,
 			    lnk->link_index);
 	bsc->start_timer.cb = start_rest;
 	bsc->start_timer.data = &bsc;