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