osmo-e1d/src/octoi/octoi_sock.c

/*
 * octoi_sock.c - OCTOI Socket handling code
 *
 * (C) 2022 by Harald Welte <laforge@osmocom.org>
 *
 * 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.
 */

#include <stdint.h>
#include <errno.h>
#include <stdbool.h>
#include <unistd.h>

#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>

#include <osmocom/core/linuxlist.h>
#include <osmocom/core/utils.h>
#include <osmocom/core/select.h>
#include <osmocom/core/socket.h>
#include <osmocom/core/sockaddr_str.h>
#include <osmocom/core/msgb.h>
#include <osmocom/core/logging.h>

#include <osmocom/octoi/e1oip_proto.h>

#include "octoi_sock.h"
#include "e1oip.h"

/* determine domain / AF of socket */
static int sock_get_domain(int fd)
{
	int domain;
	socklen_t dom_len = sizeof(domain);
	int rc;

	rc = getsockopt(fd, SOL_SOCKET, SO_DOMAIN, &domain, &dom_len);
	if (rc < 0)
		return rc;

	return domain;
}

/* typical number of bytes in IP + UDP header for given socket */
static int sock_get_iph_udph_overhead(int fd)
{
	int rc = sock_get_domain(fd);
	if (rc < 0) {
		LOGP(DLINP, LOGL_ERROR, "Unable to determine domain of socket %d: %s\n",
		     fd, strerror(errno));
		goto assume_ipv4;
	}

	switch (rc) {
	case AF_INET6:
		return 40 + 8;
	case AF_INET:
		return 20 + 8;
	default:
		LOGP(DLINP, LOGL_ERROR, "Unknown domain %d of socket %d\n", rc, fd);
		break;
	}

assume_ipv4:
	return 20 + 8;
}

/***********************************************************************
 * transmit to remote peer
 ***********************************************************************/

/* transmit something to an octoi peer */
int octoi_tx(struct octoi_peer *peer, uint8_t msg_type, uint8_t flags,
	     const void *data, size_t len)
{
	struct e1oip_hdr hdr = {
		.version = E1OIP_VERSION,
		.flags = flags & 0xf,
		.msg_type = msg_type,
	};
	struct iovec iov[2] = {
		{
			.iov_base = (void *) &hdr,
			.iov_len = sizeof(hdr),
		}, {
			.iov_base = (void *) data,
			.iov_len = len,
		}
	};
	struct msghdr msgh = {
		.msg_name = &peer->remote,
		.msg_namelen = sizeof(peer->remote),
		.msg_iov = iov,
		.msg_iovlen = ARRAY_SIZE(iov),
		.msg_control = NULL,
		.msg_controllen = 0,
		.msg_flags = 0,
	};
	int rc;

	rc = sendmsg(peer->sock->ofd.fd, &msgh, 0);
	if (rc < 0)
		LOGPEER(peer, LOGL_ERROR, "Error in sendmsg: %s\n", strerror(errno));
	else if (rc != (int) (sizeof(hdr) + len))
		LOGPEER(peer, LOGL_ERROR, "Short write in  sendmsg: %d != %zu\n", rc, sizeof(hdr)+len);

	return rc;
}

static int _octoi_tx_echo(struct octoi_peer *peer, bool is_req, uint16_t seq_nr,
			  const uint8_t *data, size_t data_len)
{
	enum e1oip_msgtype msgt;
	struct {
		struct e1oip_echo echo;
		uint8_t buf[data_len];
	} u;

	u.echo.seq_nr = htons(seq_nr);
	if (data && data_len)
		memcpy(u.echo.data, data, data_len);

	if (is_req)
		msgt = E1OIP_MSGT_ECHO_REQ;
	else
		msgt = E1OIP_MSGT_ECHO_RESP;

	return octoi_tx(peer, msgt, 0, &u, sizeof(u));
}

