utils/mctp-demux-daemon.c

#define _GNU_SOURCE

#include <assert.h>
#include <err.h>
#include <getopt.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

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

#include "libmctp.h"
#include "libmctp-serial.h"
#include "libmctp-astlpc.h"

#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))

static const mctp_eid_t local_eid_default = 8;
static char sockname[] = "\0mctp-mux";

struct binding {
	const char	*name;
	int		(*init)(struct mctp *mctp, struct binding *binding,
				mctp_eid_t eid, int n_params,
				char * const * params);
	int		(*get_fd)(struct binding *binding);
	int		(*process)(struct binding *binding);
	void		*data;
};

struct client {
	bool		active;
	int		sock;
	uint8_t		type;
};

struct ctx {
	struct mctp	*mctp;
	struct binding	*binding;
	bool		verbose;
	int		local_eid;

	int		sock;
	struct pollfd	*pollfds;

	struct client	*clients;
	int		n_clients;
};

static void tx_message(struct ctx *ctx, mctp_eid_t eid, void *msg, size_t len)
{
	mctp_message_tx(ctx->mctp, eid, msg, len);
}

static void client_remove_inactive(struct ctx *ctx)
{
	int i;

	for (i = 0; i < ctx->n_clients; i++) {
		struct client *client = &ctx->clients[i];
		if (client->active)
			continue;
		close(client->sock);

		ctx->n_clients--;
		memmove(&ctx->clients[i], &ctx->clients[i+1],
				(ctx->n_clients - i) * sizeof(*ctx->clients));
		ctx->clients = realloc(ctx->clients,
				ctx->n_clients * sizeof(*ctx->clients));
	}
}

static void rx_message(uint8_t eid, void *data, void *msg, size_t len)
{
	struct ctx *ctx = data;
	struct iovec iov[2];
	struct msghdr msghdr;
	bool removed;
	uint8_t type;
	int i, rc;

	if (len < 2)
		return;

	type = *(uint8_t *)msg;

	if (ctx->verbose)
		fprintf(stderr, "MCTP message received: len %zd, type %d\n",
				len, type);

	memset(&msghdr, 0, sizeof(msghdr));
	msghdr.msg_iov = iov;
	msghdr.msg_iovlen = 2;
	iov[0].iov_base = &eid;
	iov[0].iov_len = 1;
	iov[1].iov_base = msg;
	iov[1].iov_len = len;

	for (i = 0; i < ctx->n_clients; i++) {
		struct client *client = &ctx->clients[i];

		if (client->type != type)
			continue;

		if (ctx->verbose)
			fprintf(stderr, "  forwarding to client %d\n", i);

		rc = sendmsg(client->sock, &msghdr, 0);
		if (rc != (ssize_t)(len + 1)) {
			client->active = false;
			removed = true;
		}
	}

	if (removed)
		client_remove_inactive(ctx);

}

static int binding_serial_init(struct mctp *mctp, struct binding *binding,
		mctp_eid_t eid, int n_params, char * const *params)
{
	struct mctp_binding_serial *serial;
	const char *path;
	int rc;

	if (n_params != 1) {
		warnx("serial binding requires device param");
		return -1;
	}

	path = params[0];

	serial = mctp_serial_init();
	assert(serial);

	rc = mctp_serial_open_path(serial, path);
	if (rc)
		return -1;

	mctp_serial_register_bus(serial, mctp, eid);

	binding->data = serial;

	return 0;
}

static int binding_serial_get_fd(struct binding *binding)
{
	return mctp_serial_get_fd(binding->data);
}

static int binding_serial_process(struct binding *binding)
{
	return mctp_serial_read(binding->data);
}

