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 <unistd.h> 16 #include <syslog.h> 17 #include <fcntl.h> 18 #include <net/if_arp.h> 19 #include <sys/socket.h> 20 #include <netinet/in.h> 21 #include <string.h> 22 #include <errno.h> 23 #include <time.h> 24 #include <sys/uio.h> 25 26 #include "libnetlink.h" 27 28 int rcvbuf = 1024 * 1024; 29 30 void rtnl_close(struct rtnl_handle *rth) 31 { 32 if (rth->fd >= 0) { 33 close(rth->fd); 34 rth->fd = -1; 35 } 36 } 37 38 int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions, 39 int protocol) 40 { 41 socklen_t addr_len; 42 int sndbuf = 32768; 43 44 memset(rth, 0, sizeof(*rth)); 45 46 rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol); 47 if (rth->fd < 0) { 48 perror("Cannot open netlink socket"); 49 return -1; 50 } 51 52 if (setsockopt(rth->fd,SOL_SOCKET,SO_SNDBUF,&sndbuf,sizeof(sndbuf)) < 0) { 53 perror("SO_SNDBUF"); 54 return -1; 55 } 56 57 if (setsockopt(rth->fd,SOL_SOCKET,SO_RCVBUF,&rcvbuf,sizeof(rcvbuf)) < 0) { 58 perror("SO_RCVBUF"); 59 return -1; 60 } 61 62 memset(&rth->local, 0, sizeof(rth->local)); 63 rth->local.nl_family = AF_NETLINK; 64 rth->local.nl_groups = subscriptions; 65 66 if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) { 67 perror("Cannot bind netlink socket"); 68 return -1; 69 } 70 addr_len = sizeof(rth->local); 71 if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) { 72 perror("Cannot getsockname"); 73 return -1; 74 } 75 if (addr_len != sizeof(rth->local)) { 76 fprintf(stderr, "Wrong address length %d\n", addr_len); 77 return -1; 78 } 79 if (rth->local.nl_family != AF_NETLINK) { 80 fprintf(stderr, "Wrong address family %d\n", rth->local.nl_family); 81 return -1; 82 } 83 rth->seq = time(NULL); 84 return 0; 85 } 86 87 int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions) 88 { 89 return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE); 90 } 91 92 int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type) 93 { 94 struct { 95 struct nlmsghdr nlh; 96 struct rtgenmsg g; 97 __u16 align_rta; /* attribute has to be 32bit aligned */ 98 struct rtattr ext_req; 99 __u32 ext_filter_mask; 100 } req; 101 102 memset(&req, 0, sizeof(req)); 103 req.nlh.nlmsg_len = sizeof(req); 104 req.nlh.nlmsg_type = type; 105 req.nlh.nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; 106 req.nlh.nlmsg_pid = 0; 107 req.nlh.nlmsg_seq = rth->dump = ++rth->seq; 108 req.g.rtgen_family = family; 109 110 req.ext_req.rta_type = IFLA_EXT_MASK; 111 req.ext_req.rta_len = RTA_LENGTH(sizeof(__u32)); 112 req.ext_filter_mask = RTEXT_FILTER_VF; 113 114 return send(rth->fd, (void*)&req, sizeof(req), 0); 115 } 116 117 int rtnl_send(struct rtnl_handle *rth, const void *buf, int len) 118 { 119 return send(rth->fd, buf, len, 0); 120 } 121 122 int rtnl_send_check(struct rtnl_handle *rth, const void *buf, int len) 123 { 124 struct nlmsghdr *h; 125 int status; 126 char resp[1024]; 127 128 status = send(rth->fd, buf, len, 0); 129 if (status < 0) 130 return status; 131 132 /* Check for immediate errors */ 133 status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT|MSG_PEEK); 134 if (status < 0) { 135 if (errno == EAGAIN) 136 return 0; 137 return -1; 138 } 139 140 for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, status); 141 h = NLMSG_NEXT(h, status)) { 142 if (h->nlmsg_type == NLMSG_ERROR) { 143 struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); 144 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) 145 fprintf(stderr, "ERROR truncated\n"); 146 else 147 errno = -err->error; 148 return -1; 149 } 150 } 151 152 return 0; 153 } 154 155 int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len) 156 { 157 struct nlmsghdr nlh; 158 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 159 struct iovec iov[2] = { 160 { .iov_base = &nlh, .iov_len = sizeof(nlh) }, 161 { .iov_base = req, .iov_len = len } 162 }; 163 struct msghdr msg = { 164 .msg_name = &nladdr, 165 .msg_namelen = sizeof(nladdr), 166 .msg_iov = iov, 167 .msg_iovlen = 2, 168 }; 169 170 nlh.nlmsg_len = NLMSG_LENGTH(len); 171 nlh.nlmsg_type = type; 172 nlh.nlmsg_flags = NLM_F_DUMP|NLM_F_REQUEST; 173 nlh.nlmsg_pid = 0; 174 nlh.nlmsg_seq = rth->dump = ++rth->seq; 175 176 return sendmsg(rth->fd, &msg, 0); 177 } 178 179 int rtnl_dump_filter_l(struct rtnl_handle *rth, 180 const struct rtnl_dump_filter_arg *arg) 181 { 182 struct sockaddr_nl nladdr; 183 struct iovec iov; 184 struct msghdr msg = { 185 .msg_name = &nladdr, 186 .msg_namelen = sizeof(nladdr), 187 .msg_iov = &iov, 188 .msg_iovlen = 1, 189 }; 190 char buf[16384]; 191 192 iov.iov_base = buf; 193 while (1) { 194 int status; 195 const struct rtnl_dump_filter_arg *a; 196 int found_done = 0; 197 int msglen = 0; 198 199 iov.iov_len = sizeof(buf); 200 status = recvmsg(rth->fd, &msg, 0); 201 202 if (status < 0) { 203 if (errno == EINTR || errno == EAGAIN) 204 continue; 205 fprintf(stderr, "netlink receive error %s (%d)\n", 206 strerror(errno), errno); 207 return -1; 208 } 209 210 if (status == 0) { 211 fprintf(stderr, "EOF on netlink\n"); 212 return -1; 213 } 214 215 for (a = arg; a->filter; a++) { 216 struct nlmsghdr *h = (struct nlmsghdr*)buf; 217 msglen = status; 218 219 while (NLMSG_OK(h, msglen)) { 220 int err; 221 222 if (nladdr.nl_pid != 0 || 223 h->nlmsg_pid != rth->local.nl_pid || 224 h->nlmsg_seq != rth->dump) 225 goto skip_it; 226 227 if (h->nlmsg_type == NLMSG_DONE) { 228 found_done = 1; 229 break; /* process next filter */ 230 } 231 if (h->nlmsg_type == NLMSG_ERROR) { 232 struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); 233 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) { 234 fprintf(stderr, 235 "ERROR truncated\n"); 236 } else { 237 errno = -err->error; 238 perror("RTNETLINK answers"); 239 } 240 return -1; 241 } 242 err = a->filter(&nladdr, h, a->arg1); 243 if (err < 0) 244 return err; 245 246 skip_it: 247 h = NLMSG_NEXT(h, msglen); 248 } 249 } 250 251 if (found_done) 252 return 0; 253 254 if (msg.msg_flags & MSG_TRUNC) { 255 fprintf(stderr, "Message truncated\n"); 256 continue; 257 } 258 if (msglen) { 259 fprintf(stderr, "!!!Remnant of size %d\n", msglen); 260 exit(1); 261 } 262 } 263 } 264 265 int rtnl_dump_filter(struct rtnl_handle *rth, 266 rtnl_filter_t filter, 267 void *arg1) 268 { 269 const struct rtnl_dump_filter_arg a[2] = { 270 { .filter = filter, .arg1 = arg1, }, 271 { .filter = NULL, .arg1 = NULL, }, 272 }; 273 274 return rtnl_dump_filter_l(rth, a); 275 } 276 277 int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer, 278 unsigned groups, struct nlmsghdr *answer) 279 { 280 int status; 281 unsigned seq; 282 struct nlmsghdr *h; 283 struct sockaddr_nl nladdr; 284 struct iovec iov = { 285 .iov_base = (void*) n, 286 .iov_len = n->nlmsg_len 287 }; 288 struct msghdr msg = { 289 .msg_name = &nladdr, 290 .msg_namelen = sizeof(nladdr), 291 .msg_iov = &iov, 292 .msg_iovlen = 1, 293 }; 294 char buf[16384]; 295 296 memset(&nladdr, 0, sizeof(nladdr)); 297 nladdr.nl_family = AF_NETLINK; 298 nladdr.nl_pid = peer; 299 nladdr.nl_groups = groups; 300 301 n->nlmsg_seq = seq = ++rtnl->seq; 302 303 if (answer == NULL) 304 n->nlmsg_flags |= NLM_F_ACK; 305 306 status = sendmsg(rtnl->fd, &msg, 0); 307 308 if (status < 0) { 309 perror("Cannot talk to rtnetlink"); 310 return -1; 311 } 312 313 memset(buf,0,sizeof(buf)); 314 315 iov.iov_base = buf; 316 317 while (1) { 318 iov.iov_len = sizeof(buf); 319 status = recvmsg(rtnl->fd, &msg, 0); 320 321 if (status < 0) { 322 if (errno == EINTR || errno == EAGAIN) 323 continue; 324 fprintf(stderr, "netlink receive error %s (%d)\n", 325 strerror(errno), errno); 326 return -1; 327 } 328 if (status == 0) { 329 fprintf(stderr, "EOF on netlink\n"); 330 return -1; 331 } 332 if (msg.msg_namelen != sizeof(nladdr)) { 333 fprintf(stderr, "sender address length == %d\n", msg.msg_namelen); 334 exit(1); 335 } 336 for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) { 337 int len = h->nlmsg_len; 338 int l = len - sizeof(*h); 339 340 if (l < 0 || len>status) { 341 if (msg.msg_flags & MSG_TRUNC) { 342 fprintf(stderr, "Truncated message\n"); 343 return -1; 344 } 345 fprintf(stderr, "!!!malformed message: len=%d\n", len); 346 exit(1); 347 } 348 349 if (nladdr.nl_pid != peer || 350 h->nlmsg_pid != rtnl->local.nl_pid || 351 h->nlmsg_seq != seq) { 352 /* Don't forget to skip that message. */ 353 status -= NLMSG_ALIGN(len); 354 h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); 355 continue; 356 } 357 358 if (h->nlmsg_type == NLMSG_ERROR) { 359 struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h); 360 if (l < sizeof(struct nlmsgerr)) { 361 fprintf(stderr, "ERROR truncated\n"); 362 } else { 363 errno = -err->error; 364 if (errno == 0) { 365 if (answer) 366 memcpy(answer, h, h->nlmsg_len); 367 return 0; 368 } 369 perror("RTNETLINK answers"); 370 } 371 return -1; 372 } 373 if (answer) { 374 memcpy(answer, h, h->nlmsg_len); 375 return 0; 376 } 377 378 fprintf(stderr, "Unexpected reply!!!\n"); 379 380 status -= NLMSG_ALIGN(len); 381 h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); 382 } 383 if (msg.msg_flags & MSG_TRUNC) { 384 fprintf(stderr, "Message truncated\n"); 385 continue; 386 } 387 if (status) { 388 fprintf(stderr, "!!!Remnant of size %d\n", status); 389 exit(1); 390 } 391 } 392 } 393 394 int rtnl_listen(struct rtnl_handle *rtnl, 395 rtnl_filter_t handler, 396 void *jarg) 397 { 398 int status; 399 struct nlmsghdr *h; 400 struct sockaddr_nl nladdr; 401 struct iovec iov; 402 struct msghdr msg = { 403 .msg_name = &nladdr, 404 .msg_namelen = sizeof(nladdr), 405 .msg_iov = &iov, 406 .msg_iovlen = 1, 407 }; 408 char buf[8192]; 409 410 memset(&nladdr, 0, sizeof(nladdr)); 411 nladdr.nl_family = AF_NETLINK; 412 nladdr.nl_pid = 0; 413 nladdr.nl_groups = 0; 414 415 iov.iov_base = buf; 416 while (1) { 417 iov.iov_len = sizeof(buf); 418 status = recvmsg(rtnl->fd, &msg, 0); 419 420 if (status < 0) { 421 if (errno == EINTR || errno == EAGAIN) 422 continue; 423 fprintf(stderr, "netlink receive error %s (%d)\n", 424 strerror(errno), errno); 425 if (errno == ENOBUFS) 426 continue; 427 return -1; 428 } 429 if (status == 0) { 430 fprintf(stderr, "EOF on netlink\n"); 431 return -1; 432 } 433 if (msg.msg_namelen != sizeof(nladdr)) { 434 fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen); 435 exit(1); 436 } 437 for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) { 438 int err; 439 int len = h->nlmsg_len; 440 int l = len - sizeof(*h); 441 442 if (l<0 || len>status) { 443 if (msg.msg_flags & MSG_TRUNC) { 444 fprintf(stderr, "Truncated message\n"); 445 return -1; 446 } 447 fprintf(stderr, "!!!malformed message: len=%d\n", len); 448 exit(1); 449 } 450 451 err = handler(&nladdr, h, jarg); 452 if (err < 0) 453 return err; 454 455 status -= NLMSG_ALIGN(len); 456 h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len)); 457 } 458 if (msg.msg_flags & MSG_TRUNC) { 459 fprintf(stderr, "Message truncated\n"); 460 continue; 461 } 462 if (status) { 463 fprintf(stderr, "!!!Remnant of size %d\n", status); 464 exit(1); 465 } 466 } 467 } 468 469 int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler, 470 void *jarg) 471 { 472 int status; 473 struct sockaddr_nl nladdr; 474 char buf[8192]; 475 struct nlmsghdr *h = (void*)buf; 476 477 memset(&nladdr, 0, sizeof(nladdr)); 478 nladdr.nl_family = AF_NETLINK; 479 nladdr.nl_pid = 0; 480 nladdr.nl_groups = 0; 481 482 while (1) { 483 int err, len; 484 int l; 485 486 status = fread(&buf, 1, sizeof(*h), rtnl); 487 488 if (status < 0) { 489 if (errno == EINTR) 490 continue; 491 perror("rtnl_from_file: fread"); 492 return -1; 493 } 494 if (status == 0) 495 return 0; 496 497 len = h->nlmsg_len; 498 l = len - sizeof(*h); 499 500 if (l<0 || len>sizeof(buf)) { 501 fprintf(stderr, "!!!malformed message: len=%d @%lu\n", 502 len, ftell(rtnl)); 503 return -1; 504 } 505 506 status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl); 507 508 if (status < 0) { 509 perror("rtnl_from_file: fread"); 510 return -1; 511 } 512 if (status < l) { 513 fprintf(stderr, "rtnl-from_file: truncated message\n"); 514 return -1; 515 } 516 517 err = handler(&nladdr, h, jarg); 518 if (err < 0) 519 return err; 520 } 521 } 522 523 int addattr(struct nlmsghdr *n, int maxlen, int type) 524 { 525 return addattr_l(n, maxlen, type, NULL, 0); 526 } 527 528 int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data) 529 { 530 return addattr_l(n, maxlen, type, &data, sizeof(__u8)); 531 } 532 533 int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data) 534 { 535 return addattr_l(n, maxlen, type, &data, sizeof(__u16)); 536 } 537 538 int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data) 539 { 540 return addattr_l(n, maxlen, type, &data, sizeof(__u32)); 541 } 542 543 int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data) 544 { 545 return addattr_l(n, maxlen, type, &data, sizeof(__u64)); 546 } 547 548 int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str) 549 { 550 return addattr_l(n, maxlen, type, str, strlen(str)+1); 551 } 552 553 int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, 554 int alen) 555 { 556 int len = RTA_LENGTH(alen); 557 struct rtattr *rta; 558 559 if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { 560 fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen); 561 return -1; 562 } 563 rta = NLMSG_TAIL(n); 564 rta->rta_type = type; 565 rta->rta_len = len; 566 memcpy(RTA_DATA(rta), data, alen); 567 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); 568 return 0; 569 } 570 571 int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len) 572 { 573 if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) { 574 fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen); 575 return -1; 576 } 577 578 memcpy(NLMSG_TAIL(n), data, len); 579 memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len); 580 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len); 581 return 0; 582 } 583 584 struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type) 585 { 586 struct rtattr *nest = NLMSG_TAIL(n); 587 588 addattr_l(n, maxlen, type, NULL, 0); 589 return nest; 590 } 591 592 int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest) 593 { 594 nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest; 595 return n->nlmsg_len; 596 } 597 598 struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type, 599 const void *data, int len) 600 { 601 struct rtattr *start = NLMSG_TAIL(n); 602 603 addattr_l(n, maxlen, type, data, len); 604 addattr_nest(n, maxlen, type); 605 return start; 606 } 607 608 int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start) 609 { 610 struct rtattr *nest = (void *)start + NLMSG_ALIGN(start->rta_len); 611 612 start->rta_len = (void *)NLMSG_TAIL(n) - (void *)start; 613 addattr_nest_end(n, nest); 614 return n->nlmsg_len; 615 } 616 617 int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data) 618 { 619 int len = RTA_LENGTH(4); 620 struct rtattr *subrta; 621 622 if (RTA_ALIGN(rta->rta_len) + len > maxlen) { 623 fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen); 624 return -1; 625 } 626 subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len)); 627 subrta->rta_type = type; 628 subrta->rta_len = len; 629 memcpy(RTA_DATA(subrta), &data, 4); 630 rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len; 631 return 0; 632 } 633 634 int rta_addattr_l(struct rtattr *rta, int maxlen, int type, 635 const void *data, int alen) 636 { 637 struct rtattr *subrta; 638 int len = RTA_LENGTH(alen); 639 640 if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) { 641 fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen); 642 return -1; 643 } 644 subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len)); 645 subrta->rta_type = type; 646 subrta->rta_len = len; 647 memcpy(RTA_DATA(subrta), data, alen); 648 rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len); 649 return 0; 650 } 651 652 int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len) 653 { 654 memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); 655 while (RTA_OK(rta, len)) { 656 if ((rta->rta_type <= max) && (!tb[rta->rta_type])) 657 tb[rta->rta_type] = rta; 658 rta = RTA_NEXT(rta,len); 659 } 660 if (len) 661 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len); 662 return 0; 663 } 664 665 int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len) 666 { 667 int i = 0; 668 669 memset(tb, 0, sizeof(struct rtattr *) * max); 670 while (RTA_OK(rta, len)) { 671 if (rta->rta_type <= max && i < max) 672 tb[i++] = rta; 673 rta = RTA_NEXT(rta,len); 674 } 675 if (len) 676 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len); 677 return i; 678 } 679 680 int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta, 681 int len) 682 { 683 if (RTA_PAYLOAD(rta) < len) 684 return -1; 685 if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) { 686 rta = RTA_DATA(rta) + RTA_ALIGN(len); 687 return parse_rtattr_nested(tb, max, rta); 688 } 689 memset(tb, 0, sizeof(struct rtattr *) * (max + 1)); 690 return 0; 691 } 692
