summaryrefslogtreecommitdiffstats
path: root/src/host/gprsdecode/rlcmac.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/host/gprsdecode/rlcmac.c')
-rw-r--r--src/host/gprsdecode/rlcmac.c418
1 files changed, 418 insertions, 0 deletions
diff --git a/src/host/gprsdecode/rlcmac.c b/src/host/gprsdecode/rlcmac.c
new file mode 100644
index 00000000..d33cb1eb
--- /dev/null
+++ b/src/host/gprsdecode/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 <osmocom/core/gsmtap.h>
+
+#include "l1ctl_proto.h"
+#include "rlcmac.h"
+#include "gsmtap.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;
+}