int octoi_tx_echo_req(struct octoi_peer *peer, uint16_t seq_nr, const uint8_t *data, size_t data_len)
{
	LOGPEER(peer, LOGL_DEBUG, "Tx ECHO_REQ\n");
	return _octoi_tx_echo(peer, true, seq_nr, data, data_len);
}

int octoi_tx_echo_resp(struct octoi_peer *peer, uint16_t seq_nr, const uint8_t *data, size_t data_len)
{
	LOGPEER(peer, LOGL_DEBUG, "Tx ECHO_RESP\n");
	return _octoi_tx_echo(peer, false, seq_nr, data, data_len);
}

int octoi_tx_service_req(struct octoi_peer *peer, uint32_t service, const char *subscr_id,
			 const char *software_id, const char *software_version,
			 uint32_t capability_flags)
{
	struct e1oip_service_req service_req;

	memset(&service_req, 0, sizeof(service_req));
	service_req.requested_service = htonl(service);
	OSMO_STRLCPY_ARRAY(service_req.subscriber_id, subscr_id);
	OSMO_STRLCPY_ARRAY(service_req.software_id, software_id);
	OSMO_STRLCPY_ARRAY(service_req.software_version, software_version);
	service_req.capability_flags = htonl(capability_flags);

	LOGPEER(peer, LOGL_INFO, "Tx SERVICE_REQ\n");
	return octoi_tx(peer, E1OIP_MSGT_SERVICE_REQ, 0, &service_req, sizeof(service_req));
}

int octoi_tx_redir_cmd(struct octoi_peer *peer, const char *server_ip, uint16_t server_port)
{
	struct e1oip_redir_cmd redir;

	memset(&redir, 0, sizeof(redir));
	OSMO_STRLCPY_ARRAY(redir.server_ip, server_ip);
	redir.server_port = htons(server_port);

	LOGPEER(peer, LOGL_INFO, "Tx REDIR_CMD\n");
	return octoi_tx(peer, E1OIP_MSGT_REDIR_CMD, 0, &redir, sizeof(redir));
}

int octoi_tx_auth_req(struct octoi_peer *peer, uint8_t rand_len, const uint8_t *rand,
		      uint8_t autn_len, const uint8_t *autn)
{
	struct e1oip_auth_req areq;

	memset(&areq, 0, sizeof(areq));

	OSMO_ASSERT(rand_len <= sizeof(areq.rand));
	OSMO_ASSERT(autn_len <= sizeof(areq.autn));

	areq.rand_len = rand_len;
	memcpy(areq.rand, rand, rand_len);
	areq.autn_len = autn_len;
	memcpy(areq.autn, autn, autn_len);

	LOGPEER(peer, LOGL_INFO, "Tx AUTH_REQ\n");
	return octoi_tx(peer, E1OIP_MSGT_AUTH_REQ, 0, &areq, sizeof(areq));
}


int octoi_tx_auth_resp(struct octoi_peer *peer, uint8_t res_len, const uint8_t *res,
		      uint8_t auts_len, const uint8_t *auts)
{
	struct e1oip_auth_resp aresp;

	memset(&aresp, 0, sizeof(aresp));

	OSMO_ASSERT(res_len <= sizeof(aresp.res));
	OSMO_ASSERT(auts_len <= sizeof(aresp.auts));

	aresp.res_len = res_len;
	memcpy(aresp.res, res, res_len);
	aresp.auts_len = auts_len;
	memcpy(aresp.auts, auts, auts_len);

	LOGPEER(peer, LOGL_INFO, "Tx AUTH_RESP\n");
	return octoi_tx(peer, E1OIP_MSGT_AUTH_RESP, 0, &aresp, sizeof(aresp));
}

