mobile: implement sending CSD data over UNIX socket

Change-Id: Id054af7b3d9d0a41715f7981deb420f6e09bf30c
Related: OS#4396

3 files changed, 308 insertions, 11 deletions

diff --git a/src/host/layer23/src/mobile/Makefile.am b/src/host/layer23/src/mobile/Makefile.am
index b4e45e29..d00e45ea 100644
--- a/src/host/layer23/src/mobile/Makefile.am
+++ b/src/host/layer23/src/mobile/Makefile.am
@@ -33,6 +33,7 @@ libmobile_a_SOURCES = \
 	primitives.c \
 	tch.c \
 	tch_data.c \
+	tch_data_sock.c \
 	tch_voice.c \
 	transaction.c \
 	vty_interface.c \
diff --git a/src/host/layer23/src/mobile/tch_data.c b/src/host/layer23/src/mobile/tch_data.c
index 9f466419..c58f485d 100644
--- a/src/host/layer23/src/mobile/tch_data.c
+++ b/src/host/layer23/src/mobile/tch_data.c
@@ -72,21 +72,32 @@ const struct csd_v110_lchan_desc csd_v110_lchan_desc[256] = {
 	},
 };
 
+struct tch_csd_sock_state *tch_csd_sock_init(struct osmocom_ms *ms);
+void tch_csd_sock_recv(struct tch_csd_sock_state *state, struct msgb *msg);
+void tch_csd_sock_send(struct tch_csd_sock_state *state, struct msgb *msg);
+void tch_csd_sock_exit(struct tch_csd_sock_state *state);
+
 static void tch_soft_uart_rx_cb(void *priv, struct msgb *msg, unsigned int flags)
 {
-	LOGP(DL1C, LOGL_FATAL, "%s(): [flags=0x%08x] %s\n",
+	struct tch_data_state *state = (struct tch_data_state *)priv;
+
+	LOGP(DL1C, LOGL_DEBUG, "%s(): [flags=0x%08x] %s\n",
 	     __func__, flags, msgb_hexdump(msg));
-	msgb_free(msg);
+
+	if (state->sock != NULL && msgb_length(msg) > 0)
+		tch_csd_sock_send(state->sock, msg);
+	else
+		msgb_free(msg);
 }
 
 static void tch_soft_uart_tx_cb(void *priv, struct msgb *msg)
 {
-	const char *data = "TEST\r\n";
-	size_t n_bytes;
+	struct tch_data_state *state = (struct tch_data_state *)priv;
+
+	tch_csd_sock_recv(state->sock, msg);
 
-	n_bytes = OSMO_MIN(msg->data_len, strlen(data));
-	if (n_bytes > 0)
-		memcpy(msgb_put(msg, n_bytes), (void *)data, n_bytes);
+	LOGP(DL1C, LOGL_DEBUG, "%s(): [n_bytes=%u/%u] %s\n",
+	     __func__, msg->len, msg->data_len, msgb_hexdump(msg));
 }
 
 struct osmo_soft_uart *tch_soft_uart_alloc(struct osmocom_ms *ms,
@@ -138,12 +149,28 @@ struct osmo_soft_uart *tch_soft_uart_alloc(struct osmocom_ms *ms,
 
 static void tch_v110_ta_rx_cb(void *priv, const ubit_t *buf, size_t buf_size)
 {
-	/* TODO: send to the configured I/O handler */
+	const struct tch_data_state *state = (struct tch_data_state *)priv;
+
+	if (state->sock != NULL && buf_size > 0) {
+		struct msgb *msg = msgb_alloc(buf_size, __func__);
+		tch_csd_sock_send(state->sock, msg);
+	}
 }
 
 static void tch_v110_ta_tx_cb(void *priv, ubit_t *buf, size_t buf_size)
 {
-	/* TODO: send to the configured I/O handler */
+	const struct tch_data_state *state = (struct tch_data_state *)priv;
+
+	if (state->sock != NULL && buf_size > 0) {
+		struct msgb *msg = msgb_alloc(buf_size, __func__);
+
+		tch_csd_sock_recv(state->sock, msg);
+		if (msgb_length(msg) < buf_size) {
+			LOGP(DL1C, LOGL_NOTICE,
+			     "%s(): not enough bytes for sync Tx (%u < %zu)\n",
+			     __func__, msgb_length(msg), buf_size);
+		}
+	}
 }
 
 static void tch_v110_ta_async_rx_cb(void *priv, const ubit_t *buf, size_t buf_size)
@@ -431,7 +458,9 @@ int tch_data_state_init(struct gsm_trans *trans,
 
 	switch (state->handler) {
 	case TCH_DATA_IOH_UNIX_SOCK:
-		/* TODO: open listening socket */
+		state->sock = tch_csd_sock_init(ms);
+		if (state->sock == NULL)
+			return -ENOMEM;
 		break;
 	case TCH_DATA_IOH_LOOPBACK:
 	case TCH_DATA_IOH_NONE:
@@ -454,6 +483,8 @@ int tch_data_state_init(struct gsm_trans *trans,
 	return 0;
 
 exit_free:
+	if (state->sock != NULL)
+		tch_csd_sock_exit(state->sock);
 	if (state->suart != NULL)
 		osmo_soft_uart_free(state->suart);
 	if (state->v110_ta != NULL)
@@ -465,7 +496,8 @@ void tch_data_state_free(struct tch_data_state *state)
 {
 	switch (state->handler) {
 	case TCH_DATA_IOH_UNIX_SOCK:
-		/* TODO: close listening socket */
+		if (state->sock != NULL)
+			tch_csd_sock_exit(state->sock);
 		break;
 	default:
 		break;
@@ -476,3 +508,24 @@ void tch_data_state_free(struct tch_data_state *state)
 	if (state->v110_ta != NULL)
 		osmo_v110_ta_free(state->v110_ta);
 }
+
+void tch_csd_sock_state_cb(struct osmocom_ms *ms, bool connected)
+{
+	struct tch_data_state *state = NULL;
+
+	if (ms->tch_state == NULL || ms->tch_state->is_voice) {
+		LOGP(DCC, LOGL_INFO, "No data call is ongoing, "
+		     "ignoring [dis]connection event for CSD socket\n");
+		return;
+	}
+
+	state = &ms->tch_state->data;
+	osmo_v110_ta_set_circuit(state->v110_ta, OSMO_V110_TA_C_108, connected);
+
+	/* GSM/CSD employs the modified 60-bit V.110 frame format, which is basically
+	 * a stripped down version of the nurmal 80-bit V.110 frame without E1/E2/E3
+	 * bits and without the sync pattern.  These 60-bit V.110 frames are perfectly
+	 * aligned with the radio interface block boundaries, so we're always in sync. */
+	if (connected)
+		osmo_v110_ta_sync_ind(state->v110_ta);
+}
diff --git a/src/host/layer23/src/mobile/tch_data_sock.c b/src/host/layer23/src/mobile/tch_data_sock.c
new file mode 100644
index 00000000..7bfebf3e
--- /dev/null
+++ b/src/host/layer23/src/mobile/tch_data_sock.c
@@ -0,0 +1,243 @@
+/*
+ * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * Author: Vadim Yanitskiy <vyanitskiy@sysmocom.de>
+ *
+ * All Rights Reserved
+ *
+ * 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.
+ *
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include <stdint.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/select.h>
+#include <osmocom/core/timer.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/socket.h>
+
+#include <osmocom/bb/common/logging.h>
+#include <osmocom/bb/common/osmocom_data.h>
+#include <osmocom/bb/common/ms.h>
+#include <osmocom/bb/mobile/tch.h>
+
+struct tch_csd_sock_state {
+	struct osmocom_ms *ms;		/* the MS instance we belong to */
+	struct osmo_fd listen_bfd;	/* fd for listen socket */
+	struct osmo_fd conn_bfd;	/* fd for a client connection */
+	struct llist_head rxqueue;
+	struct llist_head txqueue;
+};
+
+void tch_csd_sock_state_cb(struct osmocom_ms *ms, bool connected);
+
+static void tch_csd_sock_close(struct tch_csd_sock_state *state)
+{
+	struct osmo_fd *bfd = &state->conn_bfd;
+
+	LOGP(DMOB, LOGL_NOTICE, "TCH CSD sock has closed connection\n");
+
+	tch_csd_sock_state_cb(state->ms, false);
+
+	osmo_fd_unregister(bfd);
+	close(bfd->fd);
+	bfd->fd = -1;
+
+	/* re-enable the generation of ACCEPT for new connections */
+	osmo_fd_read_enable(&state->listen_bfd);
+
+	/* flush the queues */
+	while (!llist_empty(&state->rxqueue))
+		msgb_free(msgb_dequeue(&state->rxqueue));
+	while (!llist_empty(&state->txqueue))
+		msgb_free(msgb_dequeue(&state->txqueue));
+}
+
+static int tch_csd_sock_read(struct osmo_fd *bfd)
+{
+	struct tch_csd_sock_state *state = (struct tch_csd_sock_state *)bfd->data;
+	struct msgb *msg;
+	int rc;
+
+	msg = msgb_alloc(256, "tch_csd_sock_rx");
+	if (!msg)
+		return -ENOMEM;
+
+	rc = recv(bfd->fd, msg->tail, msgb_tailroom(msg), 0);
+	if (rc == 0)
+		goto close;
+	if (rc < 0) {
+		if (errno == EAGAIN)
+			return 0;
+		goto close;
+	}
+
+	msgb_put(msg, rc);
+	msgb_enqueue(&state->rxqueue, msg);
+	return rc;
+
+close:
+	msgb_free(msg);
+	tch_csd_sock_close(state);
+	return -1;
+}
+
+static int tch_csd_sock_write(struct osmo_fd *bfd)
+{
+	struct tch_csd_sock_state *state = bfd->data;
+
+	while (!llist_empty(&state->txqueue)) {
+		struct msgb *msg;
+		int rc;
+
+		/* dequeue a msgb */
+		msg = msgb_dequeue(&state->txqueue);
+
+		/* try to send it over the socket */
+		rc = write(bfd->fd, msgb_data(msg), msgb_length(msg));
+		if (rc < 0 && errno == EAGAIN) {
+			llist_add(&msg->list, &state->txqueue);
+			return 0;
+		}
+		msgb_free(msg);
+		if (rc <= 0)
+			goto close;
+	}
+
+	osmo_fd_write_disable(bfd);
+	return 0;
+
+close:
+	tch_csd_sock_close(state);
+	return -1;
+}
+
+static int tch_csd_sock_cb(struct osmo_fd *bfd, unsigned int flags)
+{
+	int rc = 0;
+
+	if (flags & OSMO_FD_READ) {
+		rc = tch_csd_sock_read(bfd);
+		if (rc < 0)
+			return rc;
+	}
+
+	if (flags & OSMO_FD_WRITE)
+		rc = tch_csd_sock_write(bfd);
+
+	return rc;
+}
+
+static int tch_csd_sock_accept(struct osmo_fd *bfd, unsigned int flags)
+{
+	struct tch_csd_sock_state *state = (struct tch_csd_sock_state *)bfd->data;
+	struct osmo_fd *conn_bfd = &state->conn_bfd;
+	struct sockaddr_un un_addr;
+	socklen_t len;
+	int rc;
+
+	len = sizeof(un_addr);
+	rc = accept(bfd->fd, (struct sockaddr *)&un_addr, &len);
+	if (rc < 0) {
+		LOGP(DMOB, LOGL_ERROR, "Failed to accept() a new connection\n");
+		return -1;
+	}
+
+	if (conn_bfd->fd >= 0) {
+		LOGP(DMOB, LOGL_NOTICE, "TCH CSD sock already has an active connection\n");
+		close(rc); /* reject this connection request */
+		return 0;
+	}
+
+	osmo_fd_setup(conn_bfd, rc, OSMO_FD_READ, &tch_csd_sock_cb, state, 0);
+	if (osmo_fd_register(conn_bfd) != 0) {
+		LOGP(DMOB, LOGL_ERROR, "osmo_fd_register() failed\n");
+		close(conn_bfd->fd);
+		conn_bfd->fd = -1;
+		return -1;
+	}
+
+	LOGP(DMOB, LOGL_NOTICE, "TCH CSD sock got a connection\n");
+
+	tch_csd_sock_state_cb(state->ms, true);
+
+	return 0;
+}
+
+struct tch_csd_sock_state *tch_csd_sock_init(struct osmocom_ms *ms)
+{
+	const char *sock_path = ms->settings.tch_data.unix_socket_path;
+	struct tch_csd_sock_state *state;
+	struct osmo_fd *bfd;
+	int rc;
+
+	state = talloc_zero(ms, struct tch_csd_sock_state);
+	if (state == NULL)
+		return NULL;
+
+	INIT_LLIST_HEAD(&state->rxqueue);
+	INIT_LLIST_HEAD(&state->txqueue);
+	state->conn_bfd.fd = -1;
+	state->ms = ms;
+
+	bfd = &state->listen_bfd;
+
+	rc = osmo_sock_unix_init_ofd(bfd, SOCK_STREAM, 0, sock_path, OSMO_SOCK_F_BIND);
+	if (rc < 0) {
+		LOGP(DMOB, LOGL_ERROR, "Could not create unix socket: %s\n", strerror(errno));
+		talloc_free(state);
+		return NULL;
+	}
+
+	bfd->cb = &tch_csd_sock_accept;
+	bfd->data = state;
+
+	return state;
+}
+
+void tch_csd_sock_exit(struct tch_csd_sock_state *state)
+{
+	if (state->conn_bfd.fd > -1)
+		tch_csd_sock_close(state);
+	osmo_fd_unregister(&state->listen_bfd);
+	close(state->listen_bfd.fd);
+	talloc_free(state);
+}
+
+void tch_csd_sock_recv(struct tch_csd_sock_state *state, struct msgb *msg)
+{
+	while (msgb_tailroom(msg) > 0) {
+		struct msgb *rmsg = msgb_dequeue(&state->rxqueue);
+		if (rmsg == NULL)
+			break;
+		size_t len = OSMO_MIN(msgb_tailroom(msg), msgb_length(rmsg));
+		memcpy(msgb_put(msg, len), msgb_data(rmsg), len);
+		msgb_pull(rmsg, len);
+		if (msgb_length(rmsg) > 0)
+			llist_add(&rmsg->list, &state->rxqueue);
+		else
+			msgb_free(rmsg);
+	}
+}
+
+void tch_csd_sock_send(struct tch_csd_sock_state *state, struct msgb *msg)
+{
+	msgb_enqueue(&state->txqueue, msg);
+	osmo_fd_write_enable(&state->conn_bfd);
+}