From 6629561108d86d26dddbf4a522964c3df80dd95e Mon Sep 17 00:00:00 2001 From: Patrick McHardy Date: Tue, 16 Nov 2010 21:23:43 +0100 Subject: [PATCH] dectmon: multi-cluster support Signed-off-by: Patrick McHardy --- include/dectmon.h | 39 ++++++++--- src/debug.c | 9 +++ src/dlc.c | 18 +++-- src/dummy_ops.c | 10 +++ src/event_ops.c | 47 +++++++++---- src/mac.c | 141 +++++++++++++++++++++++-------------- src/main.c | 173 +++++++++++++++++++++++++++++++++++++++------- src/nwk.c | 36 ++++++---- 8 files changed, 351 insertions(+), 122 deletions(-) diff --git a/include/dectmon.h b/include/dectmon.h index 3c243a3..f916970 100644 --- a/include/dectmon.h +++ b/include/dectmon.h @@ -4,9 +4,10 @@ #include #include #include +#include +#include #include - -extern struct dect_handle *dh; +#include enum { DECTMON_DUMP_MAC = 0x1, @@ -26,6 +27,16 @@ extern void dect_dummy_ops_init(struct dect_ops *ops); extern void dect_hexdump(const char *prefix, const uint8_t *buf, size_t size); +struct dect_handle_priv { + struct list_head list; + const char *cluster; + struct dect_timer *lock_timer; + struct dect_ari pari; + struct list_head pt_list; + + struct dect_tbc *slots[DECT_FRAME_SIZE]; +}; + enum dect_mm_procedures { DECT_MM_NONE, DECT_MM_KEY_ALLOCATION, @@ -33,7 +44,6 @@ enum dect_mm_procedures { DECT_MM_CIPHERING, }; - struct dect_pt { struct list_head list; struct dect_ie_portable_identity *portable_identity; @@ -59,7 +69,8 @@ struct dect_dl { }; struct dect_msg_buf; -extern void dect_dl_data_ind(struct dect_dl *dl, struct dect_msg_buf *mb); +extern void dect_dl_data_ind(struct dect_handle *dh, struct dect_dl *dl, + struct dect_msg_buf *mb); struct dect_lc { uint16_t lsig; @@ -69,12 +80,12 @@ struct dect_lc { struct dect_mac_con { struct dect_lc *lc; - uint32_t pmid; struct dect_tbc *tbc; }; enum dect_data_channels; -extern void dect_mac_co_data_ind(struct dect_mac_con *mc, +extern void dect_mac_co_data_ind(struct dect_handle *dh, + struct dect_mac_con *mc, enum dect_data_channels chan, struct dect_msg_buf *mb); @@ -87,14 +98,22 @@ struct dect_mbc { }; struct dect_tbc { - struct dect_mbc mbc[2]; - struct dect_dl dl; - bool ciphered; + uint8_t slot1; + uint8_t slot2; + uint16_t fmid; + uint32_t pmid; + + struct dect_timer *timer; + struct dect_mbc mbc[2]; + + bool ciphered; uint8_t ks[2 * 45]; + + struct dect_dl dl; }; -extern void dect_mac_rcv(struct dect_msg_buf *mb, uint8_t slot); +extern void dect_mac_rcv(struct dect_handle *dh, struct dect_msg_buf *mb); /* DSC */ diff --git a/src/debug.c b/src/debug.c index 675f65f..5fd1091 100644 --- a/src/debug.c +++ b/src/debug.c @@ -1,3 +1,11 @@ +/* + * Copyright (c) 2010 Patrick McHardy + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + #include #include #include @@ -7,6 +15,7 @@ #include #include +#include #include #define BLOCKSIZE 16 diff --git a/src/dlc.c b/src/dlc.c index 99741c1..f49a119 100644 --- a/src/dlc.c +++ b/src/dlc.c @@ -43,8 +43,8 @@ static struct dect_lc *dect_lc_init(struct dect_mac_con *mc) struct dect_lc *lc; lc = calloc(1, sizeof(*lc)); - if ((mc->pmid & 0xf0000) != 0xe0000) - lc->lsig = mc->pmid; + if ((mc->tbc->pmid & 0xf0000) != 0xe0000) + lc->lsig = mc->tbc->pmid; return lc; } @@ -78,7 +78,8 @@ static const uint8_t channel_sdu_size[] = { #define roundup(x, y) ((((x) + ((y) - 1)) / (y)) * (y)) -static struct dect_msg_buf *dect_lc_reassemble(struct dect_lc *lc, +static struct dect_msg_buf *dect_lc_reassemble(struct dect_handle *dh, + struct dect_lc *lc, enum dect_data_channels chan, struct dect_msg_buf *mb) { @@ -101,10 +102,12 @@ static struct dect_msg_buf *dect_lc_reassemble(struct dect_lc *lc, mb = NULL; if (lc->rx_buf->len >= lc->rx_len) { - assert(lc->rx_buf->len == lc->rx_len); mb = lc->rx_buf; lc->rx_buf = NULL; + if (mb->len != lc->rx_len) + goto err; + if (!dect_fa_frame_csum_verify(lc, mb)) goto err; @@ -121,7 +124,8 @@ err: return NULL; } -void dect_mac_co_data_ind(struct dect_mac_con *mc, enum dect_data_channels chan, +void dect_mac_co_data_ind(struct dect_handle *dh, struct dect_mac_con *mc, + enum dect_data_channels chan, struct dect_msg_buf *mb) { struct dect_lc *lc; @@ -134,9 +138,9 @@ void dect_mac_co_data_ind(struct dect_mac_con *mc, enum dect_data_channels chan, mc->lc = lc; } - mb = dect_lc_reassemble(mc->lc, chan, mb); + mb = dect_lc_reassemble(dh, mc->lc, chan, mb); if (mb != NULL && mb->len > DECT_FA_HDR_SIZE) { dect_mbuf_pull(mb, DECT_FA_HDR_SIZE); - dect_dl_data_ind(&mc->tbc->dl, mb); + dect_dl_data_ind(dh, &mc->tbc->dl, mb); } } diff --git a/src/dummy_ops.c b/src/dummy_ops.c index 945e502..7f5c750 100644 --- a/src/dummy_ops.c +++ b/src/dummy_ops.c @@ -1,3 +1,12 @@ +/* + * Copyright (c) 2010 Patrick McHardy + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + + #include #include @@ -6,6 +15,7 @@ */ static void llme_mac_me_info_ind(struct dect_handle *dh, + const struct dect_ari *pari, const struct dect_fp_capabilities *fpc) { } diff --git a/src/event_ops.c b/src/event_ops.c index e69abe9..75c306b 100644 --- a/src/event_ops.c +++ b/src/event_ops.c @@ -1,3 +1,11 @@ +/* + * Copyright (c) 2010 Patrick McHardy + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + #include #include #include @@ -6,9 +14,15 @@ #include #include +struct io_event { + const struct dect_handle *dh; + struct event ev; +}; + static void event_io_callback(int fd, short mask, void *data) { struct dect_fd *dfd = data; + struct io_event *ioe = dect_fd_priv(dfd); uint32_t events; events = 0; @@ -17,13 +31,13 @@ static void event_io_callback(int fd, short mask, void *data) if (mask & EV_WRITE) events |= DECT_FD_WRITE; - dect_fd_process(dh, dfd, events); + dect_fd_process((struct dect_handle *)ioe->dh, dfd, events); } static int register_fd(const struct dect_handle *dh, struct dect_fd *dfd, uint32_t events) { - struct event *ev = dect_fd_priv(dfd); + struct io_event *ioe = dect_fd_priv(dfd); unsigned short mask; mask = EV_PERSIST; @@ -32,45 +46,50 @@ static int register_fd(const struct dect_handle *dh, struct dect_fd *dfd, if (events & DECT_FD_WRITE) mask |= EV_WRITE; - event_set(ev, dect_fd_num(dfd), mask, event_io_callback, dfd); - event_add(ev, NULL); + ioe->dh = dh; + event_set(&ioe->ev, dect_fd_num(dfd), mask, event_io_callback, dfd); + event_add(&ioe->ev, NULL); return 0; } static void unregister_fd(const struct dect_handle *dh, struct dect_fd *dfd) { - struct event *ev = dect_fd_priv(dfd); + struct io_event *ioe = dect_fd_priv(dfd); - event_del(ev); + event_del(&ioe->ev); } static void event_timer_callback(int fd, short mask, void *data) { - dect_timer_run(dh, data); + struct dect_timer *timer = data; + struct io_event *ioe = dect_timer_priv(timer); + + dect_timer_run((struct dect_handle *)ioe->dh, data); } static void start_timer(const struct dect_handle *dh, struct dect_timer *timer, const struct timeval *tv) { - struct event *ev = dect_timer_priv(timer); + struct io_event *ioe = dect_timer_priv(timer); - evtimer_set(ev, event_timer_callback, timer); - evtimer_add(ev, (struct timeval *)tv); + ioe->dh = dh; + evtimer_set(&ioe->ev, event_timer_callback, timer); + evtimer_add(&ioe->ev, (struct timeval *)tv); } static void stop_timer(const struct dect_handle *dh, struct dect_timer *timer) { - struct event *ev = dect_timer_priv(timer); + struct io_event *ioe = dect_timer_priv(timer); - evtimer_del(ev); + evtimer_del(&ioe->ev); } static const struct dect_event_ops dect_event_ops = { - .fd_priv_size = sizeof(struct event), + .fd_priv_size = sizeof(struct io_event), .register_fd = register_fd, .unregister_fd = unregister_fd, - .timer_priv_size = sizeof(struct event), + .timer_priv_size = sizeof(struct io_event), .start_timer = start_timer, .stop_timer = stop_timer }; diff --git a/src/mac.c b/src/mac.c index 9e9b072..14bdae3 100644 --- a/src/mac.c +++ b/src/mac.c @@ -493,7 +493,7 @@ static int dect_parse_ct_data(struct dect_tail_msg *tm, uint64_t t, uint8_t seq) return 0; } -static int dect_parse_tail_msg(struct dect_tail_msg *tm, uint8_t slot, +static int dect_parse_tail_msg(struct dect_tail_msg *tm, const struct dect_msg_buf *mb) { uint64_t t; @@ -514,7 +514,7 @@ static int dect_parse_tail_msg(struct dect_tail_msg *tm, uint8_t slot, return dect_parse_system_information(tm, t); case DECT_TI_PT: /* Paging tail in direction FP->PP, MAC control otherwise */ - if (slot < 12) + if (mb->slot < 12) return dect_parse_paging_msg(tm, t); case DECT_TI_MT: return dect_parse_mac_ctrl(tm, t); @@ -524,32 +524,77 @@ static int dect_parse_tail_msg(struct dect_tail_msg *tm, uint8_t slot, } } -static struct dect_trx_slot { - struct dect_tbc *tbc; -} slots[DECT_FRAME_SIZE]; +/* + * TBC + */ -static struct dect_tbc *dect_tbc_init(uint32_t pmid) +#define tbc_log(tbc, fmt, args...) \ + printf("TBC: PMID: %.5x FMID: %.3x: " fmt, \ + (tbc)->pmid, (tbc)->fmid, ## args) + +static void dect_tbc_release(struct dect_handle *dh, struct dect_tbc *tbc) { + struct dect_handle_priv *priv = dect_handle_priv(dh); + + tbc_log(tbc, "release\n"); + if (dect_timer_running(tbc->timer)) + dect_timer_stop(dh, tbc->timer); + priv->slots[tbc->slot1] = NULL; + priv->slots[tbc->slot2] = NULL; + free(tbc); +} + +static void dect_tbc_timeout(struct dect_handle *dh, struct dect_timer *timer) +{ + struct dect_tbc *tbc = dect_timer_data(timer); + + tbc_log(tbc, "timeout\n"); + dect_tbc_release(dh, tbc); +} + +static struct dect_tbc *dect_tbc_init(struct dect_handle *dh, + const struct dect_tail_msg *tm, + uint8_t slot) +{ + struct dect_handle_priv *priv = dect_handle_priv(dh); + uint8_t slot2 = dect_tdd_slot(slot); struct dect_tbc *tbc; tbc = calloc(1, sizeof(*tbc)); if (tbc == NULL) - return NULL; + goto err1; - tbc->dl.tbc = tbc; + tbc->slot1 = slot; + tbc->slot2 = slot2; + tbc->fmid = tm->cctl.fmid; + tbc->pmid = tm->cctl.pmid; + + tbc->timer = dect_timer_alloc(dh); + if (tbc->timer == NULL) + goto err2; + dect_timer_setup(tbc->timer, dect_tbc_timeout, tbc); + dect_timer_start(dh, tbc->timer, 5); tbc->mbc[DECT_MODE_FP].cs_seq = 1; tbc->mbc[DECT_MODE_FP].cf_seq = 1; - tbc->mbc[DECT_MODE_FP].mc.pmid = pmid; tbc->mbc[DECT_MODE_FP].mc.tbc = tbc; - tbc->mbc[DECT_MODE_PP].cs_seq = 1; tbc->mbc[DECT_MODE_PP].cf_seq = 1; - tbc->mbc[DECT_MODE_PP].mc.pmid = pmid; tbc->mbc[DECT_MODE_PP].mc.tbc = tbc; + tbc->dl.tbc = tbc; + + priv->slots[slot] = tbc; + priv->slots[slot2] = tbc; + tbc_log(tbc, "establish: slot %u/%u\n", slot, slot2); + return tbc; + +err2: + free(tbc); +err1: + return NULL; } static void dect_dsc_cipher(struct dect_tbc *tbc, struct dect_msg_buf *mb) @@ -557,7 +602,7 @@ static void dect_dsc_cipher(struct dect_tbc *tbc, struct dect_msg_buf *mb) unsigned int i; uint8_t *ks; - if (mb->slot < 12) + if (mb->slot < DECT_HALF_FRAME_SIZE) ks = tbc->ks; else ks = tbc->ks + 45; @@ -575,34 +620,40 @@ static void dect_dsc_cipher(struct dect_tbc *tbc, struct dect_msg_buf *mb) mb->data[i + 8] ^= ks[i + 5]; } -static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot, +static void dect_tbc_rcv(struct dect_handle *dh, struct dect_tbc *tbc, struct dect_msg_buf *mb, struct dect_tail_msg *tm) { enum dect_b_identifications b_id; struct dect_mbc *mbc; unsigned int i; + uint8_t slot = mb->slot; bool cf; if (tbc->ciphered) { - if (slot < 12) + if (slot < DECT_HALF_FRAME_SIZE) dect_dsc_keystream(dect_dsc_iv(mb->mfn, mb->frame), tbc->dl.pt->dck, tbc->ks, sizeof(tbc->ks)); dect_dsc_cipher(tbc, mb); } - mbc = &tbc->mbc[slot < 12 ? DECT_MODE_FP : DECT_MODE_PP]; + if (tm->type == DECT_TM_TYPE_ID) { + dect_timer_stop(dh, tbc->timer); + dect_timer_start(dh, tbc->timer, 5); + } + + mbc = &tbc->mbc[slot < DECT_HALF_FRAME_SIZE ? DECT_MODE_FP : DECT_MODE_PP]; b_id = (mb->data[0] & DECT_HDR_BA_MASK); if (tm->type == DECT_TM_TYPE_CT) { if (tm->ctd.seq != mbc->cs_seq) { - printf("CS: incorrect seq: %u\n", tm->ctd.seq); + tbc_log(tbc, "CS: incorrect seq: %u\n", tm->ctd.seq); return; } mbc->cs_seq = !mbc->cs_seq; dect_mbuf_pull(mb, 1); - dect_mac_co_data_ind(&mbc->mc, DECT_MC_C_S, mb); + dect_mac_co_data_ind(dh, &mbc->mc, DECT_MC_C_S, mb); dect_mbuf_pull(mb, 7); } else dect_mbuf_pull(mb, 8); @@ -616,21 +667,18 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot, case DECT_BI_ETYPE_CF_0: case DECT_BI_ETYPE_CF_1: if (((b_id >> DECT_HDR_BA_SHIFT) & 0x1) != mbc->cf_seq) { - printf("CF: incorrect seq: %u\n", b_id & 0x1); + tbc_log(tbc, "CF: incorrect seq: %u\n", b_id & 0x1); return; } mbc->cf_seq = !mbc->cf_seq; for (i = 0; i < mb->len / 10; i++) { if (cf) { - mac_print("CF: seq: %u\n", i); - dect_mac_co_data_ind(&mbc->mc, DECT_MC_C_F, mb); - dect_mbuf_pull(mb, 10); - continue; - } - - if (!(mb->data[0] & 0x80)) + tbc_log(tbc, "CF: seq: %u\n", i); + dect_mac_co_data_ind(dh, &mbc->mc, DECT_MC_C_F, mb); + } else if (!(mb->data[0] & 0x80)) cf = true; + dect_mbuf_pull(mb, 10); } break; @@ -641,11 +689,8 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot, switch (tm->type) { case DECT_TM_TYPE_BCCTRL: case DECT_TM_TYPE_ACCTRL: - if (tm->cctl.cmd == DECT_CCTRL_RELEASE) { - slots[slot].tbc = NULL; - slots[dect_tdd_slot(slot)].tbc = NULL; - free(tbc); - } + if (tm->cctl.cmd == DECT_CCTRL_RELEASE) + dect_tbc_release(dh, tbc); break; case DECT_TM_TYPE_ENCCTRL: switch (tm->encctl.cmd) { @@ -653,10 +698,9 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot, printf("\n"); break; case DECT_ENCCTRL_START_CONFIRM: - printf("ciphering enabled: %s\n", slot < 12 ? "FP->PP" : "PP->FP"); - break; case DECT_ENCCTRL_START_GRANT: - printf("ciphering enabled: %s\n", slot < 12 ? "FP->PP" : "PP->FP"); + tbc_log(tbc, "ciphering enabled: %s\n", + slot < 12 ? "FP->PP" : "PP->FP"); break; default: break; @@ -667,41 +711,38 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot, } } -void dect_mac_rcv(struct dect_msg_buf *mb, uint8_t slot) +void dect_mac_rcv(struct dect_handle *dh, struct dect_msg_buf *mb) { - struct dect_trx_slot *ts = &slots[slot]; + struct dect_handle_priv *priv = dect_handle_priv(dh); + struct dect_tbc *tbc = priv->slots[mb->slot]; enum dect_tail_identifications a_id; enum dect_b_identifications b_id; struct dect_tail_msg tm; - struct dect_tbc *tbc; a_id = (mb->data[0] & DECT_HDR_TA_MASK) >> DECT_HDR_TA_SHIFT; b_id = (mb->data[0] & DECT_HDR_BA_MASK) >> DECT_HDR_BA_SHIFT; - mac_print("slot: %02u A: %x B: %x ", slot, a_id, b_id); + mac_print("slot: %02u A: %x B: %x ", mb->slot, a_id, b_id); - dect_parse_tail_msg(&tm, slot, mb); + dect_parse_tail_msg(&tm, mb); //dect_hexdump("MAC RCV", mb->data, mb->len); - if (ts->tbc != NULL) - return dect_tbc_rcv(ts->tbc, slot, mb, &tm); + if (tbc != NULL) + return dect_tbc_rcv(dh, tbc, mb, &tm); - if (tm.type == DECT_TM_TYPE_BCCTRL || - tm.type == DECT_TM_TYPE_ACCTRL) { + switch (tm.type) { + case DECT_TM_TYPE_BCCTRL: + case DECT_TM_TYPE_ACCTRL: switch (tm.cctl.cmd) { case DECT_CCTRL_ACCESS_REQ: case DECT_CCTRL_BEARER_HANDOVER_REQ: case DECT_CCTRL_CONNECTION_HANDOVER_REQ: - tbc = dect_tbc_init(tm.cctl.pmid); - if (tbc == NULL) - break; - - ts->tbc = tbc; - slots[slot - 12].tbc = tbc; - printf("TBC slot %u\n", slot); + dect_tbc_init(dh, &tm, mb->slot); break; default: - printf("unknown\n"); break; } + break; + default: + break; } } diff --git a/src/main.c b/src/main.c index d671b25..889efe3 100644 --- a/src/main.c +++ b/src/main.c @@ -1,3 +1,11 @@ +/* + * Copyright (c) 2010 Patrick McHardy + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + #include #include #include @@ -7,32 +15,94 @@ #include #include -struct dect_handle *dh; +#define DECT_MAX_CLUSTERS 16 +#define DECT_LOCK_TIMEOUT 15 -static void pexit(const char *str) +#define cluster_log(priv, fmt, args...) \ + printf("%s: " fmt, \ + ((struct dect_handle_priv *)dect_handle_priv(dh))->cluster, \ + ## args) + +static LIST_HEAD(dect_handles); +static unsigned int locked; + +static struct dect_handle_priv *dect_handle_lookup(const struct dect_ari *pari) { - perror(str); - exit(1); + struct dect_handle_priv *priv; + + list_for_each_entry(priv, &dect_handles, list) + if (!dect_ari_cmp(pari, &priv->pari)) + return priv; + return NULL; } +static void dect_lock_timer(struct dect_handle *dh, struct dect_timer *timer) +{ + struct dect_handle_priv *priv = dect_handle_priv(dh); + + cluster_log(dh, "timeout, lock failed\n"); + memset(&priv->pari, 0, sizeof(priv->pari)); + dect_llme_scan_req(dh); +} + +static void dect_mac_me_info_ind(struct dect_handle *dh, + const struct dect_ari *pari, + const struct dect_fp_capabilities *fpc) +{ + struct dect_handle_priv *priv = dect_handle_priv(dh); + + if (pari != NULL) { + if (dect_handle_lookup(pari) == NULL) { + cluster_log(dh, "MAC_ME_INFO-ind: EMC: %.4x FPN: %.5x\n", + pari->emc, pari->fpn); + + dect_llme_mac_me_info_res(dh, pari); + priv->pari = *pari; + dect_timer_start(dh, priv->lock_timer, DECT_LOCK_TIMEOUT); + } + } else if (fpc->fpc != 0) { + if (dect_timer_running(priv->lock_timer)) { + locked++; + cluster_log(dh, "locked (%u): EMC: %.4x FPN: %.5x\n", + locked, priv->pari.emc, priv->pari.fpn); + + dect_timer_stop(dh, priv->lock_timer); + } + } else { + locked--; + cluster_log(dh, "unlocked (%u): EMC: %.4x FPN: %.5x\n", + locked, priv->pari.emc, priv->pari.fpn); + + memset(&priv->pari, 0, sizeof(priv->pari)); + dect_llme_scan_req(dh); + } +} + +static struct dect_llme_ops_ llme_ops = { + .mac_me_info_ind = dect_mac_me_info_ind, +}; + static void dect_raw_rcv(struct dect_handle *dh, struct dect_fd *dfd, struct dect_msg_buf *mb) { - dect_mac_rcv(mb, mb->slot); + dect_mac_rcv(dh, mb); } static struct dect_raw_ops raw_ops = { - .raw_rcv = dect_raw_rcv, + .raw_rcv = dect_raw_rcv, }; static struct dect_ops ops = { - .raw_ops = &raw_ops, + .priv_size = sizeof(struct dect_handle_priv), + .llme_ops = &llme_ops, + .raw_ops = &raw_ops, }; -#define OPTSTRING "c:m:d:n:p:h" +#define OPTSTRING "c:sm:d:n:p:h" enum { OPT_CLUSTER = 'c', + OPT_SCAN = 's', OPT_DUMP_MAC = 'm', OPT_DUMP_DLC = 'd', OPT_DUMP_NWK = 'n', @@ -42,6 +112,7 @@ enum { static const struct option dectmon_opts[] = { { .name = "cluster", .has_arg = true, .flag = 0, .val = OPT_CLUSTER, }, + { .name = "scan", .has_arg = false, .flag = 0, .val = OPT_SCAN, }, { .name = "dump-mac", .has_arg = true, .flag = 0, .val = OPT_DUMP_MAC, }, { .name = "dump-dlc", .has_arg = true, .flag = 0, .val = OPT_DUMP_DLC, }, { .name = "dump-nwk", .has_arg = true, .flag = 0, .val = OPT_DUMP_NWK, }, @@ -50,6 +121,28 @@ static const struct option dectmon_opts[] = { { }, }; +static void pexit(const char *str) +{ + perror(str); + exit(1); +} + +static void dectmon_help(const char *progname) +{ + printf("%s [ options ]\n" + "\n" + "Options:\n" + " -c/--cluster=NAME Bind to cluster NAME. May be specified more than once.\n" + " -s/--scan Scan for FPs and lock dynamically\n" + " -m/--dump-mac=yes/no Dump MAC layer messages (default: no)\n" + " -d/--dump-dlc=yes/no Dump DLC layer messages (default: no)\n" + " -n/--dump-nwk=yes/no Dump NWK layer messages (default: yes)\n" + " -p/--auth-pin=PIN Authentication PIN for Key Allocation\n" + " -h/--help Show this help text\n" + "\n", + progname); +} + static uint32_t opt_yesno(const char *arg, uint32_t opts, uint32_t flag) { if (!strcmp(arg, "yes")) @@ -65,10 +158,37 @@ static uint32_t opt_yesno(const char *arg, uint32_t opts, uint32_t flag) const char *auth_pin = "0000"; uint32_t dumpopts = DECTMON_DUMP_NWK; +static struct dect_handle *dectmon_open_handle(struct dect_ops *ops, + const char *cluster) +{ + struct dect_handle_priv *priv; + struct dect_handle *dh; + + dh = dect_open_handle(ops, cluster); + if (dh == NULL) + pexit("dect_open_handle"); + + priv = dect_handle_priv(dh); + priv->cluster = cluster; + + priv->lock_timer = dect_timer_alloc(dh); + if (priv->lock_timer == NULL) + pexit("dect_alloc_timer"); + dect_timer_setup(priv->lock_timer, dect_lock_timer, priv); + + init_list_head(&priv->pt_list); + list_add_tail(&priv->list, &dect_handles); + + return dh; +} + int main(int argc, char **argv) { - const char *cluster = NULL; + const char *cluster[DECT_MAX_CLUSTERS] = {}; + unsigned int ncluster = 0, i; + struct dect_handle *dh; struct dect_fd *dfd; + bool scan = false; int optidx = 0, c; for (;;) { @@ -78,7 +198,10 @@ int main(int argc, char **argv) switch (c) { case OPT_CLUSTER: - cluster = optarg; + cluster[ncluster++] = optarg; + break; + case OPT_SCAN: + scan = true; break; case OPT_DUMP_MAC: dumpopts = opt_yesno(optarg, dumpopts, DECTMON_DUMP_MAC); @@ -93,18 +216,10 @@ int main(int argc, char **argv) auth_pin = optarg; break; case OPT_HELP: - printf("%s [ options ]\n" - "\n" - "Options:\n" - " -m/--dump-mac=yes/no\n" - " -d/--dump-dlc=yes/no\n" - " -n/--dump-nwk=yes/no\n" - " -p/--auth-pin=PIN\n" - " -h/--help\n", - argv[0]); - + dectmon_help(argv[0]); exit(0); case '?': + dectmon_help(argv[0]); exit(1); } } @@ -112,13 +227,19 @@ int main(int argc, char **argv) dect_event_ops_init(&ops); dect_dummy_ops_init(&ops); - dh = dect_open_handle(&ops, cluster); - if (dh == NULL) - pexit("dect_init_handle"); + if (ncluster == 0) + ncluster = 1; - dfd = dect_raw_socket(dh); - if (dfd == NULL) - pexit("dect_raw_socket"); + for (i = 0; i < ncluster; i++) { + dh = dectmon_open_handle(&ops, cluster[i]); + + dfd = dect_raw_socket(dh); + if (dfd == NULL) + pexit("dect_raw_socket"); + + if (scan) + dect_llme_scan_req(dh); + } dect_event_loop(); return 0; diff --git a/src/nwk.c b/src/nwk.c index 4c307d6..f1098b2 100644 --- a/src/nwk.c +++ b/src/nwk.c @@ -17,8 +17,6 @@ #include #include -static LIST_HEAD(dect_pt_list); - static const char * const nwk_msg_types[256] = { [DECT_LCE_PAGE_RESPONSE] = "LCE-PAGE-RESPONSE", [DECT_LCE_PAGE_REJECT] = "LCE-PAGE-REJECT", @@ -134,11 +132,13 @@ err: fclose(f); } -static struct dect_pt *dect_pt_lookup(struct dect_ie_portable_identity *portable_identity) +static struct dect_pt *dect_pt_lookup(struct dect_handle *dh, + struct dect_ie_portable_identity *portable_identity) { + struct dect_handle_priv *priv = dect_handle_priv(dh); struct dect_pt *pt; - list_for_each_entry(pt, &dect_pt_list, list) { + list_for_each_entry(pt, &priv->pt_list, list) { if (!dect_ipui_cmp(&pt->portable_identity->ipui, &portable_identity->ipui)) return pt; @@ -146,8 +146,10 @@ static struct dect_pt *dect_pt_lookup(struct dect_ie_portable_identity *portable return NULL; } -static struct dect_pt *dect_pt_init(struct dect_ie_portable_identity *portable_identity) +static struct dect_pt *dect_pt_init(struct dect_handle *dh, + struct dect_ie_portable_identity *portable_identity) { + struct dect_handle_priv *priv = dect_handle_priv(dh); struct dect_pt *pt; pt = calloc(1, sizeof(*pt)); @@ -155,13 +157,14 @@ static struct dect_pt *dect_pt_init(struct dect_ie_portable_identity *portable_i return NULL; pt->portable_identity = dect_ie_hold(portable_identity); - list_add_tail(&pt->list, &dect_pt_list); + list_add_tail(&pt->list, &priv->pt_list); dect_pt_read_uak(pt); return pt; } -static void dect_pt_track_key_allocation(struct dect_pt *pt, uint8_t msgtype, +static void dect_pt_track_key_allocation(struct dect_handle *dh, + struct dect_pt *pt, uint8_t msgtype, const struct dect_sfmt_ie *ie, struct dect_ie_common *common) { @@ -235,7 +238,8 @@ release: pt->procedure = DECT_MM_NONE; } -static void dect_pt_track_auth(struct dect_pt *pt, uint8_t msgtype, +static void dect_pt_track_auth(struct dect_handle *dh, + struct dect_pt *pt, uint8_t msgtype, const struct dect_sfmt_ie *ie, struct dect_ie_common *common) { @@ -301,7 +305,8 @@ release: pt->procedure = DECT_MM_NONE; } -static void dect_pt_track_ciphering(struct dect_pt *pt, uint8_t msgtype, +static void dect_pt_track_ciphering(struct dect_handle *dh, + struct dect_pt *pt, uint8_t msgtype, const struct dect_sfmt_ie *ie, struct dect_ie_common *common) { @@ -316,7 +321,8 @@ static void dect_pt_track_ciphering(struct dect_pt *pt, uint8_t msgtype, } } -void dect_dl_data_ind(struct dect_dl *dl, struct dect_msg_buf *mb) +void dect_dl_data_ind(struct dect_handle *dh, struct dect_dl *dl, + struct dect_msg_buf *mb) { struct dect_pt *pt; struct dect_sfmt_ie ie; @@ -340,17 +346,17 @@ void dect_dl_data_ind(struct dect_dl *dl, struct dect_msg_buf *mb) return; if (ie.id == DECT_IE_PORTABLE_IDENTITY) { - pt = dect_pt_lookup((void *)common); + pt = dect_pt_lookup(dh, (void *)common); if (pt == NULL) - pt = dect_pt_init((void *)common); + pt = dect_pt_init(dh, (void *)common); dl->pt = pt; pt->dl = dl; } if (dl->pt != NULL) { - dect_pt_track_key_allocation(dl->pt, msgtype, &ie, common); - dect_pt_track_auth(dl->pt, msgtype, &ie, common); - dect_pt_track_ciphering(dl->pt, msgtype, &ie, common); + dect_pt_track_key_allocation(dh, dl->pt, msgtype, &ie, common); + dect_pt_track_auth(dh, dl->pt, msgtype, &ie, common); + dect_pt_track_ciphering(dh, dl->pt, msgtype, &ie, common); } __dect_ie_put(dh, common);