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