networking/libiproute/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 "libbb.h"
  14 #include <sys/socket.h>
  15
  16 #include <errno.h>
  17 #include <string.h>
  18 #include <time.h>
  19 #include <unistd.h>
  20
  21 #include <sys/uio.h>
  22
  23 #include "libnetlink.h"
  24
  25 void rtnl_close(struct rtnl_handle *rth)
  26 {
  27         close(rth->fd);
  28 }
  29
  30 int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
  31 {
  32         socklen_t addr_len;
  33
  34         memset(rth, 0, sizeof(rth));
  35
  36         rth->fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
  37         if (rth->fd < 0) {
  38                 bb_perror_msg("Cannot open netlink socket");
  39                 return -1;
  40         }
  41
  42         memset(&rth->local, 0, sizeof(rth->local));
  43         rth->local.nl_family = AF_NETLINK;
  44         rth->local.nl_groups = subscriptions;
  45
  46         if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) {
  47                 bb_perror_msg("Cannot bind netlink socket");
  48                 return -1;
  49         }
  50         addr_len = sizeof(rth->local);
  51         if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) {
  52                 bb_perror_msg("Cannot getsockname");
  53                 return -1;
  54         }
  55         if (addr_len != sizeof(rth->local)) {
  56                 bb_error_msg("Wrong address length %d", addr_len);
  57                 return -1;
  58         }
  59         if (rth->local.nl_family != AF_NETLINK) {
  60                 bb_error_msg("Wrong address family %d", rth->local.nl_family);
  61                 return -1;
  62         }
  63         rth->seq = time(NULL);
  64         return 0;
  65 }
  66
  67 int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
  68 {
  69         struct {
  70                 struct nlmsghdr nlh;
  71                 struct rtgenmsg g;
  72         } req;
  73         struct sockaddr_nl nladdr;
  74
  75         memset(&nladdr, 0, sizeof(nladdr));
  76         nladdr.nl_family = AF_NETLINK;
  77
  78         req.nlh.nlmsg_len = sizeof(req);
  79         req.nlh.nlmsg_type = type;
  80         req.nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
  81         req.nlh.nlmsg_pid = 0;
  82         req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
  83         req.g.rtgen_family = family;
  84
  85         return sendto(rth->fd, (void*)&req, sizeof(req), 0, (struct sockaddr*)&nladdr, sizeof(nladdr));
  86 }
  87
  88 int rtnl_send(struct rtnl_handle *rth, char *buf, int len)
  89 {
  90         struct sockaddr_nl nladdr;
  91
  92         memset(&nladdr, 0, sizeof(nladdr));
  93         nladdr.nl_family = AF_NETLINK;
  94
  95         return sendto(rth->fd, buf, len, 0, (struct sockaddr*)&nladdr, sizeof(nladdr));
  96 }
  97
  98 int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
  99 {
 100         struct nlmsghdr nlh;
 101         struct sockaddr_nl nladdr;
 102         struct iovec iov[2] = { { &nlh, sizeof(nlh) }, { req, len } };
 103         struct msghdr msg = {
 104                 (void*)&nladdr, sizeof(nladdr),
 105                 iov,    2,
 106                 NULL,   0,
 107                 0
 108         };
 109
 110         memset(&nladdr, 0, sizeof(nladdr));
 111         nladdr.nl_family = AF_NETLINK;
 112
 113         nlh.nlmsg_len = NLMSG_LENGTH(len);
 114         nlh.nlmsg_type = type;
 115         nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
 116         nlh.nlmsg_pid = 0;
 117         nlh.nlmsg_seq = rth->dump = ++rth->seq;
 118
 119         return sendmsg(rth->fd, &msg, 0);
 120 }
 121
 122 int rtnl_dump_filter(struct rtnl_handle *rth,
 123                      int (*filter)(struct sockaddr_nl *, struct nlmsghdr *n, void *),
 124                      void *arg1,
 125                      int (*junk)(struct sockaddr_nl *,struct nlmsghdr *n, void *),
 126                      void *arg2)
 127 {
 128         char    buf[8192];
 129         struct sockaddr_nl nladdr;
 130         struct iovec iov = { buf, sizeof(buf) };
 131
 132         while (1) {
 133                 int status;
 134                 struct nlmsghdr *h;
 135
 136                 struct msghdr msg = {
 137                         (void*)&nladdr, sizeof(nladdr),
 138                         &iov,   1,
 139                         NULL,   0,
 140                         0
 141                 };
 142
 143                 status = recvmsg(rth->fd, &msg, 0);
 144
 145                 if (status < 0) {
 146                         if (errno == EINTR)
 147                                 continue;
 148                         bb_perror_msg("OVERRUN");
 149                         continue;
 150                 }
 151                 if (status == 0) {
 152                         bb_error_msg("EOF on netlink");
 153                         return -1;
 154                 }
 155                 if (msg.msg_namelen != sizeof(nladdr)) {
 156                         bb_error_msg_and_die("sender address length == %d", msg.msg_namelen);
 157                 }
 158
 159                 h = (struct nlmsghdr*)buf;
 160                 while (NLMSG_OK(h, status)) {
 161                         int err;
 162
 163                         if (nladdr.nl_pid != 0 ||
 164                             h->nlmsg_pid != rth->local.nl_pid ||
 165                             h->nlmsg_seq != rth->dump) {
 166                                 if (junk) {
 167                                         err = junk(&nladdr, h, arg2);
 168                                         if (err < 0) {
 169                                                 return err;
 170                                         }
 171                                 }
 172                                 goto skip_it;
 173                         }
 174
 175                         if (h->nlmsg_type == NLMSG_DONE) {
 176                                 return 0;
 177                         }
 178                         if (h->nlmsg_type == NLMSG_ERROR) {
 179                                 struct nlmsgerr *l_err = (struct nlmsgerr*)NLMSG_DATA(h);
 180                                 if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
 181                                         bb_error_msg("ERROR truncated");
 182                                 } else {
 183                                         errno = -l_err->error;
 184                                         bb_perror_msg("RTNETLINK answers");
 185                                 }
 186                                 return -1;
 187                         }
 188                         err = filter(&nladdr, h, arg1);
 189                         if (err < 0) {
 190                                 return err;
 191                         }
 192
 193 skip_it:
 194                         h = NLMSG_NEXT(h, status);
 195                 }
 196                 if (msg.msg_flags & MSG_TRUNC) {
 197                         bb_error_msg("Message truncated");
 198                         continue;
 199                 }
 200                 if (status) {
 201                         bb_error_msg_and_die("!!!Remnant of size %d", status);
 202                 }
 203         }
 204 }
 205
 206 int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
 207               unsigned groups, struct nlmsghdr *answer,
 208               int (*junk)(struct sockaddr_nl *,struct nlmsghdr *n, void *),
 209               void *jarg)
 210 {
 211         int status;
 212         unsigned seq;
 213         struct nlmsghdr *h;
 214         struct sockaddr_nl nladdr;
 215         struct iovec iov = { (void*)n, n->nlmsg_len };
 216         char   buf[8192];
 217         struct msghdr msg = {
 218                 (void*)&nladdr, sizeof(nladdr),
 219                 &iov,   1,
 220                 NULL,   0,
 221                 0
 222         };
 223
 224         memset(&nladdr, 0, sizeof(nladdr));
 225         nladdr.nl_family = AF_NETLINK;
 226         nladdr.nl_pid = peer;
 227         nladdr.nl_groups = groups;
 228
 229         n->nlmsg_seq = seq = ++rtnl->seq;
 230         if (answer == NULL) {
 231                 n->nlmsg_flags |= NLM_F_ACK;
 232         }
 233         status = sendmsg(rtnl->fd, &msg, 0);
 234
 235         if (status < 0) {
 236                 bb_perror_msg("Cannot talk to rtnetlink");
 237                 return -1;
 238         }
 239
 240         iov.iov_base = buf;
 241
 242         while (1) {
 243                 iov.iov_len = sizeof(buf);
 244                 status = recvmsg(rtnl->fd, &msg, 0);
 245
 246                 if (status < 0) {
 247                         if (errno == EINTR) {
 248                                 continue;
 249                         }
 250                         bb_perror_msg("OVERRUN");
 251                         continue;
 252                 }
 253                 if (status == 0) {
 254                         bb_error_msg("EOF on netlink");
 255                         return -1;
 256                 }
 257                 if (msg.msg_namelen != sizeof(nladdr)) {
 258                         bb_error_msg_and_die("sender address length == %d", msg.msg_namelen);
 259                 }
 260                 for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
 261                         int l_err;
 262                         int len = h->nlmsg_len;
 263                         int l = len - sizeof(*h);
 264
 265                         if (l<0 || len>status) {
 266                                 if (msg.msg_flags & MSG_TRUNC) {
 267                                         bb_error_msg("Truncated message");
 268                                         return -1;
 269                                 }
 270                                 bb_error_msg_and_die("!!!malformed message: len=%d", len);
 271                         }
 272
 273                         if (nladdr.nl_pid != peer ||
 274                             h->nlmsg_pid != rtnl->local.nl_pid ||
 275                             h->nlmsg_seq != seq) {
 276                                 if (junk) {
 277                                         l_err = junk(&nladdr, h, jarg);
 278                                         if (l_err < 0) {
 279                                                 return l_err;
 280                                         }
 281                                 }
 282                                 continue;
 283                         }
 284
 285                         if (h->nlmsg_type == NLMSG_ERROR) {
 286                                 struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
 287                                 if (l < sizeof(struct nlmsgerr)) {
 288                                         bb_error_msg("ERROR truncated");
 289                                 } else {
 290                                         errno = -err->error;
 291                                         if (errno == 0) {
 292                                                 if (answer) {
 293                                                         memcpy(answer, h, h->nlmsg_len);
 294                                                 }
 295                                                 return 0;
 296                                         }
 297                                         bb_perror_msg("RTNETLINK answers");
 298                                 }
 299                                 return -1;
 300                         }
 301                         if (answer) {
 302                                 memcpy(answer, h, h->nlmsg_len);
 303                                 return 0;
 304                         }
 305
 306                         bb_error_msg("Unexpected reply!!!");
 307
 308                         status -= NLMSG_ALIGN(len);
 309                         h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
 310                 }
 311                 if (msg.msg_flags & MSG_TRUNC) {
 312                         bb_error_msg("Message truncated");
 313                         continue;
 314                 }
 315                 if (status) {
 316                         bb_error_msg_and_die("!!!Remnant of size %d", status);
 317                 }
 318         }
 319 }
 320
 321 int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
 322 {
 323         int len = RTA_LENGTH(4);
 324         struct rtattr *rta;
 325         if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen)
 326                 return -1;
 327         rta = (struct rtattr*)(((char*)n) + NLMSG_ALIGN(n->nlmsg_len));
 328         rta->rta_type = type;
 329         rta->rta_len = len;
 330         memcpy(RTA_DATA(rta), &data, 4);
 331         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
 332         return 0;
 333 }
 334
 335 int addattr_l(struct nlmsghdr *n, int maxlen, int type, void *data, int alen)
 336 {
 337         int len = RTA_LENGTH(alen);
 338         struct rtattr *rta;
 339
 340         if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen)
 341                 return -1;
 342         rta = (struct rtattr*)(((char*)n) + NLMSG_ALIGN(n->nlmsg_len));
 343         rta->rta_type = type;
 344         rta->rta_len = len;
 345         memcpy(RTA_DATA(rta), data, alen);
 346         n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
 347         return 0;
 348 }
 349
 350 int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
 351 {
 352         int len = RTA_LENGTH(4);
 353         struct rtattr *subrta;
 354
 355         if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
 356                 return -1;
 357         }
 358         subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
 359         subrta->rta_type = type;
 360         subrta->rta_len = len;
 361         memcpy(RTA_DATA(subrta), &data, 4);
 362         rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
 363         return 0;
 364 }
 365
 366 int rta_addattr_l(struct rtattr *rta, int maxlen, int type, void *data, int alen)
 367 {
 368         struct rtattr *subrta;
 369         int len = RTA_LENGTH(alen);
 370
 371         if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
 372                 return -1;
 373         }
 374         subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
 375         subrta->rta_type = type;
 376         subrta->rta_len = len;
 377         memcpy(RTA_DATA(subrta), data, alen);
 378         rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
 379         return 0;
 380 }
 381
 382
 383 int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
 384 {
 385         while (RTA_OK(rta, len)) {
 386                 if (rta->rta_type <= max) {
 387                         tb[rta->rta_type] = rta;
 388                 }
 389                 rta = RTA_NEXT(rta,len);
 390         }
 391         if (len) {
 392                 bb_error_msg("!!!Deficit %d, rta_len=%d", len, rta->rta_len);
 393         }
 394         return 0;
 395 }