1 /* 2 * Copyright (c) 2020 Yubico AB. All rights reserved. 3 * Use of this source code is governed by a BSD-style 4 * license that can be found in the LICENSE file. 5 */ 6 7 #include <sys/socket.h> 8 9 #include <linux/genetlink.h> 10 #include <linux/netlink.h> 11 #include <linux/nfc.h> 12 13 #include <errno.h> 14 #include <limits.h> 15 16 #include "fido.h" 17 #include "netlink.h" 18 19 #ifdef FIDO_FUZZ 20 static ssize_t (*fuzz_read)(int, void *, size_t); 21 static ssize_t (*fuzz_write)(int, const void *, size_t); 22 # define READ fuzz_read 23 # define WRITE fuzz_write 24 #else 25 # define READ read 26 # define WRITE write 27 #endif 28 29 #ifndef SOL_NETLINK 30 #define SOL_NETLINK 270 31 #endif 32 33 #define NETLINK_POLL_MS 100 34 35 /* XXX avoid signed NLA_ALIGNTO */ 36 #undef NLA_HDRLEN 37 #define NLA_HDRLEN NLMSG_ALIGN(sizeof(struct nlattr)) 38 39 typedef struct nlmsgbuf { 40 size_t siz; /* alloc size */ 41 size_t len; /* of payload */ 42 unsigned char *ptr; /* in payload */ 43 union { 44 struct nlmsghdr nlmsg; 45 char buf[NLMSG_HDRLEN]; /* align */ 46 } u; 47 unsigned char payload[]; 48 } nlmsgbuf_t; 49 50 typedef struct genlmsgbuf { 51 union { 52 struct genlmsghdr genl; 53 char buf[GENL_HDRLEN]; /* align */ 54 } u; 55 } genlmsgbuf_t; 56 57 typedef struct nlamsgbuf { 58 size_t siz; /* alloc size */ 59 size_t len; /* of payload */ 60 unsigned char *ptr; /* in payload */ 61 union { 62 struct nlattr nla; 63 char buf[NLA_HDRLEN]; /* align */ 64 } u; 65 unsigned char payload[]; 66 } nlamsgbuf_t; 67 68 typedef struct nl_family { 69 uint16_t id; 70 uint32_t mcastgrp; 71 } nl_family_t; 72 73 typedef struct nl_poll { 74 uint32_t dev; 75 unsigned int eventcnt; 76 } nl_poll_t; 77 78 typedef struct nl_target { 79 int found; 80 uint32_t *value; 81 } nl_target_t; 82 83 static const void * 84 nlmsg_ptr(const nlmsgbuf_t *m) 85 { 86 return (&m->u.nlmsg); 87 } 88 89 static size_t 90 nlmsg_len(const nlmsgbuf_t *m) 91 { 92 return (m->u.nlmsg.nlmsg_len); 93 } 94 95 static uint16_t 96 nlmsg_type(const nlmsgbuf_t *m) 97 { 98 return (m->u.nlmsg.nlmsg_type); 99 } 100 101 static nlmsgbuf_t * 102 nlmsg_new(uint16_t type, uint16_t flags, size_t len) 103 { 104 nlmsgbuf_t *m; 105 size_t siz; 106 107 if (len > SIZE_MAX - sizeof(*m) || 108 (siz = sizeof(*m) + len) > UINT16_MAX || 109 (m = calloc(1, siz)) == NULL) 110 return (NULL); 111 112 m->siz = siz; 113 m->len = len; 114 m->ptr = m->payload; 115 m->u.nlmsg.nlmsg_type = type; 116 m->u.nlmsg.nlmsg_flags = NLM_F_REQUEST | flags; 117 m->u.nlmsg.nlmsg_len = NLMSG_HDRLEN; 118 119 return (m); 120 } 121 122 static nlamsgbuf_t * 123 nla_from_buf(const unsigned char **ptr, size_t *len) 124 { 125 nlamsgbuf_t h, *a; 126 size_t nlalen, skip; 127 128 if (*len < sizeof(h.u)) 129 return (NULL); 130 131 memset(&h, 0, sizeof(h)); 132 memcpy(&h.u, *ptr, sizeof(h.u)); 133 134 if ((nlalen = h.u.nla.nla_len) < sizeof(h.u) || nlalen > *len || 135 nlalen - sizeof(h.u) > UINT16_MAX || 136 nlalen > SIZE_MAX - sizeof(*a) || 137 (skip = NLMSG_ALIGN(nlalen)) > *len || 138 (a = calloc(1, sizeof(*a) + nlalen - sizeof(h.u))) == NULL) 139 return (NULL); 140 141 memcpy(&a->u, *ptr, nlalen); 142 a->siz = sizeof(*a) + nlalen - sizeof(h.u); 143 a->ptr = a->payload; 144 a->len = nlalen - sizeof(h.u); 145 *ptr += skip; 146 *len -= skip; 147 148 return (a); 149 } 150 151 static nlamsgbuf_t * 152 nla_getattr(nlamsgbuf_t *a) 153 { 154 return (nla_from_buf((void *)&a->ptr, &a->len)); 155 } 156 157 static uint16_t 158 nla_type(const nlamsgbuf_t *a) 159 { 160 return (a->u.nla.nla_type); 161 } 162 163 static nlamsgbuf_t * 164 nlmsg_getattr(nlmsgbuf_t *m) 165 { 166 return (nla_from_buf((void *)&m->ptr, &m->len)); 167 } 168 169 static int 170 nla_read(nlamsgbuf_t *a, void *buf, size_t cnt) 171 { 172 if (cnt > a->u.nla.nla_len || 173 fido_buf_read((void *)&a->ptr, &a->len, buf, cnt) < 0) 174 return (-1); 175 176 a->u.nla.nla_len = (uint16_t)(a->u.nla.nla_len - cnt); 177 178 return (0); 179 } 180 181 static nlmsgbuf_t * 182 nlmsg_from_buf(const unsigned char **ptr, size_t *len) 183 { 184 nlmsgbuf_t h, *m; 185 size_t msglen, skip; 186 187 if (*len < sizeof(h.u)) 188 return (NULL); 189 190 memset(&h, 0, sizeof(h)); 191 memcpy(&h.u, *ptr, sizeof(h.u)); 192 193 if ((msglen = h.u.nlmsg.nlmsg_len) < sizeof(h.u) || msglen > *len || 194 msglen - sizeof(h.u) > UINT16_MAX || 195 (skip = NLMSG_ALIGN(msglen)) > *len || 196 (m = nlmsg_new(0, 0, msglen - sizeof(h.u))) == NULL) 197 return (NULL); 198 199 memcpy(&m->u, *ptr, msglen); 200 *ptr += skip; 201 *len -= skip; 202 203 return (m); 204 } 205 206 static int 207 nlmsg_read(nlmsgbuf_t *m, void *buf, size_t cnt) 208 { 209 if (cnt > m->u.nlmsg.nlmsg_len || 210 fido_buf_read((void *)&m->ptr, &m->len, buf, cnt) < 0) 211 return (-1); 212 213 m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len - cnt); 214 215 return (0); 216 } 217 218 static int 219 nlmsg_write(nlmsgbuf_t *m, const void *buf, size_t cnt) 220 { 221 if (cnt > UINT32_MAX - m->u.nlmsg.nlmsg_len || 222 fido_buf_write(&m->ptr, &m->len, buf, cnt) < 0) 223 return (-1); 224 225 m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len + cnt); 226 227 return (0); 228 } 229 230 static int 231 nlmsg_set_genl(nlmsgbuf_t *m, uint8_t cmd) 232 { 233 genlmsgbuf_t g; 234 235 memset(&g, 0, sizeof(g)); 236 g.u.genl.cmd = cmd; 237 g.u.genl.version = NFC_GENL_VERSION; 238 239 return (nlmsg_write(m, &g, sizeof(g))); 240 } 241 242 static int 243 nlmsg_get_genl(nlmsgbuf_t *m, uint8_t cmd) 244 { 245 genlmsgbuf_t g; 246 247 memset(&g, 0, sizeof(g)); 248 249 if (nlmsg_read(m, &g, sizeof(g)) < 0 || g.u.genl.cmd != cmd) 250 return (-1); 251 252 return (0); 253 } 254 255 static int 256 nlmsg_get_status(nlmsgbuf_t *m) 257 { 258 int status; 259 260 if (nlmsg_read(m, &status, sizeof(status)) < 0 || status == INT_MIN) 261 return (-1); 262 if (status < 0) 263 status = -status; 264 265 return (status); 266 } 267 268 static int 269 nlmsg_setattr(nlmsgbuf_t *m, uint16_t type, const void *ptr, size_t len) 270 { 271 int r; 272 char *padding; 273 size_t skip; 274 nlamsgbuf_t a; 275 276 if ((skip = NLMSG_ALIGN(len)) > UINT16_MAX - sizeof(a.u) || 277 skip < len || (padding = calloc(1, skip - len)) == NULL) 278 return (-1); 279 280 memset(&a, 0, sizeof(a)); 281 a.u.nla.nla_type = type; 282 a.u.nla.nla_len = (uint16_t)(len + sizeof(a.u)); 283 r = nlmsg_write(m, &a.u, sizeof(a.u)) < 0 || 284 nlmsg_write(m, ptr, len) < 0 || 285 nlmsg_write(m, padding, skip - len) < 0 ? -1 : 0; 286 287 free(padding); 288 289 return (r); 290 } 291 292 static int 293 nlmsg_set_u16(nlmsgbuf_t *m, uint16_t type, uint16_t val) 294 { 295 return (nlmsg_setattr(m, type, &val, sizeof(val))); 296 } 297 298 static int 299 nlmsg_set_u32(nlmsgbuf_t *m, uint16_t type, uint32_t val) 300 { 301 return (nlmsg_setattr(m, type, &val, sizeof(val))); 302 } 303 304 static int 305 nlmsg_set_str(nlmsgbuf_t *m, uint16_t type, const char *val) 306 { 307 return (nlmsg_setattr(m, type, val, strlen(val) + 1)); 308 } 309 310 static int 311 nla_get_u16(nlamsgbuf_t *a, uint16_t *v) 312 { 313 return (nla_read(a, v, sizeof(*v))); 314 } 315 316 static int 317 nla_get_u32(nlamsgbuf_t *a, uint32_t *v) 318 { 319 return (nla_read(a, v, sizeof(*v))); 320 } 321 322 static char * 323 nla_get_str(nlamsgbuf_t *a) 324 { 325 size_t n; 326 char *s = NULL; 327 328 if ((n = a->len) < 1 || a->ptr[n - 1] != '\0' || 329 (s = calloc(1, n)) == NULL || nla_read(a, s, n) < 0) { 330 free(s); 331 return (NULL); 332 } 333 s[n - 1] = '\0'; 334 335 return (s); 336 } 337 338 static int 339 nlmsg_tx(int fd, const nlmsgbuf_t *m) 340 { 341 ssize_t r; 342 343 if ((r = WRITE(fd, nlmsg_ptr(m), nlmsg_len(m))) == -1) { 344 fido_log_error(errno, "%s: write", __func__); 345 return (-1); 346 } 347 if (r < 0 || (size_t)r != nlmsg_len(m)) { 348 fido_log_debug("%s: %zd != %zu", __func__, r, nlmsg_len(m)); 349 return (-1); 350 } 351 fido_log_xxd(nlmsg_ptr(m), nlmsg_len(m), "%s", __func__); 352 353 return (0); 354 } 355 356 static ssize_t 357 nlmsg_rx(int fd, unsigned char *ptr, size_t len, int ms) 358 { 359 ssize_t r; 360 361 if (len > SSIZE_MAX) { 362 fido_log_debug("%s: len", __func__); 363 return (-1); 364 } 365 if (fido_hid_unix_wait(fd, ms, NULL) < 0) { 366 fido_log_debug("%s: fido_hid_unix_wait", __func__); 367 return (-1); 368 } 369 if ((r = READ(fd, ptr, len)) == -1) { 370 fido_log_error(errno, "%s: read %zd", __func__, r); 371 return (-1); 372 } 373 fido_log_xxd(ptr, (size_t)r, "%s", __func__); 374 375 return (r); 376 } 377 378 static int 379 nlmsg_iter(nlmsgbuf_t *m, void *arg, int (*parser)(nlamsgbuf_t *, void *)) 380 { 381 nlamsgbuf_t *a; 382 int r; 383 384 while ((a = nlmsg_getattr(m)) != NULL) { 385 r = parser(a, arg); 386 free(a); 387 if (r < 0) { 388 fido_log_debug("%s: parser", __func__); 389 return (-1); 390 } 391 } 392 393 return (0); 394 } 395 396 static int 397 nla_iter(nlamsgbuf_t *g, void *arg, int (*parser)(nlamsgbuf_t *, void *)) 398 { 399 nlamsgbuf_t *a; 400 int r; 401 402 while ((a = nla_getattr(g)) != NULL) { 403 r = parser(a, arg); 404 free(a); 405 if (r < 0) { 406 fido_log_debug("%s: parser", __func__); 407 return (-1); 408 } 409 } 410 411 return (0); 412 } 413 414 static int 415 nl_parse_reply(const uint8_t *blob, size_t blob_len, uint16_t msg_type, 416 uint8_t genl_cmd, void *arg, int (*parser)(nlamsgbuf_t *, void *)) 417 { 418 nlmsgbuf_t *m; 419 int r; 420 421 while (blob_len) { 422 if ((m = nlmsg_from_buf(&blob, &blob_len)) == NULL) { 423 fido_log_debug("%s: nlmsg", __func__); 424 return (-1); 425 } 426 if (nlmsg_type(m) == NLMSG_ERROR) { 427 r = nlmsg_get_status(m); 428 free(m); 429 return (r); 430 } 431 if (nlmsg_type(m) != msg_type || 432 nlmsg_get_genl(m, genl_cmd) < 0) { 433 fido_log_debug("%s: skipping", __func__); 434 free(m); 435 continue; 436 } 437 if (parser != NULL && nlmsg_iter(m, arg, parser) < 0) { 438 fido_log_debug("%s: nlmsg_iter", __func__); 439 free(m); 440 return (-1); 441 } 442 free(m); 443 } 444 445 return (0); 446 } 447 448 static int 449 parse_mcastgrp(nlamsgbuf_t *a, void *arg) 450 { 451 nl_family_t *family = arg; 452 char *name; 453 454 switch (nla_type(a)) { 455 case CTRL_ATTR_MCAST_GRP_NAME: 456 if ((name = nla_get_str(a)) == NULL || 457 strcmp(name, NFC_GENL_MCAST_EVENT_NAME) != 0) { 458 free(name); 459 return (-1); /* XXX skip? */ 460 } 461 free(name); 462 return (0); 463 case CTRL_ATTR_MCAST_GRP_ID: 464 if (family->mcastgrp) 465 break; 466 if (nla_get_u32(a, &family->mcastgrp) < 0) { 467 fido_log_debug("%s: group", __func__); 468 return (-1); 469 } 470 return (0); 471 } 472 473 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a)); 474 475 return (0); 476 } 477 478 static int 479 parse_mcastgrps(nlamsgbuf_t *a, void *arg) 480 { 481 return (nla_iter(a, arg, parse_mcastgrp)); 482 } 483 484 static int 485 parse_family(nlamsgbuf_t *a, void *arg) 486 { 487 nl_family_t *family = arg; 488 489 switch (nla_type(a)) { 490 case CTRL_ATTR_FAMILY_ID: 491 if (family->id) 492 break; 493 if (nla_get_u16(a, &family->id) < 0) { 494 fido_log_debug("%s: id", __func__); 495 return (-1); 496 } 497 return (0); 498 case CTRL_ATTR_MCAST_GROUPS: 499 return (nla_iter(a, family, parse_mcastgrps)); 500 } 501 502 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a)); 503 504 return (0); 505 } 506 507 static int 508 nl_get_nfc_family(int fd, uint16_t *type, uint32_t *mcastgrp) 509 { 510 nlmsgbuf_t *m; 511 uint8_t reply[512]; 512 nl_family_t family; 513 ssize_t r; 514 int ok; 515 516 if ((m = nlmsg_new(GENL_ID_CTRL, 0, 64)) == NULL || 517 nlmsg_set_genl(m, CTRL_CMD_GETFAMILY) < 0 || 518 nlmsg_set_u16(m, CTRL_ATTR_FAMILY_ID, GENL_ID_CTRL) < 0 || 519 nlmsg_set_str(m, CTRL_ATTR_FAMILY_NAME, NFC_GENL_NAME) < 0 || 520 nlmsg_tx(fd, m) < 0) { 521 free(m); 522 return (-1); 523 } 524 free(m); 525 memset(&family, 0, sizeof(family)); 526 if ((r = nlmsg_rx(fd, reply, sizeof(reply), -1)) < 0) { 527 fido_log_debug("%s: nlmsg_rx", __func__); 528 return (-1); 529 } 530 if ((ok = nl_parse_reply(reply, (size_t)r, GENL_ID_CTRL, 531 CTRL_CMD_NEWFAMILY, &family, parse_family)) != 0) { 532 fido_log_debug("%s: nl_parse_reply: %d", __func__, ok); 533 return (-1); 534 } 535 if (family.id == 0 || family.mcastgrp == 0) { 536 fido_log_debug("%s: missing attr", __func__); 537 return (-1); 538 } 539 *type = family.id; 540 *mcastgrp = family.mcastgrp; 541 542 return (0); 543 } 544 545 static int 546 parse_target(nlamsgbuf_t *a, void *arg) 547 { 548 nl_target_t *t = arg; 549 550 if (t->found || nla_type(a) != NFC_ATTR_TARGET_INDEX) { 551 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a)); 552 return (0); 553 } 554 if (nla_get_u32(a, t->value) < 0) { 555 fido_log_debug("%s: target", __func__); 556 return (-1); 557 } 558 t->found = 1; 559 560 return (0); 561 } 562 563 int 564 fido_nl_power_nfc(fido_nl_t *nl, uint32_t dev) 565 { 566 nlmsgbuf_t *m; 567 uint8_t reply[512]; 568 ssize_t r; 569 int ok; 570 571 if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL || 572 nlmsg_set_genl(m, NFC_CMD_DEV_UP) < 0 || 573 nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 || 574 nlmsg_tx(nl->fd, m) < 0) { 575 free(m); 576 return (-1); 577 } 578 free(m); 579 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) { 580 fido_log_debug("%s: nlmsg_rx", __func__); 581 return (-1); 582 } 583 if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type, 584 NFC_CMD_DEV_UP, NULL, NULL)) != 0 && ok != EALREADY) { 585 fido_log_debug("%s: nl_parse_reply: %d", __func__, ok); 586 return (-1); 587 } 588 589 return (0); 590 } 591 592 static int 593 nl_nfc_poll(fido_nl_t *nl, uint32_t dev) 594 { 595 nlmsgbuf_t *m; 596 uint8_t reply[512]; 597 ssize_t r; 598 int ok; 599 600 if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL || 601 nlmsg_set_genl(m, NFC_CMD_START_POLL) < 0 || 602 nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 || 603 nlmsg_set_u32(m, NFC_ATTR_PROTOCOLS, NFC_PROTO_ISO14443_MASK) < 0 || 604 nlmsg_tx(nl->fd, m) < 0) { 605 free(m); 606 return (-1); 607 } 608 free(m); 609 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) { 610 fido_log_debug("%s: nlmsg_rx", __func__); 611 return (-1); 612 } 613 if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type, 614 NFC_CMD_START_POLL, NULL, NULL)) != 0) { 615 fido_log_debug("%s: nl_parse_reply: %d", __func__, ok); 616 return (-1); 617 } 618 619 return (0); 620 } 621 622 static int 623 nl_dump_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target, int ms) 624 { 625 nlmsgbuf_t *m; 626 nl_target_t t; 627 uint8_t reply[512]; 628 ssize_t r; 629 int ok; 630 631 if ((m = nlmsg_new(nl->nfc_type, NLM_F_DUMP, 64)) == NULL || 632 nlmsg_set_genl(m, NFC_CMD_GET_TARGET) < 0 || 633 nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 || 634 nlmsg_tx(nl->fd, m) < 0) { 635 free(m); 636 return (-1); 637 } 638 free(m); 639 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), ms)) < 0) { 640 fido_log_debug("%s: nlmsg_rx", __func__); 641 return (-1); 642 } 643 memset(&t, 0, sizeof(t)); 644 t.value = target; 645 if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type, 646 NFC_CMD_GET_TARGET, &t, parse_target)) != 0) { 647 fido_log_debug("%s: nl_parse_reply: %d", __func__, ok); 648 return (-1); 649 } 650 if (!t.found) { 651 fido_log_debug("%s: target not found", __func__); 652 return (-1); 653 } 654 655 return (0); 656 } 657 658 static int 659 parse_nfc_event(nlamsgbuf_t *a, void *arg) 660 { 661 nl_poll_t *ctx = arg; 662 uint32_t dev; 663 664 if (nla_type(a) != NFC_ATTR_DEVICE_INDEX) { 665 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a)); 666 return (0); 667 } 668 if (nla_get_u32(a, &dev) < 0) { 669 fido_log_debug("%s: dev", __func__); 670 return (-1); 671 } 672 if (dev == ctx->dev) 673 ctx->eventcnt++; 674 else 675 fido_log_debug("%s: ignoring dev 0x%x", __func__, dev); 676 677 return (0); 678 } 679 680 int 681 fido_nl_get_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target) 682 { 683 uint8_t reply[512]; 684 nl_poll_t ctx; 685 ssize_t r; 686 int ok; 687 688 if (nl_nfc_poll(nl, dev) < 0) { 689 fido_log_debug("%s: nl_nfc_poll", __func__); 690 return (-1); 691 } 692 #ifndef FIDO_FUZZ 693 if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, 694 &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) { 695 fido_log_error(errno, "%s: setsockopt add", __func__); 696 return (-1); 697 } 698 #endif 699 r = nlmsg_rx(nl->fd, reply, sizeof(reply), NETLINK_POLL_MS); 700 #ifndef FIDO_FUZZ 701 if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP, 702 &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) { 703 fido_log_error(errno, "%s: setsockopt drop", __func__); 704 return (-1); 705 } 706 #endif 707 if (r < 0) { 708 fido_log_debug("%s: nlmsg_rx", __func__); 709 return (-1); 710 } 711 memset(&ctx, 0, sizeof(ctx)); 712 ctx.dev = dev; 713 if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type, 714 NFC_EVENT_TARGETS_FOUND, &ctx, parse_nfc_event)) != 0) { 715 fido_log_debug("%s: nl_parse_reply: %d", __func__, ok); 716 return (-1); 717 } 718 if (ctx.eventcnt == 0) { 719 fido_log_debug("%s: dev 0x%x not observed", __func__, dev); 720 return (-1); 721 } 722 if (nl_dump_nfc_target(nl, dev, target, -1) < 0) { 723 fido_log_debug("%s: nl_dump_nfc_target", __func__); 724 return (-1); 725 } 726 727 return (0); 728 } 729 730 void 731 fido_nl_free(fido_nl_t **nlp) 732 { 733 fido_nl_t *nl; 734 735 if (nlp == NULL || (nl = *nlp) == NULL) 736 return; 737 if (nl->fd != -1 && close(nl->fd) == -1) 738 fido_log_error(errno, "%s: close", __func__); 739 740 free(nl); 741 *nlp = NULL; 742 } 743 744 fido_nl_t * 745 fido_nl_new(void) 746 { 747 fido_nl_t *nl; 748 int ok = -1; 749 750 if ((nl = calloc(1, sizeof(*nl))) == NULL) 751 return (NULL); 752 if ((nl->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, 753 NETLINK_GENERIC)) == -1) { 754 fido_log_error(errno, "%s: socket", __func__); 755 goto fail; 756 } 757 nl->saddr.nl_family = AF_NETLINK; 758 if (bind(nl->fd, (struct sockaddr *)&nl->saddr, 759 sizeof(nl->saddr)) == -1) { 760 fido_log_error(errno, "%s: bind", __func__); 761 goto fail; 762 } 763 if (nl_get_nfc_family(nl->fd, &nl->nfc_type, &nl->nfc_mcastgrp) < 0) { 764 fido_log_debug("%s: nl_get_nfc_family", __func__); 765 goto fail; 766 } 767 768 ok = 0; 769 fail: 770 if (ok < 0) 771 fido_nl_free(&nl); 772 773 return (nl); 774 } 775 776 #ifdef FIDO_FUZZ 777 void 778 set_netlink_io_functions(ssize_t (*read_f)(int, void *, size_t), 779 ssize_t (*write_f)(int, const void *, size_t)) 780 { 781 fuzz_read = read_f; 782 fuzz_write = write_f; 783 } 784 #endif 785