Added calls to stub decryption functions for MAC resources

The tcs (Tetra Crypto State) struct now maintains information relevant for decryption, such as the current network, colour code, hyperframe, etcetera.
Also, the upper mac now calls a stub decryption function when receiving an encrypted resource.

Change-Id: I92d718789d6b7e84c1901d09165fce59cdf8c1ca

src/Makefile

@@ -16,11 +16,14 @@ libosmo-tetra-phy.a: phy/tetra_burst_sync.o phy/tetra_burst.o
 libosmo-tetra-mac.a: lower_mac/tetra_conv_enc.o lower_mac/tch_reordering.o tetra_tdma.o lower_mac/tetra_scramb.o lower_mac/tetra_rm3014.o lower_mac/tetra_interleave.o lower_mac/crc_simple.o tetra_common.o lower_mac/viterbi.o lower_mac/viterbi_cch.o lower_mac/viterbi_tch.o lower_mac/tetra_lower_mac.o tetra_upper_mac.o tetra_mac_pdu.o tetra_llc_pdu.o tetra_llc.o tetra_mle_pdu.o tetra_mle.o tetra_mm_pdu.o tetra_cmce_pdu.o tetra_sndcp_pdu.o tetra_gsmtap.o tuntap.o
 	$(AR) r $@ $^
 
+libosmo-tetra-crypto.a: crypto/tetra_crypto.o
+	$(AR) r $@ $^
+
 float_to_bits: float_to_bits.o
 
 crc_test: crc_test.o tetra_common.o libosmo-tetra-mac.a
 
-tetra-rx: tetra-rx.o libosmo-tetra-phy.a libosmo-tetra-mac.a
+tetra-rx: tetra-rx.o libosmo-tetra-phy.a libosmo-tetra-mac.a libosmo-tetra-crypto.a
 
 conv_enc_test: conv_enc_test.o testpdu.o libosmo-tetra-phy.a libosmo-tetra-mac.a
 

src/crypto/tetra_crypto.c

