diff options
author | Holger Hans Peter Freyther <zecke@selfish.org> | 2011-01-20 19:51:36 +0100 |
---|---|---|
committer | Holger Hans Peter Freyther <zecke@selfish.org> | 2011-01-20 19:51:36 +0100 |
commit | 42f9aa94ba5a3f7236ba279785f03c4608a9dc5a (patch) | |
tree | 226e9710faebb387f5d8e16175e4ac0775c8dcbf | |
parent | 460a8eb8654ae945ac73b46f5f078ced611a4478 (diff) | |
parent | 050577a088f6e8cfd02dca82fe47b4b34b53ae4e (diff) |
Merge branch 'on-waves/multiple-links'
-rw-r--r-- | include/bsc_data.h | 2 | ||||
-rw-r--r-- | include/mtp_data.h | 46 | ||||
-rw-r--r-- | include/mtp_level3.h | 10 | ||||
-rw-r--r-- | src/Makefile.am | 8 | ||||
-rw-r--r-- | src/link_udp.c | 7 | ||||
-rw-r--r-- | src/links.c | 22 | ||||
-rw-r--r-- | src/main_stp.c | 1 | ||||
-rw-r--r-- | src/mtp_layer3.c | 225 | ||||
-rw-r--r-- | src/mtp_link.c | 157 | ||||
-rw-r--r-- | tests/mtp/mtp_parse_test.c | 12 |
10 files changed, 300 insertions, 190 deletions
diff --git a/include/bsc_data.h b/include/bsc_data.h index 3d10bb3..758932f 100644 --- a/include/bsc_data.h +++ b/include/bsc_data.h @@ -112,8 +112,6 @@ struct bsc_data { /* bsc related functions */ void release_bsc_resources(struct bsc_data *bsc); -void mtp_link_down(struct mtp_link *data); -void mtp_link_up(struct mtp_link *data); void mtp_linkset_down(struct mtp_link_set *); void mtp_linkset_up(struct mtp_link_set *); diff --git a/include/mtp_data.h b/include/mtp_data.h index e00f13a..f04c07d 100644 --- a/include/mtp_data.h +++ b/include/mtp_data.h @@ -26,6 +26,7 @@ struct bsc_data; struct mtp_link; +struct mtp_level_3_mng *mng; /* MTP Level3 timers */ @@ -48,24 +49,14 @@ struct mtp_link_set { int available; int running; int sccp_up; + int linkset_up; int last_sls; - /* misc data */ - uint8_t test_ptrn[14]; - - int sltm_pending; - int sltm_once; - int was_up; - - int slta_misses; - struct timer_list t1_timer; - struct timer_list t2_timer; - - struct timer_list delay_timer; - struct llist_head links; + int nr_links; struct mtp_link *slc[16]; + int sltm_once; /* special handling */ int pass_all_isup; @@ -84,12 +75,24 @@ struct mtp_link { struct llist_head entry; int pcap_fd; - struct mtp_link_set *the_link; + struct mtp_link_set *set; int available; struct timer_list link_activate; + /* link test routine */ + uint8_t test_ptrn[14]; + + int link_no; + int sltm_pending; + int was_up; + + int slta_misses; + struct timer_list t1_timer; + struct timer_list t2_timer; + + /* callback's to implement */ int (*start)(struct mtp_link *); int (*write)(struct mtp_link *, struct msgb *msg); int (*shutdown)(struct mtp_link *); @@ -117,6 +120,19 @@ void mtp_link_set_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); -void mtp_link_set_sccp_down(struct mtp_link_set *link); + +/* link related routines */ +void mtp_link_down(struct mtp_link *data); +void mtp_link_up(struct mtp_link *data); + +void mtp_link_init(struct mtp_link *link); +void mtp_link_start_link_test(struct mtp_link *link); +void mtp_link_stop_link_test(struct mtp_link *link); +int mtp_link_slta(struct mtp_link *link, uint16_t l3_len, struct mtp_level_3_mng *mng); + +void mtp_link_failure(struct mtp_link *fail); + +/* internal routines */ +struct msgb *mtp_msg_alloc(struct mtp_link_set *link); #endif diff --git a/include/mtp_level3.h b/include/mtp_level3.h index bc2d186..f311ad5 100644 --- a/include/mtp_level3.h +++ b/include/mtp_level3.h @@ -65,7 +65,7 @@ #define SCCP_SSA 0x01 #define MTP_LINK_MASK 0x0F -#define MTP_ADDR_MASK 0x0FFF +#define MTP_ADDR_MASK 0x3FFF #define MTP_APOC_MASK 0x3f @@ -77,6 +77,10 @@ ((link) & MTP_LINK_MASK) << 28) #define MTP_MAKE_APOC(apoc) \ (apoc & 0x3fff) +#define MTP_ADDR_DPC(addr) \ + (addr & MTP_ADDR_MASK) +#define MTP_ADDR_OPC(addr) \ + ((addr >> 14) & MTP_ADDR_MASK) #elif __BYTE_ORDER == __BIG_ENDIAN static inline uint32_t c_swap_32(uint32_t in) { @@ -97,6 +101,10 @@ static inline uint16_t c_swap_16(uint16_t in) ((link) & MTP_LINK_MASK) << 28) #define MTP_MAKE_APOC(apoc) \ c_swap_16((apoc & 0x3fff)) +#define MTP_ADDR_DPC(addr) \ + (c_swap_32(addr) & MTP_ADDR_MASK) +#define MTP_ADDR_OPC(addr) \ + ((c_swap_32(addr) >> 14) & MTP_ADDR_MASK) #endif diff --git a/src/Makefile.am b/src/Makefile.am index eb6ab11..defe121 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -11,17 +11,19 @@ mgcp_mgw_LDADD = $(LAFORGE_LIBS) $(NEXUSWARE_C7_LIBS) $(NEXUSWARE_UNIPORTE_LIBS) cellmgr_ng_SOURCES = main.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \ bss_patch.c bssap_sccp.c bsc_sccp.c bsc_ussd.c links.c \ - msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c isup.c + msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c isup.c \ + mtp_link.c cellmgr_ng_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \ -lpthread -lnetsnmp -lcrypto udt_relay_SOURCES = main_udt.c mtp_layer3.c thread.c input/ipaccess.c pcap.c \ msc_conn.c link_udp.c snmp_mtp.c debug.c vty_interface.c \ - bss_patch.c isup.c links.c sctp_m2ua.c + bss_patch.c isup.c links.c sctp_m2ua.c mtp_link.c udt_relay_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \ -lpthread -lnetsnmp -lcrypto -lm2ua -lsctp osmo_stp_SOURCES = main_stp.c mtp_layer3.c thread.c pcap.c link_udp.c snmp_mtp.c \ - debug.c vty_interface.c links.c isup.c sctp_m2ua.c + debug.c vty_interface.c links.c isup.c sctp_m2ua.c \ + mtp_link.c osmo_stp_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOSCCP_LIBS) $(LIBOSMOVTY_LIBS) $(NEXUSWARE_C7_LIBS) \ -lpthread -lnetsnmp -lcrypto -lm2ua -lsctp diff --git a/src/link_udp.c b/src/link_udp.c index 01296dc..7e0b94b 100644 --- a/src/link_udp.c +++ b/src/link_udp.c @@ -81,7 +81,7 @@ static int udp_read_cb(struct bsc_fd *fd) } /* throw away data as the link is down */ - if (link->the_link->available == 0) { + if (link->set->available == 0) { LOGP(DINP, LOGL_ERROR, "The link is down. Not forwarding.\n"); rc = 0; goto exit; @@ -91,12 +91,11 @@ static int udp_read_cb(struct bsc_fd *fd) 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_down(link); - mtp_link_up(link); + mtp_link_failure(link); goto exit; } else if (hdr->data_type > UDP_DATA_MSU_PRIO_3) { LOGP(DINP, LOGL_ERROR, "Link failure. retrieved message.\n"); - mtp_link_down(link); + mtp_link_failure(link); goto exit; } diff --git a/src/links.c b/src/links.c index 8802662..6374afd 100644 --- a/src/links.c +++ b/src/links.c @@ -46,29 +46,30 @@ void mtp_link_down(struct mtp_link *link) was_up = link->available; link->available = 0; - one_up = is_one_up(link->the_link); + link->was_up = 0; + one_up = is_one_up(link->set); /* our linkset is now unsuable */ if (was_up && !one_up) - mtp_linkset_down(link->the_link); + mtp_linkset_down(link->set); link->clear_queue(link); - mtp_link_set_init_slc(link->the_link); + mtp_link_stop_link_test(link); + mtp_link_set_init_slc(link->set); } void mtp_link_up(struct mtp_link *link) { int one_up; - one_up = is_one_up(link->the_link); + one_up = is_one_up(link->set); link->available = 1; + link->was_up = 0; - mtp_link_set_init_slc(link->the_link); + mtp_link_set_init_slc(link->set); if (!one_up) - mtp_linkset_up(link->the_link); -} - -void mtp_link_set_sccp_down(struct mtp_link_set *link) -{ + mtp_linkset_up(link->set); + else + mtp_link_start_link_test(link); } void mtp_link_set_submit(struct mtp_link *link, struct msgb *msg) @@ -112,7 +113,6 @@ 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->base.the_link = bsc->link_set; lnk->bsc = bsc; lnk->link_index = 1; lnk->reset_timeout = bsc->udp_reset_timeout; diff --git a/src/main_stp.c b/src/main_stp.c index d81b860..36e04d6 100644 --- a/src/main_stp.c +++ b/src/main_stp.c @@ -254,7 +254,6 @@ int main(int argc, char **argv) lnk = sctp_m2ua_transp_create("0.0.0.0", 2904); lnk->base.pcap_fd = bsc.pcap_fd; - lnk->base.the_link = bsc.m2ua_set; mtp_link_set_add_link(bsc.m2ua_set, (struct mtp_link *) lnk); llist_for_each_entry(data, &bsc.m2ua_set->links, entry) diff --git a/src/mtp_layer3.c b/src/mtp_layer3.c index e9d01c9..67ae70e 100644 --- a/src/mtp_layer3.c +++ b/src/mtp_layer3.c @@ -36,7 +36,7 @@ static void *tall_mtp_ctx = NULL; static int mtp_int_submit(struct mtp_link_set *link, int pc, int sls, int type, const uint8_t *data, unsigned int length); -static struct msgb *mtp_msg_alloc(struct mtp_link_set *link) +struct msgb *mtp_msg_alloc(struct mtp_link_set *link) { struct mtp_level_3_hdr *hdr; struct msgb *msg = msgb_alloc_headroom(4096, 128, "mtp-msg"); @@ -53,34 +53,8 @@ static struct msgb *mtp_msg_alloc(struct mtp_link_set *link) return msg; } -static struct msgb *mtp_create_sltm(struct mtp_link_set *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->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_set *link, struct mtp_level_3_mng *in_mng, int l3_len) +static struct msgb *mtp_create_slta(struct mtp_link_set *link, int sls, + struct mtp_level_3_mng *in_mng, int l3_len) { struct mtp_level_3_hdr *hdr; struct mtp_level_3_mng *mng; @@ -91,6 +65,8 @@ static struct msgb *mtp_create_slta(struct mtp_link_set *link, struct mtp_level_ hdr = (struct mtp_level_3_hdr *) out->l2h; hdr->ser_ind = MTP_SI_MNT_REG_MSG; + hdr->addr = MTP_ADDR(sls, link->dpc, link->opc); + 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; @@ -101,6 +77,7 @@ static struct msgb *mtp_create_slta(struct mtp_link_set *link, struct mtp_level_ return out; } + static struct msgb *mtp_base_alloc(struct mtp_link_set *link, int msg, int apoc) { struct mtp_level_3_hdr *hdr; @@ -203,64 +180,6 @@ void mtp_link_set_init(void) tall_mtp_ctx = talloc_named_const(NULL, 1, "mtp-link"); } -static void mtp_send_sltm(struct mtp_link_set *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_set_submit(link->slc[0], msg); -} - -static void mtp_sltm_t1_timeout(void *_link) -{ - struct mtp_link_set *link = (struct mtp_link_set *) _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_set_sccp_down(link); - mtp_link_restart(link->slc[0]); - } -} - -static void mtp_sltm_t2_timeout(void *_link) -{ - struct mtp_link_set *link = (struct mtp_link_set *) _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_set *mtp_link_set_alloc(void) { struct mtp_link_set *link; @@ -270,12 +189,6 @@ struct mtp_link_set *mtp_link_set_alloc(void) return NULL; link->ni = MTP_NI_NATION_NET; - 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->links); return link; @@ -283,21 +196,23 @@ struct mtp_link_set *mtp_link_set_alloc(void) void mtp_link_set_stop(struct mtp_link_set *link) { - bsc_del_timer(&link->t1_timer); - bsc_del_timer(&link->t2_timer); - bsc_del_timer(&link->delay_timer); + struct mtp_link *lnk; + llist_for_each_entry(lnk, &link->links, entry) + mtp_link_stop_link_test(lnk); + link->sccp_up = 0; link->running = 0; - link->sltm_pending = 0; - - mtp_link_set_sccp_down(link); + link->linkset_up = 0; } void mtp_link_set_reset(struct mtp_link_set *link) { + struct mtp_link *lnk; mtp_link_set_stop(link); link->running = 1; - bsc_schedule_timer(&link->delay_timer, START_DELAY); + + llist_for_each_entry(lnk, &link->links, entry) + mtp_link_start_link_test(lnk); } static int send_tfp(struct mtp_link_set *link, int apoc) @@ -331,6 +246,39 @@ static int send_tfa(struct mtp_link_set *link, int opc) return 0; } +static int linkset_up(struct mtp_link_set *set) +{ + /* the link set is already up */ + if (set->linkset_up) + return 0; + + if (send_tfp(set, 0) != 0) + return -1; + if (send_tfp(set, set->opc) != 0) + return -1; + if (set->sccp_opc != set->opc && + send_tfp(set, set->sccp_opc) != 0) + return -1; + if (set->isup_opc != set->opc && + send_tfp(set, set->isup_opc) != 0) + return -1; + + /* Send the TRA for all PCs */ + if (send_tra(set, set->opc) != 0) + return -1; + if (set->sccp_opc != set->opc && + send_tfa(set, set->sccp_opc) != 0) + return -1; + if (set->isup_opc != set->opc && + send_tfa(set, set->isup_opc) != 0) + return -1; + + set->linkset_up = 1; + LOGP(DINP, LOGL_NOTICE, + "The linkset %p is considered running.\n", set); + return 0; +} + static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *hdr, int l3_len) { struct mtp_level_3_cmn *cmn; @@ -351,32 +299,7 @@ static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr * switch (cmn->h1) { case MTP_RESTR_MSG_ALLWED: LOGP(DINP, LOGL_INFO, "Received Restart Allowed. SST could be next: %p\n", link); - link->sccp_up = 0; - mtp_link_set_sccp_down(link); - - if (send_tfp(link, 0) != 0) - return -1; - if (send_tfp(link, link->opc) != 0) - return -1; - if (link->sccp_opc != link->opc && - send_tfp(link, link->sccp_opc) != 0) - return -1; - if (link->isup_opc != link->opc && - send_tfp(link, link->isup_opc) != 0) - return -1; - - /* Send the TRA for all PCs */ - if (send_tra(link, link->opc) != 0) - return -1; - if (link->sccp_opc != link->opc && - send_tfa(link, link->sccp_opc) != 0) - return -1; - if (link->isup_opc != link->opc && - send_tfa(link, link->isup_opc) != 0) - return -1; - link->sccp_up = 1; - link->was_up = 1; LOGP(DINP, LOGL_INFO, "SCCP traffic allowed. %p\n", link); return 0; break; @@ -403,17 +326,23 @@ static int mtp_link_sign_msg(struct mtp_link_set *link, struct mtp_level_3_hdr * return -1; } -static int mtp_link_regular_msg(struct mtp_link_set *link, struct mtp_level_3_hdr *hdr, int l3_len) +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->ni != link->ni || l3_len < 1) { + if (hdr->ni != link->set->ni || l3_len < 1) { LOGP(DINP, LOGL_ERROR, "Unhandled data (ni: %d len: %d)\n", hdr->ni, l3_len); return -1; } + if (MTP_ADDR_DPC(hdr->addr) != link->set->opc) { + LOGP(DINP, LOGL_ERROR, "MSG for 0x%x not handled by 0x%x\n", + MTP_ADDR_DPC(hdr->addr), link->set->opc); + 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); @@ -423,32 +352,18 @@ static int mtp_link_regular_msg(struct mtp_link_set *link, struct mtp_level_3_hd switch (mng->cmn.h1) { case MTP_TST_MSG_SLTM: /* simply respond to the request... */ - out = mtp_create_slta(link, mng, l3_len); + out = mtp_create_slta(link->set, + MTP_LINK_SLS(hdr->addr), + mng, l3_len); if (!out) return -1; - mtp_link_set_submit(link->slc[0], out); + mtp_link_set_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; - return 0; + /* If this link is proven set it up */ + if (mtp_link_slta(link, l3_len, mng) == 0) + linkset_up(link->set); break; } break; @@ -527,7 +442,7 @@ int mtp_link_set_data(struct mtp_link *link, struct msgb *msg) if (!msg->l2h || msgb_l2len(msg) < sizeof(*hdr)) return -1; - if (!link->the_link->running) { + if (!link->set->running) { LOGP(DINP, LOGL_ERROR, "Link is not running. Call mtp_link_reset first: %p\n", link); return -1; } @@ -537,17 +452,17 @@ int mtp_link_set_data(struct mtp_link *link, struct msgb *msg) switch (hdr->ser_ind) { case MTP_SI_MNT_SNM_MSG: - rc = mtp_link_sign_msg(link->the_link, hdr, l3_len); + rc = mtp_link_sign_msg(link->set, hdr, l3_len); break; case MTP_SI_MNT_REG_MSG: - rc = mtp_link_regular_msg(link->the_link, hdr, l3_len); + rc = mtp_link_regular_msg(link, hdr, l3_len); break; case MTP_SI_MNT_SCCP: - rc = mtp_link_sccp_data(link->the_link, hdr, msg, l3_len); + rc = mtp_link_sccp_data(link->set, hdr, msg, l3_len); break; case MTP_SI_MNT_ISUP: msg->l3h = &hdr->data[0]; - rc = mtp_link_set_isup(link->the_link, msg, MTP_LINK_SLS(hdr->addr)); + rc = mtp_link_set_isup(link->set, msg, MTP_LINK_SLS(hdr->addr)); break; default: fprintf(stderr, "Unhandled: %u\n", hdr->ser_ind); @@ -648,6 +563,10 @@ void mtp_link_set_init_slc(struct mtp_link_set *set) void mtp_link_set_add_link(struct mtp_link_set *set, struct mtp_link *lnk) { + lnk->set = set; + lnk->link_no = set->nr_links++; + mtp_link_init(lnk); + llist_add_tail(&lnk->entry, &set->links); mtp_link_set_init_slc(set); } diff --git a/src/mtp_link.c b/src/mtp_link.c new file mode 100644 index 0000000..5e5f418 --- /dev/null +++ b/src/mtp_link.c @@ -0,0 +1,157 @@ +/* MTP level3 link */ +/* + * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org> + * (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 Affero General Public License as published by + * the Free Software Foundation, either version 3 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <mtp_data.h> +#include <mtp_level3.h> +#include <cellmgr_debug.h> + +#include <string.h> + +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->set); + uint8_t *data; + if (!msg) + return NULL; + + hdr = (struct mtp_level_3_hdr *) msg->l2h; + hdr->ser_ind = MTP_SI_MNT_REG_MSG; + hdr->addr = MTP_ADDR(link->link_no % 16, link->set->dpc, link->set->opc); + + 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 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; + } + + link->write(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); + bsc_del_timer(&link->t2_timer); + mtp_link_failure(link); + } +} + +static void mtp_sltm_t2_timeout(void *_link) +{ + struct mtp_link *link = (struct mtp_link *) _link; + + if (!link->set->running) { + LOGP(DINP, LOGL_INFO, "The linkset is not active. Stopping SLTM now. %p\n", link); + return; + } + + link->slta_misses = 0; + mtp_send_sltm(link); + + bsc_schedule_timer(&link->t1_timer, MTP_T1); + + if (link->set->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); +} + +void mtp_link_init(struct mtp_link *link) +{ + 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; +} + +void mtp_link_stop_link_test(struct mtp_link *link) +{ + bsc_del_timer(&link->t1_timer); + bsc_del_timer(&link->t2_timer); + + link->sltm_pending = 0; +} + +void mtp_link_start_link_test(struct mtp_link *link) +{ + mtp_sltm_t2_timeout(link); +} + +int mtp_link_slta(struct mtp_link *link, uint16_t l3_len, + struct mtp_level_3_mng *mng) +{ + 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; + link->was_up = 1; + + return 0; +} + +void mtp_link_failure(struct mtp_link *link) +{ + LOGP(DINP, LOGL_ERROR, "Link has failed. Resetting it: 0x%p\n", link); + link->reset(link); +} diff --git a/tests/mtp/mtp_parse_test.c b/tests/mtp/mtp_parse_test.c index 2ef726b..cd5d603 100644 --- a/tests/mtp/mtp_parse_test.c +++ b/tests/mtp/mtp_parse_test.c @@ -589,6 +589,18 @@ int main(int argc, char **argv) tests[2].hdr.addr = MTP_ADDR(0x00, 136, 91); tests[2].prohib.apoc = MTP_MAKE_APOC(86); + addr = MTP_ADDR(0x00, 0x2AAA, 0x1555); + if (MTP_ADDR_OPC(addr) != 0x1555) { + fprintf(stderr, "Failed to extract OPC: %d\n", + MTP_ADDR_OPC(addr)); + abort(); + } + if (MTP_ADDR_DPC(addr) != 0x2AAA) { + fprintf(stderr, "Failed to extract DPC: %d\n", + MTP_ADDR_DPC(addr)); + abort(); + } + for (i = 0; i < ARRAY_SIZE(tests); ++i) { check_hdr(tests[i].input, &tests[i].hdr); if (tests[i].has_mng) |