laforge submitted this change.
added support for fill bits and basic link defragmentation (also thanks to SQ5BPF)
The upper mac now maintains a defragmentation buffer for each timeslot. Resources with length -1 (fragmentation start) are added to the buf for that slot, further mac/frag frames are appended. When a mac/end is encountered, the reconstructed l2 message is passed to rx_tm_sdu. The tetra_llc_pdu struct now uses a uint32_t for tl_sdu_len in order to account for the possibly longer sdus. Fill bits processing was required in order to reliably determine the end of a MAC PDU.
Change-Id: I41c9438b0b12c2fac9dff1b226eec5b33f30fbb4
---
M src/tetra_llc_pdu.h
M src/tetra_mac_pdu.c
M src/tetra_mac_pdu.h
M src/tetra_upper_mac.c
M src/tetra_upper_mac.h
5 files changed, 275 insertions(+), 38 deletions(-)
diff --git a/src/tetra_llc_pdu.h b/src/tetra_llc_pdu.h
index 4d260c7..9b33925 100644
--- a/src/tetra_llc_pdu.h
+++ b/src/tetra_llc_pdu.h
@@ -73,12 +73,12 @@
uint8_t ns; /* N(S) PDU number (sent) */
uint8_t ss; /* S(S) Segment (sent) */
- uint8_t have_fcs; /* 1 if LLC PDU defines FCS is present */
- uint32_t fcs; /* FCS value extracted from pdu */
- uint8_t fcs_invalid; /* 1 if extracted FCS does not match computed FCS */
+ bool have_fcs; /* 1 if LLC PDU defines FCS is present */
+ uint32_t fcs; /* FCS value extracted from pdu */
+ bool fcs_invalid; /* 1 if extracted FCS does not match computed FCS */
uint8_t *tl_sdu; /* pointer to bitbuf */
- uint8_t tl_sdu_len; /* in bits */
+ uint32_t tl_sdu_len; /* in bits */
};
/* parse a received LLC PDU and parse it into 'lpp' */
diff --git a/src/tetra_mac_pdu.c b/src/tetra_mac_pdu.c
index d114703..c71e458 100644
--- a/src/tetra_mac_pdu.c
+++ b/src/tetra_mac_pdu.c
@@ -91,7 +91,7 @@
};
/* 21.5.2 */
-static int macpdu_decode_chan_alloc(struct tetra_chan_alloc_decoded *cad, const uint8_t *bits)
+int macpdu_decode_chan_alloc(struct tetra_chan_alloc_decoded *cad, const uint8_t *bits)
{
const uint8_t *cur = bits;
@@ -160,9 +160,6 @@
return dec_tbl[in & 0xf];
}
-#define MACPDU_LEN_2ND_STOLEN -1
-#define MACPDU_LEN_START_FRAG -2
-
static int decode_length(unsigned int length_ind)
{
/* FIXME: Y2/Z2 for non-pi4 DQPSK */
diff --git a/src/tetra_mac_pdu.h b/src/tetra_mac_pdu.h
index d243062..4b1baeb 100644
--- a/src/tetra_mac_pdu.h
+++ b/src/tetra_mac_pdu.h
@@ -1,6 +1,9 @@
#ifndef TETRA_MAC_PDU
#define TETRA_MAC_PDU
+#define MACPDU_LEN_2ND_STOLEN -2
+#define MACPDU_LEN_START_FRAG -1
+
enum tetra_mac_pdu_types {
TETRA_PDU_T_MAC_RESOURCE = 0,
TETRA_PDU_T_MAC_FRAG_END = 1,
@@ -157,7 +160,7 @@
enum tetra_mac_res_addr_type {
ADDR_TYPE_NULL = 0,
ADDR_TYPE_SSI = 1,
- ADDR_TYPE_EVENT_LABEL = 2,
+ ADDR_TYPE_EVENT_LABEL = 2,
ADDR_TYPE_USSI = 3,
ADDR_TYPE_SMI = 4,
ADDR_TYPE_SSI_EVENT = 5,
@@ -234,6 +237,8 @@
};
int macpdu_decode_resource(struct tetra_resrc_decoded *rsd, const uint8_t *bits, uint8_t is_decrypted);
+int macpdu_decode_chan_alloc(struct tetra_chan_alloc_decoded *cad, const uint8_t *bits);
+
const char *tetra_addr_dump(const struct tetra_addr *addr);
const char *tetra_get_ul_dl_name(uint8_t ul_dl);
diff --git a/src/tetra_upper_mac.c b/src/tetra_upper_mac.c
index f6b34bf..033e13a 100644
--- a/src/tetra_upper_mac.c
+++ b/src/tetra_upper_mac.c
@@ -40,7 +40,53 @@
static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int len);
-static void rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
+/* FIXME move global fragslots to context variable */
+struct fragslot fragslots[FRAGSLOT_NR_SLOTS] = {0};
+
+void init_fragslot(struct fragslot *fragslot)
+{
+ if (fragslot->msgb) {
+ /* Should never be the case, but just to be sure */
+ talloc_free(fragslot->msgb);
+ memset(fragslot, 0, sizeof(struct fragslot));
+ }
+ fragslot->msgb = msgb_alloc(FRAGSLOT_MSGB_SIZE, "fragslot");
+}
+
+void cleanup_fragslot(struct fragslot *fragslot)
+{
+ if (fragslot->msgb) {
+ talloc_free(fragslot->msgb);
+ }
+ memset(fragslot, 0, sizeof(struct fragslot));
+}
+
+void age_fragslots()
+{
+ int i;
+ for (i = 0; i < FRAGSLOT_NR_SLOTS; i++) {
+ if (fragslots[i].active) {
+ fragslots[i].age++;
+ if (fragslots[i].age > N203) {
+ printf("\nFRAG: aged out old fragments for slot=%d fragments=%d length=%d timer=%d\n", i, fragslots[i].num_frags, fragslots[i].length, fragslots[i].age);
+ cleanup_fragslot(&fragslots[i]);
+ }
+ }
+ }
+}
+
+static int get_num_fill_bits(const unsigned char *l1h, int len_with_fillbits)
+{
+ for (int i = 1; i < len_with_fillbits; i++) {
+ if (l1h[len_with_fillbits - i] == 1) {
+ return i;
+ }
+ }
+ printf("WARNING get_fill_bits_len: no 1 bit within %d bytes from end\n", len_with_fillbits);
+ return 0;
+}
+
+static int rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
{
struct msgb *msg = tmvp->oph.msg;
struct tetra_si_decoded sid;
@@ -73,6 +119,7 @@
sid.mle_si.bs_service_details & (1 << i) ? 1 : 0);
memcpy(&tms->last_sid, &sid, sizeof(sid));
+ return -1; /* FIXME check this indeed fills slot */
}
const char *tetra_alloc_dump(const struct tetra_chan_alloc_decoded *cad, struct tetra_mac_state *tms)
@@ -141,8 +188,13 @@
memset(&lpp, 0, sizeof(lpp));
tetra_llc_pdu_parse(&lpp, bits, len);
- printf("TM-SDU(%s,%u,%u): ",
+ printf("TM-SDU(%s,%u,%u)",
tetra_get_llc_pdut_dec_name(lpp.pdu_type), lpp.ns, lpp.ss);
+ if (lpp.have_fcs) {
+ printf("(FCS: %s)", lpp.fcs_invalid ? "BAD" : "OK");
+ }
+ printf(": ");
+
if (lpp.tl_sdu && lpp.ss == 0) {
msg->l3h = lpp.tl_sdu;
rx_tl_sdu(tms, msg, lpp.tl_sdu_len);
@@ -150,22 +202,43 @@
return len;
}
-static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
+static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
{
struct msgb *msg = tmvp->oph.msg;
struct tetra_resrc_decoded rsd;
- int tmpdu_offset;
+ struct msgb *fragmsgb;
+ int tmpdu_offset, slot;
+ int pdu_bits; /* Full length of pdu, including fill bits */
memset(&rsd, 0, sizeof(rsd));
tmpdu_offset = macpdu_decode_resource(&rsd, msg->l1h, 0);
- msg->l2h = msg->l1h + tmpdu_offset;
- printf("RESOURCE Encr=%u, Length=%d Addr=%s ",
- rsd.encryption_mode, rsd.macpdu_length,
+ if (rsd.macpdu_length == MACPDU_LEN_2ND_STOLEN) {
+ pdu_bits = -1; /* Fills slot */
+ printf("WARNING pdu with MACPDU_LEN_2ND_STOLEN, not implemented\n");
+ } else if (rsd.macpdu_length == MACPDU_LEN_START_FRAG) {
+ pdu_bits = -1; /* Fills slot */
+ } else {
+ pdu_bits = rsd.macpdu_length * 8; /* Length given */
+ msg->tail = msg->head + pdu_bits;
+ }
+
+ /* Strip fill bits */
+ if (rsd.fill_bits) {
+ int num_fill_bits = get_num_fill_bits(msg->l1h, msgb_l1len(msg));
+ msg->tail -= num_fill_bits;
+ }
+
+ /* We now have accurate length and start of TM-SDU, set LLC start in msg->l2h */
+ msg->l2h = msg->l1h + tmpdu_offset;
+ printf("RESOURCE Encr=%u, len=%d l1_len=%d l2_len %d Addr=%s ",
+ rsd.encryption_mode, rsd.macpdu_length, msgb_l1len(msg), msgb_l2len(msg),
tetra_addr_dump(&rsd.addr));
- if (rsd.addr.type == ADDR_TYPE_NULL)
+ if (rsd.addr.type == ADDR_TYPE_NULL) {
+ pdu_bits = -1; /* No more PDUs in slot */
goto out;
+ }
if (rsd.chan_alloc_pres)
printf("ChanAlloc=%s ", tetra_alloc_dump(&rsd.cad, tms));
@@ -174,20 +247,148 @@
printf("SlotGrant=%u/%u ", rsd.slot_granting.nr_slots,
rsd.slot_granting.delay);
- if (rsd.macpdu_length > 0 && rsd.encryption_mode == 0) {
- 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, len_bits);
+ if (rsd.macpdu_length != MACPDU_LEN_START_FRAG || !REASSEMBLE_FRAGMENTS) {
+ /* Non-fragmented resource (or no reassembly desired) */
+ if (!rsd.is_encrypted) {
+ rx_tm_sdu(tms, msg, msgb_l2len(msg));
+ }
+ } else {
+ /* Fragmented resource */
+ slot = tmvp->u.unitdata.tdma_time.tn;
+ if (fragslots[slot].active) {
+ printf("\nWARNING: fragment slot still active\n");
+ cleanup_fragslot(&fragslots[slot]);
+ }
+
+ init_fragslot(&fragslots[slot]);
+ fragmsgb = fragslots[slot].msgb;
+
+ /* Copy l2 part to fragmsgb. l3h is constructed once all fragments are merged */
+ fragmsgb->l1h = msgb_put(fragmsgb, msgb_l1len(msg));
+ fragmsgb->l2h = fragmsgb->l1h + tmpdu_offset;
+ fragmsgb->l3h = 0;
+ memcpy(fragmsgb->l2h, msg->l2h, msgb_l2len(msg));
+
+ printf("\nFRAG-START slot=%d len=%d msgb=%s\n", slot, msgb_l2len(fragmsgb), osmo_ubit_dump(fragmsgb->l2h, msgb_l2len(msg)));
+ fragslots[slot].active = 1;
+ fragslots[slot].num_frags = 1;
+ fragslots[slot].length = msgb_l2len(msg);
+ fragslots[slot].encryption = rsd.encryption_mode > 0;
}
tms->ssi = rsd.addr.ssi;
out:
printf("\n");
+ return pdu_bits;
}
-static void rx_suppl(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
+void append_frag_bits(int slot, uint8_t *bits, int bitlen)
+{
+ struct msgb *fragmsgb;
+ fragmsgb = fragslots[slot].msgb;
+ if (fragmsgb->len + bitlen > fragmsgb->data_len) {
+ printf(" WARNING: FRAG LENGTH ERROR!\n");
+ return;
+ }
+ unsigned char *append_ptr = msgb_put(fragmsgb, bitlen);
+ memcpy(append_ptr, bits, bitlen);
+
+ fragslots[slot].length = fragslots[slot].length + bitlen;
+ fragslots[slot].num_frags++;
+ fragslots[slot].age = 0;
+}
+
+static int rx_macfrag(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
+{
+ struct msgb *msg = tmvp->oph.msg;
+ int slot = tmvp->u.unitdata.tdma_time.tn;
+ uint8_t *bits = msg->l1h;
+ uint8_t fillbits_present;
+ int n = 0;
+ int m = 0;
+
+ if (fragslots[slot].active) {
+ m = 2; n = n + m; /* MAC-FRAG/END (01) */
+ m = 1; n = n + m; /* MAC-FRAG (0) */
+ m = 1; fillbits_present = bits_to_uint(bits + n, m); n = n + m;
+ msg->l2h = msg->l1h + n;
+
+ /* MAC-FRAG will always fill remainder of the slot, but fill bits may be present */
+ if (fillbits_present) {
+ int num_fill_bits = get_num_fill_bits(msg->l1h, msgb_l1len(msg));
+ msgb_get(msg, num_fill_bits);
+ }
+
+ /* Add frag to fragslot buffer */
+ struct msgb *fragmsgb = fragslots[slot].msgb;
+ append_frag_bits(slot, msg->l2h, msgb_l2len(msg));
+ printf("FRAG-CONT slot=%d added=%d msgb=%s\n", slot, msgb_l2len(msg), osmo_ubit_dump(fragmsgb->l2h, msgb_l2len(fragmsgb)));
+ } else {
+ printf("WARNING got fragment without start packet for slot=%d\n", slot);
+ }
+ return -1; /* Always fills slot */
+}
+
+static int rx_macend(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
+{
+ struct msgb *msg = tmvp->oph.msg;
+ int slot = tmvp->u.unitdata.tdma_time.tn;
+ struct tetra_resrc_decoded rsd;
+ uint8_t *bits = msg->l1h;
+ uint8_t fillbits_present, chanalloc_present, length_indicator, slot_granting;
+ int num_fill_bits;
+ struct msgb *fragmsgb;
+ int n = 0;
+ int m = 0;
+
+ memset(&rsd, 0, sizeof(rsd));
+ m = 2; n = n + m; /* MAC-FRAG/END (01) */
+ m = 1; n = n + m; /* MAC-END (1) */
+ m = 1; fillbits_present = bits_to_uint(bits + n, m); n = n + m;
+ m = 1; n = n + m; /* position_of_grant */
+ m = 6; length_indicator = bits_to_uint(bits + n, m); n = n + m;
+
+ fragmsgb = fragslots[slot].msgb;
+ if (fragslots[slot].active) {
+
+ /* FIXME: handle napping bit in d8psk and qam */
+ m = 1; slot_granting = bits_to_uint(bits + n, m); n = n + m;
+ if (slot_granting) {
+ /* FIXME: multiple slot granting in qam */
+ m = 8; /* basic slot granting */ n = n + m;
+ }
+ m = 1; chanalloc_present = bits_to_uint(bits + n, m); n = n + m;
+
+ /* Determine msg len, strip fill bits if any */
+ msg->tail = msg->head + length_indicator * 8;
+ if (fillbits_present) {
+ num_fill_bits = get_num_fill_bits(msg->l1h, msgb_l1len(msg));
+ msg->tail -= num_fill_bits;
+ }
+
+ /* Parse chanalloc element (if present) and update l2 offsets */
+ if (chanalloc_present) {
+ m = macpdu_decode_chan_alloc(&rsd.cad, bits + n); n = n + m;
+ }
+ msg->l2h = msg->l1h + n;
+ append_frag_bits(slot, msg->l2h, msgb_l2len(msg));
+ printf("FRAG-END slot=%d added=%d msgb=%s\n", slot, msgb_l2len(msg), osmo_ubit_dump(fragmsgb->l2h, msgb_l2len(fragmsgb)));
+
+ /* Message is completed inside fragmsgb now */
+ if (!fragslots[slot].encryption) {
+ rx_tm_sdu(tms, fragmsgb, fragslots[slot].length);
+ }
+ } else {
+ printf("FRAG: got end frag with len %d without start packet for slot=%d\n", length_indicator * 8, slot);
+ }
+
+ cleanup_fragslot(&fragslots[slot]);
+ return length_indicator * 8;
+}
+
+
+static int rx_suppl(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
{
//struct tmv_unitdata_param *tup = &tmvp->u.unitdata;
struct msgb *msg = tmvp->oph.msg;
@@ -214,6 +415,7 @@
rx_tm_sdu(tms, msg, 100);
printf("\n");
+ return -1; /* TODO FIXME check length */
}
static void dump_access(struct tetra_access_field *acc, unsigned int num)
@@ -257,6 +459,7 @@
uint8_t pdu_type = bits_to_uint(msg->l1h, 2);
const char *pdu_name;
struct msgb *gsmtap_msg;
+ int len_parsed;
if (tup->lchan == TETRA_LC_BSCH)
pdu_name = "SYNC";
@@ -273,7 +476,7 @@
tup->crc_ok, pdu_name);
if (!tup->crc_ok)
- return 0;
+ return -1;
gsmtap_msg = tetra_gsmtap_makemsg(&tup->tdma_time, tup->lchan,
tup->tdma_time.tn-1, /* expects timeslot in 0-3 range */
@@ -282,6 +485,12 @@
if (gsmtap_msg)
tetra_gsmtap_sendmsg(gsmtap_msg);
+ /* age out old fragments */
+ if (REASSEMBLE_FRAGMENTS && tup->tdma_time.fn == 18) {
+ age_fragslots();
+ }
+
+ len_parsed = -1; /* Default for cases where slot is filled or otherwise irrelevant */
switch (tup->lchan) {
case TETRA_LC_AACH:
rx_aach(tmvp, tms);
@@ -291,24 +500,35 @@
case TETRA_LC_SCH_F:
switch (pdu_type) {
case TETRA_PDU_T_BROADCAST:
- rx_bcast(tmvp, tms);
+ len_parsed = rx_bcast(tmvp, tms);
break;
case TETRA_PDU_T_MAC_RESOURCE:
- rx_resrc(tmvp, tms);
+ len_parsed = rx_resrc(tmvp, tms);
break;
case TETRA_PDU_T_MAC_SUPPL:
- rx_suppl(tmvp, tms);
+ len_parsed = rx_suppl(tmvp, tms);
break;
case TETRA_PDU_T_MAC_FRAG_END:
- if (msg->l1h[3] == TETRA_MAC_FRAGE_FRAG) {
- printf("FRAG/END FRAG: ");
- msg->l2h = msg->l1h+4;
- rx_tm_sdu(tms, msg, 100 /*FIXME*/);
- printf("\n");
- } else
- printf("FRAG/END END\n");
+ if (REASSEMBLE_FRAGMENTS) {
+ if (msg->l1h[2] == TETRA_MAC_FRAGE_FRAG) {
+ len_parsed = rx_macfrag(tmvp, tms);
+ } else {
+ len_parsed = rx_macend(tmvp, tms);
+ }
+ } else {
+ len_parsed = -1; /* Can't determine len */
+ if (msg->l1h[3] == TETRA_MAC_FRAGE_FRAG) {
+ printf("FRAG/END FRAG: ");
+ msg->l2h = msg->l1h+4;
+ rx_tm_sdu(tms, msg, 100 /*FIXME*/);
+ printf("\n");
+ } else {
+ printf("FRAG/END END\n");
+ }
+ }
break;
default:
+ len_parsed = -1; /* Can't determine len */
printf("STRANGE pdu=%u\n", pdu_type);
break;
}
@@ -320,19 +540,19 @@
break;
}
- return 0;
+ return len_parsed;
}
int upper_mac_prim_recv(struct osmo_prim_hdr *op, void *priv)
{
struct tetra_tmvsap_prim *tmvp;
struct tetra_mac_state *tms = priv;
- int rc;
+ int pdu_bits = -1;
switch (op->sap) {
case TETRA_SAP_TMV:
tmvp = (struct tetra_tmvsap_prim *) op;
- rc = rx_tmv_unitdata_ind(tmvp, tms);
+ pdu_bits = rx_tmv_unitdata_ind(tmvp, tms);
break;
default:
printf("primitive on unknown sap\n");
@@ -342,5 +562,5 @@
talloc_free(op->msg);
talloc_free(op);
- return rc;
+ return pdu_bits;
}
diff --git a/src/tetra_upper_mac.h b/src/tetra_upper_mac.h
index 9e77b57..d7a420d 100644
--- a/src/tetra_upper_mac.h
+++ b/src/tetra_upper_mac.h
@@ -3,6 +3,21 @@
#include "tetra_prim.h"
+#define REASSEMBLE_FRAGMENTS 1 /* Set to 0 to disable reassembly functionality */
+#define FRAGSLOT_NR_SLOTS 5 /* Slot 0 is unused */
+
+#define N203 6 /* Fragslot max age, see N.203 in the tetra docs, must be 4 multiframes or greater */
+#define FRAGSLOT_MSGB_SIZE 8192
+struct fragslot {
+ bool active; /* Set to 1 when fragslot holds a partially constructed message */
+ uint32_t age; /* Maintains the number of multiframes since the last fragment */
+ int num_frags; /* Maintains the number of fragments appended in the msgb */
+ int length; /* Maintains the number of bits appended in the msgb */
+ bool encryption; /* Set to true if the fragments were received encrypted */
+ struct msgb *msgb; /* Message buffer in which fragments are appended */
+};
+
+void upper_mac_init_fragslots();
int upper_mac_prim_recv(struct osmo_prim_hdr *op, void *priv);
#endif
To view, visit change 29387. To unsubscribe, or for help writing mail filters, visit settings.