edge: Add unified decoder methods for GPRS/EGPRS

This commit adds new RLC block decoder functions that support both
GPRS and EGPRS. The code path is selected based on the value of the
GprsCodingScheme cs object.

- rlc_parse_ul_data_header
        parses the header of an RLC data block including the E and FBI/TI
        flags (currently supported CS-1 - CS-4, MCS-1 - MCS-4).

- rlc_copy_to_aligned_buffer
        copies an RLC data unit to a byte aligned buffer and returns
        the unit's length.

- rlc_get_data_aligned
        is a convenience wrapper around rlc_copy_to_aligned_buffer
        that avoids copying if the data unit is already byte aligned.

Sponsored-by: On-Waves ehf

4 files changed, 492 insertions, 0 deletions

diff --git a/src/decoding.cpp b/src/decoding.cpp
index d4f014b2..a26377bb 100644
--- a/src/decoding.cpp
+++ b/src/decoding.cpp
@@ -21,11 +21,267 @@
 #include <rlc.h>
 #include <gprs_debug.h>
 
+extern "C" {
+#include <osmocom/core/utils.h>
+}
+
 #include <arpa/inet.h>
 
 #include <errno.h>
 #include <string.h>
 
+#define LENGTH_TO_END 255
+/*
+ * \returns num extensions fields (num frames == offset) on success,
+ *          -errno otherwise.
+ */
+static int parse_extensions_egprs(const uint8_t *data, unsigned int data_len,
+	unsigned int *offs,
+	bool is_last_block,
+	Decoding::RlcData *chunks, unsigned int chunks_size)
+{
+	const struct rlc_li_field_egprs *li;
+	uint8_t e;
+	unsigned int num_chunks = 0;
+	// unsigned int data_area = 0;
+
+	e = 0;
+	while (!e) {
+		if (*offs > data_len) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
+				"but no more data\n");
+			return -EINVAL;
+		}
+
+		/* get new E */
+		li = (struct rlc_li_field_egprs *)&data[*offs];
+		e = li->e;
+		*offs += 1;
+
+		if (!chunks)
+			continue;
+
+		if (num_chunks == chunks_size) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
+				"but no more chunks possible\n");
+			return -ENOSPC;
+		}
+		if (li->li == 0 && num_chunks == 0 && li->e == 0) {
+			/* TS 44.060, table 10.4.14a.1, row 2a */
+			chunks[num_chunks].length = 0;
+			chunks[num_chunks].is_complete = true;
+		} else if (li->li == 0 && num_chunks == 0 && li->e == 1) {
+			/* TS 44.060, table 10.4.14a.1, row 4 */
+			// chunks[num_chunks].length = data_len - *offs - data_area;
+			chunks[num_chunks].length = LENGTH_TO_END;
+			chunks[num_chunks].is_complete = is_last_block;
+		} else if (li->li == 127 && li->e == 1) {
+			/* TS 44.060, table 10.4.14a.1, row 3 & 5 */
+			/* only filling bytes left */
+			break;
+		} else if (li->li > 0) {
+			/* TS 44.060, table 10.4.14a.1, row 1 & 2b */
+			chunks[num_chunks].length = li->li;
+			chunks[num_chunks].is_complete = true;
+		} else {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI contains "
+				"invalid extension octet: LI=%d, E=%d, count=%d\n",
+				li->li, li->e, num_chunks);
+			return -EINVAL;
+		}
+
+		num_chunks += 1;
+
+		if (e == 1) {
+			/* There is space after the last chunk, add a final one */
+			if (num_chunks == chunks_size) {
+				LOGP(DRLCMACUL, LOGL_NOTICE,
+					"UL DATA LI possibly extended, "
+					"but no more chunks possible\n");
+				return -ENOSPC;
+			}
+
+			// chunks[num_chunks].length = data_len - *offs - data_area;
+			chunks[num_chunks].length = LENGTH_TO_END;
+			chunks[num_chunks].is_complete = is_last_block;
+			// data_area += chunks[num_chunks].length;
+			num_chunks += 1;
+		}
+	}
+
+	return num_chunks;
+}
+
+static int parse_extensions_gprs(const uint8_t *data, unsigned int data_len,
+	unsigned int *offs,
+	bool is_last_block,
+	Decoding::RlcData *chunks, unsigned int chunks_size)
+{
+	const struct rlc_li_field *li;
+	uint8_t m, e;
+	unsigned int num_chunks = 0;
+	// unsigned int data_area = 0;
+
+	e = 0;
+	while (!e) {
+		if (*offs > data_len) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
+				"but no more data\n");
+			return -EINVAL;
+		}
+
+		/* get new E */
+		li = (const struct rlc_li_field *)&data[*offs];
+		e = li->e;
+		m = li->m;
+		*offs += 1;
+
+		if (li->li == 0) {
+			/* TS 44.060, 10.4.14, par 6 */
+			e = 1;
+			m = 0;
+		}
+
+		/* TS 44.060, table 10.4.13.1 */
+		if (m == 0 && e == 0) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA "
+				"ignored, because M='0' and E='0'.\n");
+			return 0;
+		}
+
+		if (!chunks)
+			continue;
+
+		if (num_chunks == chunks_size) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
+				"but no more chunks possible\n");
+			return -ENOSPC;
+		}
+
+		if (li->li == 0)
+			/* e is 1 here */
+			// chunks[num_chunks].length = data_len - *offs - data_area;
+			chunks[num_chunks].length = LENGTH_TO_END;
+		else
+			chunks[num_chunks].length = li->li;
+
+		chunks[num_chunks].is_complete = li->li || is_last_block;
+
+		// data_area += chunks[num_chunks].length;
+		num_chunks += 1;
+
+		if (e == 1 && m == 1) {
+			if (num_chunks == chunks_size) {
+				LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA LI extended, "
+					"but no more chunks possible\n");
+				return -ENOSPC;
+			}
+			/* TS 44.060, 10.4.13.1, row 4 */
+			// chunks[num_chunks].length = data_len - *offs - data_area;
+			chunks[num_chunks].length = LENGTH_TO_END;
+			chunks[num_chunks].is_complete = is_last_block;
+			num_chunks += 1;
+		}
+	}
+
+	return num_chunks;
+}
+
+int Decoding::rlc_data_from_ul_data(
+	const struct gprs_rlc_ul_data_block_info *rdbi, GprsCodingScheme cs,
+	const uint8_t *data, RlcData *chunks, unsigned int chunks_size,
+	uint32_t *tlli)
+{
+	uint8_t e;
+	unsigned int data_len = rdbi->data_len;
+	unsigned int num_chunks = 0, i;
+	unsigned int offs = 0;
+	bool is_last_block = (rdbi->cv == 0);
+
+	if (!chunks)
+		chunks_size = 0;
+
+	e = rdbi->e;
+	if (e) {
+		if (chunks_size > 0) {
+			chunks[num_chunks].offset = offs;
+			chunks[num_chunks].length = LENGTH_TO_END;
+			chunks[num_chunks].is_complete = is_last_block;
+			num_chunks += 1;
+		} else if (chunks) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "No extension, "
+				"but no more chunks possible\n");
+			return -ENOSPC;
+		}
+	} else if (cs.isEgprs()) {
+		/* if E is not set (LI follows), EGPRS */
+		num_chunks = parse_extensions_egprs(data, data_len, &offs,
+			is_last_block,
+			chunks, chunks_size);
+	} else {
+		/* if E is not set (LI follows), GPRS */
+		num_chunks = parse_extensions_gprs(data, data_len, &offs,
+			is_last_block,
+			chunks, chunks_size);
+	}
+
+	/* TLLI */
+	if (rdbi->ti) {
+		uint32_t tlli_enc;
+		if (offs + 4 > data_len) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA TLLI out of block "
+				"border\n");
+			return -EINVAL;
+		}
+
+		memcpy(&tlli_enc, data + offs, sizeof(tlli_enc));
+		if (cs.isGprs())
+			/* The TLLI is encoded in big endian for GPRS (see
+			 * TS 44.060, figure 10.2.2.1, note) */
+			*tlli = be32toh(tlli_enc);
+		else
+			/* The TLLI is encoded in little endian for EGPRS (see
+			 * TS 44.060, figure 10.3a.2.1, note 2) */
+			*tlli = le32toh(tlli_enc);
+
+		offs += sizeof(tlli_enc);
+	} else {
+		*tlli = 0;
+	}
+
+	/* PFI */
+	if (rdbi->pi) {
+		LOGP(DRLCMACUL, LOGL_ERROR, "ERROR: PFI not supported, "
+			"please disable in SYSTEM INFORMATION\n");
+		return -ENOTSUP;
+
+		/* TODO: Skip all extensions with E=0 (see TS 44.060, 10.4.11 */
+	}
+
+	/* LLC */
+	for (i = 0; i < num_chunks; i++) {
+		chunks[i].offset = offs;
+		if (chunks[i].length == LENGTH_TO_END) {
+			if (offs == data_len) {
+				/* There is no place for an additional chunk,
+				 * so drop it (this may happen with EGPRS since
+				 * there is no M flag. */
+				num_chunks -= 1;
+				break;;
+			}
+			chunks[i].length = data_len - offs;
+		}
+		offs += chunks[i].length;
+		if (offs > data_len) {
+			LOGP(DRLCMACUL, LOGL_NOTICE, "UL DATA out of block "
+				"border, chunk idx: %d, size: %d\n",
+				i, chunks[i].length);
+			return -EINVAL;
+		}
+	}
+
+	return num_chunks;
+}
 
 int Decoding::tlli_from_ul_data(const uint8_t *data, uint8_t len,
 					uint32_t *tlli)
