From 1f48ec86276a3ab51cd71b6285387685851f002b Mon Sep 17 00:00:00 2001 From: wbokslag Date: Fri, 16 Sep 2022 16:07:14 +0200 Subject: [PATCH] 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 --- src/tetra_llc_pdu.h | 8 +- src/tetra_mac_pdu.c | 5 +- src/tetra_mac_pdu.h | 7 +- src/tetra_upper_mac.c | 278 +++++++++++++++++++++++++++++++++++++----- src/tetra_upper_mac.h | 15 +++ 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 @@ struct tetra_llc_pdu { 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 @@ static const uint8_t addr_len_by_type[] = { }; /* 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 @@ static int decode_nr_slots(uint8_t in) 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 @@ const char *tetra_get_ul_usage_name(uint8_t num); 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 @@ struct tetra_resrc_decoded { }; 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 @@ static void rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms 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 @@ static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int 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 @@ static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int 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 @@ static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms 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 @@ static void rx_suppl(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms 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 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ 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 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ 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 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ 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 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ 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 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_ 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 @@ int upper_mac_prim_recv(struct osmo_prim_hdr *op, void *priv) 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