layer23/mobile: implement handling of TCH test loop commands

For more information, see 3GPP TS 44.014, sections:

  - 5.1 "Single-slot TCH loops", and
  - 8 "Message definitions and contents".

This feature has nothing to do with the Mobility Management, so
let's handle GSM48_PDISC_TEST messages in the Radio Resources
layer implementation (gsm48_mm.c -> gsm48_rr.c).

Change-Id: If8efc57c7017aa8ea47b37c472d1bbb1914389ca

7 files changed, 239 insertions, 15 deletions

diff --git a/src/host/layer23/include/osmocom/bb/common/l1ctl.h b/src/host/layer23/include/osmocom/bb/common/l1ctl.h
index e4dbdedc..02ffa343 100644
--- a/src/host/layer23/include/osmocom/bb/common/l1ctl.h
+++ b/src/host/layer23/include/osmocom/bb/common/l1ctl.h
@@ -49,7 +49,7 @@ int l1ctl_tx_ccch_mode_req(struct osmocom_ms *ms, uint8_t ccch_mode);
 
 /* Transmit TCH_MODE_REQ */
 int l1ctl_tx_tch_mode_req(struct osmocom_ms *ms, uint8_t tch_mode,
-	uint8_t audio_mode);
+			  uint8_t audio_mode, uint8_t tch_loop_mode);
 
 /* Transmit ECHO_REQ */
 int l1ctl_tx_echo_req(struct osmocom_ms *ms, unsigned int len);
