More comprehensive AMR handling

* parse AMR multirate config form 04.08 IE into easier format
* CMR, CMC and CMI on the L1 side are an _index_ into the current
  mode array
* Fix conversion of AMR SID frames from RTP -> L1

1 files changed, 62 insertions, 32 deletions

diff --git a/src/osmo-bts-sysmo/tch.c b/src/osmo-bts-sysmo/tch.c
index dd2ae32d..68ad0ce4 100644
--- a/src/osmo-bts-sysmo/tch.c
+++ b/src/osmo-bts-sysmo/tch.c
@@ -65,9 +65,9 @@ void osmo_nibble_shift_right(uint8_t *out, const uint8_t *in,
 	/* shift the last nibble, in case there's an odd count */
 	i = num_whole_bytes;
 	if (num_nibbles & 1)
-		out[i] = (in[i-1] & 0xF) << 4;
-	else
 		out[i] = ((in[i-1] & 0xF) << 4) | (in[i] >> 4);
+	else
+		out[i] = (in[i-1] & 0xF) << 4;
 }
 
 
@@ -201,29 +201,30 @@ static int rtppayload_to_l1_hr(uint8_t *l1_payload, uint8_t *rtp_payload,
 #define AMR_CMR_NONE	0xF
 
 static struct msgb *l1_to_rtppayload_amr(uint8_t *l1_payload, uint8_t payload_len,
-					 uint8_t active_codec_set)
+					 struct amr_multirate_conf *amr_mrc)
 {
 	struct msgb *msg = msgb_alloc_headroom(1024, 128, "L1C-to-RTP");
 	uint8_t *cur;
+	u_int8_t cmr;
 	uint8_t ft = l1_payload[2] & 0xF;
-	uint8_t cmr = l1_payload[1];
+	uint8_t cmr_idx = l1_payload[1];
 	uint8_t amr_if2_len = payload_len - 2;
 
-#warning Only use CMR when it actually appears according to TDMA time
 #if 0
-	/* CMR can never be > 7 */
-	if (cmr > 7)
+	/* CMR == Unset means CMR was not transmitted at this TDMA */
+	if (cmr_idx >= GsmL1_AmrCodecMode_Unset)
 		cmr = AMR_CMR_NONE;
-	else {
+	else if (cmr_idx >= amr_mrc->num_modes) {
 		/* Make sure the CMR of the phone is in the active codec set */
-		if (!(active_codec_set & (1 << cmr))) {
-			LOGP(DL1C, LOGL_NOTICE, "L1->RTP: overriding CMR %u\n", cmr);
-			cmr = AMR_CMR_NONE;
-		}
+		LOGP(DL1C, LOGL_NOTICE, "L1->RTP: overriding CMR IDX %u\n", cmr_idx);
+		cmr = AMR_CMR_NONE;
+	} else {
+		cmr = amr_mrc->mode[cmr_idx].mode;
 	}
 #else
 	cmr = AMR_CMR_NONE;
 #endif
+
 	/* RFC 3267  4.4.1 Payload Header */
 	msgb_put_u8(msg, (cmr << 4));
 
@@ -245,6 +246,16 @@ enum amr_frame_type {
 	AMR_FT_SID_AMR	= 8,
 };
 
+int get_amr_mode_idx(const struct amr_multirate_conf *amr_mrc, uint8_t cmi)
+{
+	unsigned int i;
+	for (i = 0; i < amr_mrc->num_modes; i++) {
+		if (amr_mrc->mode[i].mode == cmi)
+			return i;
+	}
+	return -EINVAL;
+}
+
 /*! \brief convert AMR from RTP payload to L1 format
  *  \param[out] l1_payload payload part of L1 buffer
  *  \param[in] rtp_payload pointer to RTP payload data
@@ -252,47 +263,67 @@ enum amr_frame_type {
  *  \returns number of \a l1_payload bytes filled
  */
 static int rtppayload_to_l1_amr(uint8_t *l1_payload, uint8_t *rtp_payload,
-				uint8_t payload_len, const uint8_t active_codec_set)
+				uint8_t payload_len,
+				struct amr_multirate_conf *amr_mrc)
 {
 	uint8_t ft = (rtp_payload[1] >> 3) & 0xf;
 	uint8_t cmr = rtp_payload[0] >> 4;
-	uint8_t *l1_cmi = l1_payload;
-	uint8_t *l1_cmr = l1_payload+1;
+	uint8_t cmi, sti;
+	uint8_t *l1_cmi_idx = l1_payload;
+	uint8_t *l1_cmr_idx = l1_payload+1;
 	uint8_t amr_if2_core_len = payload_len - 2;
+	int rc;
 
 	/* step1: shift everything right one nibble; make space for FT */
-	osmo_nibble_shift_right(l1_payload+2, rtp_payload+2, amr_if2_core_len*2 -1);
+	osmo_nibble_shift_right(l1_payload+2, rtp_payload+2, amr_if2_core_len*2);
 	/* step2: reverse the bit-order within every byte of the IF2
 	 * core frame contained in the RTP payload */
-	osmo_revbytebits_buf(l1_payload+2, amr_if2_core_len);
+	osmo_revbytebits_buf(l1_payload+2, amr_if2_core_len+1);
 
 	/* CMI in downlink tells the L1 encoder which encoding function
 	 * it will use, so we have to use the frame type */
 	switch (ft) {
 	case 0: case 1: case 2: case 3:
 	case 4: case 5: case 6: case 7:
-		*l1_cmi = ft;
+		cmi = ft;
+		LOGP(DL1C, LOGL_ERROR, "SPEECH frame with CMI %u\n", cmi);
 		break;
 	case AMR_FT_SID_AMR:
-		/* extract the mode indiciation from last 3 bits of
-		 * 39 bit SID frame */
-		*l1_cmi = *(l1_payload+2+4) >> 4;
+		/* extract the mode indiciation from last bits of
+		 * 39 bit SID frame (Table 6 / 26.101) */
+		cmi = (rtp_payload[2+4] >> 1) & 0x7;
+		sti = rtp_payload[2+4] & 0x10;
+		LOGP(DL1C, LOGL_ERROR, "SID %s frame with CMI %u\n",
+		     sti ? "UPDATE" : "FIRST", cmi);
 		break;
 	default:
 		LOGP(DL1C, LOGL_ERROR, "unsupported AMR FT 0x%02x\n", ft);
+		return -EINVAL;
 		break;
 	}
 
+	rc = get_amr_mode_idx(amr_mrc, cmi);
+	if (rc < 0) {
+		LOGP(DL1C, LOGL_ERROR, "AMR CMI %u not part of AMR MR set\n",
+			cmi);
+		*l1_cmi_idx = 0;
+	} else
+		*l1_cmi_idx = rc;
+
 	/* Codec Mode Request is in upper 4 bits of RTP payload header,
 	 * and we simply copy the CMR into the CMC */
-	if (cmr == 0xF)
-		*l1_cmr = 2;
-	else if (!(active_codec_set & (1 << cmr))) {
+	if (cmr == 0xF) {
 		/* FIXME: we need some state about the last codec mode */
-		LOGP(DL1C, LOGL_NOTICE, "RTP->L1: overriding CMR %u\n", cmr);
-		*l1_cmr = 2;
-	} else
-		*l1_cmr = cmr;
+		*l1_cmr_idx = 0;
+	} else {
+		rc = get_amr_mode_idx(amr_mrc, cmr);
+		if (rc < 0) {
+			/* FIXME: we need some state about the last codec mode */
+			LOGP(DL1C, LOGL_NOTICE, "RTP->L1: overriding CMR %u\n", cmr);
+			*l1_cmr_idx = 0;
+		} else
+			*l1_cmr_idx = rc;
+	}
 #if 0
 	/* check for bad quality indication */
 	if (rtp_payload[1] & AMR_TOC_QBIT) {
@@ -339,7 +370,6 @@ void bts_model_rtp_rx_cb(struct osmo_rtp_socket *rs, uint8_t *rtp_pl,
 	GsmL1_MsgUnitParam_t *msu_param = &data_req->msgUnitParam;
 	uint8_t *payload_type = &msu_param->u8Buffer[0];
 	uint8_t *l1_payload = &msu_param->u8Buffer[1];
-	uint8_t *mr_conf = &lchan->mr_conf;
 	int rc;
 
 	DEBUGP(DL1C, "%s RTP IN: %s\n", gsm_lchan_name(lchan),
@@ -366,7 +396,7 @@ void bts_model_rtp_rx_cb(struct osmo_rtp_socket *rs, uint8_t *rtp_pl,
 	case GSM48_CMODE_SPEECH_AMR:
 		*payload_type = GsmL1_TchPlType_Amr;
 		rc = rtppayload_to_l1_amr(l1_payload, rtp_pl,
-					  rtp_pl_len, mr_conf[1]);
+					  rtp_pl_len, &lchan->tch.amr_mr);
 		break;
 	default:
 		/* we don't support CSD modes */
@@ -416,7 +446,6 @@ int l1if_tch_rx(struct gsm_lchan *lchan, struct msgb *l1p_msg)
 	GsmL1_PhDataInd_t *data_ind = &l1p->u.phDataInd;
 	uint8_t payload_type = data_ind->msgUnitParam.u8Buffer[0];
 	uint8_t *payload = data_ind->msgUnitParam.u8Buffer + 1;
-	uint8_t *mr_conf = &lchan->mr_conf;
 	uint8_t payload_len;
 	struct msgb *rmsg = NULL;
 
@@ -467,7 +496,8 @@ int l1if_tch_rx(struct gsm_lchan *lchan, struct msgb *l1p_msg)
 		break;
 #endif
 	case GsmL1_TchPlType_Amr:
-		rmsg = l1_to_rtppayload_amr(payload, payload_len, mr_conf[1]);
+		rmsg = l1_to_rtppayload_amr(payload, payload_len,
+					    &lchan->tch.amr_mr);
 		break;
 	}