lib/libnetlink.c

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