@@ -114,3 +370,169 @@ void Decoding::extract_rbb(const uint8_t *rbb, char *show_rbb)
 
 	show_rbb[64] = '\0';
 }
+
+int Decoding::rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
+	const uint8_t *data, GprsCodingScheme cs)
+{
+	const struct gprs_rlc_ul_header_egprs_3 *egprs3;
+	const struct rlc_ul_header *gprs;
+	unsigned int e_ti_header;
+	unsigned int cur_bit = 0;
+	unsigned int data_len = 0;
+
+	rlc->cs = cs;
+
+	data_len = cs.maxDataBlockBytes();
+
+	switch(cs.headerTypeData()) {
+	case GprsCodingScheme::HEADER_GPRS_DATA:
+		gprs = static_cast<struct rlc_ul_header *>
+			((void *)data);
+		rlc->r      = gprs->r;
+		rlc->si     = gprs->si;
+		rlc->tfi    = gprs->tfi;
+		rlc->cps    = 0;
+		rlc->rsb    = 0;
+
+		rlc->num_data_blocks = 1;
+		rlc->block_info[0].cv  = gprs->cv;
+		rlc->block_info[0].pi  = gprs->pi;
+		rlc->block_info[0].bsn = gprs->bsn;
+		rlc->block_info[0].e   = gprs->e;
+		rlc->block_info[0].ti  = gprs->ti;
+		rlc->block_info[0].spb = 0;
+
+		cur_bit += 3 * 8;
+
+		rlc->data_offs_bits[0] = cur_bit;
+		rlc->block_info[0].data_len = data_len;
+		cur_bit += data_len * 8;
+
+		break;
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_3:
+		egprs3 = static_cast<struct gprs_rlc_ul_header_egprs_3 *>
+			((void *)data);
+		rlc->r      = egprs3->r;
+		rlc->si     = egprs3->si;
+		rlc->tfi    = (egprs3->tfi_a << 0)  | (egprs3->tfi_b << 2);
+		rlc->cps    = (egprs3->cps_a << 0)  | (egprs3->cps_b << 2);
+		rlc->rsb    = egprs3->rsb;
+
+		rlc->num_data_blocks = 1;
+		rlc->block_info[0].cv  = egprs3->cv;
+		rlc->block_info[0].pi  = egprs3->pi;
+		rlc->block_info[0].spb = egprs3->spb;
+		rlc->block_info[0].bsn =
+			(egprs3->bsn1_a << 0) | (egprs3->bsn1_b << 5);
+
+		cur_bit += 3 * 8 + 7;
+
+		e_ti_header = (data[3] + (data[4] << 8)) >> 7;
+		rlc->block_info[0].e   = !!(e_ti_header & 0x01);
+		rlc->block_info[0].ti  = !!(e_ti_header & 0x02);
+		cur_bit += 2;
+
+		rlc->data_offs_bits[0] = cur_bit;
+		rlc->block_info[0].data_len = data_len;
+		cur_bit += data_len * 8;
+
+		break;
+
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_1:
+	case GprsCodingScheme::HEADER_EGPRS_DATA_TYPE_2:
+		/* TODO: Support both header types */
+		/* fall through */
+	default:
+		LOGP(DRLCMACDL, LOGL_ERROR,
+			"Decoding of uplink %s data blocks not yet supported.\n",
+			cs.name());
+		return -ENOTSUP;
+	};
+
+	return cur_bit;
+}
+
+/**
+ * \brief Copy LSB bitstream RLC data block to byte aligned buffer.
+ *
+ * Note that the bitstream is encoded in LSB first order, so the two octets
+ * 654321xx xxxxxx87 contain the octet 87654321 starting at bit position 3
+ * (LSB has bit position 1). This is a different order than the one used by
+ * CSN.1.
+ *
+ * \param data_block_idx  The block index, 0..1 for header type 1, 0 otherwise
+ * \param src     A pointer to the start of the RLC block (incl. the header)
+ * \param buffer  A data area of a least the size of the RLC block
+ * \returns  the number of bytes copied
+ */
+unsigned int Decoding::rlc_copy_to_aligned_buffer(
+	const struct gprs_rlc_ul_header_egprs *rlc,
+	unsigned int data_block_idx,
+	const uint8_t *src, uint8_t *buffer)
+{
+	unsigned int hdr_bytes;
+	unsigned int extra_bits;
+	unsigned int i;
+
+	uint8_t c, last_c;
+	uint8_t *dst;
+	const struct gprs_rlc_ul_data_block_info *rdbi;
+
+	OSMO_ASSERT(data_block_idx < rlc->num_data_blocks);
+	rdbi = &rlc->block_info[data_block_idx];
+
+	hdr_bytes = rlc->data_offs_bits[data_block_idx] >> 3;
+	extra_bits = (rlc->data_offs_bits[data_block_idx] & 7);
+
+	if (extra_bits == 0) {
+		/* It is aligned already */
+		memmove(buffer, src + hdr_bytes, rdbi->data_len);
+		return rdbi->data_len;
+	}
+
+	dst = buffer;
+	src = src + hdr_bytes;
+	last_c = *(src++);
+
+	for (i = 0; i < rdbi->data_len; i++) {
+		c = src[i];
+		*(dst++) = (last_c >> extra_bits) | (c << (8 - extra_bits));
+		last_c = c;
+	}
+
+	return rdbi->data_len;
+}
+
+/**
+ * \brief Get a pointer to byte aligned RLC data.
+ *
+ * Since the RLC data may not be byte aligned to the RLC block data such that a
+ * single RLC data byte is spread over two RLC block bytes, this function
+ * eventually uses the provided buffer as data storage.
+ *
+ * \param src     A pointer to the start of the RLC block (incl. the header)
+ * \param buffer  A data area of a least the size of the RLC block
+ * \returns A pointer to the RLC data start within src if it is aligned, and
+ *          buffer otherwise.
+ */
+const uint8_t *Decoding::rlc_get_data_aligned(
+	const struct gprs_rlc_ul_header_egprs *rlc,
+	unsigned int data_block_idx,
+	const uint8_t *src, uint8_t *buffer)
+{
+	unsigned int hdr_bytes;
+	unsigned int extra_bits;
+
+	OSMO_ASSERT(data_block_idx < ARRAY_SIZE(rlc->data_offs_bits));
+
+	hdr_bytes = rlc->data_offs_bits[data_block_idx] >> 3;
+	extra_bits = (rlc->data_offs_bits[data_block_idx] & 7);
+
+	if (extra_bits == 0)
+		/* It is aligned already, return a pointer that refers to the
+		 * original data. */
+		return src + hdr_bytes;
+
+	Decoding::rlc_copy_to_aligned_buffer(rlc, data_block_idx, src, buffer);
+	return buffer;
+}
diff --git a/src/decoding.h b/src/decoding.h
index 03dad47e..1cda7b42 100644
--- a/src/decoding.h
+++ b/src/decoding.h
@@ -20,15 +20,37 @@
 #pragma once
 
 #include <gsm_rlcmac.h>