int octoi_tx_service_ack(struct octoi_peer *peer, uint32_t assigned_service,
			 const char *server_id, const char *software_id,
			 const char *software_version, uint32_t capability_flags)
{
	struct e1oip_service_ack service_ack;

	memset(&service_ack, 0, sizeof(service_ack));
	service_ack.assigned_service = htonl(assigned_service);
	OSMO_STRLCPY_ARRAY(service_ack.server_id, server_id);
	OSMO_STRLCPY_ARRAY(service_ack.software_id, software_id);
	OSMO_STRLCPY_ARRAY(service_ack.software_version, software_version);
	service_ack.capability_flags = htonl(capability_flags);

	LOGPEER(peer, LOGL_INFO, "Tx SERVICE_ACK\n");
	return octoi_tx(peer, E1OIP_MSGT_SERVICE_ACK, 0, &service_ack, sizeof(service_ack));
}

int octoi_tx_service_rej(struct octoi_peer *peer, uint32_t rejected_service, const char *message)
{
	struct e1oip_service_rej service_rej;

	memset(&service_rej, 0, sizeof(service_rej));
	service_rej.rejected_service = htonl(rejected_service);
	OSMO_STRLCPY_ARRAY(service_rej.reject_message, message);

	LOGPEER(peer, LOGL_INFO, "Tx SERVICE_REJ\n");
	return octoi_tx(peer, E1OIP_MSGT_SERVICE_REJ, 0, &service_rej, sizeof(service_rej));
}

int octoi_tx_error_ind(struct octoi_peer *peer, uint32_t cause, const char *message,
		       const uint8_t *orig, size_t orig_len)
{
	struct {
		struct e1oip_error_ind error_ind;
		uint8_t orig[orig_len];
	} u;

	u.error_ind.cause = htonl(cause);
	OSMO_STRLCPY_ARRAY(u.error_ind.error_message, message);
	memcpy(&u.orig, orig, orig_len);

	LOGPEER(peer, LOGL_INFO, "Tx ERROR_IND\n");
	return octoi_tx(peer, E1OIP_MSGT_ERROR_IND, 0, &u, sizeof(u));
}


/***********************************************************************
 * socket
 ***********************************************************************/

static int sockaddr_cmp(const struct sockaddr *x, const struct sockaddr *y)
{
	if (x->sa_family != y->sa_family)
		return -1;

	if (x->sa_family == AF_UNIX) {
		const struct sockaddr_un *xun = (void *)x, *yun = (void *)y;
		int r = strcmp(xun->sun_path, yun->sun_path);
		if (r != 0)
			return r;
	} else if (x->sa_family == AF_INET) {
		const struct sockaddr_in *xin = (void *)x, *yin = (void *)y;
		if (xin->sin_addr.s_addr != yin->sin_addr.s_addr)
			return -1;
		if (xin->sin_port != yin->sin_port)
			return -1;
	} else if (x->sa_family == AF_INET6) {
		const struct sockaddr_in6 *xin6 = (void *)x, *yin6 = (void *)y;
		int r = memcmp(xin6->sin6_addr.s6_addr, yin6->sin6_addr.s6_addr, sizeof(xin6->sin6_addr.s6_addr));
		if (r != 0)
			return r;
		if (xin6->sin6_port != yin6->sin6_port)
			return -1;
		if (xin6->sin6_flowinfo != yin6->sin6_flowinfo)
			return -1;
		if (xin6->sin6_scope_id != yin6->sin6_scope_id)
			return -1;
	} else {
		OSMO_ASSERT(0);
	}
	return 0;
}

static struct octoi_peer *find_peer_by_sockaddr(struct octoi_sock *sock, const struct sockaddr *sa)
{
	struct octoi_peer *peer;

	llist_for_each_entry(peer, &sock->peers, list) {
		if (!sockaddr_cmp(sa, (struct sockaddr *) &peer->remote))
			return peer;
	}
	return NULL;
}

static struct octoi_peer *
alloc_peer(struct octoi_sock *sock, const struct sockaddr *sa, socklen_t sa_len)
{
	struct octoi_peer *peer = talloc_zero(sock, struct octoi_peer);

	if (!peer)
		return NULL;

	OSMO_ASSERT(sa_len <= sizeof(peer->remote));
	memcpy(&peer->remote, sa, sa_len);

	peer->sock = sock;
	llist_add_tail(&peer->list, &sock->peers);

	return peer;
}

