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