static int binding_astlpc_init(struct mctp *mctp, struct binding *binding,
		mctp_eid_t eid, int n_params,
		char * const *params __attribute__((unused)))
{
	struct mctp_binding_astlpc *astlpc;

	if (n_params) {
		warnx("astlpc binding does not accept parameters");
		return -1;
	}

	astlpc = mctp_astlpc_init();
	if (!astlpc) {
		warnx("could not initialise astlpc binding");
		return -1;
	}

	mctp_astlpc_register_bus(astlpc, mctp, eid);
	binding->data = astlpc;
	return 0;
}

static int binding_astlpc_get_fd(struct binding *binding)
{
	return mctp_astlpc_get_fd(binding->data);
}

static int binding_astlpc_process(struct binding *binding)
{
	return mctp_astlpc_poll(binding->data);
}

struct binding bindings[] = {
	{
		.name = "serial",
		.init = binding_serial_init,
		.get_fd = binding_serial_get_fd,
		.process = binding_serial_process,
	},
	{
		.name = "astlpc",
		.init = binding_astlpc_init,
		.get_fd = binding_astlpc_get_fd,
		.process = binding_astlpc_process,
	}
};

struct binding *binding_lookup(const char *name)
{
	struct binding *binding;
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(bindings); i++) {
		binding = &bindings[i];

		if (!strcmp(binding->name, name))
			return binding;
	}

	return NULL;
}

static int socket_init(struct ctx *ctx)
{
	struct sockaddr_un addr;
	int namelen, rc;

	namelen = sizeof(sockname) - 1;
	addr.sun_family = AF_UNIX;
	memcpy(addr.sun_path, sockname, namelen);

	ctx->sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
	if (ctx->sock < 0) {
		warn("can't create socket");
		return -1;
	}

	rc = bind(ctx->sock, (struct sockaddr *)&addr,
			sizeof(addr.sun_family) + namelen);
	if (rc) {
		warn("can't bind socket");
		goto err_close;
	}

	rc = listen(ctx->sock, 1);
	if (rc) {
		warn("can't listen on socket");
		goto err_close;
	}

	return 0;

err_close:
	close(ctx->sock);
	return -1;
}

static int socket_process(struct ctx *ctx)
{
	struct client *client;
	int fd;

	fd = accept4(ctx->sock, NULL, 0, SOCK_NONBLOCK);
	if (fd < 0)
		return -1;

	ctx->n_clients++;
	ctx->clients = realloc(ctx->clients,
			ctx->n_clients * sizeof(struct client));

	client = &ctx->clients[ctx->n_clients-1];
	memset(client, 0, sizeof(client));
	client->active = true;
	client->sock = fd;

	return 0;
}

static int client_process_recv(struct ctx *ctx, int idx)
{
	struct client *client = &ctx->clients[idx];
	uint8_t buf[4096];
	int rc;

	/* are we waiting for a type message? */
	if (!client->type) {
		uint8_t type;
		rc = read(client->sock, &type, 1);
		if (rc <= 0)
			goto out_close;

		if (type == 0) {
			rc = -1;
			goto out_close;
		}
		if (ctx->verbose)
			fprintf(stderr, "client[%d] registered for type %u\n",
					idx, type);
		client->type = type;
		return 0;
	}

	/* todo: size detection through MSG_TRUNC or MSG_PEEK */
	rc = recv(client->sock, buf, sizeof(buf), 0);
	if (rc < 0) {
		warn("can't receive from client");
		goto out_close;
	}

	if (rc == 0) {
		rc = -1;
		goto out_close;
	}

	if (rc > 0) {
		uint8_t eid = buf[0];
		if (ctx->verbose)
			fprintf(stderr,
				"client[%d] sent message: dest 0x%02x len %d\n",
				idx, eid, rc - 1);

		tx_message(ctx, eid, buf + 1, rc - 1);
	}

	return 0;

out_close:
	client->active = false;
	return rc;
}

static int binding_init(struct ctx *ctx, const char *name,
		int argc, char * const *argv)
{
	int rc;

	ctx->binding = binding_lookup(name);
	if (!ctx->binding) {
		warnx("no such binding '%s'", name);
		return -1;
	}

	rc = ctx->binding->init(ctx->mctp, ctx->binding, ctx->local_eid,
			argc, argv);
	return rc;
}

