stp: Add code that allows to inject messages on any linkset

This can be useful to test out certain messages without having
any of the linksets be fully connected. It is not possible to
get the result. In the future this code should reply with an
M2UA error message if something went wrong.

4 files changed, 145 insertions, 2 deletions

diff --git a/include/bsc_data.h b/include/bsc_data.h
index fe6d295..1009d46 100644
--- a/include/bsc_data.h
+++ b/include/bsc_data.h
@@ -134,6 +134,10 @@ struct bsc_data {
 	uint16_t lac;
 
 	int forward_only;
+
+	/* inject */
+	struct bsc_fd inject_fd;
+	struct llist_head inject_list;
 };
 
 /* bsc related functions */
diff --git a/include/mtp_data.h b/include/mtp_data.h
index 0e35ab5..9314962 100644
--- a/include/mtp_data.h
+++ b/include/mtp_data.h
@@ -137,6 +137,7 @@ void mtp_link_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);
+int mtp_link_set_send(struct mtp_link_set *set, struct msgb *msg);
 
 /* link related routines */
 void mtp_link_down(struct mtp_link *data);
diff --git a/src/main_stp.c b/src/main_stp.c
index 444e7be..57f509f 100644
--- a/src/main_stp.c
+++ b/src/main_stp.c
@@ -179,6 +179,122 @@ static void handle_options(int argc, char **argv)
 	}
 }
 
+static struct mtp_link_set *find_link_set(struct bsc_data *bsc,
+					  int len, const char *buf)
+{
+	if (strncmp(buf, "mtp", len) == 0)
+		return bsc->link_set;
+	else if (strncmp(buf, "m2ua", len) == 0)
+		return bsc->m2ua_set;
+	return NULL;
+}
+
+static int inject_read_cb(struct bsc_fd *fd, unsigned int what)
+{
+	struct msgb *msg;
+	struct m2ua_msg_part *data, *link;
+	struct bsc_data *bsc;
+	struct m2ua_msg *m2ua;
+	struct mtp_link_set *out_set;
+	uint8_t buf[4096];
+
+	bsc = fd->data;
+
+	int rc = read(fd->fd, buf, sizeof(buf));
+	if (rc <= 0) {
+		LOGP(DINP, LOGL_ERROR, "Failed to read from the console.\n");
+		return -1;
+	}
+
+	m2ua = m2ua_from_msg(rc, buf);
+	if (!m2ua) {
+		LOGP(DINP, LOGL_ERROR, "Failed to parse M2UA.\n");
+		return -1;
+	}
+
+	if (m2ua->hdr.msg_class == M2UA_CLS_MAUP && m2ua->hdr.msg_type == M2UA_MAUP_DATA) {
+		data = m2ua_msg_find_tag(m2ua, M2UA_TAG_DATA);
+		if (!data) {
+			LOGP(DINP, LOGL_ERROR, "MAUP Data without data.\n");
+			goto exit;
+		}
+
+		if (data->len > 2048) {
+			LOGP(DINP, LOGL_ERROR, "Data is too big for this configuration.\n");
+			goto exit;
+		}
+
+		link = m2ua_msg_find_tag(m2ua, MUA_TAG_IDENT_TEXT);
+		if (!link) {
+			LOGP(DINP, LOGL_ERROR, "Interface Identifier Text is mandantory.\n");
+			goto exit;
+		}
+
+		if (link->len > 255) {
+			LOGP(DINP, LOGL_ERROR, "Spec violation. Ident text should be shorter than 255.\n");
+			goto exit;
+		}
+
+		out_set = find_link_set(bsc, link->len, (const char *) link->dat);
+		if (!out_set) {
+			LOGP(DINP, LOGL_ERROR, "Identified linkset does not exist.\n");
+			goto exit;
+		}
+
+		msg = msgb_alloc(2048, "inject-data");
+		if (!msg) {
+			LOGP(DINP, LOGL_ERROR, "Failed to allocate storage.\n");
+			goto exit;
+		}
+
+		msg->l2h = msgb_put(msg, data->len);
+		memcpy(msg->l2h, data->dat, data->len);
+
+		/* we are diretcly going to the output. no checking of anything  */
+		if (mtp_link_set_send(out_set, msg) != 0) {
+			LOGP(DINP, LOGL_ERROR, "Failed to send message.\n");
+			msgb_free(msg);
+		}
+	}
+
+exit:
+	m2ua_msg_free(m2ua);
+	return 0;
+}
+
+static int inject_init(struct bsc_data *bsc)
+{
+	int fd;
+	struct sockaddr_in addr;
+
+	fd = socket(PF_INET, SOCK_DGRAM, 0);
+	if (fd < 0)
+		return -1;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sin_family = AF_INET;
+	addr.sin_port = htons(5001);
+
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) != 0) {
+		LOGP(DINP, LOGL_ERROR, "Failed to bind to port 5001.\n");
+		close(fd);
+		return -1;
+	}
+
+	bsc->inject_fd.fd = fd;
+	bsc->inject_fd.when = BSC_FD_READ;
+	bsc->inject_fd.cb = inject_read_cb;
+	bsc->inject_fd.data = bsc;
+
+	if (bsc_register_fd(&bsc->inject_fd) != 0) {
+		LOGP(DINP, LOGL_ERROR, "Failed to register.\n");
+		close(fd);
+		return -1;
+	}
+
+	return 0;
+}
+
 int main(int argc, char **argv)
 {
 	int rc;
@@ -242,6 +358,11 @@ int main(int argc, char **argv)
 	if (rc < 0)
 		return rc;
 
+	if (inject_init(&bsc) != 0) {
+		LOGP(DINP, LOGL_NOTICE, "Failed to initialize inject interface.\n");
+		return -1;
+	}
+
 	if (link_init(&bsc) != 0)
 		return -1;
 
diff --git a/src/mtp_layer3.c b/src/mtp_layer3.c
index 69374aa..71db16d 100644
--- a/src/mtp_layer3.c
+++ b/src/mtp_layer3.c
@@ -1,7 +1,7 @@
 /* MTP layer3 main handling code */
 /*
- * (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
@@ -524,6 +524,23 @@ int mtp_link_set_submit_isup_data(struct mtp_link_set *link, int sls,
 	return mtp_int_submit(link, link->isup_opc, sls, MTP_SI_MNT_ISUP, data, length);
 }
 
+int mtp_link_set_send(struct mtp_link_set *set, struct msgb *msg)
+{
+	int sls;
+	struct mtp_level_3_hdr *hdr;
+
+	if (msgb_l2len(msg) < sizeof(*hdr))
+		return -1;
+
+	hdr = (struct mtp_level_3_hdr *) msg->l2h;
+	sls = MTP_LINK_SLS(hdr->addr);
+	if (!set->slc[sls])
+		return -2;
+
+	mtp_link_submit(set->slc[sls], msg);
+	return 0;
+}
+
 static int mtp_int_submit(struct mtp_link_set *link, int pc, int sls, int type,
 			  const uint8_t *data, unsigned int length)
 {