lib/libnetlink.c

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