diff --git a/include/sctp_m3ua.h b/include/sctp_m3ua.h index f59b5de..0cc405f 100644 --- a/include/sctp_m3ua.h +++ b/include/sctp_m3ua.h @@ -22,6 +22,11 @@ struct mtp_m3ua_client_link { struct sockaddr_in remote; int link_index; int routing_context; + + + /* state of the link */ + int aspsm_active; + int asptm_active; }; struct mtp_m3ua_client_link *mtp_m3ua_client_link_init(struct mtp_link *link); diff --git a/src/sctp_m3ua_client.c b/src/sctp_m3ua_client.c index 0cef103..dd378bb 100644 --- a/src/sctp_m3ua_client.c +++ b/src/sctp_m3ua_client.c @@ -18,6 +18,11 @@ #include #include #include +#include +#include + +#include +#include #include @@ -30,6 +35,20 @@ #define notImplemented() \ LOGP(DINP, LOGL_NOTICE, "%s not implemented.\n", __func__) + +/* + * State machine code + */ +static void m3ua_handle_aspsm(struct mtp_m3ua_client_link *link, struct xua_msg *msg); +static void m3ua_handle_asptm(struct mtp_m3ua_client_link *link, struct xua_msg *msg); +static void m3ua_handle_trans(struct mtp_m3ua_client_link *link, struct xua_msg *msg); +static void m3ua_send_daud(struct mtp_m3ua_client_link *link, uint32_t pc); +static void m3ua_send_aspup(struct mtp_m3ua_client_link *link); +static void m3ua_send_aspac(struct mtp_m3ua_client_link *link); + +/* + * boilerplate + */ static int m3ua_shutdown(struct mtp_link *mtp_link); static void m3ua_start(void *data); @@ -51,7 +70,30 @@ static void fail_link(struct mtp_m3ua_client_link *link) static int m3ua_conn_handle(struct mtp_m3ua_client_link *link, struct msgb *msg, struct sctp_sndrcvinfo *info) { - notImplemented(); + struct xua_msg *m3ua; + m3ua = xua_from_msg(M3UA_VERSION, msg->len, msg->data); + if (!m3ua) { + LOGP(DINP, LOGL_ERROR, "Failed to parse the message.\n"); + return -1; + } + + switch (m3ua->hdr.msg_class) { + case M3UA_CLS_ASPSM: + m3ua_handle_aspsm(link, m3ua); + break; + case M3UA_CLS_ASPTM: + m3ua_handle_asptm(link, m3ua); + break; + case M3UA_CLS_TRANS: + m3ua_handle_trans(link, m3ua); + break; + default: + LOGP(DINP, LOGL_ERROR, "Unhandled msg_class %d\n", + m3ua->hdr.msg_class); + break; + } + + xua_msg_free(m3ua); return 0; } @@ -70,6 +112,29 @@ static int m3ua_conn_write(struct osmo_fd *fd, struct msgb *msg) return 0; } +static int m3ua_conn_send(struct mtp_m3ua_client_link *link, + struct xua_msg *m3ua, + struct sctp_sndrcvinfo *info) +{ + struct msgb *msg; + msg = xua_to_msg(M3UA_VERSION, m3ua); + if (!msg) + return -1; + + /* save the OOB data in front of the message */ + msg->l2h = msg->data; + msgb_push(msg, sizeof(*info)); + memcpy(msg->data, info, sizeof(*info)); + + if (osmo_wqueue_enqueue(&link->queue, msg) != 0) { + LOGP(DINP, LOGL_ERROR, "Failed to enqueue.\n"); + msgb_free(msg); + return -1; + } + + return 0; +} + static int m3ua_conn_read(struct osmo_fd *fd) { struct sockaddr_in addr; @@ -161,11 +226,89 @@ static void m3ua_start(void *data) close(sctp); return fail_link(link); } + + /* begin the messages for bring-up */ + m3ua_send_aspup(link); } static int m3ua_write(struct mtp_link *mtp_link, struct msgb *msg) { - notImplemented(); + struct mtp_m3ua_client_link *link = mtp_link->data; + struct sctp_sndrcvinfo info; + struct xua_msg *m3ua; + struct mtp_level_3_hdr *mtp_hdr; + struct m3ua_protocol_data proto_data; + uint8_t *proto_start; + + if (!link->asptm_active) { + LOGP(DINP, LOGL_ERROR, "ASP not ready for %d/%s of %d/%s.\n", + mtp_link->nr, mtp_link->name, mtp_link->set->nr, + mtp_link->set->name); + goto clean; + } + + /* + * TODO.. we could enhance the structure of mtp_link to + * have function pointers for operations like SLTM instead + * of doing what we do here. + * The entire m3ua episode (code + reading the spec) had a + * budget of < 2 man days so the amount of architecture changes + * we can do. + */ + + /* TODO.. need to terminate MTPL3 locally... */ + + /* TODO.. extract MTP information.. */ + mtp_hdr = (struct mtp_level_3_hdr *) msg->l2h; + switch (mtp_hdr->ser_ind) { + case MTP_SI_MNT_SNM_MSG: + case MTP_SI_MNT_REG_MSG: + LOGP(DINP, LOGL_ERROR, + "Dropping SNM/REG/ISUP/??? message %d\n", mtp_hdr->ser_ind); + goto clean; + break; + case MTP_SI_MNT_ISUP: + case MTP_SI_MNT_SCCP: + default: + /* TODO... read OPC/DPC from message.. */ + memset(&proto_data, 0, sizeof(proto_data)); + proto_data.opc = htonl(mtp_link->set->sccp_opc); + proto_data.dpc = htonl(mtp_link->set->sccp_dpc == -1 ? + mtp_link->set->sccp_dpc : mtp_link->set->dpc); + proto_data.sls = MTP_LINK_SLS(mtp_hdr->addr); + proto_data.si = mtp_hdr->ser_ind; + proto_data.ni = mtp_link->set->ni; + + msg->l3h = mtp_hdr->data; + msgb_pull_to_l3(msg); + proto_start = msgb_push(msg, sizeof(proto_data)); + memcpy(proto_start, &proto_data, sizeof(proto_data)); + break; + }; + + m3ua = xua_msg_alloc(); + if (!m3ua) + goto clean; + + mtp_handle_pcap(mtp_link, NET_OUT, msg->data, msg->len); + + m3ua->hdr.msg_class = M3UA_CLS_TRANS; + m3ua->hdr.msg_type = M3UA_TRANS_DATA; + + /* + * Modify the data...to create a true protocol data.. + */ + xua_msg_add_data(m3ua, M3UA_TAG_PROTO_DATA, msg->len, msg->data); + + memset(&info, 0, sizeof(info)); + info.sinfo_stream = 0; + info.sinfo_assoc_id = 1; + info.sinfo_ppid = htonl(SCTP_PPID_M3UA); + + m3ua_conn_send(link, m3ua, &info); + xua_msg_free(m3ua); + +clean: msgb_free(msg); return 0; } @@ -180,6 +323,8 @@ static int m3ua_shutdown(struct mtp_link *mtp_link) link->queue.bfd.fd = -1; } osmo_wqueue_clear(&link->queue); + link->aspsm_active = 0; + link->asptm_active = 0; return 0; } @@ -227,3 +372,197 @@ struct mtp_m3ua_client_link *mtp_m3ua_client_link_init(struct mtp_link *blnk) lnk->queue.bfd.fd = -1; return lnk; } + + +/* + * asp handling + */ +static void m3ua_send_aspup(struct mtp_m3ua_client_link *link) +{ + struct sctp_sndrcvinfo info; + struct xua_msg *aspup; + uint32_t asp_ident; + + aspup = xua_msg_alloc(); + if (!aspup) { + fail_link(link); + return; + } + + memset(&info, 0, sizeof(info)); + info.sinfo_stream = 0; + info.sinfo_assoc_id = 1; + info.sinfo_ppid = htonl(SCTP_PPID_M3UA); + + aspup->hdr.msg_class = M3UA_CLS_ASPSM; + aspup->hdr.msg_type = M3UA_ASPSM_UP; + + asp_ident = htonl(link->link_index); + xua_msg_add_data(aspup, MUA_TAG_ASP_IDENT, 4, (uint8_t *) &asp_ident); + + m3ua_conn_send(link, aspup, &info); + xua_msg_free(aspup); +} + +static void m3ua_send_aspac(struct mtp_m3ua_client_link *link) +{ + struct sctp_sndrcvinfo info; + struct xua_msg *aspac; + uint32_t routing_ctx; + uint32_t traffic_mode; + + aspac = xua_msg_alloc(); + if (!aspac) { + fail_link(link); + return; + } + + memset(&info, 0, sizeof(info)); + info.sinfo_stream = 0; + info.sinfo_assoc_id = 1; + info.sinfo_ppid = htonl(SCTP_PPID_M3UA); + + aspac->hdr.msg_class = M3UA_CLS_ASPTM; + aspac->hdr.msg_type = M3UA_ASPTM_ACTIV; + + traffic_mode = htonl(2); + xua_msg_add_data(aspac, 11, 4, (uint8_t *) &traffic_mode); + + routing_ctx = htonl(link->routing_context); + xua_msg_add_data(aspac, MUA_TAG_ROUTING_CTX, 4, (uint8_t *) &routing_ctx); + + m3ua_conn_send(link, aspac, &info); + xua_msg_free(aspac); +} + +static void m3ua_send_daud(struct mtp_m3ua_client_link *link, uint32_t dpc) +{ + struct sctp_sndrcvinfo info; + struct xua_msg *daud; + uint32_t routing_ctx; + + daud = xua_msg_alloc(); + if (!daud) { + fail_link(link); + return; + } + + memset(&info, 0, sizeof(info)); + info.sinfo_stream = 0; + info.sinfo_assoc_id = 1; + info.sinfo_ppid = htonl(SCTP_PPID_M3UA); + + daud->hdr.msg_class = M3UA_CLS_SSNM; + daud->hdr.msg_type = M3UA_SSNM_DAUD; + + routing_ctx = htonl(link->routing_context); + xua_msg_add_data(daud, MUA_TAG_ROUTING_CTX, 4, (uint8_t *) &routing_ctx); + + dpc = htonl(dpc); + xua_msg_add_data(daud, MUA_TAG_AFF_PC, 4, (uint8_t *) &dpc); + + m3ua_conn_send(link, daud, &info); + xua_msg_free(daud); +} + +static void m3ua_handle_aspsm(struct mtp_m3ua_client_link *link, struct xua_msg *m3ua) +{ + switch (m3ua->hdr.msg_type) { + case M3UA_ASPSM_UP_ACK: + LOGP(DINP, LOGL_NOTICE, "Received ASP_UP_ACK.. sending ASPAC\n"); + link->aspsm_active = 1; + m3ua_send_aspac(link); + break; + default: + LOGP(DINP, LOGL_ERROR, "Unhandled msg_type %d\n", + m3ua->hdr.msg_type); + break; + } +} + +static void m3ua_handle_asptm(struct mtp_m3ua_client_link *link, struct xua_msg *m3ua) +{ + switch (m3ua->hdr.msg_type) { + case M3UA_ASPTM_ACTIV_ACK: + LOGP(DINP, LOGL_NOTICE, "Received ASPAC_ACK.. taking link up\n"); + link->asptm_active = 1; + mtp_link_up(link->base); + m3ua_send_daud(link, link->base->set->dpc); + if (link->base->set->sccp_dpc != -1) + m3ua_send_daud(link, link->base->set->sccp_dpc); + break; + default: + LOGP(DINP, LOGL_ERROR, "Unhandled msg_type %d\n", + m3ua->hdr.msg_type); + break; + } +} + +static void m3ua_handle_trans(struct mtp_m3ua_client_link *link, struct xua_msg *m3ua) +{ + struct msgb *msg; + struct xua_msg_part *data; + struct mtp_link *mtp_link; + struct m3ua_protocol_data *proto; + struct mtp_level_3_hdr *mtp_hdr; + uint32_t opc, dpc; + uint8_t sls, si; + + mtp_link = link->base; + + /* ignore everything if the link is blocked */ + if (mtp_link->blocked) + return; + + if (m3ua->hdr.msg_type != M3UA_TRANS_DATA) { + LOGP(DINP, LOGL_ERROR, "msg_type(%d) is not known. Ignoring\n", + m3ua->hdr.msg_type); + return; + } + + data = xua_msg_find_tag(m3ua, M3UA_TAG_PROTO_DATA); + if (!data) { + LOGP(DINP, LOGL_ERROR, "No PROTO_DATA in DATA message.\n"); + return; + } + + if (data->len > 2048) { + LOGP(DINP, LOGL_ERROR, "TOO much data for us to handle.\n"); + return; + } + + if (data->len < sizeof(struct m3ua_protocol_data)) { + LOGP(DINP, LOGL_ERROR, "Too little data..\n"); + return; + } + + msg = msgb_alloc(2048, "m3ua-data"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate storage.\n"); + return; + } + + msg->l2h = msgb_put(msg, data->len); + memcpy(msg->l2h, data->dat, data->len); + + proto = (struct m3ua_protocol_data *) msg->l2h; + opc = ntohl(proto->opc); + dpc = ntohl(proto->dpc); + sls = proto->sls; + si = proto->si; + LOGP(DINP, LOGL_DEBUG, "Got data for OPC(%d)/DPC(%d)/SLS(%d) len(%d)\n", + opc, dpc, sls, msgb_l2len(msg) - sizeof(*proto)); + + + /* put a MTP3 header in front */ + msg->l3h = proto->data; + msgb_pull_to_l3(msg); + msg->l2h = msgb_push(msg, sizeof(*mtp_hdr)); + mtp_hdr = (struct mtp_level_3_hdr *) msg->l2h; + mtp_hdr->ser_ind = si; + mtp_hdr->addr = MTP_ADDR(sls % 16, dpc, opc); + + mtp_handle_pcap(mtp_link, NET_IN, msg->l2h, msgb_l2len(msg)); + mtp_link_set_data(mtp_link, msg); + msgb_free(msg); +}