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