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