Add LLC defragmentation + SDNCP output via tun device

this allows us to look at IP messages contained in SNDCP

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

src/tetra_llc.c

@@ -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;
+}

src/tetra_llc_pdu.h

@@ -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 */

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");