utils: Add a demultiplexer daemon
As described by https://github.com/openbmc/docs/commit/bef44633c6, this
change adds a MCTP multiplexer daemon, allowing clients to register for
receiving messages of a specific type.
Signed-off-by: Jeremy Kerr <jk@ozlabs.org>
diff --git a/utils/mctp-demux-daemon.c b/utils/mctp-demux-daemon.c
new file mode 100644
index 0000000..cc2311b
--- /dev/null
+++ b/utils/mctp-demux-daemon.c
@@ -0,0 +1,492 @@
+#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;
+
+}