From 0c937878baad2c7e64844bbc92f2542eb4992a7d Mon Sep 17 00:00:00 2001 From: Vadim Yanitskiy Date: Sat, 12 May 2018 19:13:02 +0700 Subject: WIP: host/layer23: share RLC/MAC code from gprsdecode Change-Id: I78a4a53726e09ce0d918148ba1d1802271605522 --- src/host/layer23/configure.ac | 1 + src/host/layer23/include/osmocom/bb/Makefile.am | 2 +- .../layer23/include/osmocom/bb/gprs/Makefile.am | 1 + src/host/layer23/include/osmocom/bb/gprs/gsmtap.h | 9 + src/host/layer23/include/osmocom/bb/gprs/rlcmac.h | 43 +++ src/host/layer23/src/common/Makefile.am | 3 +- src/host/layer23/src/common/gsmtap.c | 104 +++++ src/host/layer23/src/common/rlcmac.c | 418 +++++++++++++++++++++ 8 files changed, 579 insertions(+), 2 deletions(-) create mode 100644 src/host/layer23/include/osmocom/bb/gprs/Makefile.am create mode 100644 src/host/layer23/include/osmocom/bb/gprs/gsmtap.h create mode 100644 src/host/layer23/include/osmocom/bb/gprs/rlcmac.h create mode 100644 src/host/layer23/src/common/gsmtap.c create mode 100644 src/host/layer23/src/common/rlcmac.c 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 +#include +#include + +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 +#include + +#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 + * (C) 2018 by Vadim Yanitskiy + * (C) 2011-2012 by Luca Melette + * + * 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 +#include +#include +#include +#include + +#include +#include +#include + +#include + +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 + * (C) 2018 by Vadim Yanitskiy + * (C) 2011-2012 by Luca Melette + * + * 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 +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +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; +} -- cgit v1.2.3