diff --git a/src/host/layer23/include/osmocom/bb/mobile/gsm48_rr.h b/src/host/layer23/include/osmocom/bb/mobile/gsm48_rr.h
index 6996ff35..9b499a6b 100644
--- a/src/host/layer23/include/osmocom/bb/mobile/gsm48_rr.h
+++ b/src/host/layer23/include/osmocom/bb/mobile/gsm48_rr.h
@@ -188,6 +188,9 @@ struct gsm48_rrlayer {
 	/* audio flow */
 	uint8_t                 audio_mode;
 
+	/* 3GPP TS 44.014 TCH test loop mode (L1CTL specific format) */
+	uint8_t			tch_loop_mode;
+
 	/* sapi 3 */
 	uint8_t			sapi3_state;
 	uint8_t			sapi3_link_id;
diff --git a/src/host/layer23/src/common/l1ctl.c b/src/host/layer23/src/common/l1ctl.c
index 54c7452a..da30767c 100644
--- a/src/host/layer23/src/common/l1ctl.c
+++ b/src/host/layer23/src/common/l1ctl.c
@@ -461,7 +461,7 @@ int l1ctl_tx_ccch_mode_req(struct osmocom_ms *ms, uint8_t ccch_mode)
 
 /* Transmit L1CTL_TCH_MODE_REQ */
 int l1ctl_tx_tch_mode_req(struct osmocom_ms *ms, uint8_t tch_mode,
-	uint8_t audio_mode)
+			  uint8_t audio_mode, uint8_t tch_loop_mode)
 {
 	struct msgb *msg;
 	struct l1ctl_tch_mode_req *req;
@@ -475,6 +475,7 @@ int l1ctl_tx_tch_mode_req(struct osmocom_ms *ms, uint8_t tch_mode,
 	req = (struct l1ctl_tch_mode_req *) msgb_put(msg, sizeof(*req));
 	req->tch_mode = tch_mode;
 	req->audio_mode = audio_mode;
+	req->tch_loop_mode = tch_loop_mode;
 
 	return osmo_send_l1(ms, msg);
 }
diff --git a/src/host/layer23/src/mobile/Makefile.am b/src/host/layer23/src/mobile/Makefile.am
index 4e80e4ea..783ae162 100644
--- a/src/host/layer23/src/mobile/Makefile.am
+++ b/src/host/layer23/src/mobile/Makefile.am
@@ -4,7 +4,7 @@ LDADD = ../common/liblayer23.a $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOG
 
 noinst_LIBRARIES = libmobile.a
 libmobile_a_SOURCES = gsm322.c gsm480_ss.c gsm411_sms.c gsm48_cc.c gsm48_mm.c \
-	gsm48_rr.c mnccms.c settings.c subscriber.c support.c \
+	gsm48_rr.c gsm414.c mnccms.c settings.c subscriber.c support.c \
 	transaction.c vty_interface.c voice.c mncc_sock.c primitives.c
 
 bin_PROGRAMS = mobile
diff --git a/src/host/layer23/src/mobile/gsm414.c b/src/host/layer23/src/mobile/gsm414.c
new file mode 100644
index 00000000..2f630df3
--- /dev/null
+++ b/src/host/layer23/src/mobile/gsm414.c
@@ -0,0 +1,220 @@
+/*
+ * (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * Author: Vadim Yanitskiy <vyanitskiy@sysmocom.de>
+ *
+ * All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <stdint.h>
+#include <errno.h>
+
+#include <osmocom/core/logging.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+
+#include <osmocom/gsm/gsm48.h>
+#include <osmocom/gsm/rsl.h>
+
+#include <osmocom/gsm/protocol/gsm_04_08.h>
+#include <osmocom/gsm/protocol/gsm_04_14.h>
+#include <osmocom/gsm/protocol/gsm_08_58.h>
+
+#include <osmocom/bb/common/osmocom_data.h>
+#include <osmocom/bb/common/logging.h>
+
+#include <l1ctl_proto.h>
+
+int gsm48_rr_tx_rr_status(struct osmocom_ms *ms, uint8_t cause);
+int gsm48_send_rsl(struct osmocom_ms *ms, uint8_t msg_type,
+		   struct msgb *msg, uint8_t link_id);
+struct msgb *gsm48_l3_msgb_alloc(void);
+
+#define loop_mode_name(mode) \
+	get_value_string(loop_mode_names, mode)
+
+static const struct value_string loop_mode_names[] = {
+	{ L1CTL_TCH_LOOP_OPEN,	"(OPEN)" },
+	{ L1CTL_TCH_LOOP_A,	"A" },
+	{ L1CTL_TCH_LOOP_B,	"B" },
+	{ L1CTL_TCH_LOOP_C,	"C" },
+	{ L1CTL_TCH_LOOP_D,	"D" },
+	{ L1CTL_TCH_LOOP_E,	"E" },
+	{ L1CTL_TCH_LOOP_F,	"F" },
+	{ L1CTL_TCH_LOOP_I,	"I" },
+	{ 0, NULL }
+};
+
+static struct msgb *alloc_gsm414_msg(uint8_t msg_type)
+{
+	struct gsm48_hdr *ngh;
+	struct msgb *nmsg;
+
+	nmsg = gsm48_l3_msgb_alloc();
+	if (nmsg == NULL)
+		return NULL;
+
+	ngh = (struct gsm48_hdr *) msgb_put(nmsg, sizeof(*ngh));
+	ngh->proto_discr = GSM48_PDISC_TEST;
+	ngh->msg_type = msg_type;
+
+	return nmsg;
+}
+
+static int handle_close_tch_loop(struct osmocom_ms *ms, const struct msgb *msg)
+{
+	struct gsm48_rrlayer *rr = &ms->rrlayer;
+	const struct gsm48_hdr *gh = msgb_l3(msg);
+	unsigned int msg_len = msgb_l3len(msg);
+	struct msgb *nmsg;
+
+	/* Make sure that we have an active connection */
+	if (rr->state != GSM48_RR_ST_DEDICATED) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop requires an active connection\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	/* Make sure that the established channel is either TCH/F or TCH/H */
+	if ((rr->cd_now.chan_nr & 0xf8) != RSL_CHAN_Bm_ACCHs
+	 && (rr->cd_now.chan_nr & 0xf0) != RSL_CHAN_Lm_ACCHs) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop requires a TCH/F or TCH/H connection\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	/* Check if a loop is already closed */
+	if (rr->tch_loop_mode != L1CTL_TCH_LOOP_OPEN) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop has already been closed\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	if ((msg_len - sizeof(*gh)) < 1)
+		return -EINVAL;
+
+	/* Parse type of the TCH test loop, convert to L1CTL format */
+	uint8_t gsm414_loop_mode = (gh->data[0] >> 1) & 0x1f;
+
+	/* NOTE: some bits are not specified, so they can be 0 or 1 */
+	if (gsm414_loop_mode == GSM414_LOOP_A)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_A;
+	else if (gsm414_loop_mode == GSM414_LOOP_B)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_B;
+	else if ((gsm414_loop_mode & 0x1e) == GSM414_LOOP_C)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_C;
+	else if ((gsm414_loop_mode & 0x1c) == GSM414_LOOP_D)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_D;
+	else if ((gsm414_loop_mode & 0x1c) == GSM414_LOOP_E)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_E;
+	else if ((gsm414_loop_mode & 0x1c) == GSM414_LOOP_F)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_F;
+	else if ((gsm414_loop_mode & 0x1c) == GSM414_LOOP_I)
+		rr->tch_loop_mode = L1CTL_TCH_LOOP_I;
+	else {
+		LOGP(DMM, LOGL_NOTICE, "Unhandled 3GPP TS 44.014 TCH loop "
+		     "mode=0x%02x => rejecting\n", gsm414_loop_mode);
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N);
+		return -ENOTSUP;
+	}
+
+	LOGP(DMM, LOGL_NOTICE, "(%s) Closing 3GPP TS 44.014 TCH loop mode '%s'\n",
+	     rsl_chan_nr_str(rr->cd_now.chan_nr), loop_mode_name(rr->tch_loop_mode));
+
+	/* Instruct the L1 to enable received TCH loopback mode
+	 * FIXME: delay applying this mode, so we can send the ACK first */
+	l1ctl_tx_tch_mode_req(ms, rr->cd_now.mode, rr->audio_mode, rr->tch_loop_mode);
+
+	/* Craft and send the ACKnowledgement */
+	nmsg = alloc_gsm414_msg(GSM414_MT_CLOSE_TCH_LOOP_ACK);
+	if (nmsg == NULL)
+		return -ENOMEM;
+
+	return gsm48_send_rsl(ms, RSL_MT_DATA_REQ, nmsg, 0);
+}
+
+static int handle_open_tch_loop(struct osmocom_ms *ms, const struct msgb *msg)
+{
+	struct gsm48_rrlayer *rr = &ms->rrlayer;
+	struct msgb *nmsg;
+
+	/* Make sure that we have an active connection */
+	if (rr->state != GSM48_RR_ST_DEDICATED) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop requires an active connection\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	/* Make sure that the established channel is either TCH/F or TCH/H */
+	if ((rr->cd_now.chan_nr & 0xf8) != RSL_CHAN_Bm_ACCHs
+	 && (rr->cd_now.chan_nr & 0xf0) != RSL_CHAN_Lm_ACCHs) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop requires a TCH/F or TCH/H connection\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	/* Check if a loop actually needs to be closed */
+	if (rr->tch_loop_mode == L1CTL_TCH_LOOP_OPEN) {
+		LOGP(DMM, LOGL_NOTICE, "TCH loop has not been closed (already open)\n");
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT);
+		return -EINVAL;
+	}
+
+	LOGP(DMM, LOGL_NOTICE, "(%s) Opening 3GPP TS 44.014 TCH loop mode '%s'\n",
+	     rsl_chan_nr_str(rr->cd_now.chan_nr), loop_mode_name(rr->tch_loop_mode));
+
+	/* Instruct the L1 to disable the TCH loopback mode */
+	l1ctl_tx_tch_mode_req(ms, rr->cd_now.mode, rr->audio_mode, L1CTL_TCH_LOOP_OPEN);
+
+	/* Only the loop mode C needs to be ACKnowledged */
+	bool needs_ack = rr->tch_loop_mode == L1CTL_TCH_LOOP_C;
+	rr->tch_loop_mode = L1CTL_TCH_LOOP_OPEN;
+	if (!needs_ack)
+		return 0;
+
+	/* Craft and send the ACKnowledgement */
+	nmsg = alloc_gsm414_msg(GSM414_MT_OPEN_LOOP_CMD);
+	if (nmsg == NULL)
+		return -ENOMEM;
+
+	msgb_put_u8(nmsg, GSM414_OPEN_LOOP_ACK_IE);
+
+	return gsm48_send_rsl(ms, RSL_MT_DATA_REQ, nmsg, 0);
+}
+
+int gsm414_rcv_test(struct osmocom_ms *ms, const struct msgb *msg)
+{
+	const struct gsm48_hdr *gh = msgb_l3(msg);
+
+	LOGP(DMM, LOGL_INFO, "Received 3GPP TS 44.014 message '%s' (0x%02x)\n",
+	     get_value_string(gsm414_msgt_names, gh->msg_type), gh->msg_type);
+
+	/* TODO: check if the test SIM (special EF.ADM) is inserted */
+	switch (gh->msg_type) {
+	case GSM414_MT_CLOSE_TCH_LOOP_CMD:
+		return handle_close_tch_loop(ms, msg);
+	case GSM414_MT_OPEN_LOOP_CMD:
+		return handle_open_tch_loop(ms, msg);
+	default:
+		LOGP(DMM, LOGL_NOTICE, "Unhandled 3GPP TS 44.014 message '%s' (0x%02x)\n",
+		     get_value_string(gsm414_msgt_names, gh->msg_type), gh->msg_type);
+		gsm48_rr_tx_rr_status(ms, GSM48_RR_CAUSE_MSG_TYPE_N);
+		return -ENOTSUP;
+	}
+}
diff --git a/src/host/layer23/src/mobile/gsm48_mm.c b/src/host/layer23/src/mobile/gsm48_mm.c
index 52c6fe9a..49cc2bc6 100644
--- a/src/host/layer23/src/mobile/gsm48_mm.c
+++ b/src/host/layer23/src/mobile/gsm48_mm.c
@@ -4076,14 +4076,9 @@ static int gsm48_mm_data_ind(struct osmocom_ms *ms, struct msgb *msg)
 		msgb_free(msg);
 		return rc;
 