@@ -0,0 +1,252 @@
+/* Cryptography related helper, key management and wrapper functions */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation, either version 3 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include <osmocom/core/utils.h>
+#include <tetra_mac_pdu.h>
+#include <phy/tetra_burst.h>
+
+#include "tetra_crypto.h"
+
+struct tetra_crypto_database _tcdb, *tcdb = &_tcdb;
+
+static const struct value_string tetra_key_types[] = {
+	{ KEYTYPE_UNDEFINED,		"UNDEFINED" },
+	{ KEYTYPE_DCK,			"DCK" },
+	{ KEYTYPE_MGCK,			"MGCK" },
+	{ KEYTYPE_CCK_SCK,		"CCK/SCK" },
+	{ KEYTYPE_GCK,			"GCK" },
+	{ 0, NULL }
+};
+
+const char *tetra_get_key_type_name(enum tetra_key_type key_type)
+{
+	return get_value_string(tetra_key_types, key_type);
+}
+
+static const struct value_string tetra_tea_types[] = {
+	{ UNKNOWN,			"UNKNOWN" },
+	{ KSG_TEA1,			"TEA1" },
+	{ KSG_TEA2,			"TEA2" },
+	{ KSG_TEA3,			"TEA3" },
+	{ KSG_TEA4,			"TEA4" },
+	{ KSG_TEA5,			"TEA5" },
+	{ KSG_TEA6,			"TEA6" },
+	{ KSG_TEA7,			"TEA7" },
+	{ KSG_PROPRIETARY,		"PROPRIETARY" },
+	{ 0, NULL }
+};
+
+const char *tetra_get_ksg_type_name(enum tetra_ksg_type ksg_type)
+{
+	if (ksg_type >= KSG_PROPRIETARY)
+		return tetra_tea_types[KSG_PROPRIETARY].str;
+	else
+		return get_value_string(tetra_tea_types, ksg_type);
+}
+
+static const struct value_string tetra_security_classes[] = {
+	{ NETWORK_CLASS_UNDEFINED,	"CLASS_UNDEFINED" },
+	{ NETWORK_CLASS_1,		"CLASS_1" },
+	{ NETWORK_CLASS_2,		"CLASS_2" },
+	{ NETWORK_CLASS_3,		"CLASS_3" },
+	{ 0, NULL }
+};
+
+const char *tetra_get_security_class_name(uint8_t pdut)
+{
+	return get_value_string(tetra_security_classes, pdut);
+}
+
+void tetra_crypto_state_init(struct tetra_crypto_state *tcs)
+{
+	/* Initialize tetra_crypto_state */
+	tcs = talloc_zero(NULL, struct tetra_crypto_state);
+	tcs->mnc = -1;
+	tcs->mcc = -1;
+	tcs->cck_id = -1;
+	tcs->hn =  -1;
+	tcs->la =  -1;
+	tcs->cc =  -1;
+	tcs->cck = 0;
+	tcs->network = 0;
+}
+
+void tetra_crypto_db_init(void)
+{
+	/* Initialize tetra_crypto_database */
+	tcdb->num_keys = 0;
+	tcdb->num_nets = 0;
+	tcdb->keys = talloc_zero_array(NULL, struct tetra_key, TCDB_ALLOC_BLOCK_SIZE);
+	tcdb->nets = talloc_zero_array(NULL, struct tetra_netinfo, TCDB_ALLOC_BLOCK_SIZE);
+
+	if (!tcdb->keys || !tcdb->nets) {
+		fprintf(stderr, "couldn't allocate memory for tetra_crypto_database\n");
+		exit(1);
+	}
+}
+
+char *dump_key(struct tetra_key *k)
+{
+	static char pbuf[1024];
+
+	int c = snprintf(pbuf, sizeof(pbuf), "MCC %4d MNC %4d key_type %02X",
+		k->mcc, k->mnc, k->key_type);
+
+	if (k->key_type & (KEYTYPE_DCK | KEYTYPE_MGCK))
+		c += snprintf(pbuf + c, sizeof(pbuf) - c, " addr: %8d", k->addr);
+
+	if (k->key_type & (KEYTYPE_CCK_SCK))
+		c += snprintf(pbuf + c, sizeof(pbuf) - c, " key_num: %4d", k->key_num);
+
+	c += snprintf(pbuf + c, sizeof(pbuf) - c, ": ");
+	for (int i = 0; i < 10; i++)
+		snprintf(pbuf + c + 2*i, sizeof(pbuf)-c-2*i, "%02X", k->key[i]);
+
+	return pbuf;
+}
+
+char *dump_network_info(struct tetra_netinfo *network)
+{
+	static char pbuf[1024];
+	snprintf(pbuf, sizeof(pbuf), "MCC %4d MNC %4d ksg_type %d security_class %d", network->mcc, network->mnc, network->ksg_type, network->security_class);
+	return pbuf;
+}
+
+uint32_t tea_build_iv(struct tetra_tdma_time *tm, uint16_t hn, uint8_t dir)
+{
+	assert(1 <= tm->tn  && tm->tn  <= 4);
+	assert(1 <= tm->fn  && tm->fn  <= 18);
+	assert(1 <= tm->mn  && tm->mn  <= 60);
+	assert(0 <= tm->hn  && tm->hn  <= 0xFFFF);
+	assert(0 <= dir && dir <= 1); // 0 = downlink, 1 = uplink
+	return ((tm->tn - 1) | (tm->fn << 2) | (tm->mn << 7) | ((hn & 0x7FFF) << 13) | (dir << 28));
+}
+
+int decrypt_identity(struct tetra_crypto_state *tcs, struct tetra_addr *addr)
+{
+	return 0;
+}
+
+int decrypt_mac_element(struct tetra_crypto_state *tcs, struct tetra_tmvsap_prim *tmvp, struct tetra_key *key, int l1_len, int tmpdu_offset)
+{
+	return 0;
+}
+
+int decrypt_voice_timeslot(struct tetra_crypto_state *tcs, struct tetra_tdma_time *tdma_time, int16_t *type1_block)
+{
+	return 0;
+}
+
+int load_keystore(char *tetra_keyfile)
+{
+	return 0;
+}
+
+struct tetra_key *get_key_by_addr(struct tetra_crypto_state *tcs, int addr, enum tetra_key_type key_type)
+{
+	for (int i = 0; i < tcdb->num_keys; i++) {
+		struct tetra_key *key = &tcdb->keys[i];
+		if (key->mnc == tcs->mnc &&
+				key->mcc == tcs->mcc &&
+				key->addr == addr &&
+				(key->key_type & key_type)) {
+			return key;
+		}
+	}
+	return 0;
+}
+
+struct tetra_key *get_ksg_key(struct tetra_crypto_state *tcs, int addr)
+{
+	/* TETRA standard part 7 Clause 6.2:
+	--------------------------------------------
+			Auth	Encr	GCK	DCK
+	Class 1:	?	-	-	-
+	Class 2:	?	+	?	-
+	Class 3:	+	+	?	+
+	--------------------------------------------
+	*/
+
+	if (!tcs->network)
+		/* No tetra_netinfo from the db set for this network */
+		return 0;
+
+	/* FIXME: add support for ISSI/GSSI range definitions and GCK bindings */
+	/* FIXME: add support for ISSI-bound DCK keys in class 3 networks */
+
+	return tcs->cck;
+}
+
+void update_current_network(struct tetra_crypto_state *tcs, int mcc, int mnc)
+{
+	// Update globals
+	tcs->mcc = mcc;
+	tcs->mnc = mnc;
+
+	// Network changed, update reference to current network
+	tcs->network = 0;
+	for (int i = 0; i < tcdb->num_nets; i++) {
+		struct tetra_netinfo *network = &tcdb->nets[i];
+		if (network->mnc == tcs->mnc && network->mcc == tcs->mcc) {
+			tcs->network = network;
+			break;
+		}
+	}
+
+	// (Try to) select new CCK/SCK
+	update_current_cck(tcs);
+}
+
+void update_current_cck(struct tetra_crypto_state *tcs)
+{
+	printf("\ntetra_crypto: update_current_cck invoked cck %d mcc %d mnc %d\n", tcs->cck_id, tcs->mcc, tcs->mnc);
+	tcs->cck = 0;
+
+	for (int i = 0; i < tcdb->num_keys; i++) {
+		struct tetra_key *key = &tcdb->keys[i];
+		/* TODO FIXME consider selecting CCK or SCK key type based on network config */
+		if (key->mcc == tcs->mcc && key->mnc == tcs->mnc && key->key_num == tcs->cck_id) {
+			if (key->key_type == KEYTYPE_CCK_SCK) {
+				tcs->cck = key;
+				printf("tetra_crypto: Set new current_cck %d (type: full)\n", i);
+				break;
+			}
+		}
+	}
+}
+
+struct tetra_netinfo *get_network_info(int mcc, int mnc)
+{
+	for (int i = 0; i < tcdb->num_nets; i++) {
+		if (tcdb->nets[i].mcc == mcc && tcdb->nets[i].mnc == mnc)
+			return &tcdb->nets[i];
+	}
+	return 0;
+}

