diff options
| -rw-r--r-- | include/bsc_data.h | 4 | ||||
| -rw-r--r-- | include/mtp_data.h | 1 | ||||
| -rw-r--r-- | src/main_stp.c | 121 | ||||
| -rw-r--r-- | src/mtp_layer3.c | 21 |
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) { |