VIRT-PHY: Add support for GPRS / TBF mode

we add a new STATE_TBF to vthe MS model and add some L1CTL primitives
to configure that TBF mode as well as to enqueue transmissions for
blocks at a given USF+TS.

Change-Id: Ie6f37090bd45f463aa25d9e00bc06f563be5264a

3 files changed, 249 insertions, 5 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 fa791270..67c24bb6 100644
--- a/src/host/virt_phy/include/virtphy/virt_l1_model.h
+++ b/src/host/virt_phy/include/virtphy/virt_l1_model.h
@@ -15,6 +15,7 @@ enum ms_state {
 	MS_STATE_IDLE_SYNCING,
 	MS_STATE_IDLE_CAMPING,
 	MS_STATE_DEDICATED,
+	MS_STATE_TBF
 };
 
 
@@ -74,6 +75,15 @@ struct l1_state_ms {
 		uint8_t tsc; // training sequence code (ununsed in virtual um)
 		uint8_t h; // hopping enabled flag (ununsed in virtual um)
 	} dedicated;
+	struct {
+		struct {
+			uint8_t usf[8];
+			struct llist_head tx_queue;
+		} ul;
+		struct {
+			uint8_t tfi[8];
+		} dl;
+	} tbf;
 
 	/* fbsb state */
 	struct {
diff --git a/src/host/virt_phy/src/gsmtapl1_if.c b/src/host/virt_phy/src/gsmtapl1_if.c
index 8202046f..2cf9d2dc 100644
--- a/src/host/virt_phy/src/gsmtapl1_if.c
+++ b/src/host/virt_phy/src/gsmtapl1_if.c
@@ -54,7 +54,7 @@ static char *pseudo_lchan_name(uint16_t arfcn, uint8_t ts, uint8_t ss, uint8_t s
 void gsmtapl1_tx_to_virt_um_inst(struct l1_model_ms *ms, uint32_t fn, uint8_t tn, struct msgb *msg)
 {
 	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *)msg->data;
-	struct l1ctl_info_ul *ul = (struct l1ctl_info_ul *)l1h->data;
+	struct l1ctl_info_ul *ul;
 	struct gsmtap_hdr *gh;
 	struct msgb *outmsg;	/* msg to send with gsmtap header prepended */
 	uint16_t arfcn = ms->state.serving_cell.arfcn;	/* arfcn of the cell we currently camp on */
@@ -68,8 +68,20 @@ void gsmtapl1_tx_to_virt_um_inst(struct l1_model_ms *ms, uint32_t fn, uint8_t tn
 	uint8_t timeslot;	/* tdma timeslot to send in (0-7) */
 	uint8_t gsmtap_chan;	/* the gsmtap channel */
 
-	rsl_dec_chan_nr(ul->chan_nr, &rsl_chantype, &subslot, &timeslot);
-	gsmtap_chan = chantype_rsl2gsmtap(rsl_chantype, ul->link_id);
+	switch (l1h->msg_type) {
+	case L1CTL_DATA_TBF_REQ:
+		ul = NULL;
+		rsl_chantype = RSL_CHAN_OSMO_PDCH;
+		timeslot = tn;
+		subslot = 0;
+		gsmtap_chan = chantype_rsl2gsmtap(rsl_chantype, 0);
+		break;
+	default:
+		ul = (struct l1ctl_info_ul *)l1h->data;
+		rsl_dec_chan_nr(ul->chan_nr, &rsl_chantype, &subslot, &timeslot);
+		gsmtap_chan = chantype_rsl2gsmtap(rsl_chantype, ul->link_id);
+		break;
+	}
 
 	/* arfcn needs to be flagged to be able to distinguish between uplink and downlink */
 	outmsg = gsmtap_makemsg(arfcn | GSMTAP_ARFCN_F_UPLINK, timeslot,
@@ -104,6 +116,77 @@ extern void prim_fbsb_sync(struct l1_model_ms *ms, struct msgb *msg);
  */
 extern uint16_t prim_pm_set_sig_strength(struct l1_model_ms *ms, uint16_t arfcn, int16_t sig_lev);
 
+/* determine if a received Downlink RLC/MAC block matches the current MS configuration */
+static bool gprs_dl_block_matches_ms(struct l1_model_ms *ms, struct msgb *msg, uint8_t timeslot)
+{
+	uint8_t payload_type;
+	uint8_t tfi;
+
+	if (msgb_length(msg) < 1)
+		return false;
+
+	/* FIXME: Ensure this will also work for EGPRS!  */
+	payload_type = msg->data[0] >> 6;
+	switch (payload_type) {
+	case 0: /* RLC Data Block */
+		/* forward all RLD Data Blocks destined for TFI of MS */
+		tfi = (msg->data[1] >> 1) & 0x1f;
+		if (ms->state.state == MS_STATE_TBF && ms->state.tbf.dl.tfi[timeslot] == tfi)
+			return true;
+		break;
+	case 1: /* RLC/MAC Control without optional octets */
+		/* forward all RLC/MAC control blocks without optional octets, i.e. not adressed
+		 * to a specific TFI */
+		return true;
+	case 2: /* RLC/MAC with optional control octets */
+		/* forward all RLD Control Blocks destined for TFI of MS */
+		tfi = (msg->data[2] >> 1) & 0x1f;
+		if (ms->state.state == MS_STATE_TBF && ms->state.tbf.dl.tfi[timeslot] == tfi)
+			return true;
+		break;
+	default:
+		break;
+	}
+	return false;
+}
+
+/* determine if given USF at given timeslot is relevant to given MS or not */
+static bool usf_matches_ms(struct l1_model_ms *ms, uint8_t usf, uint8_t timeslot)
+{
+	if (ms->state.state == MS_STATE_TBF && ms->state.tbf.ul.usf[timeslot] == usf)
+		return true;
+
+	return false;
+}
+
+/* extract USF from (E)GPRS RLC/MAC block */
+static uint8_t get_usf_from_block(struct msgb *msg)
+{
+	/* FIXME: Ensure this will also work for EGPRS!  */
+	return msg->data[0] & 0x7;
+}
+
+/* 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 USF is not for us, bail out */
+	if (!usf_matches_ms(ms, usf, timeslot))
+		return;
+
+	/* attempt to de-queue pending msgb for this UL TBF and transmit it */
+	msg = msgb_dequeue(&ms->state.tbf.ul.tx_queue);
+	if (!msg) {
+		printf("FN=%u, TN=%u, USF=%u: empty tx_queue, not transmitting\n", fn, timeslot, usf);
+		/* FIXME: send some dummy control frame? */
+	} else {
+		printf("FN=%u, TN=%u, USF=%u: transmitting queued msg\n", fn, timeslot, usf);
+		gsmtapl1_tx_to_virt_um_inst(ms, fn, timeslot, msg);
+	}
+}
+
 static void l1ctl_from_virt_um(struct l1ctl_sock_client *lsc, struct msgb *msg, uint32_t fn,
 				uint16_t arfcn, uint8_t timeslot, uint8_t subslot,
 				uint8_t gsmtap_chantype, uint8_t chan_nr, uint8_t link_id,
@@ -111,6 +194,7 @@ static void l1ctl_from_virt_um(struct l1ctl_sock_client *lsc, struct msgb *msg,
 {
 	struct l1_model_ms *ms = lsc->priv;
 	uint8_t signal_dbm = dbm2rxlev(prim_pm_set_sig_strength(ms, arfcn & GSMTAP_ARFCN_MASK, MAX_SIG_LEV_DBM));	/* Power measurement with each received massage */
+	uint8_t usf;
 
 	gsm_fn2gsmtime(&ms->state.downlink_time, fn);
 
@@ -163,10 +247,15 @@ static void l1ctl_from_virt_um(struct l1ctl_sock_client *lsc, struct msgb *msg,
 	case GSMTAP_CHANNEL_RACH:
 		LOGPMS(DVIRPHY, LOGL_NOTICE, ms, "Ignoring unexpected RACH in downlink ?!?\n");
 		break;
-	case GSMTAP_CHANNEL_SDCCH:
-	case GSMTAP_CHANNEL_CCCH:
 	case GSMTAP_CHANNEL_PACCH:
 	case GSMTAP_CHANNEL_PDCH:
+		if (gprs_dl_block_matches_ms(ms, msg, timeslot))
+			l1ctl_tx_data_ind(ms, msg, arfcn, link_id, chan_nr, fn, snr_db, signal_dbm, 0, 0);
+		usf = get_usf_from_block(msg);
+		ms_ul_tbf_may_transmit(ms, arfcn, timeslot, fn, usf);
+		break;
+	case GSMTAP_CHANNEL_SDCCH:
+	case GSMTAP_CHANNEL_CCCH:
 	case GSMTAP_CHANNEL_PTCCH:
 		LOGPMS(DVIRPHY, LOGL_NOTICE, ms, "Ignoring unsupported channel type %s\n",
 			get_value_string(gsmtap_gsm_channel_names, gsmtap_chantype));
diff --git a/src/host/virt_phy/src/l1ctl_sap.c b/src/host/virt_phy/src/l1ctl_sap.c
index d4b33f12..aac49bf0 100644
--- a/src/host/virt_phy/src/l1ctl_sap.c
+++ b/src/host/virt_phy/src/l1ctl_sap.c
@@ -27,6 +27,7 @@
 #include <osmocom/gsm/protocol/gsm_08_58.h>
 #include <osmocom/gsm/protocol/gsm_04_08.h>
 #include <osmocom/gsm/rsl.h>
+#include <osmocom/gprs/gprs_rlc.h>
 #include <stdio.h>
 #include <l1ctl_proto.h>
 #include <netinet/in.h>
@@ -39,6 +40,9 @@
 #include <virtphy/logging.h>
 #include <virtphy/virt_l1_sched.h>
 
+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 l1_model_tch_mode_set(struct l1_model_ms *ms, uint8_t tch_mode)
 {
 	if (tch_mode == GSM48_CMODE_SPEECH_V1 || tch_mode == GSM48_CMODE_SPEECH_EFR)
@@ -55,6 +59,8 @@ static void l1_model_tch_mode_set(struct l1_model_ms *ms, uint8_t tch_mode)
 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);
+
 	prim_pm_init(model);
 }
 
@@ -156,6 +162,8 @@ static bool is_l1ctl_control(uint8_t msg_type)
 	switch (msg_type) {
 	case L1CTL_DATA_REQ:
 	case L1CTL_DATA_CONF:
+	case L1CTL_DATA_TBF_REQ:
+	case L1CTL_DATA_TBF_CONF:
 	case L1CTL_TRAFFIC_REQ:
 	case L1CTL_TRAFFIC_CONF:
 	case L1CTL_TRAFFIC_IND:
@@ -238,6 +246,12 @@ void l1ctl_sap_handler(struct l1_model_ms *ms, struct msgb *msg)
 	case L1CTL_SIM_REQ:
 		l1ctl_rx_sim_req(ms, msg);
 		break;
+	case L1CTL_TBF_CFG_REQ:
+		l1ctl_rx_tbf_cfg_req(ms, msg);
+		break;
+	case L1CTL_DATA_TBF_REQ:
+		l1ctl_rx_data_tbf_req(ms, msg);
+		goto exit_nofree;
 	}
 
 exit_msgbfree:
@@ -529,12 +543,143 @@ void l1ctl_rx_sim_req(struct l1_model_ms *ms, struct msgb *msg)
 
 }
 
+static void l1ctl_tx_tbf_cfg_conf(struct l1_model_ms *ms, const struct l1ctl_tbf_cfg_req *in);
+
+static void l1ctl_rx_tbf_cfg_req(struct l1_model_ms *ms, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->data;
+	struct l1ctl_tbf_cfg_req *cfg_req = (struct l1ctl_tbf_cfg_req *) l1h->data;
+	unsigned int i;
+
+	LOGPMS(DL1C, LOGL_INFO, ms, "Rx L1CTL_TBF_CFG_REQ (tbf_id=%u, dir=%s, "
+		"usf=[%d,%d,%d,%d,%d,%d,%d,%d]\n", cfg_req->tbf_nr,
+		cfg_req->is_uplink ? "UL" : "DL", cfg_req->usf[0], cfg_req->usf[1],
+		cfg_req->usf[2], cfg_req->usf[3], cfg_req->usf[4], cfg_req->usf[5],
+		cfg_req->usf[6], cfg_req->usf[7]);
+
+	if (cfg_req->tbf_nr != 0) {
+		LOGPMS(DL1C, LOGL_ERROR, ms, "TBF_NR != 0 not supported yet!\n");
+		return;
+	}
+
+	if (ms->state.state == MS_STATE_DEDICATED)
+		LOGPMS(DL1C, LOGL_NOTICE, ms, "Harrd termiation of DEDICATED mode, fix L23!\n");
+
+	if (cfg_req->is_uplink) {
+		for (i = 0; i < 8; i++)
+			ms->state.tbf.ul.usf[i] = cfg_req->usf[i];
+	} else {
+		for (i = 0; i < 8; i++)
+			ms->state.tbf.dl.tfi[i] = cfg_req->usf[i];
+	}
+	ms->state.state = MS_STATE_TBF;
+
+	l1ctl_tx_tbf_cfg_conf(ms, cfg_req);
+}
+
+static const enum osmo_gprs_cs osmo_cs_by_l1ctl[] = {
+	[L1CTL_CS_NONE]	= OSMO_GPRS_CS_NONE,
+	[L1CTL_CS1]	= OSMO_GPRS_CS1,
+	[L1CTL_CS2]	= OSMO_GPRS_CS2,
+	[L1CTL_CS3]	= OSMO_GPRS_CS3,
+	[L1CTL_CS4]	= OSMO_GPRS_CS4,
+	[L1CTL_MCS1]	= OSMO_GPRS_MCS1,
+	[L1CTL_MCS2]	= OSMO_GPRS_MCS2,
+	[L1CTL_MCS3]	= OSMO_GPRS_MCS3,
+	[L1CTL_MCS4]	= OSMO_GPRS_MCS4,
+	[L1CTL_MCS5]	= OSMO_GPRS_MCS5,
+	[L1CTL_MCS6]	= OSMO_GPRS_MCS6,
+	[L1CTL_MCS7]	= OSMO_GPRS_MCS7,
+	[L1CTL_MCS8]	= OSMO_GPRS_MCS8,
+	[L1CTL_MCS9]	= OSMO_GPRS_MCS9,
+};
+
+static int get_osmo_cs_by_l1ctl(enum l1ctl_coding_scheme l1)
+{
+	if (l1 >= ARRAY_SIZE(osmo_cs_by_l1ctl))
+		return -1;
+	return osmo_cs_by_l1ctl[l1];
+}
+
+static const enum l1ctl_coding_scheme l1ctl_cs_by_osmo[] = {
+	[OSMO_GPRS_CS_NONE]	= L1CTL_CS_NONE,
+	[OSMO_GPRS_CS1]		= L1CTL_CS1,
+	[OSMO_GPRS_CS2]		= L1CTL_CS2,
+	[OSMO_GPRS_CS3]		= L1CTL_CS3,
+	[OSMO_GPRS_CS4]		= L1CTL_CS4,
+	[OSMO_GPRS_MCS1]	= L1CTL_MCS1,
+	[OSMO_GPRS_MCS2]	= L1CTL_MCS2,
+	[OSMO_GPRS_MCS3]	= L1CTL_MCS3,
+	[OSMO_GPRS_MCS4]	= L1CTL_MCS4,
+	[OSMO_GPRS_MCS5]	= L1CTL_MCS5,
+	[OSMO_GPRS_MCS6]	= L1CTL_MCS6,
+	[OSMO_GPRS_MCS7]	= L1CTL_MCS7,
+	[OSMO_GPRS_MCS8]	= L1CTL_MCS8,
+	[OSMO_GPRS_MCS9]	= L1CTL_MCS9,
+};
+
+static int get_l1ctl_cs_by_osmo(enum osmo_gprs_cs in)
+{
+	if (in >= ARRAY_SIZE(l1ctl_cs_by_osmo))
+		return -1;
+	return l1ctl_cs_by_osmo[in];
+}
+
+static void l1ctl_rx_data_tbf_req(struct l1_model_ms *ms, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->data;
+	struct l1ctl_info_ul_tbf *udt = (struct l1ctl_info_ul_tbf *) l1h->data;
+	enum osmo_gprs_cs osmo_cs;
+	int block_size;
+
+	msg->l2h = udt->payload;
+
+	LOGPMS(DL1P, LOGL_ERROR, ms, "Rx L1CTL_DATA_TBF_REQ (tbf_id=%d, data=%s)\n",
+		udt->tbf_nr, 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_TBF_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);
+	}
+
+	msgb_enqueue(&ms->state.tbf.ul.tx_queue, msg);
+}
+
 /***************************************************************
  * L1CTL TX ROUTINES *******************************************
  * For more routines check the respective handler classes ******
  * like virt_prim_rach.c ***************************************
  ***************************************************************/
 
+static void l1ctl_tx_tbf_cfg_conf(struct l1_model_ms *ms, const struct l1ctl_tbf_cfg_req *in)
+{
+	struct msgb *msg = l1ctl_msgb_alloc(L1CTL_TBF_CFG_CONF);
+	struct l1ctl_tbf_cfg_req *out;
+
+	/* copy over the data from the request */
+	out = (struct l1ctl_tbf_cfg_req *) msgb_put(msg, sizeof(*out));
+	*out = *in;
+
+	l1ctl_sap_tx_to_l23_inst(ms, msg);
+}
+
 /**
  * @brief Transmit L1CTL_RESET_IND or L1CTL_RESET_CONF to layer 23.
  *