+#include "rlc.h"
 
 #include <stdint.h>
 
 class Decoding {
 public:
+	struct RlcData {
+		uint8_t	offset;
+		uint8_t	length;
+		bool	is_complete;
+	};
+
 	static int tlli_from_ul_data(const uint8_t *data, uint8_t len,
 					uint32_t *tlli);
+	static int rlc_data_from_ul_data(
+		const struct gprs_rlc_ul_data_block_info *rdbi,
+		GprsCodingScheme cs, const uint8_t *data, RlcData *chunks,
+		unsigned int chunks_size, uint32_t *tlli);
 	static uint8_t get_ms_class_by_capability(MS_Radio_Access_capability_t *cap);
 	static uint8_t get_egprs_ms_class_by_capability(MS_Radio_Access_capability_t *cap);
 
 	static void extract_rbb(const uint8_t *rbb, char *extracted_rbb);
+
+	static int rlc_parse_ul_data_header(struct gprs_rlc_ul_header_egprs *rlc,
+		const uint8_t *data, GprsCodingScheme cs);
+	static unsigned int rlc_copy_to_aligned_buffer(
+		const struct gprs_rlc_ul_header_egprs *rlc,
+		unsigned int data_block_idx,
+		const uint8_t *src, uint8_t *buffer);
+	static const uint8_t *rlc_get_data_aligned(
+		const struct gprs_rlc_ul_header_egprs *rlc,
+		unsigned int data_block_idx,
+		const uint8_t *src, uint8_t *buffer);
 };