src/crypto/tetra_crypto.h

@@ -0,0 +1,130 @@
+/* Cryptography related helper, key management and wrapper functions */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation, either version 3 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+
+#ifndef TETRA_CRYPTO_H
+#define TETRA_CRYPTO_H
+
+#include <inttypes.h>
+#include <stdbool.h>
+
+#include "../tetra_prim.h"
+#include "../tetra_tdma.h"
+#include "../tetra_mac_pdu.h"
+
+#define TCDB_ALLOC_BLOCK_SIZE 16
+
+enum tetra_key_type {
+	KEYTYPE_UNDEFINED		= 0x00,
+
+	/* Keytypes usable as ECK after applying TB5 */
+	KEYTYPE_DCK			= 0x01,
+	KEYTYPE_MGCK			= 0x02,
+	KEYTYPE_CCK_SCK			= 0x04, /* SCK in class 2, CCK in class 3 networks */
+
+	/* Can be combined with SCK/CCK to get MGCK */
+	KEYTYPE_GCK			= 0x20,
+};
+
+enum tetra_ksg_type {
+	UNKNOWN				= 0,
+	KSG_TEA1			= 1, /* KSG number value 0 */
+	KSG_TEA2			= 2, /* KSG number value 1 */
+	KSG_TEA3			= 3, /* KSG number value 2 */
+	KSG_TEA4			= 4, /* KSG number value 3 */
+	KSG_TEA5			= 5, /* KSG number value 4 */
+	KSG_TEA6			= 6, /* KSG number value 5 */
+	KSG_TEA7			= 7, /* KSG number value 6 */
+	KSG_PROPRIETARY			= 8, /* KSG number values 9-15 */
+};
+
+enum tetra_security_class {
+	NETWORK_CLASS_UNDEFINED		= 0,
+	NETWORK_CLASS_1			= 1,
+	NETWORK_CLASS_2			= 2,
+	NETWORK_CLASS_3			= 3,
+};
+
+struct tetra_netinfo {
+	uint32_t mcc;
+	uint32_t mnc;
+	enum tetra_ksg_type ksg_type;			/* KSG used in this network */
+	enum tetra_security_class security_class;	/* Security class this network employs */
+};
+
+struct tetra_key {
+	uint32_t index;				/* position in key list */
+	uint32_t mnc;
+	uint32_t mcc;
+	enum tetra_key_type key_type;
+	uint32_t key_num;			/* key_vn or whatever key numbering system is relevant for this key type */
+	uint32_t addr;				/* ISSI, GSSI, or whatever address is relevant for this key type */
+	uint8_t  key[16];			/* Currently keys are 80 bits, but we may want to support 128-bit keys as well */
+	struct tetra_netinfo *network_info;	/* Network with which the key is associated */
+};
+
+struct tetra_crypto_database {
+	uint32_t num_keys;
+	struct tetra_key *keys;
+	uint32_t num_nets;
+	struct tetra_netinfo *nets;
+};
+extern struct tetra_crypto_database *tcdb;
+
+struct tetra_crypto_state {
+	int mnc;			/* Network info for selecting keys */
+	int mcc;			/* Network info for selecting keys */
+	int cck_id;			/* CCK or SCK id used on network (from SYSINFO) */
+	int hn;				/* Hyperframe number for IV (from SYSINFO) */
+	int la;				/* location area for TB5 */
+	int cn;				/* carrier number for TB5. WARNING: only set correctly if tuned to main control channel */
+	int cc;				/* colour code for TB5 */
+	struct tetra_netinfo *network;	/* pointer to network info struct loaded from file */
+	struct tetra_key *cck;		/* pointer to CCK or SCK for this network and version (from SYSINFO) */
+};
+
+const char *tetra_get_key_type_name(enum tetra_key_type);
+const char *tetra_get_ksg_type_name(enum tetra_ksg_type);
+const char *tetra_get_security_class_name(uint8_t pdut);
+
+/* Key loading / unloading */
+void tetra_crypto_state_init(struct tetra_crypto_state *tcs);
+void tetra_crypto_db_init(void);
+int load_keystore(char *filename);
+
+/* Keystream generation and decryption functions */
+uint32_t tea_build_iv(struct tetra_tdma_time *tm, uint16_t hn, uint8_t dir);
+int decrypt_identity(struct tetra_crypto_state *tcs, struct tetra_addr *addr);
+int decrypt_mac_element(struct tetra_crypto_state *tcs, struct tetra_tmvsap_prim *tmvp, struct tetra_key *key, int l1_len, int tmpdu_offset);
+int decrypt_voice_timeslot(struct tetra_crypto_state *tcs, struct tetra_tdma_time *tdma_time, int16_t *type1_bits);
+
+/* Key selection and crypto state management */
+struct tetra_netinfo *get_network_info(int mcc, int mnc);
+struct tetra_key *get_ksg_key(struct tetra_crypto_state *tcs, int addr);
+void update_current_network(struct tetra_crypto_state *tcs, int mcc, int mnc);
+void update_current_cck(struct tetra_crypto_state *tcs);
+
+char *dump_network_info(struct tetra_netinfo *network);
+char *dump_key(struct tetra_key *k);
+
+#endif /* TETRA_CRYPTO_H */

