1 /* $OpenBSD: ds.c,v 1.5 2014/10/15 21:37:27 deraadt Exp $ */ 2 3 /* 4 * Copyright (c) 2012 Mark Kettenis 5 * 6 * Permission to use, copy, modify, and distribute this software for any 7 * purpose with or without fee is hereby granted, provided that the above 8 * copyright notice and this permission notice appear in all copies. 9 * 10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 17 */ 18 19 #include <sys/types.h> 20 #include <sys/poll.h> 21 #include <sys/queue.h> 22 #include <err.h> 23 #include <fcntl.h> 24 #include <stdio.h> 25 #include <stdlib.h> 26 #include <string.h> 27 #include <unistd.h> 28 29 #include "ds.h" 30 #include "util.h" 31 32 void ldc_rx_ctrl_vers(struct ldc_conn *, struct ldc_pkt *); 33 void ldc_rx_ctrl_rtr(struct ldc_conn *, struct ldc_pkt *); 34 void ldc_rx_ctrl_rts(struct ldc_conn *, struct ldc_pkt *); 35 void ldc_rx_ctrl_rdx(struct ldc_conn *, struct ldc_pkt *); 36 37 void ldc_send_ack(struct ldc_conn *); 38 void ldc_send_rtr(struct ldc_conn *); 39 void ldc_send_rts(struct ldc_conn *); 40 void ldc_send_rdx(struct ldc_conn *); 41 42 void 43 ldc_rx_ctrl(struct ldc_conn *lc, struct ldc_pkt *lp) 44 { 45 switch (lp->ctrl) { 46 case LDC_VERS: 47 ldc_rx_ctrl_vers(lc, lp); 48 break; 49 50 case LDC_RTS: 51 ldc_rx_ctrl_rts(lc, lp); 52 break; 53 54 case LDC_RTR: 55 ldc_rx_ctrl_rtr(lc, lp); 56 break; 57 58 case LDC_RDX: 59 ldc_rx_ctrl_rdx(lc, lp); 60 break; 61 62 default: 63 DPRINTF(("CTRL/0x%02x/0x%02x\n", lp->stype, lp->ctrl)); 64 ldc_reset(lc); 65 break; 66 } 67 } 68 69 void 70 ldc_rx_ctrl_vers(struct ldc_conn *lc, struct ldc_pkt *lp) 71 { 72 struct ldc_pkt *lvp = (struct ldc_pkt *)lp; 73 74 switch (lp->stype) { 75 case LDC_INFO: 76 if (lc->lc_state == LDC_RCV_VERS) { 77 DPRINTF(("Spurious CTRL/INFO/VERS: state %d\n", 78 lc->lc_state)); 79 return; 80 } 81 DPRINTF(("CTRL/INFO/VERS\n")); 82 if (lvp->major == LDC_VERSION_MAJOR && 83 lvp->minor == LDC_VERSION_MINOR) 84 ldc_send_ack(lc); 85 else 86 /* XXX do nothing for now. */ 87 ; 88 break; 89 90 case LDC_ACK: 91 if (lc->lc_state != LDC_SND_VERS) { 92 DPRINTF(("Spurious CTRL/ACK/VERS: state %d\n", 93 lc->lc_state)); 94 ldc_reset(lc); 95 return; 96 } 97 DPRINTF(("CTRL/ACK/VERS\n")); 98 ldc_send_rts(lc); 99 break; 100 101 case LDC_NACK: 102 DPRINTF(("CTRL/NACK/VERS\n")); 103 ldc_reset(lc); 104 break; 105 106 default: 107 DPRINTF(("CTRL/0x%02x/VERS\n", lp->stype)); 108 ldc_reset(lc); 109 break; 110 } 111 } 112 113 void 114 ldc_rx_ctrl_rts(struct ldc_conn *lc, struct ldc_pkt *lp) 115 { 116 switch (lp->stype) { 117 case LDC_INFO: 118 if (lc->lc_state != LDC_RCV_VERS) { 119 DPRINTF(("Spurious CTRL/INFO/RTS: state %d\n", 120 lc->lc_state)); 121 ldc_reset(lc); 122 return; 123 } 124 DPRINTF(("CTRL/INFO/RTS\n")); 125 if (lp->env != LDC_MODE_RELIABLE) { 126 ldc_reset(lc); 127 return; 128 } 129 ldc_send_rtr(lc); 130 break; 131 132 case LDC_ACK: 133 DPRINTF(("CTRL/ACK/RTS\n")); 134 ldc_reset(lc); 135 break; 136 137 case LDC_NACK: 138 DPRINTF(("CTRL/NACK/RTS\n")); 139 ldc_reset(lc); 140 break; 141 142 default: 143 DPRINTF(("CTRL/0x%02x/RTS\n", lp->stype)); 144 ldc_reset(lc); 145 break; 146 } 147 } 148 149 void 150 ldc_rx_ctrl_rtr(struct ldc_conn *lc, struct ldc_pkt *lp) 151 { 152 switch (lp->stype) { 153 case LDC_INFO: 154 if (lc->lc_state != LDC_SND_RTS) { 155 DPRINTF(("Spurious CTRL/INFO/RTR: state %d\n", 156 lc->lc_state)); 157 ldc_reset(lc); 158 return; 159 } 160 DPRINTF(("CTRL/INFO/RTR\n")); 161 if (lp->env != LDC_MODE_RELIABLE) { 162 ldc_reset(lc); 163 return; 164 } 165 ldc_send_rdx(lc); 166 #if 0 167 lc->lc_start(lc); 168 #endif 169 break; 170 171 case LDC_ACK: 172 DPRINTF(("CTRL/ACK/RTR\n")); 173 ldc_reset(lc); 174 break; 175 176 case LDC_NACK: 177 DPRINTF(("CTRL/NACK/RTR\n")); 178 ldc_reset(lc); 179 break; 180 181 default: 182 DPRINTF(("CTRL/0x%02x/RTR\n", lp->stype)); 183 ldc_reset(lc); 184 break; 185 } 186 } 187 188 void 189 ldc_rx_ctrl_rdx(struct ldc_conn *lc, struct ldc_pkt *lp) 190 { 191 switch (lp->stype) { 192 case LDC_INFO: 193 if (lc->lc_state != LDC_SND_RTR) { 194 DPRINTF(("Spurious CTRL/INFO/RTR: state %d\n", 195 lc->lc_state)); 196 ldc_reset(lc); 197 return; 198 } 199 DPRINTF(("CTRL/INFO/RDX\n")); 200 #if 0 201 lc->lc_start(lc); 202 #endif 203 break; 204 205 case LDC_ACK: 206 DPRINTF(("CTRL/ACK/RDX\n")); 207 ldc_reset(lc); 208 break; 209 210 case LDC_NACK: 211 DPRINTF(("CTRL/NACK/RDX\n")); 212 ldc_reset(lc); 213 break; 214 215 default: 216 DPRINTF(("CTRL/0x%02x/RDX\n", lp->stype)); 217 ldc_reset(lc); 218 break; 219 } 220 } 221 222 void 223 ldc_rx_data(struct ldc_conn *lc, struct ldc_pkt *lp) 224 { 225 size_t len; 226 227 if (lp->stype != LDC_INFO && lp->stype != LDC_ACK) { 228 DPRINTF(("DATA/0x%02x\n", lp->stype)); 229 ldc_reset(lc); 230 return; 231 } 232 233 if (lc->lc_state != LDC_SND_RTR && 234 lc->lc_state != LDC_SND_RDX) { 235 DPRINTF(("Spurious DATA/INFO: state %d\n", lc->lc_state)); 236 ldc_reset(lc); 237 return; 238 } 239 240 #if 0 241 if (lp->ackid) { 242 int i; 243 244 for (i = 0; ds_service[i].ds_svc_id; i++) { 245 if (ds_service[i].ds_ackid && 246 lp->ackid >= ds_service[i].ds_ackid) { 247 ds_service[i].ds_ackid = 0; 248 ds_service[i].ds_start(lc, ds_service[i].ds_svc_handle); 249 } 250 } 251 } 252 #endif 253 if (lp->stype == LDC_ACK) 254 return; 255 256 if (lp->env & LDC_FRAG_START) { 257 lc->lc_len = (lp->env & LDC_LEN_MASK); 258 memcpy((uint8_t *)lc->lc_msg, &lp->data, lc->lc_len); 259 } else { 260 len = (lp->env & LDC_LEN_MASK); 261 if (lc->lc_len + len > sizeof(lc->lc_msg)) { 262 DPRINTF(("Buffer overrun\n")); 263 ldc_reset(lc); 264 return; 265 } 266 memcpy((uint8_t *)lc->lc_msg + lc->lc_len, &lp->data, len); 267 lc->lc_len += len; 268 } 269 270 if (lp->env & LDC_FRAG_STOP) { 271 ldc_ack(lc, lp->seqid); 272 lc->lc_rx_data(lc, lc->lc_msg, lc->lc_len); 273 } 274 } 275 276 void 277 ldc_send_vers(struct ldc_conn *lc) 278 { 279 struct ldc_pkt lp; 280 ssize_t nbytes; 281 282 bzero(&lp, sizeof(lp)); 283 lp.type = LDC_CTRL; 284 lp.stype = LDC_INFO; 285 lp.ctrl = LDC_VERS; 286 lp.major = 1; 287 lp.minor = 0; 288 289 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 290 if (nbytes != sizeof(lp)) 291 err(1, "write"); 292 293 lc->lc_state = LDC_SND_VERS; 294 } 295 296 void 297 ldc_send_ack(struct ldc_conn *lc) 298 { 299 struct ldc_pkt lp; 300 ssize_t nbytes; 301 302 bzero(&lp, sizeof(lp)); 303 lp.type = LDC_CTRL; 304 lp.stype = LDC_ACK; 305 lp.ctrl = LDC_VERS; 306 lp.major = 1; 307 lp.minor = 0; 308 309 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 310 if (nbytes != sizeof(lp)) 311 err(1, "write"); 312 313 lc->lc_state = LDC_RCV_VERS; 314 } 315 316 void 317 ldc_send_rts(struct ldc_conn *lc) 318 { 319 struct ldc_pkt lp; 320 ssize_t nbytes; 321 322 bzero(&lp, sizeof(lp)); 323 lp.type = LDC_CTRL; 324 lp.stype = LDC_INFO; 325 lp.ctrl = LDC_RTS; 326 lp.env = LDC_MODE_RELIABLE; 327 lp.seqid = lc->lc_tx_seqid++; 328 329 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 330 if (nbytes != sizeof(lp)) 331 err(1, "write"); 332 333 lc->lc_state = LDC_SND_RTS; 334 } 335 336 void 337 ldc_send_rtr(struct ldc_conn *lc) 338 { 339 struct ldc_pkt lp; 340 ssize_t nbytes; 341 342 bzero(&lp, sizeof(lp)); 343 lp.type = LDC_CTRL; 344 lp.stype = LDC_INFO; 345 lp.ctrl = LDC_RTR; 346 lp.env = LDC_MODE_RELIABLE; 347 lp.seqid = lc->lc_tx_seqid++; 348 349 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 350 if (nbytes != sizeof(lp)) 351 err(1, "write"); 352 353 lc->lc_state = LDC_SND_RTR; 354 } 355 356 void 357 ldc_send_rdx(struct ldc_conn *lc) 358 { 359 struct ldc_pkt lp; 360 ssize_t nbytes; 361 362 bzero(&lp, sizeof(lp)); 363 lp.type = LDC_CTRL; 364 lp.stype = LDC_INFO; 365 lp.ctrl = LDC_RDX; 366 lp.env = LDC_MODE_RELIABLE; 367 lp.seqid = lc->lc_tx_seqid++; 368 369 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 370 if (nbytes != sizeof(lp)) 371 err(1, "write"); 372 373 lc->lc_state = LDC_SND_RDX; 374 } 375 376 void 377 ldc_reset(struct ldc_conn *lc) 378 { 379 lc->lc_tx_seqid = 0; 380 lc->lc_state = 0; 381 #if 0 382 lc->lc_reset(lc); 383 #endif 384 } 385 386 void 387 ldc_ack(struct ldc_conn *lc, uint32_t ackid) 388 { 389 struct ldc_pkt lp; 390 ssize_t nbytes; 391 392 bzero(&lp, sizeof(lp)); 393 lp.type = LDC_DATA; 394 lp.stype = LDC_ACK; 395 lp.seqid = lc->lc_tx_seqid++; 396 lp.ackid = ackid; 397 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 398 if (nbytes != sizeof(lp)) 399 err(1, "write"); 400 } 401 402 void 403 ds_rx_msg(struct ldc_conn *lc, void *data, size_t len) 404 { 405 struct ds_conn *dc = lc->lc_cookie; 406 struct ds_msg *dm = data; 407 408 switch(dm->msg_type) { 409 case DS_INIT_REQ: 410 { 411 struct ds_init_req *dr = data; 412 413 DPRINTF(("DS_INIT_REQ %d.%d\n", dr->major_vers, 414 dr->minor_vers)); 415 if (dr->major_vers != 1 || dr->minor_vers != 0){ 416 ldc_reset(lc); 417 return; 418 } 419 ds_init_ack(lc); 420 break; 421 } 422 423 case DS_REG_REQ: 424 { 425 struct ds_reg_req *dr = data; 426 struct ds_conn_svc *dcs; 427 428 DPRINTF(("DS_REG_REQ %s %d.%d 0x%016llx\n", dr->svc_id, 429 dr->major_vers, dr->minor_vers, dr->svc_handle)); 430 TAILQ_FOREACH(dcs, &dc->services, link) { 431 if (strcmp(dr->svc_id, dcs->service->ds_svc_id) == 0) { 432 dcs->svc_handle = dr->svc_handle; 433 dcs->ackid = lc->lc_tx_seqid; 434 ds_reg_ack(lc, dcs->svc_handle); 435 dcs->service->ds_start(lc, dcs->svc_handle); 436 return; 437 } 438 } 439 440 ds_reg_nack(lc, dr->svc_handle); 441 break; 442 } 443 444 case DS_UNREG: 445 { 446 struct ds_unreg *du = data; 447 448 DPRINTF(("DS_UNREG 0x%016llx\n", du->svc_handle)); 449 ds_unreg_ack(lc, du->svc_handle); 450 break; 451 } 452 453 case DS_DATA: 454 { 455 struct ds_data *dd = data; 456 struct ds_conn_svc *dcs; 457 458 DPRINTF(("DS_DATA 0x%016llx\n", dd->svc_handle)); 459 TAILQ_FOREACH(dcs, &dc->services, link) { 460 if (dcs->svc_handle == dd->svc_handle) 461 dcs->service->ds_rx_data(lc, dd->svc_handle, 462 data, len); 463 } 464 break; 465 } 466 467 default: 468 DPRINTF(("Unknown DS message type 0x%x\n", dm->msg_type)); 469 ldc_reset(lc); 470 break; 471 } 472 } 473 474 void 475 ds_init_ack(struct ldc_conn *lc) 476 { 477 struct ds_init_ack da; 478 479 DPRINTF((" DS_INIT_ACK\n")); 480 bzero(&da, sizeof(da)); 481 da.msg_type = DS_INIT_ACK; 482 da.payload_len = sizeof(da) - 8; 483 da.minor_vers = 0; 484 ds_send_msg(lc, &da, sizeof(da)); 485 } 486 487 void 488 ds_reg_ack(struct ldc_conn *lc, uint64_t svc_handle) 489 { 490 struct ds_reg_ack da; 491 492 DPRINTF((" DS_REG_ACK 0x%016llx\n", svc_handle)); 493 bzero(&da, sizeof(da)); 494 da.msg_type = DS_REG_ACK; 495 da.payload_len = sizeof(da) - 8; 496 da.svc_handle = svc_handle; 497 da.minor_vers = 0; 498 ds_send_msg(lc, &da, sizeof(da)); 499 } 500 501 void 502 ds_reg_nack(struct ldc_conn *lc, uint64_t svc_handle) 503 { 504 struct ds_reg_nack dn; 505 506 DPRINTF((" DS_REG_NACK 0x%016llx\n", svc_handle)); 507 bzero(&dn, sizeof(dn)); 508 dn.msg_type = DS_REG_NACK; 509 dn.payload_len = sizeof(dn) - 8; 510 dn.svc_handle = svc_handle; 511 dn.result = DS_REG_VER_NACK; 512 dn.major_vers = 0; 513 ds_send_msg(lc, &dn, sizeof(dn)); 514 } 515 516 void 517 ds_unreg_ack(struct ldc_conn *lc, uint64_t svc_handle) 518 { 519 struct ds_unreg du; 520 521 DPRINTF((" DS_UNREG_ACK 0x%016llx\n", svc_handle)); 522 bzero(&du, sizeof(du)); 523 du.msg_type = DS_UNREG_ACK; 524 du.payload_len = sizeof(du) - 8; 525 du.svc_handle = svc_handle; 526 ds_send_msg(lc, &du, sizeof(du)); 527 } 528 529 void 530 ds_unreg_nack(struct ldc_conn *lc, uint64_t svc_handle) 531 { 532 struct ds_unreg du; 533 534 DPRINTF((" DS_UNREG_NACK 0x%016llx\n", svc_handle)); 535 bzero(&du, sizeof(du)); 536 du.msg_type = DS_UNREG_NACK; 537 du.payload_len = sizeof(du) - 8; 538 du.svc_handle = svc_handle; 539 ds_send_msg(lc, &du, sizeof(du)); 540 } 541 542 void 543 ds_receive_msg(struct ldc_conn *lc, void *buf, size_t len) 544 { 545 int env = LDC_FRAG_START; 546 struct ldc_pkt lp; 547 uint8_t *p = buf; 548 ssize_t nbytes; 549 550 while (len > 0) { 551 nbytes = read(lc->lc_fd, &lp, sizeof(lp)); 552 if (nbytes != sizeof(lp)) 553 err(1, "read"); 554 555 if (lp.type != LDC_DATA && 556 lp.stype != LDC_INFO) { 557 ldc_reset(lc); 558 return; 559 } 560 561 if ((lp.env & LDC_FRAG_START) != env) { 562 ldc_reset(lc); 563 return; 564 } 565 566 bcopy(&lp.data, p, (lp.env & LDC_LEN_MASK)); 567 p += (lp.env & LDC_LEN_MASK); 568 len -= (lp.env & LDC_LEN_MASK); 569 570 if (lp.env & LDC_FRAG_STOP) 571 ldc_ack(lc, lp.seqid); 572 573 env = (lp.env & LDC_FRAG_STOP) ? LDC_FRAG_START : 0; 574 } 575 } 576 577 void 578 ldc_send_msg(struct ldc_conn *lc, void *buf, size_t len) 579 { 580 struct ldc_pkt lp; 581 uint8_t *p = buf; 582 ssize_t nbytes; 583 584 while (len > 0) { 585 bzero(&lp, sizeof(lp)); 586 lp.type = LDC_DATA; 587 lp.stype = LDC_INFO; 588 lp.env = min(len, LDC_PKT_PAYLOAD); 589 if (p == buf) 590 lp.env |= LDC_FRAG_START; 591 if (len <= LDC_PKT_PAYLOAD) 592 lp.env |= LDC_FRAG_STOP; 593 lp.seqid = lc->lc_tx_seqid++; 594 bcopy(p, &lp.data, min(len, LDC_PKT_PAYLOAD)); 595 596 nbytes = write(lc->lc_fd, &lp, sizeof(lp)); 597 if (nbytes != sizeof(lp)) 598 err(1, "write"); 599 p += min(len, LDC_PKT_PAYLOAD); 600 len -= min(len, LDC_PKT_PAYLOAD); 601 } 602 } 603 604 void 605 ds_send_msg(struct ldc_conn *lc, void *buf, size_t len) 606 { 607 uint8_t *p = buf; 608 struct ldc_pkt lp; 609 ssize_t nbytes; 610 611 while (len > 0) { 612 ldc_send_msg(lc, p, min(len, LDC_MSG_MAX)); 613 p += min(len, LDC_MSG_MAX); 614 len -= min(len, LDC_MSG_MAX); 615 616 if (len > 0) { 617 /* Consume ACK. */ 618 nbytes = read(lc->lc_fd, &lp, sizeof(lp)); 619 if (nbytes != sizeof(lp)) 620 err(1, "read"); 621 #if 0 622 { 623 uint64_t *msg = (uint64_t *)&lp; 624 int i; 625 626 for (i = 0; i < 8; i++) 627 printf("%02x: %016llx\n", i, msg[i]); 628 } 629 #endif 630 } 631 } 632 } 633 634 TAILQ_HEAD(ds_conn_head, ds_conn) ds_conns = 635 TAILQ_HEAD_INITIALIZER(ds_conns); 636 int num_ds_conns; 637 638 struct ds_conn * 639 ds_conn_open(const char *path, void *cookie) 640 { 641 struct ds_conn *dc; 642 643 dc = xmalloc(sizeof(*dc)); 644 dc->path = xstrdup(path); 645 dc->cookie = cookie; 646 647 dc->fd = open(path, O_RDWR, 0); 648 if (dc->fd == -1) 649 err(1, "cannot open %s", path); 650 651 memset(&dc->lc, 0, sizeof(dc->lc)); 652 dc->lc.lc_fd = dc->fd; 653 dc->lc.lc_cookie = dc; 654 dc->lc.lc_rx_data = ds_rx_msg; 655 656 TAILQ_INIT(&dc->services); 657 TAILQ_INSERT_TAIL(&ds_conns, dc, link); 658 dc->id = num_ds_conns++; 659 return dc; 660 } 661 662 void 663 ds_conn_register_service(struct ds_conn *dc, struct ds_service *ds) 664 { 665 struct ds_conn_svc *dcs; 666 667 dcs = xzalloc(sizeof(*dcs)); 668 dcs->service = ds; 669 670 TAILQ_INSERT_TAIL(&dc->services, dcs, link); 671 } 672 673 void 674 ds_conn_handle(struct ds_conn *dc) 675 { 676 struct ldc_pkt lp; 677 ssize_t nbytes; 678 679 nbytes = read(dc->fd, &lp, sizeof(lp)); 680 if (nbytes != sizeof(lp)) { 681 ldc_reset(&dc->lc); 682 return; 683 } 684 685 switch (lp.type) { 686 case LDC_CTRL: 687 ldc_rx_ctrl(&dc->lc, &lp); 688 break; 689 case LDC_DATA: 690 ldc_rx_data(&dc->lc, &lp); 691 break; 692 default: 693 DPRINTF(("0x%02x/0x%02x/0x%02x\n", lp.type, lp.stype, 694 lp.ctrl)); 695 ldc_reset(&dc->lc); 696 break; 697 } 698 } 699 700 void 701 ds_conn_serve(void) 702 { 703 struct ds_conn *dc; 704 struct pollfd *pfd; 705 int nfds; 706 707 pfd = xreallocarray(NULL, num_ds_conns, sizeof(*pfd)); 708 TAILQ_FOREACH(dc, &ds_conns, link) { 709 pfd[dc->id].fd = dc->fd; 710 pfd[dc->id].events = POLLIN; 711 } 712 713 while (1) { 714 nfds = poll(pfd, num_ds_conns, -1); 715 if (nfds == -1 || nfds == 0) 716 errx(1, "poll"); 717 718 TAILQ_FOREACH(dc, &ds_conns, link) { 719 if (pfd[dc->id].revents) 720 ds_conn_handle(dc); 721 } 722 } 723 } 724