enum {
	FD_BINDING = 0,
	FD_SOCKET,
	FD_NR,
};

static int run_daemon(struct ctx *ctx)
{
	bool clients_changed = false;
	int rc, i;

	ctx->pollfds = malloc(FD_NR * sizeof(struct pollfd));

	ctx->pollfds[FD_BINDING].fd =
		ctx->binding->get_fd(ctx->binding);
	ctx->pollfds[FD_BINDING].events = POLLIN;

	ctx->pollfds[FD_SOCKET].fd = ctx->sock;
	ctx->pollfds[FD_SOCKET].events = POLLIN;

	mctp_set_rx_all(ctx->mctp, rx_message, ctx);

	for (;;) {
		if (clients_changed) {
			int i;

			ctx->pollfds = realloc(ctx->pollfds,
					(ctx->n_clients + FD_NR) *
						sizeof(struct pollfd));

			for (i = 0; i < ctx->n_clients; i++) {
				ctx->pollfds[FD_NR+i].fd =
					ctx->clients[i].sock;
				ctx->pollfds[FD_NR+i].events = POLLIN;
			}
			clients_changed = false;
		}

		rc = poll(ctx->pollfds, ctx->n_clients + FD_NR, -1);
		if (rc < 0) {
			warn("poll failed");
			break;
		}

		if (!rc)
			continue;

		if (ctx->pollfds[FD_BINDING].revents) {
			rc = ctx->binding->process(ctx->binding);
			if (rc)
				break;
		}

		for (i = 0; i < ctx->n_clients; i++) {
			if (!ctx->pollfds[FD_NR+i].revents)
				continue;

			rc = client_process_recv(ctx, i);
			if (rc)
				clients_changed = true;
		}

		if (ctx->pollfds[FD_SOCKET].revents) {
			rc = socket_process(ctx);
			if (rc)
				break;
			clients_changed = true;
		}

		if (clients_changed)
			client_remove_inactive(ctx);

	}


	free(ctx->pollfds);

	return rc;
}

static const struct option options[] = {
	{ "verbose", no_argument, 0, 'v' },
	{ "eid", required_argument, 0, 'e' },
	{ 0 },
};

static void usage(const char *progname)
{
	unsigned int i;

	fprintf(stderr, "usage: %s <binding> [params]\n", progname);
	fprintf(stderr, "Available bindings:\n");
	for (i = 0; i < ARRAY_SIZE(bindings); i++)
		fprintf(stderr, "  %s\n", bindings[i].name);
}

int main(int argc, char * const *argv)
{
	struct ctx *ctx, _ctx;
	int rc;

	ctx = &_ctx;
	ctx->clients = NULL;
	ctx->n_clients = 0;
	ctx->local_eid = local_eid_default;

	for (;;) {
		rc = getopt_long(argc, argv, "e:v", options, NULL);
		if (rc == -1)
			break;
		switch (rc) {
		case 'v':
			ctx->verbose = true;
			break;
		case 'e':
			ctx->local_eid = atoi(optarg);
			break;
		default:
			fprintf(stderr, "Invalid argument\n");
			return EXIT_FAILURE;
		}
	}

	if (optind >= argc) {
		fprintf(stderr, "missing binding argument\n");
		usage(argv[0]);
		return EXIT_FAILURE;
	}

	ctx->mctp = mctp_init();
	assert(ctx->mctp);

	rc = binding_init(ctx, argv[optind], argc - optind - 1, argv + optind + 1);
	if (rc)
		return EXIT_FAILURE;

	rc = socket_init(ctx);
	if (rc)
		return EXIT_FAILURE;

	rc = run_daemon(ctx);

	return rc ? EXIT_FAILURE : EXIT_SUCCESS;

}