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