1 /* SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later */ 2 3 #include <assert.h> 4 #include <errno.h> 5 #include <stdarg.h> 6 #include <stddef.h> 7 #include <stdint.h> 8 #include <stdio.h> 9 #include <stdlib.h> 10 #include <string.h> 11 12 #undef pr_fmt 13 #define pr_fmt(fmt) "core: " fmt 14 15 #include "libmctp.h" 16 #include "libmctp-alloc.h" 17 #include "libmctp-log.h" 18 #include "libmctp-cmds.h" 19 #include "range.h" 20 21 /* Internal data structures */ 22 23 enum mctp_bus_state { 24 mctp_bus_state_constructed = 0, 25 mctp_bus_state_tx_enabled, 26 mctp_bus_state_tx_disabled, 27 }; 28 29 struct mctp_bus { 30 mctp_eid_t eid; 31 struct mctp_binding *binding; 32 enum mctp_bus_state state; 33 34 struct mctp_pktbuf *tx_queue_head; 35 struct mctp_pktbuf *tx_queue_tail; 36 37 /* todo: routing */ 38 }; 39 40 struct mctp_msg_ctx { 41 uint8_t src; 42 uint8_t dest; 43 uint8_t tag; 44 uint8_t last_seq; 45 void *buf; 46 size_t buf_size; 47 size_t buf_alloc_size; 48 }; 49 50 struct mctp { 51 int n_busses; 52 struct mctp_bus *busses; 53 54 /* Message RX callback */ 55 mctp_rx_fn message_rx; 56 void *message_rx_data; 57 58 /* Message reassembly. 59 * @todo: flexible context count 60 */ 61 struct mctp_msg_ctx msg_ctxs[16]; 62 63 enum { 64 ROUTE_ENDPOINT, 65 ROUTE_BRIDGE, 66 } route_policy; 67 size_t max_message_size; 68 }; 69 70 #ifndef BUILD_ASSERT 71 #define BUILD_ASSERT(x) \ 72 do { (void)sizeof(char[0-(!(x))]); } while (0) 73 #endif 74 75 #ifndef ARRAY_SIZE 76 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) 77 #endif 78 79 /* 64kb should be sufficient for a single message. Applications 80 * requiring higher sizes can override by setting max_message_size.*/ 81 #ifndef MCTP_MAX_MESSAGE_SIZE 82 #define MCTP_MAX_MESSAGE_SIZE 65536 83 #endif 84 85 static int mctp_message_tx_on_bus(struct mctp_bus *bus, mctp_eid_t src, 86 mctp_eid_t dest, void *msg, size_t msg_len); 87 88 struct mctp_pktbuf *mctp_pktbuf_alloc(struct mctp_binding *binding, size_t len) 89 { 90 struct mctp_pktbuf *buf; 91 size_t size; 92 93 size = binding->pkt_size + binding->pkt_pad; 94 95 /* todo: pools */ 96 buf = __mctp_alloc(sizeof(*buf) + size); 97 98 buf->size = size; 99 buf->start = binding->pkt_pad; 100 buf->end = buf->start + len; 101 buf->mctp_hdr_off = buf->start; 102 buf->next = NULL; 103 104 return buf; 105 } 106 107 void mctp_pktbuf_free(struct mctp_pktbuf *pkt) 108 { 109 __mctp_free(pkt); 110 } 111 112 struct mctp_hdr *mctp_pktbuf_hdr(struct mctp_pktbuf *pkt) 113 { 114 return (void *)pkt->data + pkt->mctp_hdr_off; 115 } 116 117 void *mctp_pktbuf_data(struct mctp_pktbuf *pkt) 118 { 119 return (void *)pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr); 120 } 121 122 size_t mctp_pktbuf_size(struct mctp_pktbuf *pkt) 123 { 124 return pkt->end - pkt->start; 125 } 126 127 void *mctp_pktbuf_alloc_start(struct mctp_pktbuf *pkt, size_t size) 128 { 129 assert(size <= pkt->start); 130 pkt->start -= size; 131 return pkt->data + pkt->start; 132 } 133 134 void *mctp_pktbuf_alloc_end(struct mctp_pktbuf *pkt, size_t size) 135 { 136 void *buf; 137 138 assert(size <= (pkt->size - pkt->end)); 139 buf = pkt->data + pkt->end; 140 pkt->end += size; 141 return buf; 142 } 143 144 int mctp_pktbuf_push(struct mctp_pktbuf *pkt, void *data, size_t len) 145 { 146 void *p; 147 148 if (pkt->end + len > pkt->size) 149 return -1; 150 151 p = pkt->data + pkt->end; 152 153 pkt->end += len; 154 memcpy(p, data, len); 155 156 return 0; 157 } 158 159 /* Message reassembly */ 160 static struct mctp_msg_ctx *mctp_msg_ctx_lookup(struct mctp *mctp, 161 uint8_t src, uint8_t dest, uint8_t tag) 162 { 163 unsigned int i; 164 165 /* @todo: better lookup, if we add support for more outstanding 166 * message contexts */ 167 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 168 struct mctp_msg_ctx *ctx = &mctp->msg_ctxs[i]; 169 if (ctx->src == src && ctx->dest == dest && ctx->tag == tag) 170 return ctx; 171 } 172 173 return NULL; 174 } 175 176 static struct mctp_msg_ctx *mctp_msg_ctx_create(struct mctp *mctp, 177 uint8_t src, uint8_t dest, uint8_t tag) 178 { 179 struct mctp_msg_ctx *ctx = NULL; 180 unsigned int i; 181 182 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 183 struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i]; 184 if (!tmp->src) { 185 ctx = tmp; 186 break; 187 } 188 } 189 190 if (!ctx) 191 return NULL; 192 193 ctx->src = src; 194 ctx->dest = dest; 195 ctx->tag = tag; 196 ctx->buf_size = 0; 197 198 return ctx; 199 } 200 201 static void mctp_msg_ctx_drop(struct mctp_msg_ctx *ctx) 202 { 203 ctx->src = 0; 204 } 205 206 static void mctp_msg_ctx_reset(struct mctp_msg_ctx *ctx) 207 { 208 ctx->buf_size = 0; 209 } 210 211 static int mctp_msg_ctx_add_pkt(struct mctp_msg_ctx *ctx, 212 struct mctp_pktbuf *pkt, size_t max_size) 213 { 214 size_t len; 215 216 len = mctp_pktbuf_size(pkt) - sizeof(struct mctp_hdr); 217 218 if (ctx->buf_size + len > ctx->buf_alloc_size) { 219 size_t new_alloc_size; 220 void *lbuf; 221 222 /* @todo: finer-grained allocation */ 223 if (!ctx->buf_alloc_size) { 224 new_alloc_size = MAX(len, 4096UL); 225 } else { 226 new_alloc_size = ctx->buf_alloc_size * 2; 227 } 228 229 /* Don't allow heap to grow beyond a limit */ 230 if (new_alloc_size > max_size) 231 return -1; 232 233 234 lbuf = __mctp_realloc(ctx->buf, new_alloc_size); 235 if (lbuf) { 236 ctx->buf = lbuf; 237 ctx->buf_alloc_size = new_alloc_size; 238 } else { 239 __mctp_free(ctx->buf); 240 return -1; 241 } 242 } 243 244 memcpy(ctx->buf + ctx->buf_size, mctp_pktbuf_data(pkt), len); 245 ctx->buf_size += len; 246 247 return 0; 248 } 249 250 /* Core API functions */ 251 struct mctp *mctp_init(void) 252 { 253 struct mctp *mctp; 254 255 mctp = __mctp_alloc(sizeof(*mctp)); 256 257 if(!mctp) 258 return NULL; 259 260 memset(mctp, 0, sizeof(*mctp)); 261 mctp->max_message_size = MCTP_MAX_MESSAGE_SIZE; 262 263 return mctp; 264 } 265 266 void mctp_set_max_message_size(struct mctp *mctp, size_t message_size) 267 { 268 mctp->max_message_size = message_size; 269 } 270 271 static void mctp_bus_destroy(struct mctp_bus *bus) 272 { 273 while (bus->tx_queue_head) { 274 struct mctp_pktbuf *curr = bus->tx_queue_head; 275 276 bus->tx_queue_head = curr->next; 277 mctp_pktbuf_free(curr); 278 } 279 } 280 281 void mctp_destroy(struct mctp *mctp) 282 { 283 size_t i; 284 285 /* Cleanup message assembly contexts */ 286 BUILD_ASSERT(ARRAY_SIZE(mctp->msg_ctxs) < SIZE_MAX); 287 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 288 struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i]; 289 if (tmp->buf) 290 __mctp_free(tmp->buf); 291 } 292 293 while (mctp->n_busses--) 294 mctp_bus_destroy(&mctp->busses[mctp->n_busses]); 295 296 __mctp_free(mctp->busses); 297 __mctp_free(mctp); 298 } 299 300 int mctp_set_rx_all(struct mctp *mctp, mctp_rx_fn fn, void *data) 301 { 302 mctp->message_rx = fn; 303 mctp->message_rx_data = data; 304 return 0; 305 } 306 307 static struct mctp_bus *find_bus_for_eid(struct mctp *mctp, 308 mctp_eid_t dest __attribute__((unused))) 309 { 310 /* for now, just use the first bus. For full routing support, 311 * we will need a table of neighbours */ 312 return &mctp->busses[0]; 313 } 314 315 int mctp_register_bus(struct mctp *mctp, 316 struct mctp_binding *binding, 317 mctp_eid_t eid) 318 { 319 int rc = 0; 320 321 /* todo: multiple busses */ 322 assert(mctp->n_busses == 0); 323 mctp->n_busses = 1; 324 325 mctp->busses = __mctp_alloc(sizeof(struct mctp_bus)); 326 if (!mctp->busses) 327 return -ENOMEM; 328 329 memset(mctp->busses, 0, sizeof(struct mctp_bus)); 330 mctp->busses[0].binding = binding; 331 mctp->busses[0].eid = eid; 332 binding->bus = &mctp->busses[0]; 333 binding->mctp = mctp; 334 mctp->route_policy = ROUTE_ENDPOINT; 335 336 if (binding->start) { 337 rc = binding->start(binding); 338 if (rc < 0) { 339 mctp_prerr("Failed to start binding: %d", rc); 340 __mctp_free(mctp->busses); 341 mctp->busses = NULL; 342 mctp->n_busses = 0; 343 } 344 } 345 346 return rc; 347 } 348 349 int mctp_bridge_busses(struct mctp *mctp, 350 struct mctp_binding *b1, struct mctp_binding *b2) 351 { 352 assert(mctp->n_busses == 0); 353 mctp->busses = __mctp_alloc(2 * sizeof(struct mctp_bus)); 354 memset(mctp->busses, 0, 2 * sizeof(struct mctp_bus)); 355 mctp->n_busses = 2; 356 mctp->busses[0].binding = b1; 357 b1->bus = &mctp->busses[0]; 358 b1->mctp = mctp; 359 mctp->busses[1].binding = b2; 360 b2->bus = &mctp->busses[1]; 361 b2->mctp = mctp; 362 363 mctp->route_policy = ROUTE_BRIDGE; 364 365 if (b1->start) 366 b1->start(b1); 367 368 if (b2->start) 369 b2->start(b2); 370 371 return 0; 372 } 373 374 static inline bool mctp_ctrl_cmd_is_transport(struct mctp_ctrl_msg_hdr *hdr) 375 { 376 return ((hdr->command_code >= MCTP_CTRL_CMD_FIRST_TRANSPORT) && 377 (hdr->command_code <= MCTP_CTRL_CMD_LAST_TRANSPORT)); 378 } 379 380 static bool mctp_ctrl_handle_msg(struct mctp_bus *bus, mctp_eid_t src, 381 void *buffer, size_t length) 382 { 383 struct mctp_ctrl_msg_hdr *msg_hdr = buffer; 384 385 /* 386 * Control message is received. If a transport control message handler 387 * is provided, it will called. If there is no dedicated handler, this 388 * function returns false and data can be handled by the generic 389 * message handler. The transport control message handler will be 390 * provided with messages in the command range 0xF0 - 0xFF. 391 */ 392 if (mctp_ctrl_cmd_is_transport(msg_hdr)) { 393 if (bus->binding->control_rx != NULL) { 394 /* MCTP bus binding handler */ 395 bus->binding->control_rx(src, 396 bus->binding->control_rx_data, 397 buffer, length); 398 return true; 399 } 400 } 401 402 /* 403 * Command was not handled, due to lack of specific callback. 404 * It will be passed to regular message_rx handler. 405 */ 406 return false; 407 } 408 409 static inline bool mctp_rx_dest_is_local(struct mctp_bus *bus, mctp_eid_t dest) 410 { 411 return dest == bus->eid || dest == MCTP_EID_NULL || 412 dest == MCTP_EID_BROADCAST; 413 } 414 415 static inline bool mctp_ctrl_cmd_is_request(struct mctp_ctrl_msg_hdr *hdr) 416 { 417 return hdr->ic_msg_type == MCTP_CTRL_HDR_MSG_TYPE && 418 hdr->rq_dgram_inst & MCTP_CTRL_HDR_FLAG_REQUEST; 419 } 420 421 /* 422 * Receive the complete MCTP message and route it. 423 * Asserts: 424 * 'buf' is not NULL. 425 */ 426 static void mctp_rx(struct mctp *mctp, struct mctp_bus *bus, mctp_eid_t src, 427 mctp_eid_t dest, void *buf, size_t len) 428 { 429 assert(buf != NULL); 430 431 if (mctp->route_policy == ROUTE_ENDPOINT && 432 mctp_rx_dest_is_local(bus, dest)) { 433 /* Handle MCTP Control Messages: */ 434 if (len >= sizeof(struct mctp_ctrl_msg_hdr)) { 435 struct mctp_ctrl_msg_hdr *msg_hdr = buf; 436 437 /* 438 * Identify if this is a control request message. 439 * See DSP0236 v1.3.0 sec. 11.5. 440 */ 441 if (mctp_ctrl_cmd_is_request(msg_hdr)) { 442 bool handled; 443 handled = mctp_ctrl_handle_msg(bus, src, buf, 444 len); 445 if (handled) 446 return; 447 } 448 } 449 if (mctp->message_rx) 450 mctp->message_rx(src, mctp->message_rx_data, buf, len); 451 } 452 453 if (mctp->route_policy == ROUTE_BRIDGE) { 454 int i; 455 456 for (i = 0; i < mctp->n_busses; i++) { 457 struct mctp_bus *dest_bus = &mctp->busses[i]; 458 if (dest_bus == bus) 459 continue; 460 461 mctp_message_tx_on_bus(dest_bus, src, dest, buf, len); 462 } 463 464 } 465 } 466 467 void mctp_bus_rx(struct mctp_binding *binding, struct mctp_pktbuf *pkt) 468 { 469 struct mctp_bus *bus = binding->bus; 470 struct mctp *mctp = binding->mctp; 471 uint8_t flags, exp_seq, seq, tag; 472 struct mctp_msg_ctx *ctx; 473 struct mctp_hdr *hdr; 474 size_t len; 475 void *p; 476 int rc; 477 478 assert(bus); 479 480 /* Drop packet if it was smaller than mctp hdr size */ 481 if (mctp_pktbuf_size(pkt) <= sizeof(struct mctp_hdr)) 482 goto out; 483 484 hdr = mctp_pktbuf_hdr(pkt); 485 486 /* small optimisation: don't bother reassembly if we're going to 487 * drop the packet in mctp_rx anyway */ 488 if (mctp->route_policy == ROUTE_ENDPOINT && hdr->dest != bus->eid) 489 goto out; 490 491 flags = hdr->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM); 492 tag = (hdr->flags_seq_tag >> MCTP_HDR_TAG_SHIFT) & MCTP_HDR_TAG_MASK; 493 seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT) & MCTP_HDR_SEQ_MASK; 494 495 switch (flags) { 496 case MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM: 497 /* single-packet message - send straight up to rx function, 498 * no need to create a message context */ 499 len = pkt->end - pkt->mctp_hdr_off - sizeof(struct mctp_hdr); 500 p = pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr); 501 mctp_rx(mctp, bus, hdr->src, hdr->dest, p, len); 502 break; 503 504 case MCTP_HDR_FLAG_SOM: 505 /* start of a new message - start the new context for 506 * future message reception. If an existing context is 507 * already present, drop it. */ 508 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag); 509 if (ctx) { 510 mctp_msg_ctx_reset(ctx); 511 } else { 512 ctx = mctp_msg_ctx_create(mctp, 513 hdr->src, hdr->dest, tag); 514 } 515 516 rc = mctp_msg_ctx_add_pkt(ctx, pkt, mctp->max_message_size); 517 if (rc) { 518 mctp_msg_ctx_drop(ctx); 519 } else { 520 ctx->last_seq = seq; 521 } 522 523 break; 524 525 case MCTP_HDR_FLAG_EOM: 526 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag); 527 if (!ctx) 528 goto out; 529 530 exp_seq = (ctx->last_seq + 1) % 4; 531 532 if (exp_seq != seq) { 533 mctp_prdebug( 534 "Sequence number %d does not match expected %d", 535 seq, exp_seq); 536 mctp_msg_ctx_drop(ctx); 537 goto out; 538 } 539 540 rc = mctp_msg_ctx_add_pkt(ctx, pkt, mctp->max_message_size); 541 if (!rc) 542 mctp_rx(mctp, bus, ctx->src, ctx->dest, 543 ctx->buf, ctx->buf_size); 544 545 mctp_msg_ctx_drop(ctx); 546 break; 547 548 case 0: 549 /* Neither SOM nor EOM */ 550 ctx = mctp_msg_ctx_lookup(mctp, hdr->src,hdr->dest, tag); 551 if (!ctx) 552 goto out; 553 554 exp_seq = (ctx->last_seq + 1) % 4; 555 if (exp_seq != seq) { 556 mctp_prdebug( 557 "Sequence number %d does not match expected %d", 558 seq, exp_seq); 559 mctp_msg_ctx_drop(ctx); 560 goto out; 561 } 562 563 rc = mctp_msg_ctx_add_pkt(ctx, pkt, mctp->max_message_size); 564 if (rc) { 565 mctp_msg_ctx_drop(ctx); 566 goto out; 567 } 568 ctx->last_seq = seq; 569 570 break; 571 } 572 out: 573 mctp_pktbuf_free(pkt); 574 } 575 576 static int mctp_packet_tx(struct mctp_bus *bus, 577 struct mctp_pktbuf *pkt) 578 { 579 if (bus->state != mctp_bus_state_tx_enabled) 580 return -1; 581 582 return bus->binding->tx(bus->binding, pkt); 583 } 584 585 static void mctp_send_tx_queue(struct mctp_bus *bus) 586 { 587 struct mctp_pktbuf *pkt; 588 589 while ((pkt = bus->tx_queue_head)) { 590 int rc; 591 592 rc = mctp_packet_tx(bus, pkt); 593 if (rc) 594 break; 595 596 bus->tx_queue_head = pkt->next; 597 mctp_pktbuf_free(pkt); 598 } 599 600 if (!bus->tx_queue_head) 601 bus->tx_queue_tail = NULL; 602 603 } 604 605 void mctp_binding_set_tx_enabled(struct mctp_binding *binding, bool enable) 606 { 607 struct mctp_bus *bus = binding->bus; 608 609 switch(bus->state) { 610 case mctp_bus_state_constructed: 611 if (!enable) 612 return; 613 614 if (binding->pkt_size < MCTP_PACKET_SIZE(MCTP_BTU)) { 615 mctp_prerr("Cannot start %s binding with invalid MTU: %zu", 616 binding->name, 617 MCTP_BODY_SIZE(binding->pkt_size)); 618 return; 619 } 620 621 bus->state = mctp_bus_state_tx_enabled; 622 mctp_prinfo("%s binding started", binding->name); 623 return; 624 case mctp_bus_state_tx_enabled: 625 if (enable) 626 return; 627 628 bus->state = mctp_bus_state_tx_disabled; 629 mctp_prdebug("%s binding Tx disabled", binding->name); 630 return; 631 case mctp_bus_state_tx_disabled: 632 if (!enable) 633 return; 634 635 bus->state = mctp_bus_state_tx_enabled; 636 mctp_prdebug("%s binding Tx enabled", binding->name); 637 mctp_send_tx_queue(bus); 638 return; 639 } 640 } 641 642 static int mctp_message_tx_on_bus(struct mctp_bus *bus, mctp_eid_t src, 643 mctp_eid_t dest, void *msg, size_t msg_len) 644 { 645 size_t max_payload_len, payload_len, p; 646 struct mctp_pktbuf *pkt; 647 struct mctp_hdr *hdr; 648 int i; 649 650 if (bus->state == mctp_bus_state_constructed) 651 return -ENXIO; 652 653 max_payload_len = MCTP_BODY_SIZE(bus->binding->pkt_size); 654 655 { 656 const bool valid_mtu = max_payload_len >= MCTP_BTU; 657 assert(valid_mtu); 658 if (!valid_mtu) 659 return -EINVAL; 660 } 661 662 mctp_prdebug("%s: Generating packets for transmission of %zu byte message from %hhu to %hhu", 663 __func__, msg_len, src, dest); 664 665 /* queue up packets, each of max MCTP_MTU size */ 666 for (p = 0, i = 0; p < msg_len; i++) { 667 payload_len = msg_len - p; 668 if (payload_len > max_payload_len) 669 payload_len = max_payload_len; 670 671 pkt = mctp_pktbuf_alloc(bus->binding, 672 payload_len + sizeof(*hdr)); 673 hdr = mctp_pktbuf_hdr(pkt); 674 675 /* todo: tags */ 676 hdr->ver = bus->binding->version & 0xf; 677 hdr->dest = dest; 678 hdr->src = src; 679 hdr->flags_seq_tag = MCTP_HDR_FLAG_TO | 680 (0 << MCTP_HDR_TAG_SHIFT); 681 682 if (i == 0) 683 hdr->flags_seq_tag |= MCTP_HDR_FLAG_SOM; 684 if (p + payload_len >= msg_len) 685 hdr->flags_seq_tag |= MCTP_HDR_FLAG_EOM; 686 hdr->flags_seq_tag |= 687 (i & MCTP_HDR_SEQ_MASK) << MCTP_HDR_SEQ_SHIFT; 688 689 memcpy(mctp_pktbuf_data(pkt), msg + p, payload_len); 690 691 /* add to tx queue */ 692 if (bus->tx_queue_tail) 693 bus->tx_queue_tail->next = pkt; 694 else 695 bus->tx_queue_head = pkt; 696 bus->tx_queue_tail = pkt; 697 698 p += payload_len; 699 } 700 701 mctp_prdebug("%s: Enqueued %d packets", __func__, i); 702 703 mctp_send_tx_queue(bus); 704 705 return 0; 706 } 707 708 int mctp_message_tx(struct mctp *mctp, mctp_eid_t eid, 709 void *msg, size_t msg_len) 710 { 711 struct mctp_bus *bus; 712 713 bus = find_bus_for_eid(mctp, eid); 714 return mctp_message_tx_on_bus(bus, bus->eid, eid, msg, msg_len); 715 } 716