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