pimd/mtracebis_netlink.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 #ifdef HAVE_CONFIG_H
  14 #include "config.h"
  15 #endif
  16
  17 #ifdef __linux__
  18
  19 #include <stdio.h>
  20 #include <stdlib.h>
  21 #include <unistd.h>
  22 #include <syslog.h>
  23 #include <fcntl.h>
  24 #include <net/if_arp.h>
  25 #include <sys/socket.h>
  26 #include <netinet/in.h>
  27 #include <string.h>
  28 #include <errno.h>
  29 #include <time.h>
  30 #include <sys/uio.h>
  31 #include <assert.h>
  32
  33 #include "mtracebis_netlink.h"
  34
  35 int rcvbuf = 1024 * 1024;
  36
  37 void rtnl_close(struct rtnl_handle *rth)
  38 {
  39         if (rth->fd >= 0) {
  40                 close(rth->fd);
  41                 rth->fd = -1;
  42         }
  43 }
  44
  45 int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
  46                       int protocol)
  47 {
  48         socklen_t addr_len;
  49         int sndbuf = 32768;
  50
  51         memset(rth, 0, sizeof(*rth));
  52
  53         rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
  54         if (rth->fd < 0) {
  55                 perror("Cannot open netlink socket");
  56                 return -1;
  57         }
  58
  59         if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf))
  60             < 0) {
  61                 perror("SO_SNDBUF");
  62                 return -1;
  63         }
  64
  65         if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf))
  66             < 0) {
  67                 perror("SO_RCVBUF");
  68                 return -1;
  69         }
  70
  71         memset(&rth->local, 0, sizeof(rth->local));
  72         rth->local.nl_family = AF_NETLINK;
  73         rth->local.nl_groups = subscriptions;
  74
  75         if (bind(rth->fd, (struct sockaddr *)&rth->local, sizeof(rth->local))
  76             < 0) {
  77                 perror("Cannot bind netlink socket");
  78                 return -1;
  79         }
  80         addr_len = sizeof(rth->local);
  81         if (getsockname(rth->fd, (struct sockaddr *)&rth->local, &addr_len)
  82             < 0) {
  83                 perror("Cannot getsockname");
  84                 return -1;
  85         }
  86         if (addr_len != sizeof(rth->local)) {
  87                 fprintf(stderr, "Wrong address length %d\n", addr_len);
  88                 return -1;
  89         }
  90         if (rth->local.nl_family != AF_NETLINK) {
  91                 fprintf(stderr, "Wrong address family %d\n",
  92                         rth->local.nl_family);
  93                 return -1;
  94         }
  95         rth->seq = getpid();
  96         return 0;
  97 }
  98
  99 int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
 100 {
 101         return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
 102 }
 103
 104 int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
 105 {
 106         struct {
 107                 struct nlmsghdr nlh;
 108                 struct rtgenmsg g;
 109         } req;
 110
 111         memset(&req, 0, sizeof(req));
 112         req.nlh.nlmsg_len = sizeof(req);
 113         req.nlh.nlmsg_type = type;
 114         req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
 115         req.nlh.nlmsg_pid = 0;
 116         req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
 117         req.g.rtgen_family = family;
 118
 119         return send(rth->fd, (void *)&req, sizeof(req), 0);
 120 }
 121
 122 int rtnl_send(struct rtnl_handle *rth, const char *buf, int len)
 123 {
 124         return send(rth->fd, buf, len, 0);
 125 }
 126
 127 int rtnl_send_check(struct rtnl_handle *rth, const char *buf, int len)
 128 {
 129         struct nlmsghdr *h;
 130         int status;
 131         char resp[1024];
 132
 133         status = send(rth->fd, buf, len, 0);
 134         if (status < 0)
 135                 return status;
 136
 137         /* Check for immediate errors */
 138         status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT | MSG_PEEK);
 139         if (status < 0) {
 140                 if (errno == EAGAIN)
 141                         return 0;
 142                 return -1;
 143         }
 144
 145         for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, (uint32_t)status);
 146              h = NLMSG_NEXT(h, status)) {
 147                 if (h->nlmsg_type == NLMSG_ERROR) {
 148                         struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h);
 149                         if (h->nlmsg_len
 150                             < NLMSG_LENGTH(sizeof(struct nlmsgerr)))
 151                                 fprintf(stderr, "ERROR truncated\n");
 152                         else
 153                                 errno = -err->error;
 154                         return -1;
 155                 }
 156         }
 157
 158         return 0;
 159 }
 160
 161 int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
 162 {
 163         struct nlmsghdr nlh;
 164         struct sockaddr_nl nladdr;
 165         struct iovec iov[2] = {{.iov_base = &nlh, .iov_len = sizeof(nlh)},
 166                                {.iov_base = req, .iov_len = len}};
 167         struct msghdr msg = {
 168                 .msg_name = &nladdr,
 169                 .msg_namelen = sizeof(nladdr),
 170                 .msg_iov = iov,
 171                 .msg_iovlen = 2,
 172         };
 173
 174         memset(&nladdr, 0, sizeof(nladdr));
 175         nladdr.nl_family = AF_NETLINK;
 176
 177         nlh.nlmsg_len = NLMSG_LENGTH(len);
 178         nlh.nlmsg_type = type;
 179         nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
 180         nlh.nlmsg_pid = 0;
 181         nlh.nlmsg_seq = rth->dump = ++rth->seq;
 182
 183         return sendmsg(rth->fd, &msg, 0);
 184 }
 185
 186 int rtnl_dump_filter_l(struct rtnl_handle *rth,
 187                        const struct rtnl_dump_filter_arg *arg)
 188 {
 189         struct sockaddr_nl nladdr;
 190         char buf[16384];
 191         struct iovec iov = {
 192                 .iov_base = buf,
 193                 .iov_len = sizeof(buf),
 194         };
 195         struct msghdr msg = {
 196                 .msg_name = &nladdr,
 197                 .msg_namelen = sizeof(nladdr),
 198                 .msg_iov = &iov,
 199                 .msg_iovlen = 1,
 200         };
 201
 202         while (1) {
 203                 int status;
 204                 const struct rtnl_dump_filter_arg *a;
 205                 int found_done = 0;
 206                 int msglen = 0;
 207
 208                 iov.iov_len = sizeof(buf);
 209                 status = recvmsg(rth->fd, &msg, 0);
 210
 211                 if (status < 0) {
 212                         if (errno == EINTR || errno == EAGAIN)
 213                                 continue;
 214                         fprintf(stderr, "netlink receive error %s (%d)\n",
 215                                 strerror(errno), errno);
 216                         return -1;
 217                 }
 218
 219                 if (status == 0) {
 220                         fprintf(stderr, "EOF on netlink\n");
 221                         return -1;
 222                 }
 223
 224                 for (a = arg; a->filter; a++) {
 225                         struct nlmsghdr *h = (struct nlmsghdr *)iov.iov_base;
 226                         msglen = status;
 227
 228                         while (NLMSG_OK(h, (uint32_t)msglen)) {
 229                                 int err;
 230
 231                                 if (nladdr.nl_pid != 0
 232                                     || h->nlmsg_pid != rth->local.nl_pid
 233                                     || h->nlmsg_seq != rth->dump) {
 234                                         if (a->junk) {
 235                                                 err = a->junk(&nladdr, h,
 236                                                               a->arg2);
 237                                                 if (err < 0)
 238                                                         return err;
 239                                         }
 240                                         goto skip_it;
 241                                 }
 242
 243                                 if (h->nlmsg_type == NLMSG_DONE) {
 244                                         found_done = 1;
 245                                         break; /* process next filter */
 246                                 }
 247                                 if (h->nlmsg_type == NLMSG_ERROR) {
 248                                         struct nlmsgerr *merr =
 249                                                 (struct nlmsgerr *)NLMSG_DATA(
 250                                                         h);
 251                                         if (h->nlmsg_len
 252                                             < NLMSG_LENGTH(sizeof(
 253                                                       struct nlmsgerr))) {
 254                                                 fprintf(stderr,
 255                                                         "ERROR truncated\n");
 256                                         } else {
 257                                                 errno = -merr->error;
 258                                                 perror("RTNETLINK answers");
 259                                         }
 260                                         return -1;
 261                                 }
 262                                 err = a->filter(&nladdr, h, a->arg1);
 263                                 if (err < 0)
 264                                         return err;
 265
 266                         skip_it:
 267                                 h = NLMSG_NEXT(h, msglen);
 268                         }
 269                 }
 270
 271                 if (found_done)
 272                         return 0;
 273
 274                 if (msg.msg_flags & MSG_TRUNC) {
 275                         fprintf(stderr, "Message truncated\n");
 276                         continue;
 277                 }
 278                 if (msglen) {
 279                         fprintf(stderr, "!!!Remnant of size %d\n", msglen);
 280                         exit(1);
 281                 }
 282         }
 283 }
 284
 285 int rtnl_dump_filter(struct rtnl_handle *rth, rtnl_filter_t filter, void *arg1,
 286                      rtnl_filter_t junk, void *arg2)
 287 {
 288         const struct rtnl_dump_filter_arg a[2] = {
 289                 {.filter = filter, .arg1 = arg1, .junk = junk, .arg2 = arg2},
 290                 {.filter = NULL, .arg1 = NULL, .junk = NULL, .arg2 = NULL}};
 291
 292         return rtnl_dump_filter_l(rth, a);
 293 }
 294
 295 int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
 296               unsigned groups, struct nlmsghdr *answer, rtnl_filter_t junk,
 297               void *jarg)
 298 {
 299         int status;
 300         unsigned seq;
 301         struct nlmsghdr *h;
 302         struct sockaddr_nl nladdr;
 303         struct iovec iov = {.iov_base = (void *)n, .iov_len = n->nlmsg_len};
 304         struct msghdr msg = {
 305                 .msg_name = &nladdr,
 306                 .msg_namelen = sizeof(nladdr),
 307                 .msg_iov = &iov,
 308                 .msg_iovlen = 1,
 309         };
 310         char buf[16384];
 311
 312         memset(&nladdr, 0, sizeof(nladdr));
 313         nladdr.nl_family = AF_NETLINK;
 314         nladdr.nl_pid = peer;
 315         nladdr.nl_groups = groups;
 316
 317         n->nlmsg_seq = seq = ++rtnl->seq;
 318
 319         if (answer == NULL)
 320                 n->nlmsg_flags |= NLM_F_ACK;
 321
 322         status = sendmsg(rtnl->fd, &msg, 0);
 323
 324         if (status < 0) {
 325                 perror("Cannot talk to rtnetlink");
 326                 return -1;
 327         }
 328
 329         memset(buf, 0, sizeof(buf));
 330
 331         iov.iov_base = buf;
 332
 333         while (1) {
 334                 iov.iov_len = sizeof(buf);
 335                 status = recvmsg(rtnl->fd, &msg, 0);
 336
 337                 if (status < 0) {
 338                         if (errno == EINTR || errno == EAGAIN)
 339                                 continue;
 340                         fprintf(stderr, "netlink receive error %s (%d)\n",
 341                                 strerror(errno), errno);
 342                         return -1;
 343                 }
 344                 if (status == 0) {
 345                         fprintf(stderr, "EOF on netlink\n");
 346                         return -1;
 347                 }
 348                 if (msg.msg_namelen != sizeof(nladdr)) {
 349                         fprintf(stderr, "sender address length == %d\n",
 350                                 msg.msg_namelen);
 351                         exit(1);
 352                 }
 353                 for (h = (struct nlmsghdr *)iov.iov_base;
 354                      status >= (int)sizeof(*h);) {
 355                         int err;
 356                         int len = h->nlmsg_len;
 357                         int l = len - sizeof(*h);
 358
 359                         if (l < 0 || len > status) {
 360                                 if (msg.msg_flags & MSG_TRUNC) {
 361                                         fprintf(stderr, "Truncated message\n");
 362                                         return -1;
 363                                 }
 364                                 fprintf(stderr,
 365                                         "!!!malformed message: len=%d\n", len);
 366                                 exit(1);
 367                         }
 368
 369                         if ((int)nladdr.nl_pid != peer
 370                             || h->nlmsg_pid != rtnl->local.nl_pid
 371                             || h->nlmsg_seq != seq) {
 372                                 if (junk) {
 373                                         err = junk(&nladdr, h, jarg);
 374                                         if (err < 0)
 375                                                 return err;
 376                                 }
 377                                 /* Don't forget to skip that message. */
 378                                 status -= NLMSG_ALIGN(len);
 379                                 h = (struct nlmsghdr *)((char *)h
 380                                                         + NLMSG_ALIGN(len));
 381                                 continue;
 382                         }
 383
 384                         if (h->nlmsg_type == NLMSG_ERROR) {
 385                                 struct nlmsgerr *merr =
 386                                         (struct nlmsgerr *)NLMSG_DATA(h);
 387                                 if (l < (int)sizeof(struct nlmsgerr)) {
 388                                         fprintf(stderr, "ERROR truncated\n");
 389                                 } else {
 390                                         errno = -merr->error;
 391                                         if (errno == 0) {
 392                                                 if (answer)
 393                                                         memcpy(answer, h,
 394                                                                h->nlmsg_len);
 395                                                 return 0;
 396                                         }
 397                                         perror("RTNETLINK answers");
 398                                 }
 399                                 return -1;
 400                         }
 401                         if (answer) {
 402                                 memcpy(answer, h, h->nlmsg_len);
 403                                 return 0;
 404                         }
 405
 406                         fprintf(stderr, "Unexpected reply!!!\n");
 407
 408                         status -= NLMSG_ALIGN(len);
 409                         h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));
 410                 }
 411                 if (msg.msg_flags & MSG_TRUNC) {
 412                         fprintf(stderr, "Message truncated\n");
 413                         continue;
 414                 }
 415                 if (status) {
 416                         fprintf(stderr, "!!!Remnant of size %d\n", status);
 417                         exit(1);
 418                 }
 419         }
 420 }
 421
 422 int rtnl_listen(struct rtnl_handle *rtnl, rtnl_filter_t handler, void *jarg)
 423 {
 424         int status;
 425         struct nlmsghdr *h;
 426         struct sockaddr_nl nladdr;
 427         char buf[8192];
 428         struct iovec iov = {
 429                 .iov_base = buf,
 430                 .iov_len = sizeof(buf),
 431         };
 432         struct msghdr msg = {
 433                 .msg_name = &nladdr,
 434                 .msg_namelen = sizeof(nladdr),
 435                 .msg_iov = &iov,
 436                 .msg_iovlen = 1,
 437         };
 438
 439         memset(&nladdr, 0, sizeof(nladdr));
 440         nladdr.nl_family = AF_NETLINK;
 441         nladdr.nl_pid = 0;
 442         nladdr.nl_groups = 0;
 443
 444         while (1) {
 445                 iov.iov_len = sizeof(buf);
 446                 status = recvmsg(rtnl->fd, &msg, 0);
 447
 448                 if (status < 0) {
 449                         if (errno == EINTR || errno == EAGAIN)
 450                                 continue;
 451                         fprintf(stderr, "netlink receive error %s (%d)\n",
 452                                 strerror(errno), errno);
 453                         if (errno == ENOBUFS)
 454                                 continue;
 455                         return -1;
 456                 }
 457                 if (status == 0) {
 458                         fprintf(stderr, "EOF on netlink\n");
 459                         return -1;
 460                 }
 461                 if (msg.msg_namelen != sizeof(nladdr)) {
 462                         fprintf(stderr, "Sender address length == %d\n",
 463                                 msg.msg_namelen);
 464                         exit(1);
 465                 }
 466                 for (h = (struct nlmsghdr *)buf; status >= (int)sizeof(*h);) {
 467                         int err;
 468                         int len = h->nlmsg_len;
 469                         int l = len - sizeof(*h);
 470
 471                         if (l < 0 || len > status) {
 472                                 if (msg.msg_flags & MSG_TRUNC) {
 473                                         fprintf(stderr, "Truncated message\n");
 474                                         return -1;
 475                                 }
 476                                 fprintf(stderr,
 477                                         "!!!malformed message: len=%d\n", len);
 478                                 exit(1);
 479                         }
 480
 481                         err = handler(&nladdr, h, jarg);
 482                         if (err < 0)
 483                                 return err;
 484
 485                         status -= NLMSG_ALIGN(len);
 486                         h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));
 487                 }
 488                 if (msg.msg_flags & MSG_TRUNC) {
 489                         fprintf(stderr, "Message truncated\n");
 490                         continue;
 491                 }
 492                 if (status) {
 493                         fprintf(stderr, "!!!Remnant of size %d\n", status);
 494                         exit(1);
 495                 }
 496         }
 497 }
 498
 499 int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler, void *jarg)
 500 {
 501         struct sockaddr_nl nladdr;
 502         char buf[8192];
 503         struct nlmsghdr *h = (void *)buf;
 504
 505         memset(&nladdr, 0, sizeof(nladdr));
 506         nladdr.nl_family = AF_NETLINK;
 507         nladdr.nl_pid = 0;
 508         nladdr.nl_groups = 0;
 509
 510         while (1) {
 511                 int err;
 512                 size_t l, rl, arl;
 513
 514                 rl = sizeof(*h);
 515                 arl = fread(&buf, 1, rl, rtnl);
 516
 517                 if (arl != rl) {
 518                         if (arl == 0)
 519                                 return 0;
 520
 521                         if (ferror(rtnl))
 522                                 fprintf(stderr, "%s: header read failed\n",
 523                                         __func__);
 524                         else
 525                                 fprintf(stderr, "%s: truncated header\n",
 526                                         __func__);
 527                         return -1;
 528                 }
 529
 530                 l = h->nlmsg_len > rl ? h->nlmsg_len - rl : 0;
 531
 532                 if (l == 0 || (l + (size_t)NLMSG_HDRLEN) > sizeof(buf)) {
 533                         fprintf(stderr, "%s: malformed message: len=%zu @%lu\n",
 534                                 __func__, (size_t)h->nlmsg_len, ftell(rtnl));
 535                         return -1;
 536                 }
 537
 538                 rl = NLMSG_ALIGN(l);
 539                 arl = fread(NLMSG_DATA(h), 1, rl, rtnl);
 540
 541                 if (arl != rl) {
 542                         if (ferror(rtnl))
 543                                 fprintf(stderr, "%s: msg read failed\n",
 544                                         __func__);
 545                         else
 546                                 fprintf(stderr, "%s: truncated message\n",
 547                                         __func__);
 548                         return -1;
 549                 }
 550
 551                 err = handler(&nladdr, h, jarg);
 552                 if (err < 0)
 553                         return err;
 554         }
 555 }
 556
 557 int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
 558 {
 559         int len = RTA_LENGTH(4);
 560         struct rtattr *rta;
 561         if ((int)(NLMSG_ALIGN(n->nlmsg_len) + len) > maxlen) {
 562                 fprintf(stderr,
 563                         "addattr32: Error! max allowed bound %d exceeded\n",
 564                         maxlen);
 565                 return -1;
 566         }
 567         rta = NLMSG_TAIL(n);
 568         rta->rta_type = type;
 569         rta->rta_len = len;
 570         memcpy(RTA_DATA(rta), &data, 4);
 571         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
 572         return 0;
 573 }
 574
 575 int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
 576               int alen)
 577 {
 578         int len = RTA_LENGTH(alen);
 579         struct rtattr *rta;
 580
 581         if ((int)(NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len)) > maxlen) {
 582                 fprintf(stderr,
 583                         "addattr_l ERROR: message exceeded bound of %d\n",
 584                         maxlen);
 585                 return -1;
 586         }
 587         rta = NLMSG_TAIL(n);
 588         rta->rta_type = type;
 589         rta->rta_len = len;
 590
 591         if (data)
 592                 memcpy(RTA_DATA(rta), data, alen);
 593         else
 594                 assert(alen == 0);
 595
 596         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
 597         return 0;
 598 }
 599
 600 int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
 601 {
 602         if ((int)(NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len)) > maxlen) {
 603                 fprintf(stderr,
 604                         "addraw_l ERROR: message exceeded bound of %d\n",
 605                         maxlen);
 606                 return -1;
 607         }
 608
 609         memcpy(NLMSG_TAIL(n), data, len);
 610         memset((uint8_t *)NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
 611         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
 612         return 0;
 613 }
 614
 615 struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type)
 616 {
 617         struct rtattr *nest = NLMSG_TAIL(n);
 618
 619         addattr_l(n, maxlen, type, NULL, 0);
 620         return nest;
 621 }
 622
 623 int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest)
 624 {
 625         nest->rta_len = (uint8_t *)NLMSG_TAIL(n) - (uint8_t *)nest;
 626         return n->nlmsg_len;
 627 }
 628
 629 struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type,
 630                                    const void *data, int len)
 631 {
 632         struct rtattr *start = NLMSG_TAIL(n);
 633
 634         addattr_l(n, maxlen, type, data, len);
 635         addattr_nest(n, maxlen, type);
 636         return start;
 637 }
 638
 639 int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start)
 640 {
 641         struct rtattr *nest = start + NLMSG_ALIGN(start->rta_len);
 642
 643         start->rta_len = (uint8_t *)NLMSG_TAIL(n) - (uint8_t *)start;
 644         addattr_nest_end(n, nest);
 645         return n->nlmsg_len;
 646 }
 647
 648 int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
 649 {
 650         int len = RTA_LENGTH(4);
 651         struct rtattr *subrta;
 652
 653         if ((int)(RTA_ALIGN(rta->rta_len) + len) > maxlen) {
 654                 fprintf(stderr,
 655                         "rta_addattr32: Error! max allowed bound %d exceeded\n",
 656                         maxlen);
 657                 return -1;
 658         }
 659         subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len));
 660         subrta->rta_type = type;
 661         subrta->rta_len = len;
 662         memcpy(RTA_DATA(subrta), &data, 4);
 663         rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
 664         return 0;
 665 }
 666
 667 int rta_addattr_l(struct rtattr *rta, int maxlen, int type, const void *data,
 668                   int alen)
 669 {
 670         struct rtattr *subrta;
 671         int len = RTA_LENGTH(alen);
 672
 673         if ((int)(RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len)) > maxlen) {
 674                 fprintf(stderr,
 675                         "rta_addattr_l: Error! max allowed bound %d exceeded\n",
 676                         maxlen);
 677                 return -1;
 678         }
 679         subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len));
 680         subrta->rta_type = type;
 681         subrta->rta_len = len;
 682         memcpy(RTA_DATA(subrta), data, alen);
 683         rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
 684         return 0;
 685 }
 686
 687 int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
 688 {
 689         memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
 690         while (RTA_OK(rta, len)) {
 691                 if ((rta->rta_type <= max) && (!tb[rta->rta_type]))
 692                         tb[rta->rta_type] = rta;
 693                 rta = RTA_NEXT(rta, len);
 694         }
 695         if (len)
 696                 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
 697                         rta->rta_len);
 698         return 0;
 699 }
 700
 701 int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta,
 702                          int len)
 703 {
 704         int i = 0;
 705
 706         memset(tb, 0, sizeof(struct rtattr *) * max);
 707         while (RTA_OK(rta, len)) {
 708                 if (rta->rta_type <= max && i < max)
 709                         tb[i++] = rta;
 710                 rta = RTA_NEXT(rta, len);
 711         }
 712         if (len)
 713                 fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
 714                         rta->rta_len);
 715         return i;
 716 }
 717
 718 int __parse_rtattr_nested_compat(struct rtattr *tb[], int max,
 719                                  struct rtattr *rta, int len)
 720 {
 721         if ((int)RTA_PAYLOAD(rta) < len)
 722                 return -1;
 723         if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) {
 724                 rta = (struct rtattr *)(uint8_t *)RTA_DATA(rta)
 725                       + RTA_ALIGN(len);
 726                 return parse_rtattr_nested(tb, max, rta);
 727         }
 728         memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
 729         return 0;
 730 }
 731
 732 #endif /* __linux__ */