src/lower_mac/tetra_lower_mac.c

@@ -40,6 +40,7 @@
 #include <tetra_prim.h>
 #include "tetra_upper_mac.h"
 #include <lower_mac/viterbi.h>
+#include <crypto/tetra_crypto.h>
 
 struct tetra_blk_param {
 	const char *name;
@@ -149,6 +150,7 @@ void tp_sap_udata_ind(enum tp_sap_data_type type, int blk_num, const uint8_t *bi
 
 	const struct tetra_blk_param *tbp = &tetra_blk_param[type];
 	struct tetra_mac_state *tms = priv;
+	struct tetra_crypto_state *tcs = tms->tcs;
 	const char *time_str;
 
 	/* TMV-SAP.UNITDATA.ind primitive which we will send to the upper MAC */
@@ -299,6 +301,12 @@ void tp_sap_udata_ind(enum tp_sap_data_type type, int blk_num, const uint8_t *bi
 		/* update the PHY layer time */
 		memcpy(&t_phy_state.time, &tcd->time, sizeof(t_phy_state.time));
 		tup->lchan = TETRA_LC_BSCH;
+
+		/* Update colour code and network info for crypto IV generation */
+		tcs->cc = tcd->colour_code;
+		if (tcs->mcc != tcd->mcc || tcs->mnc != tcd->mnc)
+			update_current_network(tcs, tcd->mcc, tcd->mnc);
+
 		break;
 	case TPSAP_T_SB2:
 	case TPSAP_T_NDB:

src/tetra-rx.c

@@ -30,6 +30,7 @@
 #include <osmocom/core/talloc.h>
 
 #include "tetra_common.h"
+#include "crypto/tetra_crypto.h"
 #include <phy/tetra_burst.h>
 #include <phy/tetra_burst_sync.h>
 #include "tetra_gsmtap.h"
@@ -43,8 +44,11 @@ int main(int argc, char **argv)
 	struct tetra_rx_state *trs;
 	struct tetra_mac_state *tms;
 
+	/* Initialize tetra mac state and crypto state */
 	tms = talloc_zero(tetra_tall_ctx, struct tetra_mac_state);
 	tetra_mac_state_init(tms);
+	tms->tcs = talloc_zero(NULL, struct tetra_crypto_state);
+	tetra_crypto_state_init(tms->tcs);
 
 	trs = talloc_zero(tetra_tall_ctx, struct tetra_rx_state);
 	trs->burst_cb_priv = tms;

src/tetra_common.h

@@ -55,6 +55,8 @@ struct tetra_mac_state {
 	} cur_burst;
 	struct tetra_si_decoded last_sid;
 
+	struct tetra_crypto_state *tcs; /* contains all state relevant to encryption */
+
 	char *dumpdir;	/* Where to save traffic channel dump */
 	int ssi;	/* SSI */
 	int tsn;	/* Timeslon number */

src/tetra_upper_mac.c

@@ -27,6 +27,7 @@
 #include <osmocom/core/msgb.h>
 #include <osmocom/core/talloc.h>
 
+#include "crypto/tetra_crypto.h"
 #include "tetra_common.h"
 #include "tetra_prim.h"
 #include "tetra_upper_mac.h"
@@ -84,6 +85,7 @@ static int get_num_fill_bits(const unsigned char *l1h, int len_with_fillbits)
 static int rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 {
 	struct msgb *msg = tmvp->oph.msg;
+	struct tetra_crypto_state *tcs = tms->tcs;
 	struct tetra_si_decoded sid;
 	uint32_t dl_freq, ul_freq;
 	int i;
@@ -114,6 +116,19 @@ static int 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));
+
+	/* Update crypto state */
+	tcs->la = sid.mle_si.la;
+	tcs->cn = sid.main_carrier; /* FIXME this won't work when not tuned to the main carier */
+	if (sid.cck_valid_no_hf) {
+		if (sid.cck_id != tcs->cck_id) {
+			tcs->cck_id = sid.cck_id;
+			update_current_cck(tcs);
+		}
+	} else {
+		tcs->hn = sid.hyperframe_number;
+	}
+
 	return -1; /* FIXME check this indeed fills slot */
 }
 
@@ -142,8 +157,10 @@ const char *tetra_alloc_dump(const struct tetra_chan_alloc_decoded *cad, struct
 static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 {
 	struct msgb *msg = tmvp->oph.msg;
+	struct tetra_crypto_state *tcs = tms->tcs;
 	struct tetra_resrc_decoded rsd;
 	struct msgb *fragmsgb;
+	struct tetra_key *key = 0;
 	int tmpdu_offset, slot;
 	int pdu_bits; /* Full length of pdu, including fill bits */
 
@@ -151,12 +168,12 @@ static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 	tmpdu_offset = macpdu_decode_resource(&rsd, msg->l1h, 0);
 
 	if (rsd.macpdu_length == MACPDU_LEN_2ND_STOLEN) {
-		pdu_bits = -1; /* Fills slot */
-		printf("WARNING pdu with MACPDU_LEN_2ND_STOLEN, not implemented\n");
+		pdu_bits = -1;				/* Fills slot */
+		tms->cur_burst.blk2_stolen = true;	/* Next block is also stolen */
 	} else if (rsd.macpdu_length == MACPDU_LEN_START_FRAG) {
-		pdu_bits = -1; /* Fills slot */
+		pdu_bits = -1;				/* Fills slot */
 	} else {
-		pdu_bits = rsd.macpdu_length * 8; /* Length given */
+		pdu_bits = rsd.macpdu_length * 8;	/* Length given */
 		msg->tail = msg->head + pdu_bits;
 	}
 
@@ -166,14 +183,34 @@ static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 		msg->tail -= num_fill_bits;
 	}
 
+	/* Decrypt buffer if encrypted and key available */
+	if (rsd.is_encrypted && tcdb->num_keys) {
+		decrypt_identity(tcs, &rsd.addr);
+		key = get_ksg_key(tcs, rsd.addr.ssi);
+
+		if (key) {
+			rsd.is_encrypted = !decrypt_mac_element(tcs, tmvp, key, msgb_l1len(msg), tmpdu_offset);
+			if (rsd.chan_alloc_pres) {
+				// Re-decode the channel allocation element to get accurate L2 start
+				tmpdu_offset += macpdu_decode_chan_alloc(&rsd.cad, msg->l1h + tmpdu_offset);
+			}
+		}
+	}
+
 	/* 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_field=%d l1_len=%d l2_len %d Addr=%s",
-		rsd.encryption_mode, rsd.macpdu_length, msgb_l1len(msg), msgb_l2len(msg),
+	printf("RESOURCE Encr=%u%s len_field=%d l1_len=%d l2_len=%d Addr=%s",
+		rsd.encryption_mode,
+		rsd.encryption_mode && !rsd.is_encrypted ? " DECRYPTED" : "",
+		rsd.macpdu_length, msgb_l1len(msg), msgb_l2len(msg),
 		tetra_addr_dump(&rsd.addr));
 
-	if (rsd.chan_alloc_pres)
-		printf(" ChanAlloc=%s", tetra_alloc_dump(&rsd.cad, tms));
+	if (rsd.chan_alloc_pres) {
+		if (!rsd.is_encrypted)
+			printf(" ChanAlloc=%s", tetra_alloc_dump(&rsd.cad, tms));
+		else
+			printf(" ChanAlloc=ENCRYPTED");
+	}
 
 	if (rsd.slot_granting.pres)
 		printf(" SlotGrant=%u/%u", rsd.slot_granting.nr_slots,
@@ -183,16 +220,20 @@ static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 		pdu_bits = -1; /* No more PDUs in slot */
 		goto out;
 	}
-	if (msgb_l2len(msg) == 0) {
+	tms->ssi = rsd.addr.ssi;
+	tms->usage_marker = rsd.addr.usage_marker;
+	tms->addr_type = rsd.addr.type;
+
+	if (msgb_l2len(msg) == 0)
 		goto out; /* No l2 data */
-	}
+
+	if (rsd.is_encrypted)
+		goto out; /* Can't parse any further */
 
 	printf(": %s\n", osmo_ubit_dump(msg->l2h, msgb_l2len(msg)));
 	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));