static int octoi_fd_cb(struct osmo_fd *ofd, unsigned int what)
{
	struct octoi_sock *sock = ofd->data;
	struct msgb *msg;
	struct sockaddr_storage ss_remote;
	socklen_t ss_remote_len = sizeof(ss_remote);
	struct octoi_peer *peer;
	int rc;

	if (what & OSMO_FD_WRITE) {
		LOGP(DLINP, LOGL_INFO, "non-blocking connect succeeded\n");
		osmo_fd_write_disable(ofd);
	}


	if (what & OSMO_FD_READ) {
		msg = msgb_alloc_c(sock, 2048, "OCTOI Rx");
		OSMO_ASSERT(msg);
		rc = recvfrom(ofd->fd, msgb_data(msg), msgb_tailroom(msg), 0,
				(struct sockaddr *) &ss_remote, &ss_remote_len);
		if (rc <= 0) {
			msgb_free(msg);
			return -1;
		}
		msgb_put(msg, rc);
		msg->l1h = msg->data;
		if (msgb_l1len(msg) < sizeof(struct e1oip_hdr)) {
			msgb_free(msg);
			return -2;
		}
		msg->l2h = msg->l1h + sizeof(struct e1oip_hdr);

		/* look-up octoi_peer based on remote address */
		peer = find_peer_by_sockaddr(sock, (struct sockaddr *) &ss_remote);
		if (!peer) {
			peer = alloc_peer(sock, (struct sockaddr *) &ss_remote, ss_remote_len);
			if (peer) {
				osmo_sockaddr_str_from_sockaddr(&peer->cfg.remote, &ss_remote);
				osmo_talloc_replace_string_fmt(peer, &peer->name, OSMO_SOCKADDR_STR_FMT,
								OSMO_SOCKADDR_STR_FMT_ARGS(&peer->cfg.remote));
				LOGPEER(peer, LOGL_INFO, "peer created\n");
			}
		}
		OSMO_ASSERT(peer);

		/* dispatch received message to peer */
		rc = sock->rx_cb(peer, msg);
		if (rc < 0)
			return rc;
	}

	return 0;
}

void octoi_peer_destroy(struct octoi_peer *peer)
{
	if (!peer)
		return;

	peer->tdm_permitted = false;
	peer->sock = NULL;
	e1oip_line_destroy(peer->iline);

	llist_del(&peer->list);
	talloc_free(peer);
}

static struct octoi_sock *octoi_sock_create(void *ctx)
{
	struct octoi_sock *sock = talloc_zero(ctx, struct octoi_sock);

	if (!sock)
		return NULL;

	INIT_LLIST_HEAD(&sock->peers);
	osmo_fd_setup(&sock->ofd, -1, OSMO_FD_READ, octoi_fd_cb, sock, 0);

	return sock;
}

struct octoi_sock *octoi_sock_create_server(void *ctx, void *priv, const struct osmo_sockaddr_str *local)
{
	struct octoi_sock *sock = octoi_sock_create(ctx);
	struct sockaddr_storage sa_local;
	int rc;

	OSMO_ASSERT(sock);

	sock->priv = priv;
	sock->cfg.server_mode = true;
	sock->cfg.local = *local;

	/* bind to local addr/port; don't connect to any remote as we have many */
	osmo_sockaddr_str_to_sockaddr(&sock->cfg.local, &sa_local);
	rc = osmo_sock_init_osa_ofd(&sock->ofd, SOCK_DGRAM, IPPROTO_UDP,
				    (struct osmo_sockaddr *) &sa_local, NULL,
				    OSMO_SOCK_F_BIND | OSMO_SOCK_F_NONBLOCK);

	if (rc < 0) {
		LOGP(DLINP, LOGL_ERROR, "Unable to create OCTOI server socket\n");
		talloc_free(sock);
		return NULL;
	}

	LOGP(DLINP, LOGL_NOTICE, "OCTOI server socket at "OSMO_SOCKADDR_STR_FMT"\n",
		OSMO_SOCKADDR_STR_FMT_ARGS(local));

