3 files changed, 74 insertions, 8 deletions

diff --git a/src/osmo-bts-sysmo/l1_if.c b/src/osmo-bts-sysmo/l1_if.c
index dc97802b..ea71e2c6 100644
--- a/src/osmo-bts-sysmo/l1_if.c
+++ b/src/osmo-bts-sysmo/l1_if.c
@@ -1346,6 +1346,9 @@ static int info_compl_cb(struct gsm_bts_trx *trx, struct msgb *resp,
 		LOGP(DL1C, LOGL_FATAL, "BTS band %s not supported by hw\n",
 		     gsm_band_name(trx->bts->band));
 
+	if (l1if_dsp_ver(fl1h) < L1_VER_SHIFT(5,3,3))
+		fl1h->rtp_hr_jumble_needed = true;
+
 	/* Request the activation */
 	l1if_activate_rf(fl1h, 1);
 
diff --git a/src/osmo-bts-sysmo/l1_if.h b/src/osmo-bts-sysmo/l1_if.h
index 5f05f1df..968cd170 100644
--- a/src/osmo-bts-sysmo/l1_if.h
+++ b/src/osmo-bts-sysmo/l1_if.h
@@ -78,12 +78,27 @@ struct femtol1_hdl {
 	} hw_info;
 
 	int fixup_needed;
+	bool rtp_hr_jumble_needed;
 
 	struct calib_send_state st;
 
 	uint8_t last_rf_mute[8];
 };
 
+#define L1_VER_SHIFT(x,y,z) ((x << 16) | (y << 8) | (z))
+
+static inline uint32_t l1if_dsp_ver(struct femtol1_hdl *fl1h)
+{
+        const uint8_t *v = fl1h->hw_info.dsp_version;
+        return L1_VER_SHIFT(v[0], v[1], v[2]);
+}
+
+static inline uint32_t l1if_fpga_ver(struct femtol1_hdl *fl1h)
+{
+        const uint8_t *v = fl1h->hw_info.fpga_version;
+        return L1_VER_SHIFT(v[0], v[1], v[2]);
+}
+
 #define msgb_l1prim(msg)	((GsmL1_Prim_t *)(msg)->l1h)
 #define msgb_sysprim(msg)	((SuperFemto_Prim_t *)(msg)->l1h)
 