-	case GSM48_PDISC_TEST: /* test TS 04.14 */
-		LOGP(DMM, LOGL_NOTICE, "Test protocol 0x%02x according to "
-			"TS 04.14 is not supported.\n", pdisc);
-		goto status;
 	default:
 		LOGP(DMM, LOGL_NOTICE, "Protocol type 0x%02x unsupported.\n",
 			pdisc);
-status:
 		msgb_free(msg);
 		return gsm48_mm_tx_mm_status(ms,
 			GSM48_REJECT_MSG_TYPE_NOT_IMPLEMENTED);
diff --git a/src/host/layer23/src/mobile/gsm48_rr.c b/src/host/layer23/src/mobile/gsm48_rr.c
index 0449ee31..64d64f1a 100644
--- a/src/host/layer23/src/mobile/gsm48_rr.c
+++ b/src/host/layer23/src/mobile/gsm48_rr.c
@@ -93,6 +93,7 @@ static int gsm48_rr_tx_meas_rep(struct osmocom_ms *ms);
 static int gsm48_rr_set_mode(struct osmocom_ms *ms, uint8_t chan_nr,
 	uint8_t mode);
 static int gsm48_rr_rel_cnf(struct osmocom_ms *ms, struct msgb *msg);
+int gsm414_rcv_test(struct osmocom_ms *ms, const struct msgb *msg);
 
 /*
  * support
@@ -516,8 +517,8 @@ int gsm48_rr_upmsg(struct osmocom_ms *ms, struct msgb *msg)
 }
 
 /* push rsl header and send (RSL-SAP) */