-		}
+		rx_tm_sdu(tms, msg, msgb_l2len(msg));
 	} else {
 		/* Fragmented resource */
 		slot = tmvp->u.unitdata.tdma_time.tn;
@@ -205,6 +246,7 @@ static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 		fragmsgb = fragslots[slot].msgb;
 
 		/* Copy l2 part to fragmsgb. l3h is constructed once all fragments are merged */
+		fragmsgb = fragslots[slot].msgb;
 		fragmsgb->l1h = msgb_put(fragmsgb, msgb_l1len(msg));
 		fragmsgb->l2h = fragmsgb->l1h + tmpdu_offset;
 		fragmsgb->l3h = 0;
@@ -215,10 +257,9 @@ static int rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 		fragslots[slot].num_frags = 1;
 		fragslots[slot].length = msgb_l2len(msg);
 		fragslots[slot].encryption = rsd.encryption_mode > 0;
+		fragslots[slot].key = key;
 	}
 
-	tms->ssi = rsd.addr.ssi;
-
 out:
 	printf("\n");
 	return pdu_bits;
@@ -243,6 +284,7 @@ void append_frag_bits(int slot, uint8_t *bits, int bitlen)
 static int rx_macfrag(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 {
 	struct msgb *msg = tmvp->oph.msg;
+	struct msgb *fragmsgb;
 	int slot = tmvp->u.unitdata.tdma_time.tn;
 	uint8_t *bits = msg->l1h;
 	uint8_t fillbits_present;
@@ -258,11 +300,15 @@ static int rx_macfrag(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tm
 		/* 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);
+			msg->tail -= num_fill_bits;
 		}
 
+		/* Decrypt (if required) */
+		fragmsgb = fragslots[slot].msgb;
+		if (fragslots[slot].encryption && fragslots[slot].key)
+			decrypt_mac_element(tms->tcs, tmvp, fragslots[slot].key, msgb_l1len(msg), n);
+
 		/* 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 {
@@ -308,16 +354,21 @@ static int rx_macend(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms
 			msg->tail -= num_fill_bits;
 		}
 
+		/* Decrypt (if required) */
+		if (fragslots[slot].encryption && fragslots[slot].key)
+			decrypt_mac_element(tms->tcs, tmvp, fragslots[slot].key, msgb_l1len(msg), n);
+
 		/* 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) {
+		if (!fragslots[slot].encryption || fragslots[slot].key) {
 			rx_tm_sdu(tms, fragmsgb, fragslots[slot].length);
 		}
 	} else {
@@ -343,17 +394,17 @@ static int rx_suppl(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 	{
 		uint8_t slot_granting = *(msg->l1h + 17);
 		if (slot_granting)
-			tmpdu_offset = 17+1+8;
+			tmpdu_offset = 17 + 1 + 8;
 		else
-			tmpdu_offset = 17+1;
+			tmpdu_offset = 17 + 1;
 	}
 #endif
 
 	printf("SUPPLEMENTARY MAC-D-BLOCK ");
 
 	//if (sud.encryption_mode == 0)
-		msg->l2h = msg->l1h + tmpdu_offset;
-		rx_tm_sdu(tms, msg, 100);
+	msg->l2h = msg->l1h + tmpdu_offset;
+	rx_tm_sdu(tms, msg, 100);
 
 	printf("\n");
 	return -1; /* TODO FIXME check length */
@@ -385,10 +436,15 @@ static void rx_aach(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
 		printf("UL_USAGE: %s ", tetra_get_ul_usage_name(aad.ul_usage));
 
 	/* save the state whether the current burst is traffic or not */
-	if (aad.dl_usage > 3)
+	if (aad.dl_usage > 3) {
 		tms->cur_burst.is_traffic = aad.dl_usage;
-	else
+	} else {
 		tms->cur_burst.is_traffic = 0;
+	}
+
+	/* Reset slot stealing flags */
+	tms->cur_burst.blk1_stolen = false;
+	tms->cur_burst.blk2_stolen = false;
 
 	printf("\n");
 }
@@ -426,10 +482,11 @@ 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) {
+
+	if (tup->tdma_time.fn == 18 && REASSEMBLE_FRAGMENTS)
+		/* Age out old fragments */
+		/* FIXME: also age out old event labels */
 		age_fragslots();
-	}
 
 	len_parsed = -1; /* Default for cases where slot is filled or otherwise irrelevant */
 	switch (tup->lchan) {

src/tetra_upper_mac.h

@@ -14,6 +14,7 @@ struct fragslot {
 	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 tetra_key *key;		/* Holds pointer to the key to be used for this slot */
 	struct msgb *msgb;		/* Message buffer in which fragments are appended */
 };