WIP: host/layer23: share RLC/MAC code from gprsdecode

Change-Id: I78a4a53726e09ce0d918148ba1d1802271605522

8 files changed, 579 insertions, 2 deletions

diff --git a/src/host/layer23/configure.ac b/src/host/layer23/configure.ac
index 3e696103..d42ca301 100644
--- a/src/host/layer23/configure.ac
+++ b/src/host/layer23/configure.ac
@@ -69,6 +69,7 @@ AC_OUTPUT(
     include/osmocom/Makefile
     include/osmocom/bb/Makefile
     include/osmocom/bb/common/Makefile
+    include/osmocom/bb/gprs/Makefile
     include/osmocom/bb/misc/Makefile
     include/osmocom/bb/mobile/Makefile
     Makefile)
diff --git a/src/host/layer23/include/osmocom/bb/Makefile.am b/src/host/layer23/include/osmocom/bb/Makefile.am
index 58a5f7fb..aaaa3ce6 100644
--- a/src/host/layer23/include/osmocom/bb/Makefile.am
+++ b/src/host/layer23/include/osmocom/bb/Makefile.am
@@ -1 +1 @@
-SUBDIRS = common misc mobile
+SUBDIRS = common gprs misc mobile
diff --git a/src/host/layer23/include/osmocom/bb/gprs/Makefile.am b/src/host/layer23/include/osmocom/bb/gprs/Makefile.am
new file mode 100644
index 00000000..8610b8c3
--- /dev/null
+++ b/src/host/layer23/include/osmocom/bb/gprs/Makefile.am
@@ -0,0 +1 @@
+noinst_HEADERS = rlcmac.h gsmtap.h
diff --git a/src/host/layer23/include/osmocom/bb/gprs/gsmtap.h b/src/host/layer23/include/osmocom/bb/gprs/gsmtap.h
new file mode 100644
index 00000000..fc7dc6a2
--- /dev/null
+++ b/src/host/layer23/include/osmocom/bb/gprs/gsmtap.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+int gsmtap_init(const char *addr);
+void gsmtap_send_rlcmac(uint8_t *msg, size_t len, uint8_t ts, bool ul);
+void gsmtap_send_llc(uint8_t *data, size_t len, bool ul);
diff --git a/src/host/layer23/include/osmocom/bb/gprs/rlcmac.h b/src/host/layer23/include/osmocom/bb/gprs/rlcmac.h
new file mode 100644
index 00000000..2381d771
--- /dev/null
+++ b/src/host/layer23/include/osmocom/bb/gprs/rlcmac.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define OLD_TIME 2000
+
+struct gprs_message {
+	uint16_t arfcn;
+	uint32_t fn;
+	uint8_t tn;
+	uint8_t rxl;
+	uint8_t snr;
+	uint8_t len;
+	uint8_t msg[0];
+};
+
+struct gprs_lime {
+	uint8_t li:6,
+		m:1,
+		e:1;
+	uint8_t used;
+} __attribute__ ((packed));
+
+struct gprs_frag {
+	uint32_t fn;
+	uint8_t last;
+	uint8_t len;
+	uint8_t data[53];
+	uint8_t n_blocks;
+	struct gprs_lime blocks[20];
+} __attribute__ ((packed));
+
+struct gprs_tbf {
+	uint8_t last_bsn;
+	uint8_t start_bsn;
+	struct gprs_frag frags[128];
+} __attribute__ ((packed));
+
+void print_pkt(uint8_t *msg, size_t len);
+void process_blocks(struct gprs_tbf *t, bool ul);
+void rlc_data_handler(struct gprs_message *gm);
+int rlc_type_handler(struct gprs_message *gm);
diff --git a/src/host/layer23/src/common/Makefile.am b/src/host/layer23/src/common/Makefile.am
index a8d9f7e7..45c962b3 100644
--- a/src/host/layer23/src/common/Makefile.am
+++ b/src/host/layer23/src/common/Makefile.am
@@ -3,4 +3,5 @@ AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBGPS_CFLAGS)
 
 noinst_LIBRARIES = liblayer23.a
 liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_fsm.c sap_proto.c sap_interface.c \