diff --git a/src/rlc.cpp b/src/rlc.cpp
index 42b602cb..9f5c61d9 100644
--- a/src/rlc.cpp
+++ b/src/rlc.cpp
@@ -20,6 +20,8 @@
 #include "bts.h"
 #include "gprs_debug.h"
 
+#include <errno.h>
+
 extern "C" {
 #include <osmocom/core/utils.h>
 }
diff --git a/src/rlc.h b/src/rlc.h
index 89af2197..bafe6a88 100644
--- a/src/rlc.h
+++ b/src/rlc.h
@@ -19,6 +19,8 @@
  */
 #pragma once
 
+#include "gprs_coding_scheme.h"
+
 #include <osmocom/core/endian.h>
 
 #include <stdint.h>
@@ -53,6 +55,28 @@ static inline uint16_t mod_sns_half()
 	return (RLC_MAX_SNS / 2) - 1;
 }
 
+struct gprs_rlc_ul_data_block_info {
+	unsigned int data_len; /* EGPRS: N2, GPRS: N2-2, N-2 */
+	unsigned int bsn;
+	unsigned int ti;
+	unsigned int e;
+	unsigned int cv;
+	unsigned int pi;
+	unsigned int spb;
+};
+
+struct gprs_rlc_ul_header_egprs {
+	GprsCodingScheme cs;
+	unsigned int r;
+	unsigned int si;
+	unsigned int tfi;
+	unsigned int cps;
+	unsigned int rsb;
+	unsigned int num_data_blocks;
+	unsigned int data_offs_bits[2];
+	struct gprs_rlc_ul_data_block_info block_info[2];
+};
+
 struct gprs_rlc_data {
 	uint8_t *prepare(size_t block_data_length);
 	void put_data(const uint8_t *data, size_t len);
@@ -219,6 +243,28 @@ struct rlc_li_field {
 		 m:1,
 		 li:6;
 } __attribute__ ((packed));
+
+struct rlc_li_field_egprs {
+	uint8_t	e:1,
+		 li:7;
+} __attribute__ ((packed));
+
+struct gprs_rlc_ul_header_egprs_3 {
+	uint8_t r:1,
+		si:1,
+		cv:4,
+		tfi_a:2;
+	uint8_t tfi_b:3,
+		bsn1_a:5;
+	uint8_t bsn1_b:6,
+		cps_a:2;
+	uint8_t cps_b:2,
+		spb:2,
+		rsb:1,
+		pi:1,
+		spare:1,
+		dummy:1;
+} __attribute__ ((packed));
 #else
 #  error "Only little endian headers are supported yet. TODO: add missing structs"
 #endif