-static int gsm48_send_rsl(struct osmocom_ms *ms, uint8_t msg_type,
-				struct msgb *msg, uint8_t link_id)
+int gsm48_send_rsl(struct osmocom_ms *ms, uint8_t msg_type,
+		   struct msgb *msg, uint8_t link_id)
 {
 	struct gsm48_rrlayer *rr = &ms->rrlayer;
 
@@ -879,7 +880,7 @@ static void stop_rr_t3126(struct gsm48_rrlayer *rr)
  */
 
 /* send rr status request */
-static int gsm48_rr_tx_rr_status(struct osmocom_ms *ms, uint8_t cause)
+int gsm48_rr_tx_rr_status(struct osmocom_ms *ms, uint8_t cause)
 {
 	struct msgb *nmsg;
 	struct gsm48_hdr *gh;
@@ -3433,7 +3434,7 @@ static int gsm48_rr_set_mode(struct osmocom_ms *ms, uint8_t chan_nr,
 	/* setting (new) timing advance */
 	LOGP(DRR, LOGL_INFO, "setting TCH mode to %d, audio mode to %d\n",
 		mode, rr->audio_mode);
-	l1ctl_tx_tch_mode_req(ms, mode, rr->audio_mode);
+	l1ctl_tx_tch_mode_req(ms, mode, rr->audio_mode, rr->tch_loop_mode);
 
 	return 0;
 }
@@ -4627,9 +4628,9 @@ static int gsm48_rr_data_ind(struct osmocom_ms *ms, struct msgb *msg)
 	struct gsm48_hdr *gh = msgb_l3(msg);
 	struct gsm48_rr_hdr *rrh;
 	uint8_t pdisc = gh->proto_discr & 0x0f;
+	int rc = -EINVAL;
 
 	if (pdisc == GSM48_PDISC_RR) {
-		int rc = -EINVAL;
 		uint8_t skip_ind = (gh->proto_discr & 0xf0) >> 4;
 
 		/* ignore if skip indicator is not B'0000' */
@@ -4676,6 +4677,10 @@ static int gsm48_rr_data_ind(struct osmocom_ms *ms, struct msgb *msg)
 
 		msgb_free(msg);
 		return rc;
+	} else if (pdisc == GSM48_PDISC_TEST) {
+		rc = gsm414_rcv_test(ms, msg);
+		msgb_free(msg);
+		return rc;
 	}
 
 	/* pull off RSL header up to L3 message */
@@ -5505,6 +5510,7 @@ int gsm48_rr_init(struct osmocom_ms *ms)
 	start_rr_t_meas(rr, 1, 0);
 
 	rr->audio_mode = AUDIO_TX_MICROPHONE | AUDIO_RX_SPEAKER;
+	rr->tch_loop_mode = L1CTL_TCH_LOOP_OPEN;
 
 	return 0;
 }
@@ -5707,6 +5713,5 @@ int gsm48_rr_audio_mode(struct osmocom_ms *ms, uint8_t mode)
 	 && ch_type != RSL_CHAN_Lm_ACCHs)
 		return 0;
 
-	return l1ctl_tx_tch_mode_req(ms, rr->cd_now.mode, mode);
+	return l1ctl_tx_tch_mode_req(ms, rr->cd_now.mode, mode, rr->tch_loop_mode);
 }
-