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