3 files changed, 94 insertions, 0 deletions

diff --git a/src/host/virt_phy/include/virtphy/virt_l1_model.h b/src/host/virt_phy/include/virtphy/virt_l1_model.h
index 67c24bb6..d774756a 100644
--- a/src/host/virt_phy/include/virtphy/virt_l1_model.h
+++ b/src/host/virt_phy/include/virtphy/virt_l1_model.h
@@ -84,6 +84,8 @@ struct l1_state_ms {
 			uint8_t tfi[8];
 		} dl;
 	} tbf;
+	/* PS related transmit requests with absolute frame number */
+	struct llist_head tx_queue_abs;
 
 	/* fbsb state */
 	struct {
diff --git a/src/host/virt_phy/src/gsmtapl1_if.c b/src/host/virt_phy/src/gsmtapl1_if.c
index 2cf9d2dc..8fcccbbb 100644
--- a/src/host/virt_phy/src/gsmtapl1_if.c
+++ b/src/host/virt_phy/src/gsmtapl1_if.c
@@ -70,6 +70,7 @@ void gsmtapl1_tx_to_virt_um_inst(struct l1_model_ms *ms, uint32_t fn, uint8_t tn
 
 	switch (l1h->msg_type) {
 	case L1CTL_DATA_TBF_REQ:
+	case L1CTL_DATA_ABS_REQ:
 		ul = NULL;
 		rsl_chantype = RSL_CHAN_OSMO_PDCH;
 		timeslot = tn;
@@ -166,12 +167,58 @@ static uint8_t get_usf_from_block(struct msgb *msg)
 	return msg->data[0] & 0x7;
 }
 
+/* get first message in queue, but don't de-queue */
+static struct msgb *my_msgb_queue_peek(struct llist_head *queue)
+{
+	struct llist_head *lh;
+
+	if (llist_empty(queue))
+		return NULL;
+
+	lh = queue->next;
+	if (!lh)
+		return NULL;
+
+	return llist_entry(lh, struct msgb, list);
+}
+
+/* Check if we have any uplink transfer queued for this frame number. If so, transmit */
+static bool ms_ul_abs_may_transmit(struct l1_model_ms *ms, uint32_t fn)
+{
+	struct llist_head *queue = &ms->state.tx_queue_abs;
+	struct msgb *msg;
+
+	/* First check if we have any pending uplink blocks for this absolute FN */
+	msg = my_msgb_queue_peek(queue);
+	if (msg) {
+		struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->data;
+		struct l1ctl_info_ul_abs *abs = (struct l1ctl_info_ul_abs *) l1h->data;
+		if (abs->fn < fn) {
+			printf("----Dropping UL ABS, %u < %u\n", abs->fn, fn);
+			LOGPMS(DVIRPHY, LOGL_ERROR, ms, "Dropping UL ABS, %u < %u\n", abs->fn, fn);
+			OSMO_ASSERT(msgb_dequeue(queue) == msg);
+			msgb_free(msg);
+		} else if (abs->fn == fn) {
+			printf("----dequeueing abs for fn %u tn %u\n", abs->fn, abs->ts_nr);
+			OSMO_ASSERT(msgb_dequeue(queue) == msg);
+			gsmtapl1_tx_to_virt_um_inst(ms, fn, abs->ts_nr, msg);
+			return true;
+		}
+	}
+	/* TODO: what in case we have multiple for same FN but different TN? */
+	return false;
+}
+
 /* MS is authorized to transmit a block in uplink for given USF on timeslot+arfcn at FN */
 static void ms_ul_tbf_may_transmit(struct l1_model_ms *ms, uint16_t arfcn, uint8_t timeslot,
 				   uint32_t fn, uint8_t usf)
 {
 	struct msgb *msg;
 
+	/* If we have any ABS uplink transmit queued, this has higher precedence than USF */
+	if (ms_ul_abs_may_transmit(ms, fn))
+		return;
+
 	/* If USF is not for us, bail out */
 	if (!usf_matches_ms(ms, usf, timeslot))
 		return;
diff --git a/src/host/virt_phy/src/l1ctl_sap.c b/src/host/virt_phy/src/l1ctl_sap.c
index aac49bf0..276d7f5b 100644
--- a/src/host/virt_phy/src/l1ctl_sap.c
+++ b/src/host/virt_phy/src/l1ctl_sap.c
@@ -42,6 +42,7 @@
 
 static void l1ctl_rx_tbf_cfg_req(struct l1_model_ms *ms, struct msgb *msg);
 static void l1ctl_rx_data_tbf_req(struct l1_model_ms *ms, struct msgb *msg);
+static void l1ctl_rx_data_abs_req(struct l1_model_ms *ms, struct msgb *msg);
 
 static void l1_model_tch_mode_set(struct l1_model_ms *ms, uint8_t tch_mode)
 {
@@ -60,6 +61,7 @@ void l1ctl_sap_init(struct l1_model_ms *model)
 {
 	INIT_LLIST_HEAD(&model->state.sched.mframe_items);
 	INIT_LLIST_HEAD(&model->state.tbf.ul.tx_queue);
+	INIT_LLIST_HEAD(&model->state.tx_queue_abs);
 
 	prim_pm_init(model);
 }
@@ -252,6 +254,9 @@ void l1ctl_sap_handler(struct l1_model_ms *ms, struct msgb *msg)
 	case L1CTL_DATA_TBF_REQ:
 		l1ctl_rx_data_tbf_req(ms, msg);
 		goto exit_nofree;
+	case L1CTL_DATA_ABS_REQ:
+		l1ctl_rx_data_abs_req(ms, msg);
+		goto exit_nofree;
 	}
 
 exit_msgbfree:
@@ -662,6 +667,46 @@ static void l1ctl_rx_data_tbf_req(struct l1_model_ms *ms, struct msgb *msg)
 	msgb_enqueue(&ms->state.tbf.ul.tx_queue, msg);
 }
 
+static void l1ctl_rx_data_abs_req(struct l1_model_ms *ms, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->data;
+	struct l1ctl_info_ul_abs *udt = (struct l1ctl_info_ul_abs *) l1h->data;
+	enum osmo_gprs_cs osmo_cs;
+	int block_size;
+
+	msg->l2h = udt->payload;
+
+	LOGPMS(DL1P, LOGL_ERROR, ms, "Rx L1CTL_DATA_ABS_REQ (tbf_id=%d, ts=%u, fn=%u, data=%s)\n",
+		udt->tbf_nr, udt->ts_nr, udt->fn, osmo_hexdump(msg->l2h, msgb_l2len(msg)));
+	if (udt->tbf_nr != 0) {
+		LOGPMS(DL1C, LOGL_ERROR, ms, "TBF_NR != 0 not supported yet!\n");
+		return;
+	}
+
+	if (ms->state.state != MS_STATE_TBF) {
+		LOGPMS(DL1P, LOGL_ERROR, ms, "DATA_ABS_REQ in state != TBF\n");
+		return;
+	}
+
+	osmo_cs = get_l1ctl_cs_by_osmo(udt->coding_scheme);
+	if (osmo_cs < 0) {
+		LOGPMS(DL1P, LOGL_ERROR, ms, "DATA_RBF_REQ with invalid CS\n");
+		return;
+	}
+	block_size = osmo_gprs_ul_block_size_bytes(osmo_cs);
+
+	if (msgb_l2len(msg) < block_size) {
+		int pad_len = block_size - msgb_l2len(msg);
+		uint8_t *pad = msgb_put(msg, pad_len);
+		memset(pad, GSM_MACBLOCK_PADDING, pad_len);
+	}
+
+	printf("----enqueueing abs for fn %u, ts %u: %s\n", udt->fn, udt->ts_nr, msgb_hexdump(msg));
+	/* FIXME: ordered insert?  But then, the L1CTL_DATA_ABS_REQ should already
+	 * arrive in ordered fashion */
+	msgb_enqueue(&ms->state.tx_queue_abs, msg);
+}
+
 /***************************************************************
  * L1CTL TX ROUTINES *******************************************
  * For more routines check the respective handler classes ******