daemon/gtp_endpoint.c

/* SPDX-License-Identifier: GPL-2.0 */
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>

#include <pthread.h>

#include <osmocom/core/linuxlist.h>
#include <osmocom/core/socket.h>
#include <osmocom/core/talloc.h>
#include <osmocom/core/logging.h>

#include "gtp.h"
#include "internal.h"

#define LOGEP(ep, lvl, fmt, args ...) \
	LOGP(DEP, lvl, "%s: " fmt, (ep)->name, ## args)

/***********************************************************************
 * GTP Endpoint (UDP socket)
 ***********************************************************************/

/* one thread for reading from each GTP/UDP socket (GTP decapsulation -> tun) */
static void *gtp_endpoint_thread(void *arg)
{
	struct gtp_endpoint *ep = (struct gtp_endpoint *)arg;
	struct gtp_daemon *d = ep->d;

	uint8_t buffer[MAX_UDP_PACKET+sizeof(struct gtp1_header)];

	while (1) {
		struct gtp_tunnel *t;
		const struct gtp1_header *gtph;
		int rc, nread, outfd;
		uint32_t teid;

		/* 1) read GTP packet from UDP socket */
		rc = recvfrom(ep->fd, buffer, sizeof(buffer), 0, (struct sockaddr *)NULL, 0);
		if (rc < 0) {
			LOGEP(ep, LOGL_FATAL, "Error reading from UDP socket: %s\n", strerror(errno));
			exit(1);
		}
		nread = rc;
		if (nread < sizeof(*gtph)) {
			LOGEP(ep, LOGL_NOTICE, "Short read: %d < %lu\n", nread, sizeof(*gtph));
			continue;
		}
		gtph = (struct gtp1_header *)buffer;

		/* check GTP heaader contents */
		if (gtph->flags != 0x30) {
			LOGEP(ep, LOGL_NOTICE, "Unexpected GTP Flags: 0x%02x\n", gtph->flags);
			continue;
		}
		if (gtph->type != GTP_TPDU) {
			LOGEP(ep, LOGL_NOTICE, "Unexpected GTP Message Type: 0x%02x\n", gtph->type);
			continue;
		}
		if (sizeof(*gtph)+ntohs(gtph->length) > nread) {
			LOGEP(ep, LOGL_NOTICE, "Shotr GTP Message: %lu < len=%d\n",
				sizeof(*gtph)+ntohs(gtph->length), nread);
			continue;
		}
		teid = ntohl(gtph->tid);

		/* 2) look-up tunnel based on TEID */
		pthread_rwlock_rdlock(&d->rwlock);
		t = _gtp_tunnel_find_r(d, teid, ep);
		if (!t) {
			pthread_rwlock_unlock(&d->rwlock);
			LOGEP(ep, LOGL_NOTICE, "Unable to find tunnel for TEID=0x%08x\n", teid);
			continue;
		}
		outfd = t->tun_dev->fd;
		pthread_rwlock_unlock(&d->rwlock);

		/* 3) write to TUN device */
		rc = write(outfd, buffer+sizeof(*gtph), ntohs(gtph->length));
		if (rc < nread-sizeof(struct gtp1_header)) {
			LOGEP(ep, LOGL_FATAL, "Error writing to tun device %s\n", strerror(errno));
			exit(1);
		}
	}
}

static struct gtp_endpoint *
_gtp_endpoint_create(struct gtp_daemon *d, const struct sockaddr_storage *bind_addr)
{
	struct gtp_endpoint *ep = talloc_zero(d, struct gtp_endpoint);
	char ipstr[INET6_ADDRSTRLEN];
	char portstr[8];
	int rc;

	if (!ep)
		return NULL;

	rc = getnameinfo((struct sockaddr *)bind_addr, sizeof(*bind_addr),
			 ipstr, sizeof(ipstr), portstr, sizeof(portstr), NI_NUMERICHOST|NI_NUMERICSERV);
	if (rc != 0)
		goto out_free;
	ep->name = talloc_asprintf(ep, "%s:%s", ipstr, portstr);

	ep->d = d;
	ep->use_count = 1;
	ep->bind_addr = *bind_addr;
	ep->fd = socket(ep->bind_addr.ss_family, SOCK_DGRAM, IPPROTO_UDP);
	if (ep->fd < 0) {
		LOGEP(ep, LOGL_ERROR, "Cannot create UDP socket: %s\n", strerror(errno));
		goto out_free;
	}
	rc = bind(ep->fd, (struct sockaddr *) &ep->bind_addr, sizeof(ep->bind_addr));
	if (rc < 0) {
		LOGEP(ep, LOGL_ERROR, "Cannot bind UDP socket: %s\n", strerror(errno));
		goto out_close;
	}

	if (pthread_create(&ep->thread, NULL, gtp_endpoint_thread, ep)) {
		LOGEP(ep, LOGL_ERROR, "Cannot start GTP thread: %s\n", strerror(errno));
		goto out_close;
	}

	llist_add_tail(&ep->list, &d->gtp_endpoints);
	LOGEP(ep, LOGL_INFO, "Created\n");

	return ep;

out_close:
	close(ep->fd);
out_free:
	talloc_free(ep);
	return NULL;
}

struct gtp_endpoint *
_gtp_endpoint_find(struct gtp_daemon *d, const struct sockaddr_storage *bind_addr)
{
	struct gtp_endpoint *ep;

	llist_for_each_entry(ep, &d->gtp_endpoints, list) {
		if (sockaddr_equals((const struct sockaddr *) &ep->bind_addr,
				    (const struct sockaddr *) bind_addr)) {
			return ep;
		}
	}
	return NULL;
}

struct gtp_endpoint *
gtp_endpoint_find_or_create(struct gtp_daemon *d, const struct sockaddr_storage *bind_addr)
{
	struct gtp_endpoint *ep;

	/* talloc is not thread safe, all alloc/free must come from main thread */
	ASSERT_MAIN_THREAD(d);

	pthread_rwlock_wrlock(&d->rwlock);
	ep = _gtp_endpoint_find(d, bind_addr);
	if (ep)
		ep->use_count++;
	else
		ep = _gtp_endpoint_create(d, bind_addr);
	pthread_rwlock_unlock(&d->rwlock);

	return ep;
}

/* UNLOCKED hard/forced destroy; caller must make sure references are cleaned up */
static void _gtp_endpoint_destroy(struct gtp_endpoint *ep)
{
	/* talloc is not thread safe, all alloc/free must come from main thread */
	ASSERT_MAIN_THREAD(ep->d);

	if (ep->use_count)
		LOGEP(ep, LOGL_ERROR, "Destroying despite use_count %lu != 0\n", ep->use_count);
	else
		LOGEP(ep, LOGL_INFO, "Destroying\n");

	pthread_cancel(ep->thread);
	llist_del(&ep->list);
	close(ep->fd);
	talloc_free(ep);
}

/* UNLOCKED remove all objects referencing this ep and then destroy */
void _gtp_endpoint_deref_destroy(struct gtp_endpoint *ep)
{
	struct gtp_daemon *d = ep->d;
	struct sockaddr_storage ss = ep->bind_addr;
	struct gtp_tunnel *t, *t2;
	struct gtp_endpoint *ep2;

	/* talloc is not thread safe, all alloc/free must come from main thread */
	ASSERT_MAIN_THREAD(ep->d);

	/* iterate over all tunnels; delete all references to ep */
	llist_for_each_entry_safe(t, t2, &d->gtp_tunnels, list) {
		if (t->gtp_ep == ep)
			_gtp_tunnel_destroy(t);
	}

	/* _gtp_endpoint_destroy may already have been called via
	 * _gtp_tunnel_destroy -> gtp_endpoint_release, so we have to
	 * check if the ep can still be found in the list */
	ep2 = _gtp_endpoint_find(d, &ss);
	if (ep2 && ep2 == ep)
		_gtp_endpoint_destroy(ep2);
}

/* UNLOCKED release a reference; destroy if refcount drops to 0 */
bool _gtp_endpoint_release(struct gtp_endpoint *ep)
{
	bool released = false;

	/* talloc is not thread safe, all alloc/free must come from main thread */
	ASSERT_MAIN_THREAD(ep->d);

	ep->use_count--;
	if (ep->use_count == 0) {
		_gtp_endpoint_destroy(ep);
		released = true;
	} else
		LOGEP(ep, LOGL_DEBUG, "Release; new use_count=%lu\n", ep->use_count);

	return released;
}


/* release a reference; destroy if refcount drops to 0 */
bool gtp_endpoint_release(struct gtp_endpoint *ep)
{
	struct gtp_daemon *d = ep->d;
	bool released;

	/* talloc is not thread safe, all alloc/free must come from main thread */
	ASSERT_MAIN_THREAD(ep->d);

	pthread_rwlock_wrlock(&d->rwlock);
	released = _gtp_endpoint_release(ep);
	pthread_rwlock_unlock(&d->rwlock);

	return released;
}