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