VIRT-PHY: Fixed power management and PCS sync.

PCS flag was ignored in fbsb. Power management returned a perfect link
for all possible arfcns causing the mobile trying to sync to all these
afterwards. This took too long and PM now only returnes a good link
quality for arfcns configured as available.

Power management was also extracted to an own file.

Change-Id: Ia1b79aa47c9df3b1e316122455ceccb4a66724e0

6 files changed, 127 insertions, 82 deletions

diff --git a/src/host/virt_phy/include/virtphy/l1ctl_sap.h b/src/host/virt_phy/include/virtphy/l1ctl_sap.h
index 71d47659..a1261287 100644
--- a/src/host/virt_phy/include/virtphy/l1ctl_sap.h
+++ b/src/host/virt_phy/include/virtphy/l1ctl_sap.h
@@ -23,6 +23,7 @@ void prim_rach_init(struct l1_model_ms *model);
 void prim_data_init(struct l1_model_ms *model);
 void prim_traffic_init(struct l1_model_ms *model);
 void prim_fbsb_init(struct l1_model_ms *model);
+void prim_pm_init(struct l1_model_ms *model);
 void l1ctl_sap_tx_to_l23_inst(struct l1ctl_sock_inst *lsi, struct msgb *msg);
 void l1ctl_sap_tx_to_l23(struct msgb *msg);
 void l1ctl_sap_rx_from_l23_inst_cb(struct l1ctl_sock_inst *lsi,
diff --git a/src/host/virt_phy/src/Makefile.am b/src/host/virt_phy/src/Makefile.am
index ae14ef6f..d34b8adc 100644
--- a/src/host/virt_phy/src/Makefile.am
+++ b/src/host/virt_phy/src/Makefile.am
@@ -4,7 +4,7 @@ AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include  -I$(top_srcdir)/../layer2
 CFLAGS = "-g -O0"
 
 sbin_PROGRAMS = virtphy
-virtphy_SOURCES = virtphy.c l1ctl_sock.c gsmtapl1_if.c l1ctl_sap.c virt_prim_fbsb.c virt_prim_rach.c virt_prim_data.c virt_prim_traffic.c virt_l1_sched_simple.c logging.c virt_l1_model.c shared/virtual_um.c shared/osmo_mcast_sock.c
+virtphy_SOURCES = virtphy.c l1ctl_sock.c gsmtapl1_if.c l1ctl_sap.c virt_prim_pm.c virt_prim_fbsb.c virt_prim_rach.c virt_prim_data.c virt_prim_traffic.c virt_l1_sched_simple.c logging.c virt_l1_model.c shared/virtual_um.c shared/osmo_mcast_sock.c
 virtphy_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) 
 virtphy_LDFLAGS = -pthread
 
diff --git a/src/host/virt_phy/src/gsmtapl1_if.c b/src/host/virt_phy/src/gsmtapl1_if.c
index 3cb0df91..155b4dc5 100644
--- a/src/host/virt_phy/src/gsmtapl1_if.c
+++ b/src/host/virt_phy/src/gsmtapl1_if.c
@@ -166,12 +166,6 @@ void gsmtapl1_rx_from_virt_um_inst_cb(struct virt_um_inst *vui,
 		return;
 	}
 
-	// forward msg to fbsb sync routine if we are in sync state
-	if (l1_model_ms->state->state == MS_STATE_IDLE_SYNCING) {
-		prim_fbsb_sync(msg);
-		return;
-	}
-
 	struct gsmtap_hdr *gh = msgb_l1(msg);
 	struct msgb *l1ctl_msg = NULL;
 	uint32_t fn = ntohl(gh->frame_number); // frame number of the rcv msg
