1 /* 2 * libnetlink.c RTnetlink service routines. 3 * 4 * This program is free software; you can redistribute it and/or 5 * modify it under the terms of the GNU General Public License 6 * as published by the Free Software Foundation; either version 7 * 2 of the License, or (at your option) any later version. 8 * 9 * Authors: Alexey Kuznetsov, <kuznet (at) ms2.inr.ac.ru> 10 * 11 */ 12 13 #include <stdio.h> 14 #include <stdlib.h> 15 #include <stdbool.h> 16 #include <unistd.h> 17 #include <syslog.h> 18 #include <fcntl.h> 19 #include <net/if_arp.h> 20 #include <sys/socket.h> 21 #include <netinet/in.h> 22 #include <string.h> 23 #include <errno.h> 24 #include <time.h> 25 #include <sys/uio.h> 26 27 #include "libnetlink.h" 28 29 #ifndef SOL_NETLINK 30 #define SOL_NETLINK 270 31 #endif 32 33 #ifndef MIN 34 #define MIN(a, b) ((a) < (b) ? (a) : (b)) 35 #endif 36 37 int rcvbuf = 1024 * 1024; 38 39 #ifdef HAVE_LIBMNL 40 #include <libmnl/libmnl.h> 41 42 static const enum mnl_attr_data_type extack_policy[NLMSGERR_ATTR_MAX + 1] = { 43 [NLMSGERR_ATTR_MSG] = MNL_TYPE_NUL_STRING, 44 [NLMSGERR_ATTR_OFFS] = MNL_TYPE_U32, 45 }; 46 47 static int err_attr_cb(const struct nlattr *attr, void *data) 48 { 49 const struct nlattr **tb = data; 50 uint16_t type; 51 52 if (mnl_attr_type_valid(attr, NLMSGERR_ATTR_MAX) < 0) { 53 fprintf(stderr, "Invalid extack attribute\n"); 54 return MNL_CB_ERROR; 55 } 56 57 type = mnl_attr_get_type(attr); 58 if (mnl_attr_validate(attr, extack_policy[type]) < 0) { 59 fprintf(stderr, "extack attribute %d failed validation\n", 60 type); 61 return MNL_CB_ERROR; 62 } 63 64 tb[type] = attr; 65 return MNL_CB_OK; 66 } 67 68 /* dump netlink extended ack error message */ 69 static int nl_dump_ext_err(const struct nlmsghdr *nlh, nl_ext_ack_fn_t errfn) 70 { 71 struct nlattr *tb[NLMSGERR_ATTR_MAX + 1] = {}; 72 const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh); 73 const struct nlmsghdr *err_nlh = NULL; 74 unsigned int hlen = sizeof(*err); 75 const char *errmsg = NULL; 76 uint32_t off = 0; 77 78 /* no TLVs, nothing to do here */ 79 if (!(nlh->nlmsg_flags & NLM_F_ACK_TLVS)) 80 return 0; 81 82 /* if NLM_F_CAPPED is set then the inner err msg was capped */ 83 if (!(nlh->nlmsg_flags & NLM_F_CAPPED)) 84 hlen += mnl_nlmsg_get_payload_len(&err->msg); 85 86 if (mnl_attr_parse(nlh, hlen, err_attr_cb, tb) != MNL_CB_OK) 87 return 0; 88 89 if (tb[NLMSGERR_ATTR_MSG]) 90 errmsg = mnl_attr_get_str(tb[NLMSGERR_ATTR_MSG]); 91 92 if (tb[NLMSGERR_ATTR_OFFS]) { 93 off = mnl_attr_get_u32(tb[NLMSGERR_ATTR_OFFS]); 94 95 if (off > nlh->nlmsg_len) { 96 fprintf(stderr, 97 "Invalid offset for NLMSGERR_ATTR_OFFS\n"); 98 off = 0; 99 } else if (!(nlh->nlmsg_flags & NLM_F_CAPPED)) 100 err_nlh = &err->msg; 101 } 102 103 if (errfn) 104 return errfn(errmsg, off, err_nlh); 105 106 if (errmsg && *errmsg != '\0') { 107 fprintf(stderr, "Error: %s", errmsg); 108 if (errmsg[strlen(errmsg) - 1] != '.') 109 fprintf(stderr, "."); 110 fprintf(stderr, "\n"); 111 112 return 1; 113 } 114 115 return 0; 116 } 117 #else 118 #ifndef ANDROID 119 #warning "libmnl required for error support" 120 #endif 121 122 /* No extended error ack without libmnl */ 123 static int nl_dump_ext_err(const struct nlmsghdr *nlh, nl_ext_ack_fn_t errfn) 124 { 125 return 0; 126 } 127 #endif 128 129 void rtnl_close(struct rtnl_handle *rth) 130 { 131 if (rth->fd >= 0) { 132 close(rth->fd); 133 rth->fd = -1; 134 } 135 } 136 137 int rtnl_open_byproto(struct rtnl_handle *rth, unsigned int subscriptions, 138 int protocol) 139 { 140 socklen_t addr_len; 141 int sndbuf = 32768; 142 int one = 1; 143 144 memset(rth, 0, sizeof(*rth)); 145 146 rth->proto = protocol; 147 rth->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, protocol); 148 if (rth->fd < 0) { 149 perror("Cannot open netlink socket"); 150 return -1; 151 } 152 153 if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF, 154 &sndbuf, sizeof(sndbuf)) < 0) { 155 perror("SO_SNDBUF"); 156 return -1; 157 } 158 159 if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, 160 &rcvbuf, sizeof(rcvbuf)) < 0) { 161 perror("SO_RCVBUF"); 162 return -1; 163 } 164 165 /* Older kernels may no support extended ACK reporting */ 166 setsockopt(rth->fd, SOL_NETLINK, NETLINK_EXT_ACK, 167 &one, sizeof(one)); 168 169 memset(&rth->local, 0, sizeof(rth->local)); 170 rth->local.nl_family = AF_NETLINK; 171 rth->local.nl_groups = subscriptions; 172 173 if (bind(rth->fd, (struct sockaddr *)&rth->local, 174 sizeof(rth->local)) < 0) { 175 perror("Cannot bind netlink socket"); 176 return -1; 177 } 178 addr_len = sizeof(rth->local); 179 if (getsockname(rth->fd, (struct sockaddr *)&rth->local, 180 &addr_len) < 0) { 181 perror("Cannot getsockname"); 182 return -1; 183 } 184 if (addr_len != sizeof(rth->local)) { 185 fprintf(stderr, "Wrong address length %d\n", addr_len); 186 return -1; 187 } 188 if (rth->local.nl_family != AF_NETLINK) { 189 fprintf(stderr, "Wrong address family %d\n", 190 rth->local.nl_family); 191 return -1; 192 } 193 rth->seq = time(NULL); 194 return 0; 195 } 196 197 int rtnl_open(struct rtnl_handle *rth, unsigned int subscriptions) 198 { 199 return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE); 200 } 201 202 int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type) 203 { 204 return rtnl_wilddump_req_filter(rth, family, type, RTEXT_FILTER_VF); 205 } 206 207 int rtnl_wilddump_req_filter(struct rtnl_handle *rth, int family, int type, 208 __u32 filt_mask) 209 { 210 struct { 211 struct nlmsghdr nlh; 212 struct ifinfomsg ifm; 213 /* attribute has to be NLMSG aligned */ 214 struct rtattr ext_req __attribute__ ((aligned(NLMSG_ALIGNTO))); 215 __u32 ext_filter_mask; 216 } req = { 217 .nlh.nlmsg_len = sizeof(req), 218 .nlh.nlmsg_type = type, 219 .nlh.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST, 220 .nlh.nlmsg_seq = rth->dump = ++rth->seq, 221 .ifm.ifi_family = family, 222 .ext_req.rta_type = IFLA_EXT_MASK, 223 .ext_req.rta_len = RTA_LENGTH(sizeof(__u32)), 224 .ext_filter_mask = filt_mask, 225 }; 226 227 return send(rth->fd, &req, sizeof(req), 0); 228 } 229 230 int rtnl_wilddump_req_filter_fn(struct rtnl_handle *rth, int family, int type, 231 req_filter_fn_t filter_fn) 232 { 233 struct { 234 struct nlmsghdr nlh; 235 struct ifinfomsg ifm; 236 char buf[1024]; 237 } req = { 238 .nlh.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg)), 239 .nlh.nlmsg_type = type, 240 .nlh.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST, 241 .nlh.nlmsg_seq = rth->dump = ++rth->seq, 242 .ifm.ifi_family = family, 243 }; 244 int err; 245 246 if (!filter_fn) 247 return -EINVAL; 248 249 err = filter_fn(&req.nlh, sizeof(req)); 250 if (err) 251 return err; 252 253 return send(rth->fd, &req, req.nlh.nlmsg_len, 0); 254 } 255 256 int rtnl_wilddump_stats_req_filter(struct rtnl_handle *rth, int fam, int type, 257 __u32 filt_mask) 258 { 259 struct { 260 struct nlmsghdr nlh; 261 struct if_stats_msg ifsm; 262 } req; 263 264 memset(&req, 0, sizeof(req)); 265 req.nlh.nlmsg_len = NLMSG_LENGTH(sizeof(struct if_stats_msg)); 266 req.nlh.nlmsg_type = type; 267 req.nlh.nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; 268 req.nlh.nlmsg_pid = 0; 269 req.nlh.nlmsg_seq = rth->dump = ++rth->seq; 270 req.ifsm.family = fam; 271 req.ifsm.filter_mask = filt_mask; 272 273 return send(rth->fd, &req, sizeof(req), 0); 274 } 275 276 int rtnl_send(struct rtnl_handle *rth, const void *buf, int len) 277 { 278 return send(rth->fd, buf, len, 0); 279 } 280 281 int rtnl_send_check(struct rtnl_handle *rth, const void *buf, int len) 282 { 283 struct nlmsghdr *h; 284 int status; 285 char resp[1024]; 286 287 status = send(rth->fd, buf, len, 0); 288 if (status < 0) 289 return status; 290 291 /* Check for immediate errors */ 292 status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT|MSG_PEEK); 293 if (status < 0) { 294 if (errno == EAGAIN) 295 return 0; 296 return -1; 297 } 298 299 for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, status); 300 h = NLMSG_NEXT(h, status)) { 301 if (h->nlmsg_type == NLMSG_ERROR) { 302 struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); 303 304 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) 305 fprintf(stderr, "ERROR truncated\n"); 306 else 307 errno = -err->error; 308 return -1; 309 } 310 } 311 312 return 0; 313 } 314 315 int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len) 316 { 317 struct nlmsghdr nlh = { 318 .nlmsg_len = NLMSG_LENGTH(len), 319 .nlmsg_type = type, 320 .nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST, 321 .nlmsg_seq = rth->dump = ++rth->seq, 322 }; 323 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 324 struct iovec iov[2] = { 325 { .iov_base = &nlh, .iov_len = sizeof(nlh) }, 326 { .iov_base = req, .iov_len = len } 327 }; 328 struct msghdr msg = { 329 .msg_name = &nladdr, 330 .msg_namelen = sizeof(nladdr), 331 .msg_iov = iov, 332 .msg_iovlen = 2, 333 }; 334 335 return sendmsg(rth->fd, &msg, 0); 336 } 337 338 int rtnl_dump_request_n(struct rtnl_handle *rth, struct nlmsghdr *n) 339 { 340 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 341 struct iovec iov = { 342 .iov_base = n, 343 .iov_len = n->nlmsg_len 344 }; 345 struct msghdr msg = { 346 .msg_name = &nladdr, 347 .msg_namelen = sizeof(nladdr), 348 .msg_iov = &iov, 349 .msg_iovlen = 1, 350 }; 351 352 n->nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; 353 n->nlmsg_pid = 0; 354 n->nlmsg_seq = rth->dump = ++rth->seq; 355 356 return sendmsg(rth->fd, &msg, 0); 357 } 358 359 static int rtnl_dump_done(struct nlmsghdr *h) 360 { 361 int len = *(int *)NLMSG_DATA(h); 362 363 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(int))) { 364 fprintf(stderr, "DONE truncated\n"); 365 return -1; 366 } 367 368 if (len < 0) { 369 errno = -len; 370 switch (errno) { 371 case ENOENT: 372 case EOPNOTSUPP: 373 return -1; 374 case EMSGSIZE: 375 fprintf(stderr, 376 "Error: Buffer too small for object.\n"); 377 break; 378 default: 379 perror("RTNETLINK answers"); 380 } 381 return len; 382 } 383 384 return 0; 385 } 386 387 static void rtnl_dump_error(const struct rtnl_handle *rth, 388 struct nlmsghdr *h) 389 { 390 391 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) { 392 fprintf(stderr, "ERROR truncated\n"); 393 } else { 394 const struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); 395 396 errno = -err->error; 397 if (rth->proto == NETLINK_SOCK_DIAG && 398 (errno == ENOENT || 399 errno == EOPNOTSUPP)) 400 return; 401 402 if (!(rth->flags & RTNL_HANDLE_F_SUPPRESS_NLERR)) 403 perror("RTNETLINK answers"); 404 } 405 } 406 407 int rtnl_dump_filter_l(struct rtnl_handle *rth, 408 const struct rtnl_dump_filter_arg *arg) 409 { 410 struct sockaddr_nl nladdr; 411 struct iovec iov; 412 struct msghdr msg = { 413 .msg_name = &nladdr, 414 .msg_namelen = sizeof(nladdr), 415 .msg_iov = &iov, 416 .msg_iovlen = 1, 417 }; 418 char buf[32768]; 419 int dump_intr = 0; 420 421 iov.iov_base = buf; 422 while (1) { 423 int status; 424 const struct rtnl_dump_filter_arg *a; 425 int found_done = 0; 426 int msglen = 0; 427 428 iov.iov_len = sizeof(buf); 429 status = recvmsg(rth->fd, &msg, 0); 430 431 if (status < 0) { 432 if (errno == EINTR || errno == EAGAIN) 433 continue; 434 fprintf(stderr, "netlink receive error %s (%d)\n", 435 strerror(errno), errno); 436 return -1; 437 } 438 439 if (status == 0) { 440 fprintf(stderr, "EOF on netlink\n"); 441 return -1; 442 } 443 444 if (rth->dump_fp) 445 fwrite(buf, 1, NLMSG_ALIGN(status), rth->dump_fp); 446 447 for (a = arg; a->filter; a++) { 448 struct nlmsghdr *h = (struct nlmsghdr *)buf; 449 450 msglen = status; 451 452 while (NLMSG_OK(h, msglen)) { 453 int err = 0; 454 455 h->nlmsg_flags &= ~a->nc_flags; 456 457 if (nladdr.nl_pid != 0 || 458 h->nlmsg_pid != rth->local.nl_pid || 459 h->nlmsg_seq != rth->dump) 460 goto skip_it; 461 462 if (h->nlmsg_flags & NLM_F_DUMP_INTR) 463 dump_intr = 1; 464 465 if (h->nlmsg_type == NLMSG_DONE) { 466 err = rtnl_dump_done(h); 467 if (err < 0) 468 return -1; 469 470 found_done = 1; 471 break; /* process next filter */ 472 } 473 474 if (h->nlmsg_type == NLMSG_ERROR) { 475 rtnl_dump_error(rth, h); 476 return -1; 477 } 478 479 if (!rth->dump_fp) { 480 err = a->filter(&nladdr, h, a->arg1); 481 if (err < 0) 482 return err; 483 } 484 485 skip_it: 486 h = NLMSG_NEXT(h, msglen); 487 } 488 } 489 490 if (found_done) { 491 if (dump_intr) 492 fprintf(stderr, 493 "Dump was interrupted and may be inconsistent.\n"); 494 return 0; 495 } 496 497 if (msg.msg_flags & MSG_TRUNC) { 498 fprintf(stderr, "Message truncated\n"); 499 continue; 500 } 501 if (msglen) { 502 fprintf(stderr, "!!!Remnant of size %d\n", msglen); 503 exit(1); 504 } 505 } 506 } 507 508 int rtnl_dump_filter_nc(struct rtnl_handle *rth, 509 rtnl_filter_t filter, 510 void *arg1, __u16 nc_flags) 511 { 512 const struct rtnl_dump_filter_arg a[2] = { 513 { .filter = filter, .arg1 = arg1, .nc_flags = nc_flags, }, 514 { .filter = NULL, .arg1 = NULL, .nc_flags = 0, }, 515 }; 516 517 return rtnl_dump_filter_l(rth, a); 518 } 519 520 static void rtnl_talk_error(struct nlmsghdr *h, struct nlmsgerr *err, 521 nl_ext_ack_fn_t errfn) 522 { 523 if (nl_dump_ext_err(h, errfn)) 524 return; 525 526 fprintf(stderr, "RTNETLINK answers: %s\n", 527 strerror(-err->error)); 528 } 529 530 static int __rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, 531 struct nlmsghdr *answer, size_t maxlen, 532 bool show_rtnl_err, nl_ext_ack_fn_t errfn) 533 { 534 int status; 535 unsigned int seq; 536 struct nlmsghdr *h; 537 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 538 struct iovec iov = { 539 .iov_base = n, 540 .iov_len = n->nlmsg_len 541 }; 542 struct msghdr msg = { 543 .msg_name = &nladdr, 544 .msg_namelen = sizeof(nladdr), 545 .msg_iov = &iov, 546 .msg_iovlen = 1, 547 }; 548 char buf[32768] = {}; 549 550 n->nlmsg_seq = seq = ++rtnl->seq; 551 552 if (answer == NULL) 553 n->nlmsg_flags |= NLM_F_ACK; 554 555 status = sendmsg(rtnl->fd, &msg, 0); 556 if (status < 0) { 557 perror("Cannot talk to rtnetlink"); 558 return -1; 559 } 560 561 iov.iov_base = buf; 562 while (1) { 563 iov.iov_len = sizeof(buf); 564 status = recvmsg(rtnl->fd, &msg, 0); 565 566 if (status < 0) { 567 if (errno == EINTR || errno == EAGAIN) 568 continue; 569 fprintf(stderr, "netlink receive error %s (%d)\n", 570 strerror(errno), errno); 571 return -1; 572 } 573 if (status == 0) { 574 fprintf(stderr, "EOF on netlink\n"); 575 return -1; 576 } 577 if (msg.msg_namelen != sizeof(nladdr)) { 578 fprintf(stderr, 579 "sender address length == %d\n", 580 msg.msg_namelen); 581 exit(1); 582 } 583 for (h = (struct nlmsghdr *)buf; status >= sizeof(*h); ) { 584 int len = h->nlmsg_len; 585 int l = len - sizeof(*h); 586 587 if (l < 0 || len > status) { 588 if (msg.msg_flags & MSG_TRUNC) { 589 fprintf(stderr, "Truncated message\n"); 590 return -1; 591 } 592 fprintf(stderr, 593 "!!!malformed message: len=%d\n", 594 len); 595 exit(1); 596 } 597 598 if (nladdr.nl_pid != 0 || 599 h->nlmsg_pid != rtnl->local.nl_pid || 600 h->nlmsg_seq != seq) { 601 /* Don't forget to skip that message. */ 602 status -= NLMSG_ALIGN(len); 603 h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); 604 continue; 605 } 606 607 if (h->nlmsg_type == NLMSG_ERROR) { 608 struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); 609 610 if (l < sizeof(struct nlmsgerr)) { 611 fprintf(stderr, "ERROR truncated\n"); 612 } else if (!err->error) { 613 if (answer) 614 memcpy(answer, h, 615 MIN(maxlen, h->nlmsg_len)); 616 return 0; 617 } 618 619 if (rtnl->proto != NETLINK_SOCK_DIAG && 620 show_rtnl_err) 621 rtnl_talk_error(h, err, errfn); 622 623 errno = -err->error; 624 return -1; 625 } 626 627 if (answer) { 628 memcpy(answer, h, 629 MIN(maxlen, h->nlmsg_len)); 630 return 0; 631 } 632 633 fprintf(stderr, "Unexpected reply!!!\n"); 634 635 status -= NLMSG_ALIGN(len); 636 h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); 637 } 638 639 if (msg.msg_flags & MSG_TRUNC) { 640 fprintf(stderr, "Message truncated\n"); 641 continue; 642 } 643 644 if (status) { 645 fprintf(stderr, "!!!Remnant of size %d\n", status); 646 exit(1); 647 } 648 } 649 } 650 651 int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, 652 struct nlmsghdr *answer, size_t maxlen) 653 { 654 return __rtnl_talk(rtnl, n, answer, maxlen, true, NULL); 655 } 656 657 int rtnl_talk_extack(struct rtnl_handle *rtnl, struct nlmsghdr *n, 658 struct nlmsghdr *answer, size_t maxlen, 659 nl_ext_ack_fn_t errfn) 660 { 661 return __rtnl_talk(rtnl, n, answer, maxlen, true, errfn); 662 } 663 664 int rtnl_talk_suppress_rtnl_errmsg(struct rtnl_handle *rtnl, struct nlmsghdr *n, 665 struct nlmsghdr *answer, size_t maxlen) 666 { 667 return __rtnl_talk(rtnl, n, answer, maxlen, false, NULL); 668 } 669 670 int rtnl_listen_all_nsid(struct rtnl_handle *rth) 671 { 672 unsigned int on = 1; 673 674 if (setsockopt(rth->fd, SOL_NETLINK, NETLINK_LISTEN_ALL_NSID, &on, 675 sizeof(on)) < 0) { 676 perror("NETLINK_LISTEN_ALL_NSID"); 677 return -1; 678 } 679 rth->flags |= RTNL_HANDLE_F_LISTEN_ALL_NSID; 680 return 0; 681 } 682 683 int rtnl_listen(struct rtnl_handle *rtnl, 684 rtnl_listen_filter_t handler, 685 void *jarg) 686 { 687 int status; 688 struct nlmsghdr *h; 689 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 690 struct iovec iov; 691 struct msghdr msg = { 692 .msg_name = &nladdr, 693 .msg_namelen = sizeof(nladdr), 694 .msg_iov = &iov, 695 .msg_iovlen = 1, 696 }; 697 char buf[16384]; 698 char cmsgbuf[BUFSIZ]; 699 700 if (rtnl->flags & RTNL_HANDLE_F_LISTEN_ALL_NSID) { 701 msg.msg_control = &cmsgbuf; 702 msg.msg_controllen = sizeof(cmsgbuf); 703 } 704 705 iov.iov_base = buf; 706 while (1) { 707 struct rtnl_ctrl_data ctrl; 708 struct cmsghdr *cmsg; 709 710 iov.iov_len = sizeof(buf); 711 status = recvmsg(rtnl->fd, &msg, 0); 712 713 if (status < 0) { 714 if (errno == EINTR || errno == EAGAIN) 715 continue; 716 fprintf(stderr, "netlink receive error %s (%d)\n", 717 strerror(errno), errno); 718 if (errno == ENOBUFS) 719 continue; 720 return -1; 721 } 722 if (status == 0) { 723 fprintf(stderr, "EOF on netlink\n"); 724 return -1; 725 } 726 if (msg.msg_namelen != sizeof(nladdr)) { 727 fprintf(stderr, 728 "Sender address length == %d\n", 729 msg.msg_namelen); 730 exit(1); 731 } 732 733 if (rtnl->flags & RTNL_HANDLE_F_LISTEN_ALL_NSID) { 734 memset(&ctrl, 0, sizeof(ctrl)); 735 ctrl.nsid = -1; 736 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; 737 cmsg = CMSG_NXTHDR(&msg, cmsg)) 738 if (cmsg->cmsg_level == SOL_NETLINK && 739 cmsg->cmsg_type == NETLINK_LISTEN_ALL_NSID && 740 cmsg->cmsg_len == CMSG_LEN(sizeof(int))) { 741 int *data = (int *)CMSG_DATA(cmsg); 742 743 ctrl.nsid = *data; 744 } 745 } 746 747 for (h = (struct nlmsghdr *)buf; status >= sizeof(*h); ) { 748 int err; 749 int len = h->nlmsg_len; 750 int l = len - sizeof(*h); 751 752 if (l < 0 || len > status) { 753 if (msg.msg_flags & MSG_TRUNC) { 754 fprintf(stderr, "Truncated message\n"); 755 return -1; 756 } 757 fprintf(stderr, 758 "!!!malformed message: len=%d\n", 759 len); 760 exit(1); 761 } 762 763 err = handler(&nladdr, &ctrl, h, jarg); 764 if (err < 0) 765 return err; 766 767 status -= NLMSG_ALIGN(len); 768 h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); 769 } 770 if (msg.msg_flags & MSG_TRUNC) { 771 fprintf(stderr, "Message truncated\n"); 772 continue; 773 } 774 if (status) { 775 fprintf(stderr, "!!!Remnant of size %d\n", status); 776 exit(1); 777 } 778 } 779 } 780 781 int rtnl_from_file(FILE *rtnl, rtnl_listen_filter_t handler, 782 void *jarg) 783 { 784 int status; 785 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 786 char buf[16384]; 787 struct nlmsghdr *h = (struct nlmsghdr *)buf; 788 789 while (1) { 790 int err, len; 791 int l; 792 793 status = fread(&buf, 1, sizeof(*h), rtnl); 794 795 if (status < 0) { 796 if (errno == EINTR) 797 continue; 798 perror("rtnl_from_file: fread"); 799 return -1; 800 } 801 if (status == 0) 802 return 0; 803 804 len = h->nlmsg_len; 805 l = len - sizeof(*h); 806 807 if (l < 0 || len > sizeof(buf)) { 808 fprintf(stderr, "!!!malformed message: len=%d @%lu\n", 809 len, ftell(rtnl)); 810 return -1; 811 } 812 813 status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl); 814 815 if (status < 0) { 816 perror("rtnl_from_file: fread"); 817 return -1; 818 } 819 if (status < l) { 820 fprintf(stderr, "rtnl-from_file: truncated message\n"); 821 return -1; 822 } 823 824 err = handler(&nladdr, NULL, h, jarg); 825 if (err < 0) 826 return err; 827 } 828 } 829 830 int addattr(struct nlmsghdr *n, int maxlen, int type) 831 { 832 return addattr_l(n, maxlen, type, NULL, 0); 833 } 834 835 int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data) 836 { 837 return addattr_l(n, maxlen, type, &data, sizeof(__u8)); 838 } 839 840 int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data) 841 { 842 return addattr_l(n, maxlen, type, &data, sizeof(__u16)); 843 } 844 845 int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data) 846 { 847 return addattr_l(n, maxlen, type, &data, sizeof(__u32)); 848 } 849 850 int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data) 851 { 852 return addattr_l(n, maxlen, type, &data, sizeof(__u64)); 853 } 854 855 int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str) 856 { 857 return addattr_l(n, maxlen, type, str, strlen(str)+1); 858 } 859 860 int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, 861 int alen) 862 { 863 int len = RTA_LENGTH(alen); 864 struct rtattr *rta; 865 866 if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { 867 fprintf(stderr, 868 "addattr_l ERROR: message exceeded bound of %d\n", 869 maxlen); 870 return -1; 871 } 872 rta = NLMSG_TAIL(n); 873 rta->rta_type = type; 874 rta->rta_len = len; 875 if (alen) 876 memcpy(RTA_DATA(rta), data, alen); 877 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); 878 return 0; 879 } 880 881 int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len) 882 { 883 if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) { 884 fprintf(stderr, 885 "addraw_l ERROR: message exceeded bound of %d\n", 886 maxlen); 887 return -1; 888 } 889 890 memcpy(NLMSG_TAIL(n), data, len); 891 memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len); 892 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len); 893 return 0; 894 } 895 896 struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type) 897 { 898 struct rtattr *nest = NLMSG_TAIL(n); 899 900 addattr_l(n, maxlen, type, NULL, 0); 901 return nest; 902 } 903 904 int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest) 905 { 906 nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest; 907 return n->nlmsg_len; 908 } 909 910 struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type, 911 const void *data, int len) 912 { 913 struct rtattr *start = NLMSG_TAIL(n); 914 915 addattr_l(n, maxlen, type, data, len); 916 addattr_nest(n, maxlen, type); 917 return start; 918 } 919 920 int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start) 921 { 922 struct rtattr *nest = (void *)start + NLMSG_ALIGN(start->rta_len); 923 924 start->rta_len = (void *)NLMSG_TAIL(n) - (void *)start; 925 addattr_nest_end(n, nest); 926 return n->nlmsg_len; 927 } 928 929 int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data) 930 { 931 int len = RTA_LENGTH(4); 932 struct rtattr *subrta; 933 934 if (RTA_ALIGN(rta->rta_len) + len > maxlen) { 935 fprintf(stderr, 936 "rta_addattr32: Error! max allowed bound %d exceeded\n", 937 maxlen); 938 return -1; 939 } 940 subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len)); 941 subrta->rta_type = type; 942 subrta->rta_len = len; 943 memcpy(RTA_DATA(subrta), &data, 4); 944 rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len; 945 return 0; 946 } 947 948 int rta_addattr_l(struct rtattr *rta, int maxlen, int type, 949 const void *data, int alen) 950 { 951 struct rtattr *subrta; 952 int len = RTA_LENGTH(alen); 953 954 if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) { 955 fprintf(stderr, 956 "rta_addattr_l: Error! max allowed bound %d exceeded\n", 957 maxlen); 958 return -1; 959 } 960 subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len)); 961 subrta->rta_type = type; 962 subrta->rta_len = len; 963 if (alen) 964 memcpy(RTA_DATA(subrta), data, alen); 965 rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len); 966 return 0; 967 } 968 969 int rta_addattr8(struct rtattr *rta, int maxlen, int type, __u8 data) 970 { 971 return rta_addattr_l(rta, maxlen, type, &data, sizeof(__u8)); 972 } 973 974 int rta_addattr16(struct rtattr *rta, int maxlen, int type, __u16 data) 975 { 976 return rta_addattr_l(rta, maxlen, type, &data, sizeof(__u16)); 977 } 978 979 int rta_addattr64(struct rtattr *rta, int maxlen, int type, __u64 data) 980 { 981 return rta_addattr_l(rta, maxlen, type, &data, sizeof(__u64)); 982 } 983 984 struct rtattr *rta_nest(struct rtattr *rta, int maxlen, int type) 985 { 986 struct rtattr *nest = RTA_TAIL(rta); 987 988 rta_addattr_l(rta, maxlen, type, NULL, 0); 989 990 return nest; 991 } 992 993 int rta_nest_end(struct rtattr *rta, struct rtattr *nest) 994 { 995 nest->rta_len = (void *)RTA_TAIL(rta) - (void *)nest; 996 997 return rta->rta_len; 998 } 999 1000 int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len) 1001 { 1002 return parse_rtattr_flags(tb, max, rta, len, 0); 1003 } 1004 1005 int parse_rtattr_flags(struct rtattr *tb[], int max, struct rtattr *rta, 1006 int len, unsigned short flags) 1007 { 1008 unsigned short type; 1009 1010 memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); 1011 while (RTA_OK(rta, len)) { 1012 type = rta->rta_type & ~flags; 1013 if ((type <= max) && (!tb[type])) 1014 tb[type] = rta; 1015 rta = RTA_NEXT(rta, len); 1016 } 1017 if (len) 1018 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", 1019 len, rta->rta_len); 1020 return 0; 1021 } 1022 1023 int parse_rtattr_byindex(struct rtattr *tb[], int max, 1024 struct rtattr *rta, int len) 1025 { 1026 int i = 0; 1027 1028 memset(tb, 0, sizeof(struct rtattr *) * max); 1029 while (RTA_OK(rta, len)) { 1030 if (rta->rta_type <= max && i < max) 1031 tb[i++] = rta; 1032 rta = RTA_NEXT(rta, len); 1033 } 1034 if (len) 1035 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", 1036 len, rta->rta_len); 1037 return i; 1038 } 1039 1040 struct rtattr *parse_rtattr_one(int type, struct rtattr *rta, int len) 1041 { 1042 while (RTA_OK(rta, len)) { 1043 if (rta->rta_type == type) 1044 return rta; 1045 rta = RTA_NEXT(rta, len); 1046 } 1047 1048 if (len) 1049 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", 1050 len, rta->rta_len); 1051 return NULL; 1052 } 1053 1054 int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, 1055 struct rtattr *rta, 1056 int len) 1057 { 1058 if (RTA_PAYLOAD(rta) < len) 1059 return -1; 1060 if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) { 1061 rta = RTA_DATA(rta) + RTA_ALIGN(len); 1062 return parse_rtattr_nested(tb, max, rta); 1063 } 1064 memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); 1065 return 0; 1066 } 1067
