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