@@ -185,6 +179,29 @@ void gsmtapl1_rx_from_virt_um_inst_cb(struct virt_um_inst *vui,
 	uint8_t link_id; // rsl link id tells if this is an ssociated or dedicated link
 	uint8_t chan_nr; // encoded rsl channel type, timeslot and mf subslot
 
+	// generally ignore all uplink messages received
+	if (arfcn & GSMTAP_ARFCN_F_UPLINK) {
+		LOGP(DVIRPHY, LOGL_NOTICE,
+		     "Ignoring gsmtap msg from virt um - uplink flag set!\n");
+		goto nomessage;
+	}
+
+	// forward downlink msg to fbsb sync routine if we are in sync state
+	if (l1_model_ms->state->state == MS_STATE_IDLE_SYNCING) {
+		prim_fbsb_sync(msg);
+		return;
+	}
+
+	// generally ignore all messages coming from another arfcn than the camped one
+	if (l1_model_ms->state->serving_cell.arfcn != arfcn) {
+		LOGP(DVIRPHY,
+		     LOGL_NOTICE,
+		     "Ignoring gsmtap msg from virt um - msg arfcn=%d not equal synced arfcn=%d!\n",
+		     arfcn,
+		     l1_model_ms->state->serving_cell.arfcn);
+		goto nomessage;
+	}
+
 	msg->l2h = msgb_pull(msg, sizeof(*gh));
 	chantype_gsmtap2rsl(gsmtap_chantype, &rsl_chantype, &link_id);
 	// see GSM 8.58 -> 9.3.1 for channel number encoding
@@ -200,23 +217,6 @@ void gsmtapl1_rx_from_virt_um_inst_cb(struct virt_um_inst *vui,
 	       get_value_string(gsmtap_channels, gsmtap_chantype), timeslot,
 	       subslot, rsl_chantype, link_id, chan_nr);
 
-	// generally ignore all messages coming from another arfcn than the camped one
-	if (l1_model_ms->state->serving_cell.arfcn != (arfcn & GSMTAP_ARFCN_MASK)) {
-		LOGP(DVIRPHY,
-		     LOGL_NOTICE,
-		     "Ignoring gsmtap msg from virt um - msg arfcn=%d not equal synced arfcn=%d!\n",
-		     arfcn & GSMTAP_ARFCN_MASK,
-		     l1_model_ms->state->serving_cell.arfcn);
-		goto nomessage;
-	}
-
-	// generally ignore all uplink messages received
-	if (arfcn & GSMTAP_ARFCN_F_UPLINK) {
-		LOGP(DVIRPHY, LOGL_NOTICE,
-		     "Ignoring gsmtap msg from virt um - uplink flag set!\n");
-		goto nomessage;
-	}
-
 	// switch case with removed acch flag
 	switch (gsmtap_chantype & ~GSMTAP_CHANNEL_ACCH & 0xff) {
 	case GSMTAP_CHANNEL_TCH_H:
diff --git a/src/host/virt_phy/src/l1ctl_sap.c b/src/host/virt_phy/src/l1ctl_sap.c
index c03da3b1..bd6051be 100644
--- a/src/host/virt_phy/src/l1ctl_sap.c
+++ b/src/host/virt_phy/src/l1ctl_sap.c
@@ -377,60 +377,6 @@ void l1ctl_rx_param_req(struct msgb *msg)
 }
 
 /**
- * @brief Handler for received L1CTL_PM_REQ from L23.
- *
- * -- power measurement request --
- *
- * @param [in] msg the received message.
- *
- * Process power measurement for a given range of arfcns to calculate signal power and connection quality.
- *
- * Note: We do not need to calculate that for the virtual physical layer,
- * but l23 apps can expect a response. So this response is mocked here.
- * TODO: Might be possible to sync to different virtual BTS. Mapping from arfcn to mcast address would be needed. Configurable rx_lev for each mcast address.
- */
-void l1ctl_rx_pm_req(struct msgb *msg)
-{
-	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *)msg->data;
-	struct l1ctl_pm_req *pm_req = (struct l1ctl_pm_req *)l1h->data;
-	struct msgb *resp_msg = l1ctl_msgb_alloc(L1CTL_PM_CONF);
-	uint16_t arfcn_next;
-	// convert to host order
-	pm_req->range.band_arfcn_from = ntohs(pm_req->range.band_arfcn_from);
-	pm_req->range.band_arfcn_to = ntohs(pm_req->range.band_arfcn_to);
-
-	DEBUGP(DL1C,
-	                "Received from l23 - L1CTL_PM_REQ TYPE=%u, FROM=%d, TO=%d\n",
-	                pm_req->type, pm_req->range.band_arfcn_from,
-	                pm_req->range.band_arfcn_to);
-
-	for (arfcn_next = pm_req->range.band_arfcn_from;
-	                arfcn_next <= pm_req->range.band_arfcn_to;
-	                ++arfcn_next) {
-		struct l1ctl_pm_conf *pm_conf =
-		                (struct l1ctl_pm_conf *)msgb_put(resp_msg,
-		                                sizeof(*pm_conf));
-		pm_conf->band_arfcn = htons(arfcn_next);
-		// rxlev 63 is great, 0 is bad the two values are min and max
-		pm_conf->pm[0] = 63;
-		pm_conf->pm[1] = 63;
-		if (arfcn_next == pm_req->range.band_arfcn_to) {
-			struct l1ctl_hdr *resp_l1h = msgb_l1(resp_msg);
-			resp_l1h->flags |= L1CTL_F_DONE;
-		}
-		// no more space to hold mor pm info in msgb, flush to l23
-		if (msgb_tailroom(resp_msg) < sizeof(*pm_conf)) {
-			l1ctl_sap_tx_to_l23(resp_msg);
-			resp_msg = l1ctl_msgb_alloc(L1CTL_PM_CONF);
-		}
-	}
-	// transmit the remaining part of pm response to l23
-	if (resp_msg) {
-		l1ctl_sap_tx_to_l23(resp_msg);
-	}
-}
-
-/**
  * @brief Handler for received L1CTL_RESET_REQ from L23.
  *
  * -- reset request --
diff --git a/src/host/virt_phy/src/virt_prim_fbsb.c b/src/host/virt_phy/src/virt_prim_fbsb.c
index 6a678d08..f0c7f528 100644
--- a/src/host/virt_phy/src/virt_prim_fbsb.c
+++ b/src/host/virt_phy/src/virt_prim_fbsb.c
@@ -13,6 +13,7 @@
 #include <l1ctl_proto.h>
 
 static struct l1_model_ms *l1_model_ms = NULL;
+static uint16_t sync_count = 0;
 
 /**
  * @brief Handler for received L1CTL_FBSB_REQ from L23.
@@ -53,11 +54,17 @@ void prim_fbsb_sync(struct msgb *msg)
 	uint16_t arfcn = ntohs(gh->arfcn); // arfcn of the received msg
 
 	// ignore messages from other arfcns as the one requested to sync to by l23
-	if (l1_model_ms->state->fbsb.arfcn != (arfcn & GSMTAP_ARFCN_MASK)) {
+	if (l1_model_ms->state->fbsb.arfcn != arfcn) {
 		talloc_free(msg);
+		// cancel sync if we did not receive a msg on dl from the requested arfcn that we can sync to
+		if(sync_count++ > 100) {
+			sync_count = 0;
+			l1_model_ms->state->state = MS_STATE_IDLE_SEARCHING;
+			l1ctl_tx_fbsb_conf(1, (l1_model_ms->state->fbsb.arfcn));
+		}
 		return;
 	}
-	l1_model_ms->state->serving_cell.arfcn = (arfcn & GSMTAP_ARFCN_MASK);
+	l1_model_ms->state->serving_cell.arfcn = arfcn;
 	l1_model_ms->state->state = MS_STATE_IDLE_CAMPING;
 	/* Not needed in virtual phy */
 	l1_model_ms->state->serving_cell.fn_offset = 0;
