host/trxcon/scheduler: fix chan_nr parsing

Previously, the chan_nr, received from L2, was parsed in a wrong
way, so in some cases only one logical channel was activated or
some messages (such as Measurement Requests) were sent on
incorrect channel (e.g. on SDCCH instead of SACCH).

This change reimplements the sched_trx_chan_nr2lchan_type(), and
introduces a new function sched_trx_set_lchans(), whics will
parse chan_nr and (de)activate required channels.

Change-Id: I480311c65ef93bbc1644ec708dd2a68fd33091e0

3 files changed, 64 insertions, 27 deletions

diff --git a/src/host/trxcon/l1ctl.c b/src/host/trxcon/l1ctl.c
index b6bc404a..ed30205f 100644
--- a/src/host/trxcon/l1ctl.c
+++ b/src/host/trxcon/l1ctl.c
@@ -486,7 +486,6 @@ exit:
 static int l1ctl_rx_dm_est_req(struct l1ctl_link *l1l, struct msgb *msg)
 {
 	enum gsm_phys_chan_config config;
-	enum trx_lchan_type lchan_type;
 	struct l1ctl_dm_est_req *est_req;
 	struct l1ctl_info_ul *ul;
 	struct trx_ts *ts;
@@ -529,14 +528,6 @@ static int l1ctl_rx_dm_est_req(struct l1ctl_link *l1l, struct msgb *msg)
 		goto exit;
 	}
 
-	/* Determine lchan type */
-	lchan_type = sched_trx_chan_nr2lchan_type(chan_nr);
-	if (!lchan_type) {
-		LOGP(DL1C, LOGL_ERROR, "Couldn't determine lchan type\n");
-		rc = -EINVAL;
-		goto exit;
-	}
-
 	/* Configure requested TS */
 	rc = sched_trx_configure_ts(l1l->trx, tn, config);
 	ts = l1l->trx->ts_list[tn];
@@ -545,11 +536,13 @@ static int l1ctl_rx_dm_est_req(struct l1ctl_link *l1l, struct msgb *msg)
 		goto exit;
 	}
 
-	/* Activate only requested lchan, disabling others */
+	/* Deactivate all lchans */
 	sched_trx_deactivate_all_lchans(ts);
-	rc = sched_trx_activate_lchan(ts, lchan_type);
+
+	/* Activate only requested lchans */
+	rc = sched_trx_set_lchans(ts, chan_nr, 1);
 	if (rc) {
-		LOGP(DL1C, LOGL_ERROR, "Couldn't activate lchan\n");
+		LOGP(DL1C, LOGL_ERROR, "Couldn't activate requested lchans\n");
 		rc = -EINVAL;
 		goto exit;
 	}
@@ -578,15 +571,19 @@ static int l1ctl_rx_data_req(struct l1ctl_link *l1l, struct msgb *msg)
 	struct l1ctl_info_ul *ul;
 	struct l1ctl_data_ind *data_ind;
 	enum trx_lchan_type lchan_type;
-	uint8_t chan_nr, tn;
+	uint8_t chan_nr, link_id, tn;
 	size_t len;
 	int rc = 0;
 
 	ul = (struct l1ctl_info_ul *) msg->l1h;
 	data_ind = (struct l1ctl_data_ind *) ul->payload;
+
+	/* Obtain channel description */
 	chan_nr = ul->chan_nr;
+	link_id = ul->link_id & 0x40;
 
-	LOGP(DL1C, LOGL_DEBUG, "Recv Data Req (chan_nr=0x%02x)\n", chan_nr);
+	LOGP(DL1C, LOGL_DEBUG, "Recv Data Req (chan_nr=0x%02x, "
+		"link_id=0x%02x)\n", chan_nr, link_id);
 
 	/* Determine TS index */
 	tn = chan_nr & 0x7;
@@ -597,9 +594,10 @@ static int l1ctl_rx_data_req(struct l1ctl_link *l1l, struct msgb *msg)
 	}
 
 	/* Determine lchan type */
-	lchan_type = sched_trx_chan_nr2lchan_type(chan_nr);
+	lchan_type = sched_trx_chan_nr2lchan_type(chan_nr, link_id);
 	if (!lchan_type) {
-		LOGP(DL1C, LOGL_ERROR, "Couldn't determine lchan type\n");
+		LOGP(DL1C, LOGL_ERROR, "Couldn't determine lchan type "
+			"for chan_nr=%02x and link_id=%02x\n", chan_nr, link_id);
 		rc = -EINVAL;
 		goto exit;
 	}
diff --git a/src/host/trxcon/sched_trx.c b/src/host/trxcon/sched_trx.c
index 76112f1a..40d14465 100644
--- a/src/host/trxcon/sched_trx.c
+++ b/src/host/trxcon/sched_trx.c
@@ -296,6 +296,35 @@ struct trx_lchan_state *sched_trx_find_lchan(struct trx_ts *ts,
 	return NULL;
 }
 
