trxcon/l1sched: fix handling of UL FACCH on TCH/A[FH]S

In ad8f7794 I changed both tx_tch[fh]_fn() to use a switch statement
and introduced a regression by removing special treatment of FACCH:

@@ -238,10 +237,16 @@ int tx_tchf_fn(struct l1sched_lchan_state *lchan,
-       if (msgb_l2len(lchan->prim) == GSM_MACBLOCK_LEN) {
-               /* Encode payload */
-               rc = gsm0503_tch_fr_encode(buffer, msgb_l2(lchan->prim), GSM_MACBLOCK_LEN, 1);
-       } else if (lchan->tch_mode == GSM48_CMODE_SPEECH_AMR) {
@@ -459,10 +458,15 @@ int tx_tchh_fn(struct l1sched_lchan_state *lchan,
-       if (msgb_l2len(lchan->prim) == GSM_MACBLOCK_LEN) {
-               rc = gsm0503_tch_hr_encode(buffer, msgb_l2(lchan->prim), GSM_MACBLOCK_LEN);
-               lchan->ul_facch_blocks = 6;
-       } else if (lchan->tch_mode == GSM48_CMODE_SPEECH_AMR) {

Now if the channel mode is GSM48_CMODE_SPEECH_AMR, UL FACCH/[FH] frames
will be fed to osmo_amr_rtp_dec(), which is definitely wrong.  Fix this
by doing all AMR specific checks in a separate function, which is
called only for speech frames.

Change-Id: Ie217bbb389b5abb95d241781ffe3f5c7b1c188c0
Fixes: ad8f7794 "trxcon/l1sched: remove redundant TCH/[FH] prim length checks"
Related: OS#4396

4 files changed, 70 insertions, 92 deletions

diff --git a/src/host/trxcon/include/osmocom/bb/l1sched/l1sched.h b/src/host/trxcon/include/osmocom/bb/l1sched/l1sched.h
index d3b079ca..715b9a04 100644
--- a/src/host/trxcon/include/osmocom/bb/l1sched/l1sched.h
+++ b/src/host/trxcon/include/osmocom/bb/l1sched/l1sched.h
@@ -392,6 +392,7 @@ extern const uint8_t l1sched_nb_training_bits[8][26];
 
 const char *l1sched_burst_mask2str(const uint8_t *mask, int bits);
 size_t l1sched_bad_frame_ind(uint8_t *l2, struct l1sched_lchan_state *lchan);
+bool l1sched_lchan_amr_prim_is_valid(struct l1sched_lchan_state *lchan, bool is_cmr);
 
 /* Interleaved TCH/H block TDMA frame mapping */
 bool l1sched_tchh_block_map_fn(enum l1sched_lchan_type chan,
diff --git a/src/host/trxcon/src/sched_lchan_common.c b/src/host/trxcon/src/sched_lchan_common.c
index cddc9e5d..26d8f22b 100644
--- a/src/host/trxcon/src/sched_lchan_common.c
+++ b/src/host/trxcon/src/sched_lchan_common.c
@@ -141,3 +141,48 @@ size_t l1sched_bad_frame_ind(uint8_t *l2, struct l1sched_lchan_state *lchan)
 		return 0;
 	}
 }
+
+bool l1sched_lchan_amr_prim_is_valid(struct l1sched_lchan_state *lchan, bool is_cmr)
+{
+	enum osmo_amr_type ft_codec;
+	enum osmo_amr_quality bfi;
+	uint8_t cmr_codec;
+	int ft, cmr, len;
+	int8_t sti, cmi;
+
+	len = osmo_amr_rtp_dec(msgb_l2(lchan->prim), msgb_l2len(lchan->prim),
+			       &cmr_codec, &cmi, &ft_codec, &bfi, &sti);
+	if (len < 0) {
+		LOGP_LCHAND(lchan, LOGL_ERROR, "Cannot send invalid AMR payload (%u): %s\n",
+			    msgb_l2len(lchan->prim), msgb_hexdump_l2(lchan->prim));
+		return false;
+	}
+	ft = -1;
+	cmr = -1;
+	for (unsigned int i = 0; i < lchan->amr.codecs; i++) {
+		if (lchan->amr.codec[i] == ft_codec)
+			ft = i;
+		if (lchan->amr.codec[i] == cmr_codec)
+			cmr = i;
+	}
+	if (ft < 0) {
+		LOGP_LCHAND(lchan, LOGL_ERROR,
+			    "Codec (FT = %d) of RTP frame not in list\n", ft_codec);
+		return false;
+	}
+	if (is_cmr && lchan->amr.ul_ft != ft) {
+		LOGP_LCHAND(lchan, LOGL_ERROR,
+			    "Codec (FT = %d) of RTP cannot be changed now, but in next frame\n",
+			    ft_codec);
+		return false;
+	}
+	lchan->amr.ul_ft = ft;
+	if (cmr < 0) {
+		LOGP_LCHAND(lchan, LOGL_ERROR,
+			    "Codec (CMR = %d) of RTP frame not in list\n", cmr_codec);
+	} else {
+		lchan->amr.ul_cmr = cmr;
+	}
+
+	return true;
+}
diff --git a/src/host/trxcon/src/sched_lchan_tchf.c b/src/host/trxcon/src/sched_lchan_tchf.c
index a4aa80cb..9e8256fc 100644
--- a/src/host/trxcon/src/sched_lchan_tchf.c
+++ b/src/host/trxcon/src/sched_lchan_tchf.c
@@ -244,54 +244,20 @@ int tx_tchf_fn(struct l1sched_lchan_state *lchan,
 		break;
 	case GSM48_CMODE_SPEECH_AMR:
 	{
-		int len;
-		uint8_t cmr_codec;
-		int ft, cmr, i;
-		enum osmo_amr_type ft_codec;
-		enum osmo_amr_quality bfi;
-		int8_t sti, cmi;
-		bool amr_fn_is_cmr;
-		/* the first FN 0,8,17 defines that CMI is included in frame,
-		 * the first FN 4,13,21 defines that CMR is included in frame.
-		 */
-		amr_fn_is_cmr = !sched_tchf_ul_amr_cmi_map[br->fn % 26];
-
-		len = osmo_amr_rtp_dec(msgb_l2(lchan->prim), msgb_l2len(lchan->prim),
-				       &cmr_codec, &cmi, &ft_codec, &bfi, &sti);
-		if (len < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR, "Cannot send invalid AMR payload (%u): %s\n",
-				    msgb_l2len(lchan->prim), msgb_hexdump_l2(lchan->prim));
-			goto free_bad_msg;
-		}
-		ft = -1;
-		cmr = -1;
-		for (i = 0; i < lchan->amr.codecs; i++) {
-			if (lchan->amr.codec[i] == ft_codec)
-				ft = i;
-			if (lchan->amr.codec[i] == cmr_codec)
-				cmr = i;
-		}
-		if (ft < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (FT = %d) of RTP frame not in list\n", ft_codec);
-			goto free_bad_msg;
-		}
-		if (amr_fn_is_cmr && lchan->amr.ul_ft != ft) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (FT = %d) of RTP cannot be changed now, but in next frame\n",
-				    ft_codec);
-			goto free_bad_msg;
-		}
-		lchan->amr.ul_ft = ft;
-		if (cmr < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (CMR = %d) of RTP frame not in list\n", cmr_codec);
-		} else {
-			lchan->amr.ul_cmr = cmr;
+		bool amr_fn_is_cmr = !sched_tchf_ul_amr_cmi_map[br->fn % 26];
+		const uint8_t *data = msgb_l2(lchan->prim);
+		size_t data_len = msgb_l2len(lchan->prim);
+
+		if (data_len != GSM_MACBLOCK_LEN) { /* TCH/AFS: speech */
+			if (!l1sched_lchan_amr_prim_is_valid(lchan, amr_fn_is_cmr))
+				goto free_bad_msg;
+			/* pull the AMR header - sizeof(struct amr_hdr) */
+			data_len -= 2;
+			data += 2;
 		}
+
 		rc = gsm0503_tch_afs_encode(bursts_p,
-					    msgb_l2(lchan->prim) + 2,
-					    msgb_l2len(lchan->prim) - 2,
+					    data, data_len,
 					    amr_fn_is_cmr,
 					    lchan->amr.codec,
 					    lchan->amr.codecs,
diff --git a/src/host/trxcon/src/sched_lchan_tchh.c b/src/host/trxcon/src/sched_lchan_tchh.c
index 30d623b5..6973f919 100644
--- a/src/host/trxcon/src/sched_lchan_tchh.c
+++ b/src/host/trxcon/src/sched_lchan_tchh.c
@@ -457,54 +457,20 @@ int tx_tchh_fn(struct l1sched_lchan_state *lchan,
 		break;
 	case GSM48_CMODE_SPEECH_AMR:
 	{
-		int len;
-		uint8_t cmr_codec;
-		int ft, cmr, i;
-		enum osmo_amr_type ft_codec;
-		enum osmo_amr_quality bfi;
-		int8_t sti, cmi;
-		bool amr_fn_is_cmr;
-		/* the first FN 0,8,17 defines that CMI is included in frame,
-		 * the first FN 4,13,21 defines that CMR is included in frame.
-		 */
-		amr_fn_is_cmr = !sched_tchh_ul_amr_cmi_map[br->fn % 26];
-
-		len = osmo_amr_rtp_dec(msgb_l2(lchan->prim), msgb_l2len(lchan->prim),
-				       &cmr_codec, &cmi, &ft_codec, &bfi, &sti);
-		if (len < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR, "Cannot send invalid AMR payload (%u): %s\n",
-				    msgb_l2len(lchan->prim), msgb_hexdump_l2(lchan->prim));
-			goto free_bad_msg;
-		}
-		ft = -1;
-		cmr = -1;
-		for (i = 0; i < lchan->amr.codecs; i++) {
-			if (lchan->amr.codec[i] == ft_codec)
-				ft = i;
-			if (lchan->amr.codec[i] == cmr_codec)
-				cmr = i;
-		}
-		if (ft < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (FT = %d) of RTP frame not in list\n", ft_codec);
-			goto free_bad_msg;
-		}
-		if (amr_fn_is_cmr && lchan->amr.ul_ft != ft) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (FT = %d) of RTP cannot be changed now, but in next frame\n",
-				    ft_codec);
-			goto free_bad_msg;
-		}
-		lchan->amr.ul_ft = ft;
-		if (cmr < 0) {
-			LOGP_LCHAND(lchan, LOGL_ERROR,
-				    "Codec (CMR = %d) of RTP frame not in list\n", cmr_codec);
-		} else {
-			lchan->amr.ul_cmr = cmr;
+		bool amr_fn_is_cmr = !sched_tchh_ul_amr_cmi_map[br->fn % 26];
+		const uint8_t *data = msgb_l2(lchan->prim);
+		size_t data_len = msgb_l2len(lchan->prim);
+
+		if (data_len != GSM_MACBLOCK_LEN) { /* TCH/AHS: speech */
+			if (!l1sched_lchan_amr_prim_is_valid(lchan, amr_fn_is_cmr))
+				goto free_bad_msg;
+			/* pull the AMR header - sizeof(struct amr_hdr) */
+			data_len -= 2;
+			data += 2;
 		}
+
 		rc = gsm0503_tch_ahs_encode(bursts_p,
-					    msgb_l2(lchan->prim) + 2,
-					    msgb_l2len(lchan->prim) - 2,
+					    data, data_len,
 					    amr_fn_is_cmr,
 					    lchan->amr.codec,
 					    lchan->amr.codecs,