@@ -68,7 +75,7 @@ void prim_fbsb_sync(struct msgb *msg)
 	/* Restart scheduler */
 	virt_l1_sched_restart(l1_model_ms->state->downlink_time);
 	talloc_free(msg);
-	l1ctl_tx_fbsb_conf(0, (arfcn & GSMTAP_ARFCN_MASK));
+	l1ctl_tx_fbsb_conf(0, arfcn);
 }
 
 /**
@@ -103,7 +110,7 @@ void l1ctl_tx_fbsb_conf(uint8_t res, uint16_t arfcn)
 	l1ctl_sap_tx_to_l23(msg);
 }
 /**
- * @brief Initialize virtual prim rach.
+ * @brief Initialize virtual prim pm.
  *
  * @param [in] model the l1 model instance
  */
diff --git a/src/host/virt_phy/src/virt_prim_pm.c b/src/host/virt_phy/src/virt_prim_pm.c
new file mode 100644
index 00000000..c08ce03b
--- /dev/null
+++ b/src/host/virt_phy/src/virt_prim_pm.c
@@ -0,0 +1,91 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <osmocom/gsm/gsm_utils.h>
+#include <osmocom/gsm/protocol/gsm_08_58.h>
+#include <osmocom/core/msgb.h>
+#include <virtphy/l1ctl_sap.h>
+#include <virtphy/virt_l1_sched.h>
+#include <osmocom/core/gsmtap.h>
+#include <virtphy/logging.h>
+#include <l1ctl_proto.h>
+
+static struct l1_model_ms *l1_model_ms = NULL;
+// FIXME: ugly to configure that in code. Either make a config file or change power selection to automatically check which arfcns can be received.
+static uint16_t available_arfcns[] = {666};
+
+/**
+ * @brief Handler for received L1CTL_PM_REQ from L23.
+ *
+ * -- power measurement request --
+ *
+ * @param [in] msg the received message.
+ *
+ * Process power measurement for a given range of arfcns to calculate signal power and connection quality.
+ *
+ * Note: We do not need to calculate that for the virtual physical layer,
+ * but l23 apps can expect a response. So this response is mocked here.
+ * For available arfcns we always return a perfect link quality, for all other the worst.
+ *
+ * TODO: Change PM so that we check the downlink first for for some time to get the arfcns we receive. Then return a good link for that and a bad for all others.
+ */
+void l1ctl_rx_pm_req(struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h = (struct l1ctl_hdr *)msg->data;
+	struct l1ctl_pm_req *pm_req = (struct l1ctl_pm_req *)l1h->data;
+	struct msgb *resp_msg = l1ctl_msgb_alloc(L1CTL_PM_CONF);
+	uint16_t arfcn_next;
+	// convert to host order
+	pm_req->range.band_arfcn_from = ntohs(pm_req->range.band_arfcn_from);
+	pm_req->range.band_arfcn_to = ntohs(pm_req->range.band_arfcn_to);
+
+	DEBUGP(DL1C,
+	                "Received from l23 - L1CTL_PM_REQ TYPE=%u, FROM=%d, TO=%d\n",
+	                pm_req->type, pm_req->range.band_arfcn_from,
+	                pm_req->range.band_arfcn_to);
+
+	for (arfcn_next = pm_req->range.band_arfcn_from;
+	                arfcn_next <= pm_req->range.band_arfcn_to;
+	                ++arfcn_next) {
+		struct l1ctl_pm_conf *pm_conf =
+		                (struct l1ctl_pm_conf *)msgb_put(resp_msg,
+		                                sizeof(*pm_conf));
+		int cnt, available = 0;
+		pm_conf->band_arfcn = htons(arfcn_next);
+		// check if arfcn is available
+		for(cnt = 0; cnt < sizeof(available_arfcns) / sizeof(uint16_t); cnt++) {
+			if(arfcn_next == available_arfcns[cnt]) {
+				available = 1;
+				break;
+			}
+		}
+		// rxlev 63 is great, 0 is bad the two values are min and max
+		pm_conf->pm[0] = available ? 63 : 0;
+		pm_conf->pm[1] = available ? 63 : 0;
+		if (arfcn_next == pm_req->range.band_arfcn_to) {
+			struct l1ctl_hdr *resp_l1h = msgb_l1(resp_msg);
+			resp_l1h->flags |= L1CTL_F_DONE;
+		}
+		// no more space to hold mor pm info in msgb, flush to l23
+		if (msgb_tailroom(resp_msg) < sizeof(*pm_conf)) {
+			l1ctl_sap_tx_to_l23(resp_msg);
+			resp_msg = l1ctl_msgb_alloc(L1CTL_PM_CONF);
+		}
+	}
+	// transmit the remaining part of pm response to l23
+	if (resp_msg) {
+		l1ctl_sap_tx_to_l23(resp_msg);
+	}
+}
+
+/**
+ * @brief Initialize virtual prim pm.
+ *
+ * @param [in] model the l1 model instance
+ */
+void prim_pm_init(struct l1_model_ms *model)
+{
+	l1_model_ms = model;
+}