-	logging.c networks.c sim.c sysinfo.c gps.c l1ctl_lapdm_glue.c utils.c
+	logging.c networks.c sim.c sysinfo.c gps.c l1ctl_lapdm_glue.c utils.c \
+	gsmtap.c rlcmac.c
diff --git a/src/host/layer23/src/common/gsmtap.c b/src/host/layer23/src/common/gsmtap.c
new file mode 100644
index 00000000..9d7c9105
--- /dev/null
+++ b/src/host/layer23/src/common/gsmtap.c
@@ -0,0 +1,104 @@
+/*
+ * (C) 2017-2018 by sysmocom - s.f.m.c. GmbH, Author: Max <msuraev@sysmocom.de>
+ * (C) 2018 by Vadim Yanitskiy <axilirator@gmail.com>
+ * (C) 2011-2012 by Luca Melette <luca@srlabs.de>
+ *
+ * All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <stdbool.h>
+#include <arpa/inet.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/bb/gprs/gsmtap.h>
+
+static struct gsmtap_inst *gti = NULL;
+
+int gsmtap_init(const char *addr)
+{
+	gti = gsmtap_source_init(addr, GSMTAP_UDP_PORT, 0);
+	if (!gti)
+		return -EIO;
+
+	gsmtap_source_add_sink(gti);
+	return 0;
+}
+
+void gsmtap_send_rlcmac(uint8_t *msg, size_t len, uint8_t ts, bool ul)
+{
+	if (!gti)
+		return;
+
+	/* FIXME: explain params */
+	gsmtap_send(gti,
+		ul ? GSMTAP_ARFCN_F_UPLINK : 0,
+		ts, GSMTAP_CHANNEL_PACCH, 0, 0, 0, 0, msg, len);
+}
+
+void gsmtap_send_llc(uint8_t *data, size_t len, bool ul)
+{
+	struct gsmtap_hdr *gh;
+	struct msgb *msg;
+	uint8_t *dst;
+
+	if (!gti)
+		return;
+
+	/* Skip null frames */
+	if ((data[0] == 0x43) &&
+	    (data[1] == 0xc0) &&
+	    (data[2] == 0x01))
+		return;
+
+	/* Allocate a new message buffer */
+	msg = msgb_alloc(sizeof(*gh) + len, "gsmtap_tx");
+	if (!msg)
+	        return;
+
+	/* Put header in front */
+	gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh));
+
+	/* Fill in header */
+	gh->version = GSMTAP_VERSION;
+	gh->hdr_len = sizeof(*gh) / 4;
+	gh->type = GSMTAP_TYPE_GB_LLC;
+	gh->timeslot = 0;
+	gh->sub_slot = 0;
+	gh->arfcn = ul ? htons(GSMTAP_ARFCN_F_UPLINK) : 0;
+	gh->snr_db = 0;
+	gh->signal_dbm = 0;
+	gh->frame_number = 0;
+	gh->sub_type = 0;
+	gh->antenna_nr = 0;
+
+	/* Put and fill the payload */
+	dst = msgb_put(msg, len);
+	memcpy(dst, data, len);
+
+	/* Finally, send to the sink */
+	gsmtap_sendmsg(gti, msg);
+}
+
diff --git a/src/host/layer23/src/common/rlcmac.c b/src/host/layer23/src/common/rlcmac.c
new file mode 100644
index 00000000..fdb210e5
--- /dev/null
+++ b/src/host/layer23/src/common/rlcmac.c
@@ -0,0 +1,418 @@
+/*
+ * (C) 2017-2018 by sysmocom - s.f.m.c. GmbH, Author: Max <msuraev@sysmocom.de>
+ * (C) 2018 by Vadim Yanitskiy <axilirator@gmail.com>
+ * (C) 2011-2012 by Luca Melette <luca@srlabs.de>
+ *
+ * All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/time.h>
+
+#include <l1ctl_proto.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/bb/gprs/gsmtap.h>
+#include <osmocom/bb/gprs/rlcmac.h>
+
+static struct gprs_tbf tbf_table[32 * 2];
+
+static inline int too_old(uint32_t current_fn, uint32_t test_fn)
+{
+	uint32_t delta = (current_fn - test_fn) & 0xffffffff;
+
+	/* More and less 30 seconds from now */
+	return abs(delta) > OLD_TIME;
+}
+
+static inline int bsn_is_next(uint8_t first, uint8_t second)
+{
+	return ((first + 1) % 128) == second;
+}
+
+void print_pkt(uint8_t *msg, size_t len)
+{
+	size_t i;
+
+	printf("MSG: ");
+	for (i = 0; i < len; i++)
+		printf("%.02x", msg[i]);
+	printf("\n");
+}
+
+void process_blocks(struct gprs_tbf *t, bool ul)
+{
+	uint8_t llc_data[65536], llc_first_bsn, llc_last_bsn = 0;
+	unsigned skip, llc_len = 0;
+	uint8_t bsn, bsn2, li_off;
+	uint32_t current_fn;
+	struct gprs_frag *f;
+	struct gprs_lime *l;
+
+	/* Get current "time", oldest unreassembled frag */
+	bsn = t->start_bsn;
+	while (t->frags[bsn].len == 0) {
+		bsn = (bsn + 1) % 128;
+		if (bsn == t->start_bsn) {
+			printf("no valid  blocks in current TBF!\n");
+			fflush(stdout);
+			return;
+		}
+	}
+	current_fn = t->frags[bsn].fn;
+	t->start_bsn = bsn;
+
+	/* Walk through fragments, mark reassembled/used blocks */
+	skip = 0;
+	for (bsn = t->start_bsn; bsn != ((t->last_bsn + 1) % 128); bsn = (bsn + 1) % 128) {
+		/* Get fragment descriptor */
+		f = &t->frags[bsn];
+
+		printf(" bsn %d ", bsn);
+		fflush(stdout);
+
+		/* Already processed or null */
+		if (!f->len) {
+			printf("null\n");
+			fflush(stdout);
+			llc_len = 0;
+			skip = 1;
+			continue;
+		}
+
+		/* Check fragment age */
+		if (too_old(current_fn, f->fn)) {
+			printf("old segment\n");
+			fflush(stdout);
+			llc_len = 0;
+			skip = 1;
+			continue;
+		}
+
+		/* Update "time" */
+		current_fn = f->fn;
+
+		if (llc_len && !bsn_is_next(llc_last_bsn, bsn)) {
+			printf("missing bsn, previous %d\n", llc_last_bsn);
+			fflush(stdout);
+			llc_len = 0;
+			skip = 1;
+			continue;
+		}
+
+		/* Check for multiple blocks/parts */
+		if (f->n_blocks == 0) {
+			/* Check if first part of message */
+			if (!llc_len)
+				llc_first_bsn = bsn;
+
+			/* Append data to buffer */
+			memcpy(&llc_data[llc_len], f->data, f->len);
+
+			llc_len += f->len;
+
+			llc_last_bsn = bsn;
+
+			/* Last TBF block? (very rare condition) */
+			if (f->last) {
+				printf("end of TBF\n");
+				fflush(stdout);
+				print_pkt(llc_data, llc_len);
+
+				gsmtap_send_llc(llc_data, llc_len, ul);
+
+				/* Reset all fragments */
+				for (bsn2 = 0; bsn2 < 128; bsn2++) {
+					f = &t->frags[bsn2];
+					f->len = 0;
+					f->n_blocks = 0;
+				}
+
+				/* Reset buffer state */
+				llc_len = 0;
+				t->start_bsn = 0;
+			}
+		} else {
+			/* Multiple data parts */
+			unsigned i;
+			li_off = 0;
+			for (i = 0; i < f->n_blocks; i++) {
+				printf("\nlime %d\n", i);
+				fflush(stdout);
+				l = &f->blocks[i];
+				if (l->used) {
+					if (llc_len) {
+						printf("\nlime error!\n");
+						fflush(stdout);
+						llc_len = 0;
+					}
+				} else {
+					if (!llc_len)
+						llc_first_bsn = bsn;
+
+					/* Append data to buffer */
+					memcpy(&llc_data[llc_len], &f->data[li_off], l->li);
+
+					llc_len += l->li;
+
+					llc_last_bsn = bsn;
+
+					if (!l->e || !l->m || (l->e && l->m)) {
+						/* Message ends here */
+						printf("end of message reached\n");
+						fflush(stdout);
+						print_pkt(llc_data, llc_len);
+
+						gsmtap_send_llc(llc_data, llc_len, ul);
+
+						/* Mark frags as used */
+						l->used = 1;
+						if (llc_first_bsn != bsn) {
+						}
+
+
+						llc_len = 0;
+						if (!skip)
+							t->start_bsn = bsn;
+					}
+				}
+
+				li_off += l->li;
+			}
+
+			/* Is spare data valid? */
+			if (l->m) {
+				if (llc_len) {
+					printf("spare and buffer not empty!\n");
+					print_pkt(llc_data, llc_len);
+					fflush(stdout);
+				}
+				if ((f->len > li_off) && (f->len-li_off < 65536)) {
+					memcpy(llc_data, &f->data[li_off], f->len-li_off);
+					llc_len = f->len - li_off;
+					llc_first_bsn = bsn;
+					llc_last_bsn = bsn;
+					t->start_bsn = bsn;
+				}
+			}
+
+		}
+	}
+
+	/* Shift window if needed */
+	if (((t->last_bsn - t->start_bsn) % 128) > 64) {
+		t->start_bsn = (t->last_bsn - 64) % 128;
+		printf("shifting window\n");
+		fflush(stdout);
+	}
+}
+
+void rlc_data_handler(struct gprs_message *gm)
+{
+	int ul, off, d_bsn;
+	uint8_t tfi, bsn, cv = 1, fbi = 0;
+	uint32_t d_same_bsn, d_last_bsn;
+	struct gprs_tbf *t, *t_prev;
+	struct gprs_frag *f;
+	struct gprs_lime *l;
+
+	tfi = (gm->msg[1] & 0x3e) >> 1;
+	bsn = (gm->msg[2] & 0xfe) >> 1;
+
+	/* Get "end of TBF" according to direction */
+	ul = !!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK);
+	if (ul) {
+		cv = (gm->msg[0] & 0x3c) >> 2;
+		printf("TFI %d BSN %d CV %d ", tfi, bsn, cv);
+	} else {
+		fbi = (gm->msg[1] & 0x01);
+		printf("TFI %d BSN %d FBI %d ", tfi, bsn, fbi);
+	}
+
+	/* Get TBF descriptor for TFI,UL couple */
+	t = &tbf_table[2 * tfi + ul];
+
+	d_same_bsn = (gm->fn - t->frags[bsn].fn) & 0xffffffff;
+	d_last_bsn = (gm->fn - t->frags[t->last_bsn].fn) & 0xffffffff;
+	d_bsn = (bsn - t->last_bsn) % 128;
+
+	printf("\nfn_same_bsn %d fn_last_bsn %d delta_bsn %d old_len %d\n",
+		d_same_bsn, d_last_bsn, d_bsn, t->frags[bsn].len);
+
+	/* New / old fragment decision */
+	if (d_same_bsn > OLD_TIME) {
+		if (d_last_bsn > OLD_TIME) {
+			/* New TBF is starting, close old one... */
+			t_prev = &tbf_table[2 * ((tfi + 1) % 32) + ul];
+			printf("clearing TBF %d, first %d last %d\n",
+				(tfi + 1) % 32, t_prev->start_bsn, t_prev->last_bsn);
+			f = &t_prev->frags[t_prev->last_bsn];
+
+			/* ...only if data is present */
+			if (f->len) {
+				f->last = 1;
+				process_blocks(t_prev, ul);
+			}
+
+			printf("new TBF, starting from %d\n", bsn);
+			t->start_bsn = 0;
+			t->last_bsn = bsn;
+			memset(t->frags, 0, 128 * sizeof(struct gprs_frag));
+		} else {
+			/* Fresh frag, current TBF */
+			if ((d_bsn >= 0) || (d_bsn < -64)) {
+				/* New frag */
+				t->last_bsn = bsn;
+			} else {
+				/* Out of sequence / duplicate */
+				t->frags[bsn].fn = gm->fn;
+				printf("duplicate\n");
+				fflush(stdout);
+				return;
+			}
+		}
+	} else {
+		if (d_last_bsn > OLD_TIME) {
+			printf("fucking error last_bsn!\n");
+			fflush(stdout);
+			return;
+		} else {
+			/* Fresh frag, current TBF */
+			if (d_bsn > 0) {
+				printf("fucking error d_bsn!\n");
+				fflush(stdout);
+				return;
+			} else {
+				if (d_bsn < -64) {
+					/* New frag */
+					t->last_bsn = bsn;
+				} else {
+					/* Duplicate */
+					t->frags[bsn].fn = gm->fn;
+					printf("duplicate2\n");
+					fflush(stdout);
+					return;
+				}
+			}
+		}
+	}
+
+	/* Get fragment struct for current BSN */
+	f = &t->frags[bsn];
+
+	/* Scan for LI_M_E entries */
+	off = 2;
+	f->n_blocks = 0;
+	while (!(gm->msg[off++] & 0x01)) {
+		l = &f->blocks[f->n_blocks++];
+		l->li = (gm->msg[off] & 0xfc) >> 2;
+		l->m = (gm->msg[off] & 0x02) >> 1;
+		l->e = (gm->msg[off] & 0x01);
+		l->used = 0;
+	}
+
+	/* End of TBF? */
+	f->last = (!cv || fbi) ? 1 : 0;
+
+	/* Optional fields for uplink, indicated in TI and PI */
+	if (ul) {
+		if (gm->msg[1] & 0x01) {
+			printf("TLLI 0x%.02x%.02x%.02x%.02x ", gm->msg[off],
+				gm->msg[off+1], gm->msg[off + 2], gm->msg[off + 3]);
+			off += 4;
+		}
+		if (gm->msg[1] & 0x40) {
+			printf("PFI %d ", gm->msg[off]);
+			off += 1;
+		}
+	}
+
+	/* Copy data part of message */
+	f->len = gm->len - off;
+	f->fn = gm->fn;
+	memcpy(f->data, &gm->msg[off], f->len);
+
+	process_blocks(t, ul);
+}
+
+int rlc_type_handler(struct gprs_message *gm)
+{
+	bool ul = !!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK);
+	uint8_t rlc_type = (gm->msg[0] & 0xc0) >> 6;
+	int rc = 0;
+
+	/* Determine the RLC type */
+	switch (rlc_type) {
+	case 0:
+		printf("TS %d ", gm->tn);
+
+		switch(gm->len) {
+		case 23:
+			printf("CS1 ");
+			break;
+		case 33:
+			printf("CS2 ");
+			break;
+		case 39:
+			printf("CS3 ");
+			break;
+		case 53:
+			printf("CS4 ");
+			break;
+		default:
+			printf("unknown (M)CS ");
+		}
+
+		printf(ul ? "UL " : "DL ");
+
+		gsmtap_send_rlcmac(gm->msg, gm->len, gm->tn, ul);
+
+		printf("DATA ");
+		rlc_data_handler(gm);
+		printf("\n");
+		fflush(stdout);
+		break;
+
+	/* Control block */
+	case 1:
+	case 2:
+		gsmtap_send_rlcmac(gm->msg, gm->len, gm->tn,
+			!!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK));
+		rc = 0;
+		break;
+
+	/* Reserved */
+	case 3:
+		printf("RLC type: reserved\n");
+		rc = 0;
+		break;
+
+	default:
+		printf("Unrecognized RLC type: %d\n", rlc_type);
+		return -EINVAL;
+	}
+
+	return rc;
+}