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