1 /* SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later */ 2 3 #define _GNU_SOURCE 4 5 #include <assert.h> 6 #include <err.h> 7 #include <errno.h> 8 #include <getopt.h> 9 #include <limits.h> 10 #include <poll.h> 11 #include <stdbool.h> 12 #include <stdio.h> 13 #include <stdlib.h> 14 #include <string.h> 15 #include <unistd.h> 16 17 #include <sys/socket.h> 18 #include <sys/un.h> 19 20 #include "libmctp.h" 21 #include "libmctp-serial.h" 22 #include "libmctp-astlpc.h" 23 24 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) 25 #define __unused __attribute__((unused)) 26 27 static const mctp_eid_t local_eid_default = 8; 28 static char sockname[] = "\0mctp-mux"; 29 30 struct binding { 31 const char *name; 32 int (*init)(struct mctp *mctp, struct binding *binding, 33 mctp_eid_t eid, int n_params, 34 char * const * params); 35 int (*get_fd)(struct binding *binding); 36 int (*process)(struct binding *binding); 37 void *data; 38 }; 39 40 struct client { 41 bool active; 42 int sock; 43 uint8_t type; 44 }; 45 46 struct ctx { 47 struct mctp *mctp; 48 struct binding *binding; 49 bool verbose; 50 int local_eid; 51 void *buf; 52 size_t buf_size; 53 54 int sock; 55 struct pollfd *pollfds; 56 57 struct client *clients; 58 int n_clients; 59 }; 60 61 static void tx_message(struct ctx *ctx, mctp_eid_t eid, void *msg, size_t len) 62 { 63 int rc; 64 65 rc = mctp_message_tx(ctx->mctp, eid, msg, len); 66 if (rc) 67 warnx("Failed to send message: %d", rc); 68 } 69 70 static void client_remove_inactive(struct ctx *ctx) 71 { 72 int i; 73 74 for (i = 0; i < ctx->n_clients; i++) { 75 struct client *client = &ctx->clients[i]; 76 if (client->active) 77 continue; 78 close(client->sock); 79 80 ctx->n_clients--; 81 memmove(&ctx->clients[i], &ctx->clients[i+1], 82 (ctx->n_clients - i) * sizeof(*ctx->clients)); 83 ctx->clients = realloc(ctx->clients, 84 ctx->n_clients * sizeof(*ctx->clients)); 85 } 86 } 87 88 static void rx_message(uint8_t eid, void *data, void *msg, size_t len) 89 { 90 struct ctx *ctx = data; 91 struct iovec iov[2]; 92 struct msghdr msghdr; 93 bool removed; 94 uint8_t type; 95 int i, rc; 96 97 if (len < 2) 98 return; 99 100 type = *(uint8_t *)msg; 101 102 if (ctx->verbose) 103 fprintf(stderr, "MCTP message received: len %zd, type %d\n", 104 len, type); 105 106 memset(&msghdr, 0, sizeof(msghdr)); 107 msghdr.msg_iov = iov; 108 msghdr.msg_iovlen = 2; 109 iov[0].iov_base = &eid; 110 iov[0].iov_len = 1; 111 iov[1].iov_base = msg; 112 iov[1].iov_len = len; 113 114 for (i = 0; i < ctx->n_clients; i++) { 115 struct client *client = &ctx->clients[i]; 116 117 if (client->type != type) 118 continue; 119 120 if (ctx->verbose) 121 fprintf(stderr, " forwarding to client %d\n", i); 122 123 rc = sendmsg(client->sock, &msghdr, 0); 124 if (rc != (ssize_t)(len + 1)) { 125 client->active = false; 126 removed = true; 127 } 128 } 129 130 if (removed) 131 client_remove_inactive(ctx); 132 133 } 134 135 static int binding_null_init(struct mctp *mctp __unused, 136 struct binding *binding __unused, 137 mctp_eid_t eid __unused, 138 int n_params, char * const *params __unused) 139 { 140 if (n_params != 0) { 141 warnx("null binding doesn't accept parameters"); 142 return -1; 143 } 144 return 0; 145 } 146 147 static int binding_serial_init(struct mctp *mctp, struct binding *binding, 148 mctp_eid_t eid, int n_params, char * const *params) 149 { 150 struct mctp_binding_serial *serial; 151 const char *path; 152 int rc; 153 154 if (n_params != 1) { 155 warnx("serial binding requires device param"); 156 return -1; 157 } 158 159 path = params[0]; 160 161 serial = mctp_serial_init(); 162 assert(serial); 163 164 rc = mctp_serial_open_path(serial, path); 165 if (rc) 166 return -1; 167 168 mctp_register_bus(mctp, mctp_binding_serial_core(serial), eid); 169 170 binding->data = serial; 171 172 return 0; 173 } 174 175 static int binding_serial_get_fd(struct binding *binding) 176 { 177 return mctp_serial_get_fd(binding->data); 178 } 179 180 static int binding_serial_process(struct binding *binding) 181 { 182 return mctp_serial_read(binding->data); 183 } 184 185 static int binding_astlpc_init(struct mctp *mctp, struct binding *binding, 186 mctp_eid_t eid, int n_params, 187 char * const *params __attribute__((unused))) 188 { 189 struct mctp_binding_astlpc *astlpc; 190 191 if (n_params) { 192 warnx("astlpc binding does not accept parameters"); 193 return -1; 194 } 195 196 astlpc = mctp_astlpc_init_fileio(); 197 if (!astlpc) { 198 warnx("could not initialise astlpc binding"); 199 return -1; 200 } 201 202 mctp_register_bus(mctp, mctp_binding_astlpc_core(astlpc), eid); 203 204 binding->data = astlpc; 205 return 0; 206 } 207 208 static int binding_astlpc_get_fd(struct binding *binding) 209 { 210 return mctp_astlpc_get_fd(binding->data); 211 } 212 213 static int binding_astlpc_process(struct binding *binding) 214 { 215 return mctp_astlpc_poll(binding->data); 216 } 217 218 struct binding bindings[] = { 219 { 220 .name = "null", 221 .init = binding_null_init, 222 }, 223 { 224 .name = "serial", 225 .init = binding_serial_init, 226 .get_fd = binding_serial_get_fd, 227 .process = binding_serial_process, 228 }, 229 { 230 .name = "astlpc", 231 .init = binding_astlpc_init, 232 .get_fd = binding_astlpc_get_fd, 233 .process = binding_astlpc_process, 234 } 235 }; 236 237 struct binding *binding_lookup(const char *name) 238 { 239 struct binding *binding; 240 unsigned int i; 241 242 for (i = 0; i < ARRAY_SIZE(bindings); i++) { 243 binding = &bindings[i]; 244 245 if (!strcmp(binding->name, name)) 246 return binding; 247 } 248 249 return NULL; 250 } 251 252 static int socket_init(struct ctx *ctx) 253 { 254 struct sockaddr_un addr; 255 int namelen, rc; 256 257 namelen = sizeof(sockname) - 1; 258 addr.sun_family = AF_UNIX; 259 memcpy(addr.sun_path, sockname, namelen); 260 261 ctx->sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); 262 if (ctx->sock < 0) { 263 warn("can't create socket"); 264 return -1; 265 } 266 267 rc = bind(ctx->sock, (struct sockaddr *)&addr, 268 sizeof(addr.sun_family) + namelen); 269 if (rc) { 270 warn("can't bind socket"); 271 goto err_close; 272 } 273 274 rc = listen(ctx->sock, 1); 275 if (rc) { 276 warn("can't listen on socket"); 277 goto err_close; 278 } 279 280 return 0; 281 282 err_close: 283 close(ctx->sock); 284 return -1; 285 } 286 287 static int socket_process(struct ctx *ctx) 288 { 289 struct client *client; 290 int fd; 291 292 fd = accept4(ctx->sock, NULL, 0, SOCK_NONBLOCK); 293 if (fd < 0) 294 return -1; 295 296 ctx->n_clients++; 297 ctx->clients = realloc(ctx->clients, 298 ctx->n_clients * sizeof(struct client)); 299 300 client = &ctx->clients[ctx->n_clients-1]; 301 memset(client, 0, sizeof(*client)); 302 client->active = true; 303 client->sock = fd; 304 305 return 0; 306 } 307 308 static int client_process_recv(struct ctx *ctx, int idx) 309 { 310 struct client *client = &ctx->clients[idx]; 311 uint8_t eid; 312 ssize_t len; 313 int rc; 314 315 /* are we waiting for a type message? */ 316 if (!client->type) { 317 uint8_t type; 318 rc = read(client->sock, &type, 1); 319 if (rc <= 0) 320 goto out_close; 321 322 if (type == 0) { 323 rc = -1; 324 goto out_close; 325 } 326 if (ctx->verbose) 327 fprintf(stderr, "client[%d] registered for type %u\n", 328 idx, type); 329 client->type = type; 330 return 0; 331 } 332 333 len = recv(client->sock, NULL, 0, MSG_PEEK | MSG_TRUNC); 334 if (len < 0) { 335 if (errno != ECONNRESET) 336 warn("can't receive (peek) from client"); 337 338 rc = -1; 339 goto out_close; 340 } 341 342 if ((size_t)len > ctx->buf_size) { 343 void *tmp; 344 345 tmp = realloc(ctx->buf, len); 346 if (!tmp) { 347 warn("can't allocate for incoming message"); 348 rc = -1; 349 goto out_close; 350 } 351 ctx->buf = tmp; 352 ctx->buf_size = len; 353 } 354 355 rc = recv(client->sock, ctx->buf, ctx->buf_size, 0); 356 if (rc < 0) { 357 if (errno != ECONNRESET) 358 warn("can't receive from client"); 359 rc = -1; 360 goto out_close; 361 } 362 363 if (rc <= 0) { 364 rc = -1; 365 goto out_close; 366 } 367 368 eid = *(uint8_t *)ctx->buf; 369 370 if (ctx->verbose) 371 fprintf(stderr, 372 "client[%d] sent message: dest 0x%02x len %d\n", 373 idx, eid, rc - 1); 374 375 376 if (eid == ctx->local_eid) 377 rx_message(eid, ctx, ctx->buf + 1, rc - 1); 378 else 379 tx_message(ctx, eid, ctx->buf + 1, rc - 1); 380 381 return 0; 382 383 out_close: 384 client->active = false; 385 return rc; 386 } 387 388 static int binding_init(struct ctx *ctx, const char *name, 389 int argc, char * const *argv) 390 { 391 int rc; 392 393 ctx->binding = binding_lookup(name); 394 if (!ctx->binding) { 395 warnx("no such binding '%s'", name); 396 return -1; 397 } 398 399 rc = ctx->binding->init(ctx->mctp, ctx->binding, ctx->local_eid, 400 argc, argv); 401 return rc; 402 } 403 404 enum { 405 FD_BINDING = 0, 406 FD_SOCKET, 407 FD_NR, 408 }; 409 410 static int run_daemon(struct ctx *ctx) 411 { 412 bool clients_changed = false; 413 int rc, i; 414 415 ctx->pollfds = malloc(FD_NR * sizeof(struct pollfd)); 416 417 if (ctx->binding->get_fd) { 418 ctx->pollfds[FD_BINDING].fd = 419 ctx->binding->get_fd(ctx->binding); 420 ctx->pollfds[FD_BINDING].events = POLLIN; 421 } else { 422 ctx->pollfds[FD_BINDING].fd = -1; 423 ctx->pollfds[FD_BINDING].events = 0; 424 } 425 426 ctx->pollfds[FD_SOCKET].fd = ctx->sock; 427 ctx->pollfds[FD_SOCKET].events = POLLIN; 428 429 mctp_set_rx_all(ctx->mctp, rx_message, ctx); 430 431 for (;;) { 432 if (clients_changed) { 433 int i; 434 435 ctx->pollfds = realloc(ctx->pollfds, 436 (ctx->n_clients + FD_NR) * 437 sizeof(struct pollfd)); 438 439 for (i = 0; i < ctx->n_clients; i++) { 440 ctx->pollfds[FD_NR+i].fd = 441 ctx->clients[i].sock; 442 ctx->pollfds[FD_NR+i].events = POLLIN; 443 } 444 clients_changed = false; 445 } 446 447 rc = poll(ctx->pollfds, ctx->n_clients + FD_NR, -1); 448 if (rc < 0) { 449 warn("poll failed"); 450 break; 451 } 452 453 if (!rc) 454 continue; 455 456 if (ctx->pollfds[FD_BINDING].revents) { 457 rc = 0; 458 if (ctx->binding->process) 459 rc = ctx->binding->process(ctx->binding); 460 if (rc) 461 break; 462 } 463 464 for (i = 0; i < ctx->n_clients; i++) { 465 if (!ctx->pollfds[FD_NR+i].revents) 466 continue; 467 468 rc = client_process_recv(ctx, i); 469 if (rc) 470 clients_changed = true; 471 } 472 473 if (ctx->pollfds[FD_SOCKET].revents) { 474 rc = socket_process(ctx); 475 if (rc) 476 break; 477 clients_changed = true; 478 } 479 480 if (clients_changed) 481 client_remove_inactive(ctx); 482 483 } 484 485 486 free(ctx->pollfds); 487 488 return rc; 489 } 490 491 static const struct option options[] = { 492 { "verbose", no_argument, 0, 'v' }, 493 { "eid", required_argument, 0, 'e' }, 494 { 0 }, 495 }; 496 497 static void usage(const char *progname) 498 { 499 unsigned int i; 500 501 fprintf(stderr, "usage: %s <binding> [params]\n", progname); 502 fprintf(stderr, "Available bindings:\n"); 503 for (i = 0; i < ARRAY_SIZE(bindings); i++) 504 fprintf(stderr, " %s\n", bindings[i].name); 505 } 506 507 int main(int argc, char * const *argv) 508 { 509 struct ctx *ctx, _ctx; 510 int rc; 511 512 ctx = &_ctx; 513 ctx->clients = NULL; 514 ctx->n_clients = 0; 515 ctx->local_eid = local_eid_default; 516 ctx->verbose = false; 517 518 for (;;) { 519 rc = getopt_long(argc, argv, "e:v", options, NULL); 520 if (rc == -1) 521 break; 522 switch (rc) { 523 case 'v': 524 ctx->verbose = true; 525 break; 526 case 'e': 527 ctx->local_eid = atoi(optarg); 528 break; 529 default: 530 fprintf(stderr, "Invalid argument\n"); 531 return EXIT_FAILURE; 532 } 533 } 534 535 if (optind >= argc) { 536 fprintf(stderr, "missing binding argument\n"); 537 usage(argv[0]); 538 return EXIT_FAILURE; 539 } 540 541 /* setup initial buffer */ 542 ctx->buf_size = 4096; 543 ctx->buf = malloc(ctx->buf_size); 544 545 mctp_set_log_stdio(ctx->verbose ? MCTP_LOG_DEBUG : MCTP_LOG_WARNING); 546 547 ctx->mctp = mctp_init(); 548 assert(ctx->mctp); 549 550 rc = binding_init(ctx, argv[optind], argc - optind - 1, argv + optind + 1); 551 if (rc) 552 return EXIT_FAILURE; 553 554 rc = socket_init(ctx); 555 if (rc) 556 return EXIT_FAILURE; 557 558 rc = run_daemon(ctx); 559 560 return rc ? EXIT_FAILURE : EXIT_SUCCESS; 561 562 } 563