+int sched_trx_set_lchans(struct trx_ts *ts, uint8_t chan_nr, int active)
+{
+	const struct trx_lchan_desc *lchan_desc;
+	struct trx_lchan_state *lchan;
+	int len, i, rc = 0;
+
+	/* Prevent NULL-pointer deference */
+	if (ts == NULL || ts->lchans == NULL) {
+		LOGP(DSCH, LOGL_ERROR, "Timeslot isn't configured\n");
+		return -EINVAL;
+	}
+
+	/* Iterate over all allocated lchans */
+	len = talloc_array_length(ts->lchans);
+	for (i = 0; i < len; i++) {
+		lchan = ts->lchans + i;
+		lchan_desc = &trx_lchan_desc[lchan->type];
+
+		if (lchan_desc->chan_nr == (chan_nr & 0xf8)) {
+			if (active)
+				rc |= sched_trx_activate_lchan(ts, lchan->type);
+			else
+				rc |= sched_trx_deactivate_lchan(ts, lchan->type);
+		}
+	}
+
+	return rc;
+}
+
 int sched_trx_activate_lchan(struct trx_ts *ts, enum trx_lchan_type chan)
 {
 	const struct trx_lchan_desc *lchan_desc = &trx_lchan_desc[chan];
@@ -312,6 +341,9 @@ int sched_trx_activate_lchan(struct trx_ts *ts, enum trx_lchan_type chan)
 		return -EINVAL;
 	}
 
+	LOGP(DSCH, LOGL_NOTICE, "Activating lchan=%s "
+		"on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
+
 	/* Conditionally allocate memory for bursts */
 	if (lchan_desc->rx_fn && lchan_desc->burst_buf_size > 0) {
 		lchan->rx_bursts = talloc_zero_size(ts->lchans,
@@ -348,6 +380,9 @@ int sched_trx_deactivate_lchan(struct trx_ts *ts, enum trx_lchan_type chan)
 		return -EINVAL;
 	}
 
+	LOGP(DSCH, LOGL_DEBUG, "Deactivating lchan=%s "
+		"on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
+
 	/* Free memory */
 	talloc_free(lchan->rx_bursts);
 	talloc_free(lchan->tx_bursts);
@@ -362,6 +397,9 @@ void sched_trx_deactivate_all_lchans(struct trx_ts *ts)
 	struct trx_lchan_state *lchan;
 	int i, len;
 
+	LOGP(DSCH, LOGL_DEBUG, "Deactivating all logical channels "
+		"on ts=%d\n", ts->index);
+
 	len = talloc_array_length(ts->lchans);
 	for (i = 0; i < len; i++) {
 		lchan = ts->lchans + i;
@@ -389,18 +427,16 @@ enum gsm_phys_chan_config sched_trx_chan_nr2pchan_config(uint8_t chan_nr)
 	return GSM_PCHAN_NONE;
 }
 
-enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr)
+enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr,
+	uint8_t link_id)
 {
-	uint8_t cbits = chan_nr >> 3;
+	int i;
 
-	if (cbits == 0x01)
-		return TRXC_TCHF;
-	else if ((cbits & 0x1e) == 0x02)
-		return TRXC_TCHH_0 + (cbits & 0x1);
-	else if ((cbits & 0x1c) == 0x04)
-		return TRXC_SDCCH4_0 + (cbits & 0x3);
-	else if ((cbits & 0x18) == 0x08)
-		return TRXC_SDCCH8_0 + (cbits & 0x7);
+	/* Iterate over all known lchan types */
+	for (i = 0; i < _TRX_CHAN_MAX; i++)
+		if (trx_lchan_desc[i].chan_nr == (chan_nr & 0xf8))
+			if (trx_lchan_desc[i].link_id == link_id)
+				return i;
 
 	return TRXC_IDLE;
 }
diff --git a/src/host/trxcon/sched_trx.h b/src/host/trxcon/sched_trx.h
index e733de8f..7ebfa152 100644
--- a/src/host/trxcon/sched_trx.h
+++ b/src/host/trxcon/sched_trx.h
@@ -258,8 +258,11 @@ int sched_trx_configure_ts(struct trx_instance *trx, int tn,
 
 /* Logical channel management functions */
 enum gsm_phys_chan_config sched_trx_chan_nr2pchan_config(uint8_t chan_nr);
-enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr);
+enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr,
+	uint8_t link_id);
+
 void sched_trx_deactivate_all_lchans(struct trx_ts *ts);
+int sched_trx_set_lchans(struct trx_ts *ts, uint8_t chan_nr, int active);
 int sched_trx_activate_lchan(struct trx_ts *ts, enum trx_lchan_type chan);
 int sched_trx_deactivate_lchan(struct trx_ts *ts, enum trx_lchan_type chan);
 struct trx_lchan_state *sched_trx_find_lchan(struct trx_ts *ts,