diff --git a/src/osmo-bts-sysmo/tch.c b/src/osmo-bts-sysmo/tch.c
index 9e8903b2..5506e155 100644
--- a/src/osmo-bts-sysmo/tch.c
+++ b/src/osmo-bts-sysmo/tch.c
@@ -189,7 +189,45 @@ static int rtppayload_to_l1_efr(uint8_t *l1_payload, const uint8_t *rtp_payload,
 #warning No EFR support in L1
 #endif /* L1_HAS_EFR */
 
-static struct msgb *l1_to_rtppayload_hr(uint8_t *l1_payload, uint8_t payload_len)
+#ifdef USE_L1_RTP_MODE
+/* change the bit-order of each unaligned field inside the HR codec
+ * payload from little-endian bit-ordering to bit-endian and vice-versa.
+ * This is required on all sysmoBTS DSP versions < 5.3.3 in order to
+ * be compliant with ETSI TS 101 318 Chapter 5.2 */
+static void hr_jumble(uint8_t *dst, const uint8_t *src)
+{
+	/* Table 2 / Section 5.2.1 of ETSI TS 101 381 */
+	const int p_unvoiced[] =
+		{ 5, 11,  9,  8,  1,  2,  7,  7,  5,  7,  7,  5,  7,  7,  5,  7,  7,  5 };
+	/* Table 3 / Section 5.2.1 of ETSI TS 101 381 */
+	const int p_voiced[] =
+		{ 5, 11,  9,  8,  1,  2,  8,  9,  5,  4,  9,  5,  4,  9,  5,  4,  9,  5 };
+
+	int base, i, j, l, si, di;
+	const int *p;
+
+	memset(dst, 0x00, GSM_HR_BYTES);
+
+	p = (src[4] & 0x30) ? p_voiced : p_unvoiced;
+
+	base = 0;
+	for (i = 0; i < 18; i++) {
+		l = p[i];
+		for (j = 0; j < l; j++) {
+			si = base + j;
+			di = base + l - j - 1;
+
+			if (src[si >> 3] & (1 << (7 - (si & 7))))
+				dst[di >> 3] |= (1 << (7 - (di & 7)));
+		}
+
+		base += l;
+	}
+}
+#endif /* USE_L1_RTP_MODE */
+
+static struct msgb *l1_to_rtppayload_hr(uint8_t *l1_payload, uint8_t payload_len,
+					struct gsm_lchan *lchan)
 {
 	struct msgb *msg;
 	uint8_t *cur;
@@ -205,9 +243,14 @@ static struct msgb *l1_to_rtppayload_hr(uint8_t *l1_payload, uint8_t payload_len
 	}
 
 	cur = msgb_put(msg, GSM_HR_BYTES);
+#ifdef USE_L1_RTP_MODE
+	struct femtol1_hdl *fl1h = trx_femtol1_hdl(lchan->ts->trx);
+	if (fl1h->rtp_hr_jumble_needed)
+		hr_jumble(cur, l1_payload);
+	else
+		memcpy(cur, l1_payload, GSM_HR_BYTES);
+#else /* USE_L1_RTP_MODE */
 	memcpy(cur, l1_payload, GSM_HR_BYTES);
-
-#ifndef USE_L1_RTP_MODE
 	/* reverse the bit-order of each payload byte */
 	osmo_revbytebits_buf(cur, GSM_HR_BYTES);
 #endif /* USE_L1_RTP_MODE */
@@ -222,7 +265,7 @@ static struct msgb *l1_to_rtppayload_hr(uint8_t *l1_payload, uint8_t payload_len
  *  \returns number of \a l1_payload bytes filled
  */
 static int rtppayload_to_l1_hr(uint8_t *l1_payload, const uint8_t *rtp_payload,
-				unsigned int payload_len)
+				unsigned int payload_len, struct gsm_lchan *lchan)
 {
 
 	if (payload_len != GSM_HR_BYTES) {
@@ -231,9 +274,14 @@ static int rtppayload_to_l1_hr(uint8_t *l1_payload, const uint8_t *rtp_payload,
 		return 0;
 	}
 
+#ifdef USE_L1_RTP_MODE
+	struct femtol1_hdl *fl1h = trx_femtol1_hdl(lchan->ts->trx);
+	if (fl1h->rtp_hr_jumble_needed)
+		hr_jumble(l1_payload, rtp_payload);
+	else
+		memcpy(l1_payload, rtp_payload, GSM_HR_BYTES);
+#else /* USE_L1_RTP_MODE */
 	memcpy(l1_payload, rtp_payload, GSM_HR_BYTES);
-
-#ifndef USE_L1_RTP_MODE
 	/* reverse the bit-order of each payload byte */
 	osmo_revbytebits_buf(l1_payload, GSM_HR_BYTES);
 #endif /* USE_L1_RTP_MODE */
@@ -475,7 +523,7 @@ void bts_model_rtp_rx_cb(struct osmo_rtp_socket *rs, const uint8_t *rtp_pl,
 		} else{
 			*payload_type = GsmL1_TchPlType_Hr;
 			rc = rtppayload_to_l1_hr(l1_payload,
-						 rtp_pl, rtp_pl_len);
+						 rtp_pl, rtp_pl_len, lchan);
 		}
 		break;
 #if defined(L1_HAS_EFR) && defined(USE_L1_RTP_MODE)
@@ -620,7 +668,7 @@ int l1if_tch_rx(struct gsm_lchan *lchan, struct msgb *l1p_msg)
 		rmsg = l1_to_rtppayload_fr(payload, payload_len);
 		break;
 	case GsmL1_TchPlType_Hr:
-		rmsg = l1_to_rtppayload_hr(payload, payload_len);
+		rmsg = l1_to_rtppayload_hr(payload, payload_len, lchan);
 		break;
 #if defined(L1_HAS_EFR) && defined(USE_L1_RTP_MODE)
 	case GsmL1_TchPlType_Efr: