diff --git a/src/lower_mac/tetra_lower_mac.c b/src/lower_mac/tetra_lower_mac.c
index bf0896b..2a2cbbe 100644
--- a/src/lower_mac/tetra_lower_mac.c
+++ b/src/lower_mac/tetra_lower_mac.c
@@ -307,10 +307,32 @@ void tp_sap_udata_ind(enum tp_sap_data_type type, int blk_num, const uint8_t *bi
 		/* FIXME: do something */
 		break;
 	}
-	/* send Rx time along with the TMV-UNITDATA.ind primitive */
-	memcpy(&tup->tdma_time, &tcd->time, sizeof(tup->tdma_time));
 
-	upper_mac_prim_recv(&ttp->oph, tms);
+	int pdu_bits = 0;
+	uint32_t offset = 0;
+	uint8_t *orig_head = msg->head;
+	uint8_t *orig_tail = msg->tail;
+	while (offset < tbp->type1_bits - 16) {
+		/* send Rx time along with the TMV-UNITDATA.ind primitive */
+		memcpy(&tup->tdma_time, &tcd->time, sizeof(tup->tdma_time));
+		pdu_bits = upper_mac_prim_recv(&ttp->oph, tms);
+
+		if (pdu_bits < 0) {
+			/* -1 is returned when pdu fills slot or length could not be determined */
+			break;
+		}
+
+		/* Increment head and l1h ptrs and reset tail to end of msg (may be altered by removing FCS) */
+		offset += pdu_bits;
+		msg->head = orig_head + offset;
+		msg->tail = orig_tail;
+		msg->len = msg->tail - msg->head;
+		msg->l1h = msg->head;
+		msg->l2h = 0;
+		msg->l3h = 0;
+		msg->l4h = 0;
+	}
+
+	talloc_free(msg);
+	talloc_free(ttp);
 }
-
-
diff --git a/src/tetra_gsmtap.c b/src/tetra_gsmtap.c
index 3f872c5..3e06223 100644
--- a/src/tetra_gsmtap.c
+++ b/src/tetra_gsmtap.c
@@ -64,16 +64,11 @@ struct msgb *tetra_gsmtap_makemsg(struct tetra_tdma_time *tm, enum tetra_log_cha
 
 int tetra_gsmtap_sendmsg(struct msgb *msg)
 {
-	int rc;
-
 	if (g_gti) {
-		rc = gsmtap_sendmsg_free(g_gti, msg);
+		return gsmtap_sendmsg(g_gti, msg);
 	} else {
-		msgb_free(msg);
-		rc = 0;
+		return 0;
 	}
-
-	return rc;
 }
 
 int tetra_gsmtap_init(const char *host, uint16_t port)
diff --git a/src/tetra_upper_mac.c b/src/tetra_upper_mac.c
index 033e13a..86c4439 100644
--- a/src/tetra_upper_mac.c
+++ b/src/tetra_upper_mac.c
@@ -559,8 +559,5 @@ int upper_mac_prim_recv(struct osmo_prim_hdr *op, void *priv)
 		break;
 	}
 
-	talloc_free(op->msg);
-	talloc_free(op);
-
 	return pdu_bits;
 }