	sock->iph_udph_size = sock_get_iph_udph_overhead(sock->ofd.fd);

	return sock;
}

struct octoi_sock *octoi_sock_create_client(void *ctx, void *priv, const struct osmo_sockaddr_str *local,
					    const struct osmo_sockaddr_str *remote)
{
	struct octoi_sock *sock = octoi_sock_create(ctx);
	struct sockaddr_storage sa_remote;
	struct octoi_peer *peer;
	int rc;

	OSMO_ASSERT(sock);

	sock->priv = priv;
	sock->cfg.server_mode = false;
	if (local)
		sock->cfg.local = *local;

	/* bind to local addr/port; don't connect to any remote as we have many */
	osmo_sockaddr_str_to_sockaddr(remote, &sa_remote);
	if (local) {
		struct sockaddr_storage sa_local;
		osmo_sockaddr_str_to_sockaddr(&sock->cfg.local, &sa_local);
		rc = osmo_sock_init_osa_ofd(&sock->ofd, SOCK_DGRAM, IPPROTO_UDP,
					    (struct osmo_sockaddr *) &sa_local,
					    (struct osmo_sockaddr *) &sa_remote,
					    OSMO_SOCK_F_CONNECT | OSMO_SOCK_F_BIND | OSMO_SOCK_F_NONBLOCK);
	} else {
		rc = osmo_sock_init_osa_ofd(&sock->ofd, SOCK_DGRAM, IPPROTO_UDP,
					    NULL, (struct osmo_sockaddr *) &sa_remote,
					    OSMO_SOCK_F_CONNECT | OSMO_SOCK_F_NONBLOCK);
	}
	if (rc < 0) {
		LOGP(DLINP, LOGL_ERROR, "Unable to create OCTOI client socket\n");
		talloc_free(sock);
		return NULL;
	}

	LOGP(DLINP, LOGL_NOTICE, "OCTOI client socket to "OSMO_SOCKADDR_STR_FMT"\n",
		OSMO_SOCKADDR_STR_FMT_ARGS(remote));

	sock->iph_udph_size = sock_get_iph_udph_overhead(sock->ofd.fd);

	/* create [the only] peer */
	peer = alloc_peer(sock, (struct sockaddr *) &sa_remote, sizeof(sa_remote));
	peer->cfg.remote = *remote;
	osmo_talloc_replace_string_fmt(peer, &peer->name, OSMO_SOCKADDR_STR_FMT,
					OSMO_SOCKADDR_STR_FMT_ARGS(remote));

	return sock;
}

int octoi_sock_set_dscp(struct octoi_sock *sock, uint8_t dscp)
{
	return osmo_sock_set_dscp(sock->ofd.fd, dscp);
}

int octoi_sock_set_priority(struct octoi_sock *sock, uint8_t priority)
{
	return osmo_sock_set_priority(sock->ofd.fd, priority);
}

void octoi_sock_destroy(struct octoi_sock *sock)
{
	struct octoi_peer *p1, *p2;

	if (!sock)
		return;

	llist_for_each_entry_safe(p1, p2, &sock->peers, list) {
		OSMO_ASSERT(p1->sock == sock);
		p1->sock = NULL;
		/* FIXME: destroy FSM / priv */
		llist_del(&p1->list);
		talloc_free(p1);
	}

	osmo_fd_unregister(&sock->ofd);
	close(sock->ofd.fd);

	LOGP(DLINP, LOGL_NOTICE, "OCTOI %s socket destroyed\n",
		sock->cfg.server_mode ? "server" : "client");

	talloc_free(sock);
}

/* return the (only) peer of a octoi_sock client */
struct octoi_peer *octoi_sock_client_get_peer(struct octoi_sock *sock)
{
	if (!sock)
		return NULL;
	OSMO_ASSERT(!sock->cfg.server_mode);
	OSMO_ASSERT(llist_count(&sock->peers) == 1);
	return llist_entry(sock->peers.next, struct octoi_peer, list);
}