1 /* SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later */ 2 3 #include <assert.h> 4 #include <stdarg.h> 5 #include <stddef.h> 6 #include <stdint.h> 7 #include <stdio.h> 8 #include <stdlib.h> 9 #include <string.h> 10 11 #undef pr_fmt 12 #define pr_fmt(fmt) "core: " fmt 13 14 #include "libmctp.h" 15 #include "libmctp-alloc.h" 16 #include "libmctp-log.h" 17 18 /* Internal data structures */ 19 20 struct mctp_bus { 21 mctp_eid_t eid; 22 struct mctp_binding *binding; 23 bool tx_enabled; 24 25 struct mctp_pktbuf *tx_queue_head; 26 struct mctp_pktbuf *tx_queue_tail; 27 28 /* todo: routing */ 29 }; 30 31 struct mctp_msg_ctx { 32 uint8_t src; 33 uint8_t dest; 34 uint8_t tag; 35 uint8_t last_seq; 36 void *buf; 37 size_t buf_size; 38 size_t buf_alloc_size; 39 }; 40 41 struct mctp { 42 int n_busses; 43 struct mctp_bus *busses; 44 45 /* Message RX callback */ 46 mctp_rx_fn message_rx; 47 void *message_rx_data; 48 49 /* Message reassembly. 50 * @todo: flexible context count 51 */ 52 struct mctp_msg_ctx msg_ctxs[16]; 53 54 enum { 55 ROUTE_ENDPOINT, 56 ROUTE_BRIDGE, 57 } route_policy; 58 }; 59 60 #ifndef BUILD_ASSERT 61 #define BUILD_ASSERT(x) \ 62 do { (void)sizeof(char[0-(!(x))]); } while (0) 63 #endif 64 65 #ifndef ARRAY_SIZE 66 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) 67 #endif 68 69 static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus, 70 mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len); 71 72 struct mctp_pktbuf *mctp_pktbuf_alloc(struct mctp_binding *binding, size_t len) 73 { 74 struct mctp_pktbuf *buf; 75 size_t size; 76 77 size = binding->pkt_size + binding->pkt_pad; 78 79 /* todo: pools */ 80 buf = __mctp_alloc(sizeof(*buf) + size); 81 82 buf->size = size; 83 buf->start = binding->pkt_pad; 84 buf->end = buf->start + len; 85 buf->mctp_hdr_off = buf->start; 86 buf->next = NULL; 87 88 return buf; 89 } 90 91 void mctp_pktbuf_free(struct mctp_pktbuf *pkt) 92 { 93 __mctp_free(pkt); 94 } 95 96 struct mctp_hdr *mctp_pktbuf_hdr(struct mctp_pktbuf *pkt) 97 { 98 return (void *)pkt->data + pkt->mctp_hdr_off; 99 } 100 101 void *mctp_pktbuf_data(struct mctp_pktbuf *pkt) 102 { 103 return (void *)pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr); 104 } 105 106 uint8_t mctp_pktbuf_size(struct mctp_pktbuf *pkt) 107 { 108 return pkt->end - pkt->start; 109 } 110 111 void *mctp_pktbuf_alloc_start(struct mctp_pktbuf *pkt, size_t size) 112 { 113 assert(size <= pkt->start); 114 pkt->start -= size; 115 return pkt->data + pkt->start; 116 } 117 118 void *mctp_pktbuf_alloc_end(struct mctp_pktbuf *pkt, size_t size) 119 { 120 void *buf; 121 122 assert(size < (pkt->size - pkt->end)); 123 buf = pkt->data + pkt->end; 124 pkt->end += size; 125 return buf; 126 } 127 128 int mctp_pktbuf_push(struct mctp_pktbuf *pkt, void *data, size_t len) 129 { 130 void *p; 131 132 if (pkt->end + len > pkt->size) 133 return -1; 134 135 p = pkt->data + pkt->end; 136 137 pkt->end += len; 138 memcpy(p, data, len); 139 140 return 0; 141 } 142 143 /* Message reassembly */ 144 static struct mctp_msg_ctx *mctp_msg_ctx_lookup(struct mctp *mctp, 145 uint8_t src, uint8_t dest, uint8_t tag) 146 { 147 unsigned int i; 148 149 /* @todo: better lookup, if we add support for more outstanding 150 * message contexts */ 151 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 152 struct mctp_msg_ctx *ctx = &mctp->msg_ctxs[i]; 153 if (ctx->src == src && ctx->dest == dest && ctx->tag == tag) 154 return ctx; 155 } 156 157 return NULL; 158 } 159 160 static struct mctp_msg_ctx *mctp_msg_ctx_create(struct mctp *mctp, 161 uint8_t src, uint8_t dest, uint8_t tag) 162 { 163 struct mctp_msg_ctx *ctx = NULL; 164 unsigned int i; 165 166 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 167 struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i]; 168 if (!tmp->src) { 169 ctx = tmp; 170 break; 171 } 172 } 173 174 if (!ctx) 175 return NULL; 176 177 ctx->src = src; 178 ctx->dest = dest; 179 ctx->tag = tag; 180 ctx->buf_size = 0; 181 182 return ctx; 183 } 184 185 static void mctp_msg_ctx_drop(struct mctp_msg_ctx *ctx) 186 { 187 ctx->src = 0; 188 } 189 190 static void mctp_msg_ctx_reset(struct mctp_msg_ctx *ctx) 191 { 192 ctx->buf_size = 0; 193 } 194 195 static int mctp_msg_ctx_add_pkt(struct mctp_msg_ctx *ctx, 196 struct mctp_pktbuf *pkt) 197 { 198 size_t len; 199 200 len = mctp_pktbuf_size(pkt) - sizeof(struct mctp_hdr); 201 202 if (ctx->buf_size + len > ctx->buf_alloc_size) { 203 size_t new_alloc_size; 204 void *lbuf; 205 206 /* @todo: finer-grained allocation, size limits */ 207 if (!ctx->buf_alloc_size) { 208 new_alloc_size = 4096; 209 } else { 210 new_alloc_size = ctx->buf_alloc_size * 2; 211 } 212 213 lbuf = __mctp_realloc(ctx->buf, new_alloc_size); 214 if (lbuf) { 215 ctx->buf = lbuf; 216 ctx->buf_alloc_size = new_alloc_size; 217 } else { 218 __mctp_free(ctx->buf); 219 return -1; 220 } 221 } 222 223 memcpy(ctx->buf + ctx->buf_size, mctp_pktbuf_data(pkt), len); 224 ctx->buf_size += len; 225 226 return 0; 227 } 228 229 /* Core API functions */ 230 struct mctp *mctp_init(void) 231 { 232 struct mctp *mctp; 233 234 mctp = __mctp_alloc(sizeof(*mctp)); 235 memset(mctp, 0, sizeof(*mctp)); 236 237 return mctp; 238 } 239 240 void mctp_destroy(struct mctp *mctp) 241 { 242 int i; 243 244 /* Cleanup message assembly contexts */ 245 for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) { 246 struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i]; 247 if (tmp->buf) 248 __mctp_free(tmp->buf); 249 } 250 251 __mctp_free(mctp->busses); 252 __mctp_free(mctp); 253 } 254 255 int mctp_set_rx_all(struct mctp *mctp, mctp_rx_fn fn, void *data) 256 { 257 mctp->message_rx = fn; 258 mctp->message_rx_data = data; 259 return 0; 260 } 261 262 static struct mctp_bus *find_bus_for_eid(struct mctp *mctp, 263 mctp_eid_t dest __attribute__((unused))) 264 { 265 /* for now, just use the first bus. For full routing support, 266 * we will need a table of neighbours */ 267 return &mctp->busses[0]; 268 } 269 270 int mctp_register_bus(struct mctp *mctp, 271 struct mctp_binding *binding, 272 mctp_eid_t eid) 273 { 274 /* todo: multiple busses */ 275 assert(mctp->n_busses == 0); 276 mctp->n_busses = 1; 277 mctp->busses = __mctp_alloc(sizeof(struct mctp_bus)); 278 memset(mctp->busses, 0, sizeof(struct mctp_bus)); 279 mctp->busses[0].binding = binding; 280 mctp->busses[0].eid = eid; 281 binding->bus = &mctp->busses[0]; 282 binding->mctp = mctp; 283 mctp->route_policy = ROUTE_ENDPOINT; 284 285 if (binding->start) 286 binding->start(binding); 287 288 return 0; 289 } 290 291 int mctp_bridge_busses(struct mctp *mctp, 292 struct mctp_binding *b1, struct mctp_binding *b2) 293 { 294 assert(mctp->n_busses == 0); 295 mctp->busses = __mctp_alloc(2 * sizeof(struct mctp_bus)); 296 memset(mctp->busses, 0, 2 * sizeof(struct mctp_bus)); 297 mctp->n_busses = 2; 298 mctp->busses[0].binding = b1; 299 b1->bus = &mctp->busses[0]; 300 b1->mctp = mctp; 301 mctp->busses[1].binding = b2; 302 b2->bus = &mctp->busses[1]; 303 b2->mctp = mctp; 304 305 mctp->route_policy = ROUTE_BRIDGE; 306 307 if (b1->start) 308 b1->start(b1); 309 310 if (b2->start) 311 b2->start(b2); 312 313 return 0; 314 } 315 316 static void mctp_rx(struct mctp *mctp, struct mctp_bus *bus, 317 mctp_eid_t src, mctp_eid_t dest, void *buf, size_t len) 318 { 319 if (mctp->route_policy == ROUTE_ENDPOINT && 320 dest == bus->eid && mctp->message_rx) 321 mctp->message_rx(src, mctp->message_rx_data, buf, len); 322 323 if (mctp->route_policy == ROUTE_BRIDGE) { 324 int i; 325 326 for (i = 0; i < mctp->n_busses; i++) { 327 struct mctp_bus *dest_bus = &mctp->busses[i]; 328 if (dest_bus == bus) 329 continue; 330 331 mctp_message_tx_on_bus(mctp, dest_bus, 332 src, dest, buf, len); 333 } 334 335 } 336 } 337 338 void mctp_bus_rx(struct mctp_binding *binding, struct mctp_pktbuf *pkt) 339 { 340 struct mctp_bus *bus = binding->bus; 341 struct mctp *mctp = binding->mctp; 342 uint8_t flags, exp_seq, seq, tag; 343 struct mctp_msg_ctx *ctx; 344 struct mctp_hdr *hdr; 345 size_t len; 346 void *p; 347 int rc; 348 349 assert(bus); 350 351 hdr = mctp_pktbuf_hdr(pkt); 352 353 /* small optimisation: don't bother reassembly if we're going to 354 * drop the packet in mctp_rx anyway */ 355 if (mctp->route_policy == ROUTE_ENDPOINT && hdr->dest != bus->eid) 356 goto out; 357 358 flags = hdr->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM); 359 tag = (hdr->flags_seq_tag >> MCTP_HDR_TAG_SHIFT) & MCTP_HDR_TAG_MASK; 360 seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT) & MCTP_HDR_SEQ_MASK; 361 362 switch (flags) { 363 case MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM: 364 /* single-packet message - send straight up to rx function, 365 * no need to create a message context */ 366 len = pkt->end - pkt->mctp_hdr_off - sizeof(struct mctp_hdr); 367 p = pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr), 368 mctp_rx(mctp, bus, hdr->src, hdr->dest, p, len); 369 break; 370 371 case MCTP_HDR_FLAG_SOM: 372 /* start of a new message - start the new context for 373 * future message reception. If an existing context is 374 * already present, drop it. */ 375 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag); 376 if (ctx) { 377 mctp_msg_ctx_reset(ctx); 378 } else { 379 ctx = mctp_msg_ctx_create(mctp, 380 hdr->src, hdr->dest, tag); 381 } 382 383 rc = mctp_msg_ctx_add_pkt(ctx, pkt); 384 if (rc) { 385 mctp_msg_ctx_drop(ctx); 386 } else { 387 ctx->last_seq = seq; 388 } 389 390 break; 391 392 case MCTP_HDR_FLAG_EOM: 393 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag); 394 if (!ctx) 395 goto out; 396 397 exp_seq = (ctx->last_seq + 1) % 4; 398 399 if (exp_seq != seq) { 400 mctp_prdebug( 401 "Sequence number %d does not match expected %d", 402 seq, exp_seq); 403 mctp_msg_ctx_drop(ctx); 404 goto out; 405 } 406 407 rc = mctp_msg_ctx_add_pkt(ctx, pkt); 408 if (!rc) 409 mctp_rx(mctp, bus, ctx->src, ctx->dest, 410 ctx->buf, ctx->buf_size); 411 412 mctp_msg_ctx_drop(ctx); 413 break; 414 415 case 0: 416 /* Neither SOM nor EOM */ 417 ctx = mctp_msg_ctx_lookup(mctp, hdr->src,hdr->dest, tag); 418 if (!ctx) 419 goto out; 420 421 exp_seq = (ctx->last_seq + 1) % 4; 422 if (exp_seq != seq) { 423 mctp_prdebug( 424 "Sequence number %d does not match expected %d", 425 seq, exp_seq); 426 mctp_msg_ctx_drop(ctx); 427 goto out; 428 } 429 430 rc = mctp_msg_ctx_add_pkt(ctx, pkt); 431 if (rc) { 432 mctp_msg_ctx_drop(ctx); 433 goto out; 434 } 435 ctx->last_seq = seq; 436 437 break; 438 } 439 out: 440 mctp_pktbuf_free(pkt); 441 } 442 443 static int mctp_packet_tx(struct mctp_bus *bus, 444 struct mctp_pktbuf *pkt) 445 { 446 if (!bus->tx_enabled) 447 return -1; 448 449 return bus->binding->tx(bus->binding, pkt); 450 } 451 452 static void mctp_send_tx_queue(struct mctp_bus *bus) 453 { 454 struct mctp_pktbuf *pkt; 455 456 while ((pkt = bus->tx_queue_head)) { 457 int rc; 458 459 rc = mctp_packet_tx(bus, pkt); 460 if (rc) 461 break; 462 463 bus->tx_queue_head = pkt->next; 464 mctp_pktbuf_free(pkt); 465 } 466 467 if (!bus->tx_queue_head) 468 bus->tx_queue_tail = NULL; 469 470 } 471 472 void mctp_binding_set_tx_enabled(struct mctp_binding *binding, bool enable) 473 { 474 struct mctp_bus *bus = binding->bus; 475 bus->tx_enabled = enable; 476 if (enable) 477 mctp_send_tx_queue(bus); 478 } 479 480 static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus, 481 mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len) 482 { 483 size_t max_payload_len, payload_len, p; 484 struct mctp_pktbuf *pkt; 485 struct mctp_hdr *hdr; 486 int i; 487 488 max_payload_len = bus->binding->pkt_size - sizeof(*hdr); 489 490 mctp_prdebug("%s: Generating packets for transmission of %zu byte message from %hhu to %hhu", 491 __func__, msg_len, src, dest); 492 493 /* queue up packets, each of max MCTP_MTU size */ 494 for (p = 0, i = 0; p < msg_len; i++) { 495 payload_len = msg_len - p; 496 if (payload_len > max_payload_len) 497 payload_len = max_payload_len; 498 499 pkt = mctp_pktbuf_alloc(bus->binding, 500 payload_len + sizeof(*hdr)); 501 hdr = mctp_pktbuf_hdr(pkt); 502 503 /* todo: tags */ 504 hdr->ver = bus->binding->version & 0xf; 505 hdr->dest = dest; 506 hdr->src = src; 507 hdr->flags_seq_tag = MCTP_HDR_FLAG_TO | 508 (0 << MCTP_HDR_TAG_SHIFT); 509 510 if (i == 0) 511 hdr->flags_seq_tag |= MCTP_HDR_FLAG_SOM; 512 if (p + payload_len >= msg_len) 513 hdr->flags_seq_tag |= MCTP_HDR_FLAG_EOM; 514 hdr->flags_seq_tag |= 515 (i & MCTP_HDR_SEQ_MASK) << MCTP_HDR_SEQ_SHIFT; 516 517 memcpy(mctp_pktbuf_data(pkt), msg + p, payload_len); 518 519 /* add to tx queue */ 520 if (bus->tx_queue_tail) 521 bus->tx_queue_tail->next = pkt; 522 else 523 bus->tx_queue_head = pkt; 524 bus->tx_queue_tail = pkt; 525 526 p += payload_len; 527 } 528 529 mctp_prdebug("%s: Enqueued %d packets", __func__, i); 530 531 mctp_send_tx_queue(bus); 532 533 return 0; 534 } 535 536 int mctp_message_tx(struct mctp *mctp, mctp_eid_t eid, 537 void *msg, size_t msg_len) 538 { 539 struct mctp_bus *bus; 540 541 bus = find_bus_for_eid(mctp, eid); 542 return mctp_message_tx_on_bus(mctp, bus, bus->eid, eid, msg, msg_len); 543 } 544