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