From ee7a645ca4879f558ce01d562be984b7d56dc054 Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Fri, 27 May 2011 09:37:06 +0200 Subject: [PATCH] Add LLC defragmentation + SDNCP output via tun device this allows us to look at IP messages contained in SNDCP --- src/Makefile | 2 +- src/tetra_llc.c | 157 ++++++++++++++++++++++++++++++++++++++++++ src/tetra_llc_pdu.h | 27 +++++++- src/tetra_upper_mac.c | 29 ++++++-- 4 files changed, 205 insertions(+), 10 deletions(-) create mode 100644 src/tetra_llc.c diff --git a/src/Makefile b/src/Makefile index fe2044c..19887e5 100644 --- a/src/Makefile +++ b/src/Makefile @@ -9,7 +9,7 @@ all: conv_enc_test crc_test tetra-rx float_to_bits tunctl libosmo-tetra-phy.a: phy/tetra_burst_sync.o phy/tetra_burst.o $(AR) r $@ $^ -libosmo-tetra-mac.a: lower_mac/tetra_conv_enc.o tetra_tdma.o lower_mac/tetra_scramb.o lower_mac/tetra_rm3014.o lower_mac/tetra_interleave.o lower_mac/crc_simple.o tetra_common.o lower_mac/viterbi.o lower_mac/viterbi_cch.o lower_mac/viterbi_tch.o lower_mac/tetra_lower_mac.o tetra_upper_mac.o tetra_mac_pdu.o tetra_llc_pdu.o tetra_mle_pdu.o tetra_mm_pdu.o tetra_cmce_pdu.o tetra_sndcp_pdu.o tetra_gsmtap.o +libosmo-tetra-mac.a: lower_mac/tetra_conv_enc.o tetra_tdma.o lower_mac/tetra_scramb.o lower_mac/tetra_rm3014.o lower_mac/tetra_interleave.o lower_mac/crc_simple.o tetra_common.o lower_mac/viterbi.o lower_mac/viterbi_cch.o lower_mac/viterbi_tch.o lower_mac/tetra_lower_mac.o tetra_upper_mac.o tetra_mac_pdu.o tetra_llc_pdu.o tetra_llc.o tetra_mle_pdu.o tetra_mm_pdu.o tetra_cmce_pdu.o tetra_sndcp_pdu.o tetra_gsmtap.o tuntap.o $(AR) r $@ $^ float_to_bits: float_to_bits.o diff --git a/src/tetra_llc.c b/src/tetra_llc.c new file mode 100644 index 0000000..432f5a7 --- /dev/null +++ b/src/tetra_llc.c @@ -0,0 +1,157 @@ +/* TETRA LLC Layer */ + +/* (C) 2011 by Harald Welte + * 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 . + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include "tetra_llc_pdu.h" + +static int tun_fd = -1; + +static struct tllc_state g_llcs = { + .rx.defrag_list = LLIST_HEAD_INIT(g_llcs.rx.defrag_list), +}; + +int rx_tl_sdu(struct msgb *msg, unsigned int len); + +static struct tllc_defrag_q_e * +get_dqe_for_ns(struct tllc_state *llcs, uint8_t ns, int alloc_if_missing) +{ + struct tllc_defrag_q_e *dqe; + llist_for_each_entry(dqe, &llcs->rx.defrag_list, list) { + if (dqe->ns == ns) + return dqe; + } + if (alloc_if_missing) { + dqe = talloc_zero(NULL, struct tllc_defrag_q_e); + dqe->ns = ns; + dqe->tl_sdu = msgb_alloc(4096, "LLC defrag"); + llist_add(&dqe->list, &llcs->rx.defrag_list); + } else + dqe = NULL; + + return dqe; +} + +static int tllc_defrag_in(struct tllc_state *llcs, + struct tetra_llc_pdu *lpp, + struct msgb *msg, unsigned int len) +{ + struct tllc_defrag_q_e *dqe; + + dqe = get_dqe_for_ns(llcs, lpp->ns, 1); + + /* check if this is the first segment, or the next + * expected segment */ + if (!dqe->last_ss || + (dqe->last_ss == lpp->ss - 1)) { + /* FIXME: append */ + printf("<> ", lpp->ss); + dqe->last_ss = lpp->ss; + memcpy(msgb_put(dqe->tl_sdu, len), msg->l3h, len); + } else + printf("<> ", dqe->last_ss, lpp->ss); + + return 0; +} + +static int tllc_defrag_out(struct tllc_state *llcs, + struct tetra_llc_pdu *lpp) +{ + struct tllc_defrag_q_e *dqe; + struct msgb *msg; + + dqe = get_dqe_for_ns(llcs, lpp->ns, 0); + msg = dqe->tl_sdu; + + printf("<> "); + msg->l3h = msg->data; + rx_tl_sdu(msg, msgb_l3len(msg)); + + if (tun_fd < 0) + tun_fd = tun_alloc("tun0"); + fprintf(stderr, "tun_fd=%d\n", tun_fd); + if (tun_fd >= 0) { + uint8_t buf[4096]; + int len = osmo_ubit2pbit(buf, msg->l3h+3+4+4+4+4, msgb_l3len(msg)-3-4-4-4-4); + write(tun_fd, buf, len); + } + + llist_del(&dqe->list); + talloc_free(msg); + talloc_free(dqe); +} + +/* Receive TM-SDU (MAC SDU == LLC PDU) */ +/* this resembles TMA-UNITDATA.ind (TM-SDU / length) */ +int rx_tm_sdu(struct msgb *msg, unsigned int len) +{ + struct tetra_llc_pdu lpp; + + memset(&lpp, 0, sizeof(lpp)); + tetra_llc_pdu_parse(&lpp, msg->l2h, len); + msg->l3h = lpp.tl_sdu; + + printf("TM-SDU(%s,%u,%u): ", + tetra_get_llc_pdut_dec_name(lpp.pdu_type), lpp.ns, lpp.ss); + + switch (lpp.pdu_type) { + case TLLC_PDUT_DEC_BL_ADATA: + case TLLC_PDUT_DEC_BL_DATA: + case TLLC_PDUT_DEC_BL_UDATA: + case TLLC_PDUT_DEC_BL_ACK: + case TLLC_PDUT_DEC_AL_SETUP: + case TLLC_PDUT_DEC_AL_ACK: + case TLLC_PDUT_DEC_AL_RNR: + case TLLC_PDUT_DEC_AL_RECONNECT: + case TLLC_PDUT_DEC_AL_DISC: + /* directly hand it to MLE */ + rx_tl_sdu(msg, lpp.tl_sdu_len); + break; + case TLLC_PDUT_DEC_AL_DATA: + case TLLC_PDUT_DEC_AL_UDATA: + case TLLC_PDUT_DEC_ALX_DATA: + case TLLC_PDUT_DEC_ALX_UDATA: + /* input into LLC defragmenter */ + tllc_defrag_in(&g_llcs, &lpp, msg, len); + break; + case TLLC_PDUT_DEC_AL_FINAL: + case TLLC_PDUT_DEC_AL_UFINAL: + case TLLC_PDUT_DEC_ALX_FINAL: + case TLLC_PDUT_DEC_ALX_UFINAL: + /* input into LLC defragmenter */ + tllc_defrag_in(&g_llcs, &lpp, msg, len); + /* check if the fragment is complete and hand it off*/ + tllc_defrag_out(&g_llcs, &lpp); + break; + } + + if (lpp.tl_sdu && lpp.ss == 0) { + /* this resembles TMA-UNITDATA.ind */ + //rx_tl_sdu(msg, lpp.tl_sdu_len); + } + return len; +} diff --git a/src/tetra_llc_pdu.h b/src/tetra_llc_pdu.h index c96bdcf..e2ce6fd 100644 --- a/src/tetra_llc_pdu.h +++ b/src/tetra_llc_pdu.h @@ -1,6 +1,8 @@ #ifndef TETRA_LLC_PDU_H #define TETRA_LLC_PDU_H +#include + /* Table 21.1 */ enum tetra_llc_pdu_t { TLLC_PDUT_BL_ADATA = 0, @@ -64,17 +66,36 @@ enum tllc_pdut_dec { }; const char *tetra_get_llc_pdut_dec_name(enum tllc_pdut_dec pdut); +/* decoded/parsed PDU with easier encoding */ struct tetra_llc_pdu { enum tllc_pdut_dec pdu_type; - uint8_t nr; - uint8_t ns; - uint8_t ss; + uint8_t nr; /* N(R) PDU number (receive) */ + uint8_t ns; /* N(S) PDU number (sent) */ + uint8_t ss; /* S(S) Segment (sent) */ uint32_t _fcs; uint32_t *fcs; uint8_t *tl_sdu; /* pointer to bitbuf */ uint8_t tl_sdu_len; /* in bits */ }; +/* parse a received LLC PDU and parse it into 'lpp' */ int tetra_llc_pdu_parse(struct tetra_llc_pdu *lpp, uint8_t *buf, int len); +/* TETRA LLC state */ +struct tllc_state { + struct llist_head list; + + struct { + struct llist_head defrag_list; + } rx; +}; + +/* entry in the defragmentation queue */ +struct tllc_defrag_q_e { + struct llist_head list; + unsigned int ns; /* current de-fragmenting */ + unsigned int last_ss; /* last received S(S) */ + + struct msgb *tl_sdu; +}; #endif /* TETRA_LLC_PDU_H */ diff --git a/src/tetra_upper_mac.c b/src/tetra_upper_mac.c index 19f4693..f0d8c52 100644 --- a/src/tetra_upper_mac.c +++ b/src/tetra_upper_mac.c @@ -38,6 +38,8 @@ #include "tetra_mle_pdu.h" #include "tetra_gsmtap.h" +static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int len); + /* FIXME: this is ugly */ static struct tetra_si_decoded g_last_sid; @@ -97,8 +99,10 @@ const char *tetra_alloc_dump(const struct tetra_chan_alloc_decoded *cad) return buf; } -static int rx_tl_sdu(struct tetra_mac_state *tms, uint8_t *bits, unsigned int len) +/* Receive TL-SDU (LLC SDU == MLE PDU) */ +static int rx_tl_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int len) { + uint8_t *bits = msg->l3h; uint8_t mle_pdisc = bits_to_uint(bits, 3); printf("TL-SDU(%s): %s", tetra_get_mle_pdisc_name(mle_pdisc), @@ -112,6 +116,15 @@ static int rx_tl_sdu(struct tetra_mac_state *tms, uint8_t *bits, unsigned int le break; case TMLE_PDISC_SNDCP: printf(" %s", tetra_get_sndcp_pdut_name(bits_to_uint(bits+3, 4), 0)); + printf(" NSAPI=%u PCOMP=%u, DCOMP=%u", + bits_to_uint(bits+3+4, 4), + bits_to_uint(bits+3+4+4, 4), + bits_to_uint(bits+3+4+4+4, 4)); + printf(" V%u, IHL=%u", + bits_to_uint(bits+3+4+4+4+4, 4), + 4*bits_to_uint(bits+3+4+4+4+4+4, 4)); + printf(" Proto=%u", + bits_to_uint(bits+3+4+4+4+4+4+4+64, 8)); break; case TMLE_PDISC_MLE: printf(" %s", tetra_get_mle_pdut_name(bits_to_uint(bits+3, 3), 0)); @@ -122,9 +135,10 @@ static int rx_tl_sdu(struct tetra_mac_state *tms, uint8_t *bits, unsigned int le return len; } -static int rx_tm_sdu(struct tetra_mac_state *tms, uint8_t *bits, unsigned int len) +static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int len) { struct tetra_llc_pdu lpp; + uint8_t *bits = msg->l2h; memset(&lpp, 0, sizeof(lpp)); tetra_llc_pdu_parse(&lpp, bits, len); @@ -132,7 +146,8 @@ static int rx_tm_sdu(struct tetra_mac_state *tms, uint8_t *bits, unsigned int le printf("TM-SDU(%s,%u,%u): ", tetra_get_llc_pdut_dec_name(lpp.pdu_type), lpp.ns, lpp.ss); if (lpp.tl_sdu && lpp.ss == 0) { - rx_tl_sdu(tms, lpp.tl_sdu, lpp.tl_sdu_len); + msg->l3h = lpp.tl_sdu; + rx_tl_sdu(tms, msg, lpp.tl_sdu_len); } return len; } @@ -165,7 +180,7 @@ static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms int len_bits = rsd.macpdu_length*8; if (msg->l2h + len_bits > msg->l1h + msgb_l1len(msg)) len_bits = msgb_l1len(msg) - tmpdu_offset; - rx_tm_sdu(tms, msg->l2h, len_bits); + rx_tm_sdu(tms, msg, len_bits); } out: @@ -195,7 +210,8 @@ static void rx_suppl(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms printf("SUPPLEMENTARY MAC-D-BLOCK "); //if (sud.encryption_mode == 0) - rx_tm_sdu(tms, msg->l1h + tmpdu_offset, 100); + msg->l2h = msg->l1h + tmpdu_offset; + rx_tm_sdu(tms, msg, 100); printf("\n"); } @@ -280,7 +296,8 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ case TETRA_PDU_T_MAC_FRAG_END: if (msg->l1h[3] == TETRA_MAC_FRAGE_FRAG) { printf("FRAG/END FRAG: "); - rx_tm_sdu(tms, msg->l1h+4, 100 /*FIXME*/); + msg->l2h = msg->l1h+4; + rx_tm_sdu(tms, msg, 100 /*FIXME*/); printf("\n"); } else printf("FRAG/END END\n");