From 00e6f692e9ee7bc7001e5c31b519abe711585503 Mon Sep 17 00:00:00 2001 From: Holger Hans Peter Freyther Date: Sun, 1 Aug 2010 16:58:34 +0800 Subject: mtp: Add the MTP Level3 code to the SCCP repoistory SCCP can be wrapped inside the MTP Level3, and one can use it for link testing as well. This repository should be renamed to libosmo-itu or libosmo-ss7 and be a host to SS7 related encapsulation... The code is coming from the cellmgr-ng code. --- Makefile.am | 2 +- configure.ac | 3 + include/Makefile.am | 2 +- include/mtp/Makefile.am | 2 + include/mtp/mtp_data.h | 85 ++++++ include/mtp/mtp_level3.h | 165 ++++++++++++ include/mtp/mtp_pcap.h | 29 +++ libosmo-mtp.pc.in | 10 + src/Makefile.am | 3 +- src/mtp_level3.c | 512 ++++++++++++++++++++++++++++++++++++ src/mtp_pcap.c | 86 +++++++ tests/Makefile.am | 2 +- tests/mtp/Makefile.am | 4 + tests/mtp/mtp_parse_test.c | 629 +++++++++++++++++++++++++++++++++++++++++++++ 14 files changed, 1530 insertions(+), 4 deletions(-) create mode 100644 include/mtp/Makefile.am create mode 100644 include/mtp/mtp_data.h create mode 100644 include/mtp/mtp_level3.h create mode 100644 include/mtp/mtp_pcap.h create mode 100644 libosmo-mtp.pc.in create mode 100644 src/mtp_level3.c create mode 100644 src/mtp_pcap.c create mode 100644 tests/mtp/Makefile.am create mode 100644 tests/mtp/mtp_parse_test.c diff --git a/Makefile.am b/Makefile.am index a0f4730..51d65f8 100644 --- a/Makefile.am +++ b/Makefile.am @@ -4,7 +4,7 @@ INCLUDES = $(all_includes) -I$(top_srcdir)/include SUBDIRS = include src tests pkgconfigdir = $(libdir)/pkgconfig -pkgconfig_DATA = libosmo-sccp.pc +pkgconfig_DATA = libosmo-sccp.pc libosmo-mtp.pc BUILT_SOURCES = $(top_srcdir)/.version $(top_srcdir)/.version: diff --git a/configure.ac b/configure.ac index 4fb8ce5..94969f7 100644 --- a/configure.ac +++ b/configure.ac @@ -29,10 +29,13 @@ AC_SUBST(SYMBOL_VISIBILITY) AC_OUTPUT( libosmo-sccp.pc + libosmo-mtp.pc include/sccp/Makefile + include/mtp/Makefile include/Makefile src/Makefile tests/Makefile tests/sccp/Makefile + tests/mtp/Makefile Makefile) diff --git a/include/Makefile.am b/include/Makefile.am index d0ed7f4..bf7855d 100644 --- a/include/Makefile.am +++ b/include/Makefile.am @@ -1 +1 @@ -SUBDIRS = sccp +SUBDIRS = sccp mtp diff --git a/include/mtp/Makefile.am b/include/mtp/Makefile.am new file mode 100644 index 0000000..077805b --- /dev/null +++ b/include/mtp/Makefile.am @@ -0,0 +1,2 @@ +mtp_HEADERS = mtp_data.h mtp_level3.h mtp_pcap.h +mtpdir = $(includedir)/mtp diff --git a/include/mtp/mtp_data.h b/include/mtp/mtp_data.h new file mode 100644 index 0000000..b8f6af0 --- /dev/null +++ b/include/mtp/mtp_data.h @@ -0,0 +1,85 @@ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (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 General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ +#ifndef mtp_data_h +#define mtp_data_h + +#include +#include +#include + +/* MTP Level3 timers */ + +/* Timers for SS7 */ +#define MTP_T1 12, 0 +#define MTP_T2 30, 0 +#define START_DELAY 8, 0 + +/** + * The state of the mtp_link in terms of layer3 and upwards + */ +struct mtp_link { + /* routing info.. */ + int dpc, opc; + + /* internal state */ + /* the MTP1 link is up */ + int available; + int running; + int sccp_up; + + /* misc data */ + uint8_t test_ptrn[14]; + + int sltm_pending; + struct llist_head pending_msgs; + int sltm_once; + int was_up; + + + /* the associated link */ + int link; + + int slta_misses; + struct timer_list t1_timer; + struct timer_list t2_timer; + + struct timer_list delay_timer; +}; + + +struct mtp_link *mtp_link_alloc(void); +void mtp_link_stop(struct mtp_link *link); +void mtp_link_reset(struct mtp_link *link); +int mtp_link_data(struct mtp_link *link, struct msgb *msg); +int mtp_link_submit_sccp_data(struct mtp_link *link, int sls, const uint8_t *data, unsigned int length); + + +/* one time init function */ +void mtp_link_init(void); + +/* to be implemented for MSU sending */ +void mtp_link_submit(struct mtp_link *link, struct msgb *msg); +void mtp_link_forward_sccp(struct mtp_link *link, struct msgb *msg, int sls); +void mtp_link_restart(struct mtp_link *link); +void mtp_link_slta_recv(struct mtp_link *link); +void mtp_link_sccp_down(struct mtp_link *link); + +#endif diff --git a/include/mtp/mtp_level3.h b/include/mtp/mtp_level3.h new file mode 100644 index 0000000..7d72d9e --- /dev/null +++ b/include/mtp/mtp_level3.h @@ -0,0 +1,165 @@ +/* Q.701-Q.704, Q.706, Q.707 handling code */ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (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 General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#ifndef mtp_level3_h +#define mtp_level3_h + +#include +#include +#include + + +/* + * pssible service information octets.. + */ +#define MTP_NI_NATION_NET 0x02 + +#define MTP_SI_MNT_SNM_MSG 0x00 +#define MTP_SI_MNT_REG_MSG 0x01 +#define MTP_SI_MNT_SCCP 0x03 + +/* + * h0 contains the group, h1 the semantic of it + */ + +#define MTP_TST_MSG_GRP 0x01 +#define MTP_PROHIBIT_MSG_GRP 0x04 +#define MTP_TRF_RESTR_MSG_GRP 0x07 + +/* h1 values for different groups */ +#define MTP_TST_MSG_SLTM 0x01 +#define MTP_TST_MSG_SLTA 0x02 + +#define MTP_RESTR_MSG_ALLWED 0x01 + +#define MTP_PROHIBIT_MSG_SIG 0x01 + + +#define SCCP_SST 0x03 +#define SCCP_SSA 0x01 + +#define MTP_LINK_MASK 0x0F +#define MTP_ADDR_MASK 0x0FFF +#define MTP_APOC_MASK 0x3f + + +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define MTP_LINK_SLS(addr) ((addr >>28) & MTP_LINK_MASK) +#define MTP_ADDR(link, dpc, opc) \ + (((dpc) & MTP_ADDR_MASK) << 0 | \ + ((opc) & MTP_ADDR_MASK) << 14| \ + ((link) & MTP_LINK_MASK) << 28) +#define MTP_MAKE_APOC(apoc) \ + (apoc & 0x3fff) +#elif __BYTE_ORDER == __BIG_ENDIAN +static inline uint32_t c_swap_32(uint32_t in) +{ + return (((in & 0x000000ff) << 24) | + ((in & 0x0000ff00) << 8) | + ((in & 0x00ff0000) >> 8) | + ((in & 0xff000000) >> 24)); +} +static inline uint16_t c_swap_16(uint16_t in) +{ + return (((in & 0x00ff) << 8) | + (in & 0xff00) >> 8); +} +#define MTP_LINK_SLS(addr) ((c_swap_32(addr)>>28) & MTP_LINK_MASK) +#define MTP_ADDR(link, dpc, opc) \ + c_swap_32(((dpc) & MTP_ADDR_MASK) << 0 | \ + ((opc) & MTP_ADDR_MASK) << 14| \ + ((link) & MTP_LINK_MASK) << 28) +#define MTP_MAKE_APOC(apoc) \ + c_swap_16((apoc & 0x3fff)) +#endif + + + +/* + * not the on wire address... + */ +struct mtp_addr { + uint16_t dpc; + uint16_t opc; + uint8_t link; +} __attribute__((packed)); + +/* + * the struct is defined in Q.704 and can be seen in the + * wireshark dissectors too + */ +struct mtp_level_3_hdr { +#if __BYTE_ORDER == __LITTLE_ENDIAN + uint8_t ser_ind : 4, + spare : 2, + ni : 2; +#elif __BYTE_ORDER == __BIG_ENDIAN + uint8_t ni : 2, + spare : 2, + ser_ind : 4; +#endif + uint32_t addr; + uint8_t data[0]; +} __attribute__((packed)); + +struct mtp_level_3_cmn { +#if __BYTE_ORDER == __LITTLE_ENDIAN + uint8_t h0 : 4, + h1 : 4; +#elif __BYTE_ORDER == __BIG_ENDIAN + uint8_t h1 : 4, + h0 : 4; +#endif +} __attribute__((packed)); + +struct mtp_level_3_mng { + struct mtp_level_3_cmn cmn; +#if __BYTE_ORDER == __LITTLE_ENDIAN + uint8_t spare : 4, + length : 4; +#elif __BYTE_ORDER == __BIG_ENDIAN + uint8_t length : 4, + spare : 4; +#endif + uint8_t data[0]; +} __attribute__((packed)); + +struct mtp_level_3_prohib { + struct mtp_level_3_cmn cmn; + + uint16_t apoc; +} __attribute__((packed)); + +struct sccp_con_ctrl_prt_mgt { + uint8_t sst; + uint8_t assn; /* affected sub system number */ + uint16_t apoc; +#if __BYTE_ORDER == __LITTLE_ENDIAN + uint8_t mul_ind : 2, + spare : 6; +#elif __BYTE_ORDER == __BIG_ENDIAN + uint8_t spare : 6, + mul_ind : 2; +#endif +} __attribute__((packed)); + +#endif diff --git a/include/mtp/mtp_pcap.h b/include/mtp/mtp_pcap.h new file mode 100644 index 0000000..5e8f7d3 --- /dev/null +++ b/include/mtp/mtp_pcap.h @@ -0,0 +1,29 @@ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (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 General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ +#ifndef mtp_pcap_h +#define mtp_pcap_h + +#include + +int mtp_pcap_write_header(int fd); +int mtp_pcap_write_msu(int fd, const uint8_t *data, int length); + +#endif diff --git a/libosmo-mtp.pc.in b/libosmo-mtp.pc.in new file mode 100644 index 0000000..675d0d3 --- /dev/null +++ b/libosmo-mtp.pc.in @@ -0,0 +1,10 @@ +prefix=@prefix@ +exec_prefix=@exec_prefix@ +libdir=@libdir@ +includedir=@includedir@ + +Name: Osmo MTP Lib +Description: Osmo MTP Lib +Version: @VERSION@ +Libs: -L${libdir} -lmtp +Cflags: -I${includedir}/ diff --git a/src/Makefile.am b/src/Makefile.am index 7e01537..963dea5 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -2,6 +2,7 @@ INCLUDES = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir) AM_CFLAGS=-Wall $(LIBOSMOCORE_CFLAGS) sccpdir = $(libdir) -sccp_LIBRARIES = libsccp.a +sccp_LIBRARIES = libsccp.a libmtp.a libsccp_a_SOURCES = sccp.c +libmtp_a_SOURCES = mtp_pcap.c mtp_level3.c diff --git a/src/mtp_level3.c b/src/mtp_level3.c new file mode 100644 index 0000000..d1101eb --- /dev/null +++ b/src/mtp_level3.c @@ -0,0 +1,512 @@ +/* MTP level3 main handling code */ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (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 General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ +#include +#include + +#include +#include + +#include + +#include + +#include + +static void *tall_mtp_ctx = NULL; + +// HACK +#define DINP 0 + +static struct msgb *mtp_msg_alloc(struct mtp_link *link) +{ + struct mtp_level_3_hdr *hdr; + struct msgb *msg = msgb_alloc_headroom(4096, 128, "mtp-msg"); + if (!msg) { + LOGP(DINP, LOGL_ERROR, "Failed to allocate mtp msg\n"); + return NULL; + } + + msg->l2h = msgb_put(msg, sizeof(*hdr)); + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->addr = MTP_ADDR(0x0, link->dpc, link->opc); + return msg; +} + +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); + uint8_t *data; + if (!msg) + return NULL; + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->ni = MTP_NI_NATION_NET; + 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 *link, struct mtp_level_3_mng *in_mng, int l3_len) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_mng *mng; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_REG_MSG; + 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; + mng->length = l3_len - 2; + msgb_put(out, mng->length); + memcpy(mng->data, in_mng->data, mng->length); + + return out; +} + +static struct msgb *mtp_tfp_alloc(struct mtp_link *link, int apoc) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_prohib *prb; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SNM_MSG; + prb = (struct mtp_level_3_prohib *) msgb_put(out, sizeof(*prb)); + prb->cmn.h0 = MTP_PROHIBIT_MSG_GRP; + prb->cmn.h1 = MTP_PROHIBIT_MSG_SIG; + prb->apoc = MTP_MAKE_APOC(apoc); + return out; +} + +static struct msgb *mtp_tra_alloc(struct mtp_link *link) +{ + struct mtp_level_3_hdr *hdr; + struct mtp_level_3_cmn *cmn; + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SNM_MSG; + cmn = (struct mtp_level_3_cmn *) msgb_put(out, sizeof(*cmn)); + cmn->h0 = MTP_TRF_RESTR_MSG_GRP; + cmn->h1 = MTP_RESTR_MSG_ALLWED; + return out; +} + +static struct msgb *mtp_sccp_alloc_ssa(struct mtp_link *link, int sls) +{ + struct sccp_data_unitdata *udt; + struct sccp_con_ctrl_prt_mgt *prt; + struct mtp_level_3_hdr *hdr; + uint8_t *data; + + + struct msgb *out = mtp_msg_alloc(link); + + if (!out) + return NULL; + + hdr = (struct mtp_level_3_hdr *) out->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SCCP; + + /* this appears to be round robin or such.. */ + hdr->addr = MTP_ADDR(sls % 16, link->dpc, link->opc); + + /* generate the UDT message... libsccp does not offer formating yet */ + udt = (struct sccp_data_unitdata *) msgb_put(out, sizeof(*udt)); + udt->type = SCCP_MSG_TYPE_UDT; + udt->proto_class = SCCP_PROTOCOL_CLASS_0; + udt->variable_called = 3; + udt->variable_calling = 5; + udt->variable_data = 7; + + /* put the called and calling address. It is LV */ + data = msgb_put(out, 2 + 1); + data[0] = 2; + data[1] = 0x42; + data[2] = 0x1; + + data = msgb_put(out, 2 + 1); + data[0] = 2; + data[1] = 0x42; + data[2] = 0x1; + + data = msgb_put(out, 1); + data[0] = sizeof(*prt); + + prt = (struct sccp_con_ctrl_prt_mgt *) msgb_put(out, sizeof(*prt)); + prt->sst = SCCP_SSA; + prt->assn = 254; + prt->apoc = MTP_MAKE_APOC(link->opc); + prt->mul_ind = 0; + + return out; +} + +void mtp_link_init(void) +{ + tall_mtp_ctx = talloc_named_const(NULL, 1, "mtp-link"); +} + +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; + } + + mtp_link_submit(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); + link->sccp_up = 0; + link->running = 0; + bsc_del_timer(&link->t2_timer); + mtp_link_sccp_down(link); + mtp_link_restart(link); + } +} + +static void mtp_sltm_t2_timeout(void *_link) +{ + struct mtp_link *link = (struct mtp_link *) _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 *mtp_link_alloc(void) +{ + struct mtp_link *link; + + link = talloc_zero(tall_mtp_ctx, struct mtp_link); + if (!link) + return NULL; + + 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->pending_msgs); + return link; +} + +void mtp_link_stop(struct mtp_link *link) +{ + bsc_del_timer(&link->t1_timer); + bsc_del_timer(&link->t2_timer); + bsc_del_timer(&link->delay_timer); + link->sccp_up = 0; + link->running = 0; + link->sltm_pending = 0; + + mtp_link_sccp_down(link); +} + +void mtp_link_reset(struct mtp_link *link) +{ + mtp_link_stop(link); + link->running = 1; + bsc_schedule_timer(&link->delay_timer, START_DELAY); +} + +static int mtp_link_sign_msg(struct mtp_link *link, struct mtp_level_3_hdr *hdr, int l3_len) +{ + struct msgb *msg; + struct mtp_level_3_cmn *cmn; + + if (hdr->spare != 0 || hdr->ni != MTP_NI_NATION_NET || l3_len < 1) { + LOGP(DINP, LOGL_ERROR, "Unhandled data (%d, %d, %d)\n", + hdr->spare, hdr->ni, l3_len); + return -1; + } + + cmn = (struct mtp_level_3_cmn *) &hdr->data[0]; + LOGP(DINP, LOGL_DEBUG, "reg msg: h0: 0x%x h1: 0x%x\n", + cmn->h0, cmn->h1); + + switch (cmn->h0) { + case MTP_TRF_RESTR_MSG_GRP: + switch (cmn->h1) { + case MTP_RESTR_MSG_ALLWED: + LOGP(DINP, LOGL_INFO, "Received Restart Allowed. SST should be next: %p\n", link); + link->sccp_up = 0; + mtp_link_sccp_down(link); + + msg = mtp_tfp_alloc(link, 0); + if (!msg) + return -1; + mtp_link_submit(link, msg); + + msg = mtp_tra_alloc(link); + if (!msg) + return -1; + + mtp_link_submit(link, msg); + return 0; + break; + } + break; + } + + abort(); + return -1; +} + +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->spare != 0 || hdr->ni != MTP_NI_NATION_NET || l3_len < 2) { + LOGP(DINP, LOGL_ERROR, "Unhandled data (%d, %d, %d)\n", + hdr->spare, hdr->ni, l3_len); + 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); + + switch (mng->cmn.h0) { + case MTP_TST_MSG_GRP: + switch (mng->cmn.h1) { + case MTP_TST_MSG_SLTM: + /* simply respond to the request... */ + out = mtp_create_slta(link, mng, l3_len); + if (!out) + return -1; + mtp_link_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; + mtp_link_slta_recv(link); + return 0; + break; + } + break; + } + + return -1; +} + +static int mtp_link_sccp_data(struct mtp_link *link, struct mtp_level_3_hdr *hdr, struct msgb *msg, int l3_len) +{ + struct msgb *out; + struct sccp_con_ctrl_prt_mgt *prt; + + msg->l2h = &hdr->data[0]; + if (msgb_l2len(msg) != l3_len) { + LOGP(DINP, LOGL_ERROR, "Size is wrong after playing with the l2h header.\n"); + return -1; + } + + + if (link->sccp_up) { + mtp_link_forward_sccp(link, msg, MTP_LINK_SLS(hdr->addr)); + return 0; + } else { + struct sccp_parse_result sccp; + memset(&sccp, 0, sizeof(sccp)); + if (sccp_parse_header(msg, &sccp) != 0) { + LOGP(DINP, LOGL_ERROR, "Failed to parsed SCCP header.\n"); + return -1; + } + + if (sccp_determine_msg_type(msg) != SCCP_MSG_TYPE_UDT) { + LOGP(DINP, LOGL_ERROR, "Dropping sccp data: 0x%x\n", + sccp_determine_msg_type(msg)); + return -1; + } + + if (msgb_l3len(msg) != 5) { + LOGP(DINP, LOGL_ERROR, "SCCP UDT msg of unexpected size: %u\n", + msgb_l3len(msg)); + return -1; + } + + if (msg->l3h[0] != SCCP_SST) { + LOGP(DINP, LOGL_ERROR, "Expected SCCP SST but got 0x%x\n", + msg->l3h[0]); + return -1; + } + + prt = (struct sccp_con_ctrl_prt_mgt *) &msg->l3h[0]; + if (prt->assn != 254 || prt->apoc != MTP_MAKE_APOC(link->opc)) { + LOGP(DINP, LOGL_ERROR, "Unknown SSN/APOC assn: %u, apoc: %u/%u\n", + prt->assn, ntohs(prt->apoc), prt->apoc); + return -1; + } + + out = mtp_sccp_alloc_ssa(link, MTP_LINK_SLS(hdr->addr)); + if (!out) + return -1; + + link->sccp_up = 1; + link->was_up = 1; + LOGP(DINP, LOGL_INFO, "SCCP is established. %p\n", link); + mtp_link_submit(link, out); + } + return 0; +} + +int mtp_link_data(struct mtp_link *link, struct msgb *msg) +{ + int rc = -1; + struct mtp_level_3_hdr *hdr; + int l3_len; + + if (!msg->l2h || msgb_l2len(msg) < sizeof(*hdr)) + return -1; + + if (!link->running) { + LOGP(DINP, LOGL_ERROR, "Link is not running. Call mtp_link_reset first: %p\n", link); + return -1; + } + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + l3_len = msgb_l2len(msg) - sizeof(*hdr); + + switch (hdr->ser_ind) { + case MTP_SI_MNT_SNM_MSG: + rc = mtp_link_sign_msg(link, hdr, l3_len); + break; + case MTP_SI_MNT_REG_MSG: + rc = mtp_link_regular_msg(link, hdr, l3_len); + break; + case MTP_SI_MNT_SCCP: + rc = mtp_link_sccp_data(link, hdr, msg, l3_len); + break; + default: + fprintf(stderr, "Unhandled: %u\n", hdr->ser_ind); + break; + } + + return rc; +} + +int mtp_link_submit_sccp_data(struct mtp_link *link, int sls, const uint8_t *data, unsigned int length) +{ + uint8_t *put_ptr; + struct mtp_level_3_hdr *hdr; + struct msgb *msg; + + if (!link->sccp_up) { + LOGP(DINP, LOGL_ERROR, "SCCP msg after TRA and before SSA. Dropping it.\n"); + return -1; + } + + msg = mtp_msg_alloc(link); + if (!msg) + return -1; + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->ni = MTP_NI_NATION_NET; + hdr->ser_ind = MTP_SI_MNT_SCCP; + + hdr->addr = MTP_ADDR(sls % 16, link->dpc, link->opc); + + /* copy the raw sccp data */ + put_ptr = msgb_put(msg, length); + memcpy(put_ptr, data, length); + + mtp_link_submit(link, msg); + return 0; +} diff --git a/src/mtp_pcap.c b/src/mtp_pcap.c new file mode 100644 index 0000000..052813f --- /dev/null +++ b/src/mtp_pcap.c @@ -0,0 +1,86 @@ +/* PCAP code from OpenBSC done by Holger Freyther */ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (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 General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include + +#include + +#include + +#define static_assert(exp, name) typedef int dummy##name [(exp) ? 1 : -1]; + +/* + * pcap writing of the misdn load + * pcap format is from http://wiki.wireshark.org/Development/LibpcapFileFormat + */ +struct pcap_hdr { + uint32_t magic_number; + uint16_t version_major; + uint16_t version_minor; + int32_t thiszone; + uint32_t sigfigs; + uint32_t snaplen; + uint32_t network; +} __attribute__((packed)); + +struct pcaprec_hdr { + uint32_t ts_sec; + uint32_t ts_usec; + uint32_t incl_len; + uint32_t orig_len; +} __attribute__((packed)); + +int mtp_pcap_write_header(int fd) +{ + static struct pcap_hdr hdr = { + .magic_number = 0xa1b2c3d4, + .version_major = 2, + .version_minor = 4, + .thiszone = 0, + .sigfigs = 0, + .snaplen = 65535, + .network = 141, + }; + + return write(fd, &hdr, sizeof(hdr)); +} + +int mtp_pcap_write_msu(int fd, const uint8_t *data, int length) +{ + int rc_h, rc_d; + struct timeval tv; + struct pcaprec_hdr payload_header = { + .ts_sec = 0, + .ts_usec = 0, + .incl_len = length, + .orig_len = length, + }; + + gettimeofday(&tv, NULL); + payload_header.ts_sec = tv.tv_sec; + payload_header.ts_usec = tv.tv_usec; + + rc_h = write(fd, &payload_header, sizeof(payload_header)); + rc_d = write(fd, data, length); + + return rc_h == sizeof(payload_header) && rc_d == length; +} diff --git a/tests/Makefile.am b/tests/Makefile.am index d0ed7f4..bf7855d 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -1 +1 @@ -SUBDIRS = sccp +SUBDIRS = sccp mtp diff --git a/tests/mtp/Makefile.am b/tests/mtp/Makefile.am new file mode 100644 index 0000000..1f364b6 --- /dev/null +++ b/tests/mtp/Makefile.am @@ -0,0 +1,4 @@ +INCLUDES = $(all_includes) -I$(top_srcdir)/include -Wall +noinst_PROGRAMS = mtp_parse_test + +mtp_parse_test_SOURCES = mtp_parse_test.c diff --git a/tests/mtp/mtp_parse_test.c b/tests/mtp/mtp_parse_test.c new file mode 100644 index 0000000..434f825 --- /dev/null +++ b/tests/mtp/mtp_parse_test.c @@ -0,0 +1,629 @@ +/* MTP Layer3 parsing tests */ +#include + +#include + +#include +#include +#include + +#define ARRAY_SIZE(a) (sizeof(a)/sizeof(a[0])) + +struct mtp_test { + const uint8_t *input; + const uint16_t length; + struct mtp_level_3_hdr hdr; + + int has_mng; + struct mtp_level_3_mng mng; + + int has_prohib; + struct mtp_level_3_prohib prohib; +}; + +static const unsigned char pkt1[] = { +0x81, 0x88, 0xc0, 0x16, 0x00, 0x11, 0xe0, 0x62, +0x61, 0x72, 0x62, 0x61, 0x6e, 0x6f, 0x62, 0x61, +0x72, 0x6e, 0x61, 0x62, 0x6f }; + +static const unsigned char pkt2[] = { +0x81, 0x5b, 0x00, 0x22, 0x00, 0x11, 0xe0, 0x41, +0x6d, 0x69, 0x74, 0x20, 0x43, 0x68, 0x61, 0x6e, +0x64, 0x72, 0x61, 0x00, 0x00 }; + +static const unsigned char pkt3[] = { +0x81, 0x88, 0xc0, 0x16, 0x00, 0x21, 0xe0, 0x41, +0x6d, 0x69, 0x74, 0x20, 0x43, 0x68, 0x61, 0x6e, +0x64, 0x72, 0x61, 0x00, 0x00 }; + +static const unsigned char pkt4[] = { +0x81, 0x5b, 0x00, 0x22, 0x00, 0x21, 0xe0, 0x62, +0x61, 0x72, 0x62, 0x61, 0x6e, 0x6f, 0x62, 0x61, +0x72, 0x6e, 0x61, 0x62, 0x6f }; + +static const unsigned char pkt5[] = { +0x81, 0x88, 0xc0, 0x16, 0x00, 0x21, 0xe0, 0x62, +0x61, 0x72, 0x62, 0x61, 0x6e, 0x6f, 0x62, 0x61, +0x72, 0x6e, 0x61, 0x62, 0x6f }; + +static const unsigned char pkt6[] = { +0x80, 0x5b, 0x00, 0x22, 0x00, 0x17 }; + +static const unsigned char pkt7[] = { +0x80, 0x88, 0xc0, 0x16, 0x00, 0x14, 0x56, 0x00 }; + +static const unsigned char pkt8[] = { +0x80, 0x88, 0xc0, 0x16, 0x00, 0x14, 0x00, 0x00 }; + +static const unsigned char pkt9[] = { +0x80, 0x88, 0xc0, 0x16, 0x00, 0x17 }; + +static const unsigned char pkt10[] = { +0x83, 0x5b, 0x00, 0x22, 0x20, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0x01, 0x04, +0xc3, 0x88, 0x00, 0x01, 0x05, 0x03, 0xfe, 0x5b, +0x00, 0x01 }; + +static const unsigned char pkt11[] = { +0x83, 0x88, 0xc0, 0x16, 0xd0, 0x09, 0x00, 0x03, +0x05, 0x07, 0x02, 0x42, 0x01, 0x02, 0x42, 0x01, +0x05, 0x01, 0xfe, 0x5b, 0x00, 0x00 }; + +static const unsigned char pkt12[] = { +0x83, 0x5b, 0x00, 0x22, 0x30, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x30, +0x04, 0x01, 0x20 }; + +static const unsigned char pkt13[] = { +0x83, 0x5b, 0x00, 0x22, 0x40, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x0f, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt14[] = { +0x83, 0x5b, 0x00, 0x22, 0x50, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x10, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt15[] = { +0x83, 0x5b, 0x00, 0x22, 0x60, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x11, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt16[] = { +0x83, 0x5b, 0x00, 0x22, 0x70, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x12, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt17[] = { +0x83, 0x5b, 0x00, 0x22, 0x80, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x13, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt18[] = { +0x83, 0x5b, 0x00, 0x22, 0x90, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x14, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt19[] = { +0x83, 0x5b, 0x00, 0x22, 0xa0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x15, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt20[] = { +0x83, 0x5b, 0x00, 0x22, 0xb0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x16, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt21[] = { +0x83, 0x5b, 0x00, 0x22, 0xc0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x17, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt22[] = { +0x83, 0x5b, 0x00, 0x22, 0xd0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x18, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt23[] = { +0x83, 0x5b, 0x00, 0x22, 0xe0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x19, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt24[] = { +0x83, 0x5b, 0x00, 0x22, 0xf0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1a, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt25[] = { +0x83, 0x5b, 0x00, 0x22, 0x00, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1b, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt26[] = { +0x83, 0x5b, 0x00, 0x22, 0x10, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1c, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt27[] = { +0x83, 0x5b, 0x00, 0x22, 0x20, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1d, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt28[] = { +0x83, 0x5b, 0x00, 0x22, 0x30, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1e, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt29[] = { +0x83, 0x88, 0xc0, 0x16, 0x60, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x0f }; + +static const unsigned char pkt30[] = { +0x83, 0x88, 0xc0, 0x16, 0x70, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x10 }; + +static const unsigned char pkt31[] = { +0x83, 0x88, 0xc0, 0x16, 0x80, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x11 }; + +static const unsigned char pkt32[] = { +0x83, 0x88, 0xc0, 0x16, 0x90, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x12 }; + +static const unsigned char pkt33[] = { +0x83, 0x88, 0xc0, 0x16, 0xa0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x13 }; + +static const unsigned char pkt34[] = { +0x83, 0x88, 0xc0, 0x16, 0xb0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x14 }; + +static const unsigned char pkt35[] = { +0x83, 0x88, 0xc0, 0x16, 0xc0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x15 }; + +static const unsigned char pkt36[] = { +0x83, 0x88, 0xc0, 0x16, 0xd0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x16 }; + +static const unsigned char pkt37[] = { +0x83, 0x88, 0xc0, 0x16, 0xe0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x17 }; + +static const unsigned char pkt38[] = { +0x83, 0x88, 0xc0, 0x16, 0xf0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x18 }; + +static const unsigned char pkt39[] = { +0x83, 0x88, 0xc0, 0x16, 0x00, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x19 }; + +static const unsigned char pkt40[] = { +0x83, 0x88, 0xc0, 0x16, 0x10, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1a }; + +static const unsigned char pkt41[] = { +0x83, 0x88, 0xc0, 0x16, 0x20, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1b }; + +static const unsigned char pkt42[] = { +0x83, 0x88, 0xc0, 0x16, 0x30, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1c }; + +static const unsigned char pkt43[] = { +0x83, 0x88, 0xc0, 0x16, 0x40, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1d }; + +static const unsigned char pkt44[] = { +0x83, 0x88, 0xc0, 0x16, 0x50, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1e }; + +static const unsigned char pkt45[] = { +0x83, 0x5b, 0x00, 0x22, 0x40, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x0f, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt46[] = { +0x83, 0x5b, 0x00, 0x22, 0x50, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x10, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt47[] = { +0x83, 0x5b, 0x00, 0x22, 0x60, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x11, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt48[] = { +0x83, 0x5b, 0x00, 0x22, 0x70, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x12, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt49[] = { +0x83, 0x5b, 0x00, 0x22, 0x80, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x13, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt50[] = { +0x83, 0x5b, 0x00, 0x22, 0x90, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x14, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt51[] = { +0x83, 0x5b, 0x00, 0x22, 0xa0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x15, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt52[] = { +0x83, 0x5b, 0x00, 0x22, 0xb0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x16, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt53[] = { +0x83, 0x5b, 0x00, 0x22, 0xc0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x17, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt54[] = { +0x83, 0x5b, 0x00, 0x22, 0xd0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x18, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt55[] = { +0x83, 0x5b, 0x00, 0x22, 0xe0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x19, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt56[] = { +0x83, 0x5b, 0x00, 0x22, 0xf0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1a, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt57[] = { +0x83, 0x5b, 0x00, 0x22, 0x00, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1b, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt58[] = { +0x83, 0x5b, 0x00, 0x22, 0x10, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1c, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt59[] = { +0x83, 0x5b, 0x00, 0x22, 0x20, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1d, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt60[] = { +0x83, 0x5b, 0x00, 0x22, 0x30, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0xc3, 0x5b, 0x00, 0xfe, 0x04, +0x43, 0x88, 0x00, 0xfe, 0x09, 0x00, 0x07, 0x40, +0x01, 0x00, 0x1e, 0x04, 0x01, 0x07 }; + +static const unsigned char pkt61[] = { +0x83, 0x88, 0xc0, 0x16, 0x60, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x0f }; + +static const unsigned char pkt62[] = { +0x83, 0x88, 0xc0, 0x16, 0x70, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x10 }; + +static const unsigned char pkt63[] = { +0x83, 0x88, 0xc0, 0x16, 0x80, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x11 }; + +static const unsigned char pkt64[] = { +0x83, 0x88, 0xc0, 0x16, 0x90, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x12 }; + +static const unsigned char pkt65[] = { +0x83, 0x88, 0xc0, 0x16, 0xa0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x13 }; + +static const unsigned char pkt66[] = { +0x83, 0x88, 0xc0, 0x16, 0xb0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x14 }; + +static const unsigned char pkt67[] = { +0x83, 0x88, 0xc0, 0x16, 0xc0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x15 }; + +static const unsigned char pkt68[] = { +0x83, 0x88, 0xc0, 0x16, 0xd0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x16 }; + +static const unsigned char pkt69[] = { +0x83, 0x88, 0xc0, 0x16, 0xe0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x17 }; + +static const unsigned char pkt70[] = { +0x83, 0x88, 0xc0, 0x16, 0xf0, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x18 }; + +static const unsigned char pkt71[] = { +0x83, 0x88, 0xc0, 0x16, 0x00, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x19 }; + +static const unsigned char pkt72[] = { +0x83, 0x88, 0xc0, 0x16, 0x10, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1a }; + +static const unsigned char pkt73[] = { +0x83, 0x88, 0xc0, 0x16, 0x20, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1b }; + +static const unsigned char pkt74[] = { +0x83, 0x88, 0xc0, 0x16, 0x30, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1c }; + +static const unsigned char pkt75[] = { +0x83, 0x88, 0xc0, 0x16, 0x40, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1d }; + +static const unsigned char pkt76[] = { +0x83, 0x88, 0xc0, 0x16, 0x50, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x06, 0x00, 0x04, 0x48, +0x01, 0x00, 0x1e }; + +static const unsigned char pkt77[] = { +0x83, 0x88, 0xc0, 0x16, 0x60, 0x09, 0x00, 0x03, +0x07, 0x0b, 0x04, 0x43, 0x88, 0x00, 0xfe, 0x04, +0x43, 0x5b, 0x00, 0xfe, 0x03, 0x00, 0x01, 0x31 }; + +static struct mtp_test tests[] = { + { + .input = pkt1, + .length = sizeof(pkt1), + .hdr = { + .ni = 0x02, + .spare = 0x00, + .ser_ind = 0x01, + }, + .has_mng = 1, + .mng = { + .cmn = { + .h0 = 0x01, + .h1 = 0x01, + }, + .length = 14, + }, + }, + + { + .input = pkt3, + .length = sizeof(pkt3), + .hdr = { + .ni = 0x02, + .spare = 0x00, + .ser_ind = 0x01, + }, + .has_mng = 1, + .mng = { + .cmn = { + .h0 = 0x01, + .h1 = 0x02, + }, + + .length = 14, + }, + }, + + { + .input = pkt7, + .length = sizeof(pkt7), + .hdr = { + .ni = 2, + .spare = 0, + .ser_ind = 0, + }, + + .has_prohib = 1, + .prohib = { + .cmn = { + .h0 = 0x04, + .h1 = 0x1, + }, + + }, + } +}; + +static void check_hdr(const uint8_t *data, const struct mtp_level_3_hdr *t_hdr) +{ + struct mtp_level_3_hdr *hdr; + hdr = (struct mtp_level_3_hdr *) data; + if (memcmp(hdr, t_hdr, sizeof(*hdr)) == 0) + return; + + if (hdr->ni != t_hdr->ni) + fprintf(stderr, "NI failed.\n"); + if (hdr->spare != t_hdr->spare) + fprintf(stderr, "spare not equal\n"); + if (hdr->ser_ind != t_hdr->ser_ind) + fprintf(stderr, "ser_ind not equal\n"); + if (hdr->addr != t_hdr->addr) + fprintf(stderr, "routing data not equal\n"); + + fprintf(stderr, "FAIL: Comparing headers failed.\n"); + abort(); +} + +static void check_mng(const uint8_t *data, const struct mtp_level_3_mng *t_mng) +{ + struct mtp_level_3_hdr *hdr = (struct mtp_level_3_hdr *) data; + struct mtp_level_3_mng *mng = (struct mtp_level_3_mng *) &hdr->data[0]; + + if (memcmp(mng, t_mng, sizeof(*mng)) == 0) + return; + + if (mng->cmn.h0 != t_mng->cmn.h0) + fprintf(stderr, "h0 not equal.\n"); + if (mng->cmn.h1 != t_mng->cmn.h1) + fprintf(stderr, "h1 not equal.\n"); + if (mng->length != t_mng->length) + fprintf(stderr, "length not euqal.\n"); + fprintf(stderr, "FAIL: Comparing the mtp_level_3_mng\n"); + abort(); +} + +static void check_prohib(const uint8_t *data, const struct mtp_level_3_prohib *t_prohib) +{ + struct mtp_level_3_hdr *hdr = (struct mtp_level_3_hdr *) data; + struct mtp_level_3_prohib *prohib = (struct mtp_level_3_prohib *) &hdr->data[0]; + + if (memcmp(prohib, t_prohib, sizeof(*prohib)) == 0) + return; + + if (prohib->cmn.h0 != t_prohib->cmn.h0) + fprintf(stderr, "h0 not equal.\n"); + if (prohib->cmn.h1 != t_prohib->cmn.h1) + fprintf(stderr, "h1 not equal.\n"); + if (ntohs(prohib->apoc) != t_prohib->apoc) + fprintf(stderr, "apoc not euqal.\n"); + fprintf(stderr, "FAIL: Comparing the mtp_level_3_prohib\n"); + abort(); +} + +int main(int argc, char **argv) +{ + uint32_t addr; + int i; + + /* set the addresses here due big endian MTP_ADDRESS macro */ + tests[0].hdr.addr = MTP_ADDR(0x00, 136, 91); + tests[1].hdr.addr = MTP_ADDR(0x00, 136, 91); + tests[2].hdr.addr = MTP_ADDR(0x00, 136, 91); + tests[2].prohib.apoc = MTP_MAKE_APOC(86); + + for (i = 0; i < ARRAY_SIZE(tests); ++i) { + check_hdr(tests[i].input, &tests[i].hdr); + if (tests[i].has_mng) + check_mng(tests[i].input, &tests[i].mng); + if (tests[i].has_prohib) + check_prohib(tests[i].input, &tests[i].prohib); + } + + /* check the SCCP unitdata */ + { + struct sccp_con_ctrl_prt_mgt prt = { + .sst = 0x03, + .assn = 254, + .apoc = MTP_MAKE_APOC(91), + .mul_ind = 1, + }; + + uint8_t data[] = { 0x03, 0xfe, 0x5b, 0x00, 0x01 }; + if (memcmp(&prt, data, 5) != 0) { + uint8_t *d = (uint8_t *) &prt; + fprintf(stderr, "GOT: 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n", + d[0], d[1], d[2], d[3], d[4]); + abort(); + } + } + + /* verify decoding of the sls */ + for (i = 0; i < 16; ++i) { + addr = MTP_ADDR(i, 136, 91); + if (MTP_LINK_SLS(addr) != i) { + fprintf(stderr, "0x%x/0x%x does not match 0x%x\n", addr, MTP_LINK_SLS(addr), i); + abort(); + } + } + + fprintf(stderr, "SUCCESS\n"); + return 0; +} -- cgit v1.2.3