mirror of https://gerrit.osmocom.org/osmo-tetra
Add LLC defragmentation + SDNCP output via tun device
this allows us to look at IP messages contained in SNDCP
This commit is contained in:
parent
13657cf8d2
commit
ee7a645ca4
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,157 @@
|
|||
/* TETRA LLC Layer */
|
||||
|
||||
/* (C) 2011 by Harald Welte <laforge@gnumonks.org>
|
||||
* 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 <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <osmocom/core/msgb.h>
|
||||
#include <osmocom/core/talloc.h>
|
||||
#include <osmocom/core/bits.h>
|
||||
|
||||
#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("<<APPEND:%u>> ", lpp->ss);
|
||||
dqe->last_ss = lpp->ss;
|
||||
memcpy(msgb_put(dqe->tl_sdu, len), msg->l3h, len);
|
||||
} else
|
||||
printf("<<MISS:%u-%u>> ", 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("<<REMOVE>> ");
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,6 +1,8 @@
|
|||
#ifndef TETRA_LLC_PDU_H
|
||||
#define TETRA_LLC_PDU_H
|
||||
|
||||
#include <osmocom/core/linuxlist.h>
|
||||
|
||||
/* 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 */
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
Loading…
Reference in New Issue