tools/testing/selftests/net/mptcp/pm_nl_ctl.c

   1 // SPDX-License-Identifier: GPL-2.0
   2
   3 #include <errno.h>
   4 #include <error.h>
   5 #include <stdio.h>
   6 #include <stdlib.h>
   7 #include <string.h>
   8 #include <unistd.h>
   9 #include <limits.h>
  10
  11 #include <sys/socket.h>
  12 #include <sys/types.h>
  13
  14 #include <arpa/inet.h>
  15 #include <net/if.h>
  16
  17 #include <linux/rtnetlink.h>
  18 #include <linux/genetlink.h>
  19
  20 #include "linux/mptcp.h"
  21
  22 #ifndef MPTCP_PM_NAME
  23 #define MPTCP_PM_NAME           "mptcp_pm"
  24 #endif
  25 #ifndef MPTCP_PM_EVENTS
  26 #define MPTCP_PM_EVENTS         "mptcp_pm_events"
  27 #endif
  28 #ifndef IPPROTO_MPTCP
  29 #define IPPROTO_MPTCP 262
  30 #endif
  31
  32 static void syntax(char *argv[])
  33 {
  34         fprintf(stderr, "%s add|ann|rem|csf|dsf|get|set|del|flush|dump|events|listen|accept [<args>]\n", argv[0]);
  35         fprintf(stderr, "\tadd [flags signal|subflow|backup|fullmesh] [id <nr>] [dev <name>] <ip>\n");
  36         fprintf(stderr, "\tann <local-ip> id <local-id> token <token> [port <local-port>] [dev <name>]\n");
  37         fprintf(stderr, "\trem id <local-id> token <token>\n");
  38         fprintf(stderr, "\tcsf lip <local-ip> lid <local-id> rip <remote-ip> rport <remote-port> token <token>\n");
  39         fprintf(stderr, "\tdsf lip <local-ip> lport <local-port> rip <remote-ip> rport <remote-port> token <token>\n");
  40         fprintf(stderr, "\tdel <id> [<ip>]\n");
  41         fprintf(stderr, "\tget <id>\n");
  42         fprintf(stderr, "\tset [<ip>] [id <nr>] flags [no]backup|[no]fullmesh [port <nr>] [token <token>] [rip <ip>] [rport <port>]\n");
  43         fprintf(stderr, "\tflush\n");
  44         fprintf(stderr, "\tdump\n");
  45         fprintf(stderr, "\tlimits [<rcv addr max> <subflow max>]\n");
  46         fprintf(stderr, "\tevents\n");
  47         fprintf(stderr, "\tlisten <local-ip> <local-port>\n");
  48         exit(0);
  49 }
  50
  51 static int init_genl_req(char *data, int family, int cmd, int version)
  52 {
  53         struct nlmsghdr *nh = (void *)data;
  54         struct genlmsghdr *gh;
  55         int off = 0;
  56
  57         nh->nlmsg_type = family;
  58         nh->nlmsg_flags = NLM_F_REQUEST;
  59         nh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
  60         off += NLMSG_ALIGN(sizeof(*nh));
  61
  62         gh = (void *)(data + off);
  63         gh->cmd = cmd;
  64         gh->version = version;
  65         off += NLMSG_ALIGN(sizeof(*gh));
  66         return off;
  67 }
  68
  69 static void nl_error(struct nlmsghdr *nh)
  70 {
  71         struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh);
  72         int len = nh->nlmsg_len - sizeof(*nh);
  73         uint32_t off;
  74
  75         if (len < sizeof(struct nlmsgerr))
  76                 error(1, 0, "netlink error message truncated %d min %ld", len,
  77                       sizeof(struct nlmsgerr));
  78
  79         if (!err->error) {
  80                 /* check messages from kernel */
  81                 struct rtattr *attrs = (struct rtattr *)NLMSG_DATA(nh);
  82
  83                 while (RTA_OK(attrs, len)) {
  84                         if (attrs->rta_type == NLMSGERR_ATTR_MSG)
  85                                 fprintf(stderr, "netlink ext ack msg: %s\n",
  86                                         (char *)RTA_DATA(attrs));
  87                         if (attrs->rta_type == NLMSGERR_ATTR_OFFS) {
  88                                 memcpy(&off, RTA_DATA(attrs), 4);
  89                                 fprintf(stderr, "netlink err off %d\n",
  90                                         (int)off);
  91                         }
  92                         attrs = RTA_NEXT(attrs, len);
  93                 }
  94         } else {
  95                 fprintf(stderr, "netlink error %d", err->error);
  96         }
  97 }
  98
  99 static int capture_events(int fd, int event_group)
 100 {
 101         u_int8_t buffer[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 102                         NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 1024];
 103         struct genlmsghdr *ghdr;
 104         struct rtattr *attrs;
 105         struct nlmsghdr *nh;
 106         int ret = 0;
 107         int res_len;
 108         int msg_len;
 109         fd_set rfds;
 110
 111         if (setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
 112                        &event_group, sizeof(event_group)) < 0)
 113                 error(1, errno, "could not join the " MPTCP_PM_EVENTS " mcast group");
 114
 115         do {
 116                 FD_ZERO(&rfds);
 117                 FD_SET(fd, &rfds);
 118                 res_len = NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 119                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 1024;
 120
 121                 ret = select(FD_SETSIZE, &rfds, NULL, NULL, NULL);
 122
 123                 if (ret < 0)
 124                         error(1, ret, "error in select() on NL socket");
 125
 126                 res_len = recv(fd, buffer, res_len, 0);
 127                 if (res_len < 0)
 128                         error(1, res_len, "error on recv() from NL socket");
 129
 130                 nh = (struct nlmsghdr *)buffer;
 131
 132                 for (; NLMSG_OK(nh, res_len); nh = NLMSG_NEXT(nh, res_len)) {
 133                         if (nh->nlmsg_type == NLMSG_ERROR)
 134                                 error(1, NLMSG_ERROR, "received invalid NL message");
 135
 136                         ghdr = (struct genlmsghdr *)NLMSG_DATA(nh);
 137
 138                         if (ghdr->cmd == 0)
 139                                 continue;
 140
 141                         fprintf(stderr, "type:%d", ghdr->cmd);
 142
 143                         msg_len = nh->nlmsg_len - NLMSG_LENGTH(GENL_HDRLEN);
 144
 145                         attrs = (struct rtattr *) ((char *) ghdr + GENL_HDRLEN);
 146                         while (RTA_OK(attrs, msg_len)) {
 147                                 if (attrs->rta_type == MPTCP_ATTR_TOKEN)
 148                                         fprintf(stderr, ",token:%u", *(__u32 *)RTA_DATA(attrs));
 149                                 else if (attrs->rta_type == MPTCP_ATTR_FAMILY)
 150                                         fprintf(stderr, ",family:%u", *(__u16 *)RTA_DATA(attrs));
 151                                 else if (attrs->rta_type == MPTCP_ATTR_LOC_ID)
 152                                         fprintf(stderr, ",loc_id:%u", *(__u8 *)RTA_DATA(attrs));
 153                                 else if (attrs->rta_type == MPTCP_ATTR_REM_ID)
 154                                         fprintf(stderr, ",rem_id:%u", *(__u8 *)RTA_DATA(attrs));
 155                                 else if (attrs->rta_type == MPTCP_ATTR_SADDR4) {
 156                                         u_int32_t saddr4 = ntohl(*(__u32 *)RTA_DATA(attrs));
 157
 158                                         fprintf(stderr, ",saddr4:%u.%u.%u.%u", saddr4 >> 24,
 159                                                (saddr4 >> 16) & 0xFF, (saddr4 >> 8) & 0xFF,
 160                                                (saddr4 & 0xFF));
 161                                 } else if (attrs->rta_type == MPTCP_ATTR_SADDR6) {
 162                                         char buf[INET6_ADDRSTRLEN];
 163
 164                                         if (inet_ntop(AF_INET6, RTA_DATA(attrs), buf,
 165                                                       sizeof(buf)) != NULL)
 166                                                 fprintf(stderr, ",saddr6:%s", buf);
 167                                 } else if (attrs->rta_type == MPTCP_ATTR_DADDR4) {
 168                                         u_int32_t daddr4 = ntohl(*(__u32 *)RTA_DATA(attrs));
 169
 170                                         fprintf(stderr, ",daddr4:%u.%u.%u.%u", daddr4 >> 24,
 171                                                (daddr4 >> 16) & 0xFF, (daddr4 >> 8) & 0xFF,
 172                                                (daddr4 & 0xFF));
 173                                 } else if (attrs->rta_type == MPTCP_ATTR_DADDR6) {
 174                                         char buf[INET6_ADDRSTRLEN];
 175
 176                                         if (inet_ntop(AF_INET6, RTA_DATA(attrs), buf,
 177                                                       sizeof(buf)) != NULL)
 178                                                 fprintf(stderr, ",daddr6:%s", buf);
 179                                 } else if (attrs->rta_type == MPTCP_ATTR_SPORT)
 180                                         fprintf(stderr, ",sport:%u",
 181                                                 ntohs(*(__u16 *)RTA_DATA(attrs)));
 182                                 else if (attrs->rta_type == MPTCP_ATTR_DPORT)
 183                                         fprintf(stderr, ",dport:%u",
 184                                                 ntohs(*(__u16 *)RTA_DATA(attrs)));
 185                                 else if (attrs->rta_type == MPTCP_ATTR_BACKUP)
 186                                         fprintf(stderr, ",backup:%u", *(__u8 *)RTA_DATA(attrs));
 187                                 else if (attrs->rta_type == MPTCP_ATTR_ERROR)
 188                                         fprintf(stderr, ",error:%u", *(__u8 *)RTA_DATA(attrs));
 189                                 else if (attrs->rta_type == MPTCP_ATTR_SERVER_SIDE)
 190                                         fprintf(stderr, ",server_side:%u", *(__u8 *)RTA_DATA(attrs));
 191
 192                                 attrs = RTA_NEXT(attrs, msg_len);
 193                         }
 194                 }
 195                 fprintf(stderr, "\n");
 196         } while (1);
 197
 198         return 0;
 199 }
 200
 201 /* do a netlink command and, if max > 0, fetch the reply  */
 202 static int do_nl_req(int fd, struct nlmsghdr *nh, int len, int max)
 203 {
 204         struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK };
 205         socklen_t addr_len;
 206         void *data = nh;
 207         int rem, ret;
 208         int err = 0;
 209
 210         nh->nlmsg_len = len;
 211         ret = sendto(fd, data, len, 0, (void *)&nladdr, sizeof(nladdr));
 212         if (ret != len)
 213                 error(1, errno, "send netlink: %uB != %uB\n", ret, len);
 214         if (max == 0)
 215                 return 0;
 216
 217         addr_len = sizeof(nladdr);
 218         rem = ret = recvfrom(fd, data, max, 0, (void *)&nladdr, &addr_len);
 219         if (ret < 0)
 220                 error(1, errno, "recv netlink: %uB\n", ret);
 221
 222         /* Beware: the NLMSG_NEXT macro updates the 'rem' argument */
 223         for (; NLMSG_OK(nh, rem); nh = NLMSG_NEXT(nh, rem)) {
 224                 if (nh->nlmsg_type == NLMSG_ERROR) {
 225                         nl_error(nh);
 226                         err = 1;
 227                 }
 228         }
 229         if (err)
 230                 error(1, 0, "bailing out due to netlink error[s]");
 231         return ret;
 232 }
 233
 234 static int genl_parse_getfamily(struct nlmsghdr *nlh, int *pm_family,
 235                                 int *events_mcast_grp)
 236 {
 237         struct genlmsghdr *ghdr = NLMSG_DATA(nlh);
 238         int len = nlh->nlmsg_len;
 239         struct rtattr *attrs;
 240         struct rtattr *grps;
 241         struct rtattr *grp;
 242         int got_events_grp;
 243         int got_family;
 244         int grps_len;
 245         int grp_len;
 246
 247         if (nlh->nlmsg_type != GENL_ID_CTRL)
 248                 error(1, errno, "Not a controller message, len=%d type=0x%x\n",
 249                       nlh->nlmsg_len, nlh->nlmsg_type);
 250
 251         len -= NLMSG_LENGTH(GENL_HDRLEN);
 252
 253         if (len < 0)
 254                 error(1, errno, "wrong controller message len %d\n", len);
 255
 256         if (ghdr->cmd != CTRL_CMD_NEWFAMILY)
 257                 error(1, errno, "Unknown controller command %d\n", ghdr->cmd);
 258
 259         attrs = (struct rtattr *) ((char *) ghdr + GENL_HDRLEN);
 260         got_family = 0;
 261         got_events_grp = 0;
 262
 263         while (RTA_OK(attrs, len)) {
 264                 if (attrs->rta_type == CTRL_ATTR_FAMILY_ID) {
 265                         *pm_family = *(__u16 *)RTA_DATA(attrs);
 266                         got_family = 1;
 267                 } else if (attrs->rta_type == CTRL_ATTR_MCAST_GROUPS) {
 268                         grps = RTA_DATA(attrs);
 269                         grps_len = RTA_PAYLOAD(attrs);
 270
 271                         while (RTA_OK(grps, grps_len)) {
 272                                 grp = RTA_DATA(grps);
 273                                 grp_len = RTA_PAYLOAD(grps);
 274                                 got_events_grp = 0;
 275
 276                                 while (RTA_OK(grp, grp_len)) {
 277                                         if (grp->rta_type == CTRL_ATTR_MCAST_GRP_ID)
 278                                                 *events_mcast_grp = *(__u32 *)RTA_DATA(grp);
 279                                         else if (grp->rta_type == CTRL_ATTR_MCAST_GRP_NAME &&
 280                                                  !strcmp(RTA_DATA(grp), MPTCP_PM_EVENTS))
 281                                                 got_events_grp = 1;
 282
 283                                         grp = RTA_NEXT(grp, grp_len);
 284                                 }
 285
 286                                 if (got_events_grp)
 287                                         break;
 288
 289                                 grps = RTA_NEXT(grps, grps_len);
 290                         }
 291                 }
 292
 293                 if (got_family && got_events_grp)
 294                         return 0;
 295
 296                 attrs = RTA_NEXT(attrs, len);
 297         }
 298
 299         error(1, errno, "can't find CTRL_ATTR_FAMILY_ID attr");
 300         return -1;
 301 }
 302
 303 static int resolve_mptcp_pm_netlink(int fd, int *pm_family, int *events_mcast_grp)
 304 {
 305         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 306                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 307                   1024];
 308         struct nlmsghdr *nh;
 309         struct rtattr *rta;
 310         int namelen;
 311         int off = 0;
 312
 313         memset(data, 0, sizeof(data));
 314         nh = (void *)data;
 315         off = init_genl_req(data, GENL_ID_CTRL, CTRL_CMD_GETFAMILY, 0);
 316
 317         rta = (void *)(data + off);
 318         namelen = strlen(MPTCP_PM_NAME) + 1;
 319         rta->rta_type = CTRL_ATTR_FAMILY_NAME;
 320         rta->rta_len = RTA_LENGTH(namelen);
 321         memcpy(RTA_DATA(rta), MPTCP_PM_NAME, namelen);
 322         off += NLMSG_ALIGN(rta->rta_len);
 323
 324         do_nl_req(fd, nh, off, sizeof(data));
 325         return genl_parse_getfamily((void *)data, pm_family, events_mcast_grp);
 326 }
 327
 328 int dsf(int fd, int pm_family, int argc, char *argv[])
 329 {
 330         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 331                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 332                   1024];
 333         struct rtattr *rta, *addr;
 334         u_int16_t family, port;
 335         struct nlmsghdr *nh;
 336         u_int32_t token;
 337         int addr_start;
 338         int off = 0;
 339         int arg;
 340
 341         const char *params[5];
 342
 343         memset(params, 0, 5 * sizeof(const char *));
 344
 345         memset(data, 0, sizeof(data));
 346         nh = (void *)data;
 347         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_SUBFLOW_DESTROY,
 348                             MPTCP_PM_VER);
 349
 350         if (argc < 12)
 351                 syntax(argv);
 352
 353         /* Params recorded in this order:
 354          * <local-ip>, <local-port>, <remote-ip>, <remote-port>, <token>
 355          */
 356         for (arg = 2; arg < argc; arg++) {
 357                 if (!strcmp(argv[arg], "lip")) {
 358                         if (++arg >= argc)
 359                                 error(1, 0, " missing local IP");
 360
 361                         params[0] = argv[arg];
 362                 } else if (!strcmp(argv[arg], "lport")) {
 363                         if (++arg >= argc)
 364                                 error(1, 0, " missing local port");
 365
 366                         params[1] = argv[arg];
 367                 } else if (!strcmp(argv[arg], "rip")) {
 368                         if (++arg >= argc)
 369                                 error(1, 0, " missing remote IP");
 370
 371                         params[2] = argv[arg];
 372                 } else if (!strcmp(argv[arg], "rport")) {
 373                         if (++arg >= argc)
 374                                 error(1, 0, " missing remote port");
 375
 376                         params[3] = argv[arg];
 377                 } else if (!strcmp(argv[arg], "token")) {
 378                         if (++arg >= argc)
 379                                 error(1, 0, " missing token");
 380
 381                         params[4] = argv[arg];
 382                 } else
 383                         error(1, 0, "unknown keyword %s", argv[arg]);
 384         }
 385
 386         for (arg = 0; arg < 4; arg = arg + 2) {
 387                 /*  addr header */
 388                 addr_start = off;
 389                 addr = (void *)(data + off);
 390                 addr->rta_type = NLA_F_NESTED |
 391                         ((arg == 0) ? MPTCP_PM_ATTR_ADDR : MPTCP_PM_ATTR_ADDR_REMOTE);
 392                 addr->rta_len = RTA_LENGTH(0);
 393                 off += NLMSG_ALIGN(addr->rta_len);
 394
 395                 /*  addr data */
 396                 rta = (void *)(data + off);
 397                 if (inet_pton(AF_INET, params[arg], RTA_DATA(rta))) {
 398                         family = AF_INET;
 399                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
 400                         rta->rta_len = RTA_LENGTH(4);
 401                 } else if (inet_pton(AF_INET6, params[arg], RTA_DATA(rta))) {
 402                         family = AF_INET6;
 403                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
 404                         rta->rta_len = RTA_LENGTH(16);
 405                 } else
 406                         error(1, errno, "can't parse ip %s", params[arg]);
 407                 off += NLMSG_ALIGN(rta->rta_len);
 408
 409                 /* family */
 410                 rta = (void *)(data + off);
 411                 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
 412                 rta->rta_len = RTA_LENGTH(2);
 413                 memcpy(RTA_DATA(rta), &family, 2);
 414                 off += NLMSG_ALIGN(rta->rta_len);
 415
 416                 /*  port */
 417                 port = atoi(params[arg + 1]);
 418                 rta = (void *)(data + off);
 419                 rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
 420                 rta->rta_len = RTA_LENGTH(2);
 421                 memcpy(RTA_DATA(rta), &port, 2);
 422                 off += NLMSG_ALIGN(rta->rta_len);
 423
 424                 addr->rta_len = off - addr_start;
 425         }
 426
 427         /* token */
 428         token = strtoul(params[4], NULL, 10);
 429         rta = (void *)(data + off);
 430         rta->rta_type = MPTCP_PM_ATTR_TOKEN;
 431         rta->rta_len = RTA_LENGTH(4);
 432         memcpy(RTA_DATA(rta), &token, 4);
 433         off += NLMSG_ALIGN(rta->rta_len);
 434
 435         do_nl_req(fd, nh, off, 0);
 436
 437         return 0;
 438 }
 439
 440 int csf(int fd, int pm_family, int argc, char *argv[])
 441 {
 442         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 443                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 444                   1024];
 445         const char *params[5];
 446         struct nlmsghdr *nh;
 447         struct rtattr *addr;
 448         struct rtattr *rta;
 449         u_int16_t family;
 450         u_int32_t token;
 451         u_int16_t port;
 452         int addr_start;
 453         u_int8_t id;
 454         int off = 0;
 455         int arg;
 456
 457         memset(params, 0, 5 * sizeof(const char *));
 458
 459         memset(data, 0, sizeof(data));
 460         nh = (void *)data;
 461         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_SUBFLOW_CREATE,
 462                             MPTCP_PM_VER);
 463
 464         if (argc < 12)
 465                 syntax(argv);
 466
 467         /* Params recorded in this order:
 468          * <local-ip>, <local-id>, <remote-ip>, <remote-port>, <token>
 469          */
 470         for (arg = 2; arg < argc; arg++) {
 471                 if (!strcmp(argv[arg], "lip")) {
 472                         if (++arg >= argc)
 473                                 error(1, 0, " missing local IP");
 474
 475                         params[0] = argv[arg];
 476                 } else if (!strcmp(argv[arg], "lid")) {
 477                         if (++arg >= argc)
 478                                 error(1, 0, " missing local id");
 479
 480                         params[1] = argv[arg];
 481                 } else if (!strcmp(argv[arg], "rip")) {
 482                         if (++arg >= argc)
 483                                 error(1, 0, " missing remote ip");
 484
 485                         params[2] = argv[arg];
 486                 } else if (!strcmp(argv[arg], "rport")) {
 487                         if (++arg >= argc)
 488                                 error(1, 0, " missing remote port");
 489
 490                         params[3] = argv[arg];
 491                 } else if (!strcmp(argv[arg], "token")) {
 492                         if (++arg >= argc)
 493                                 error(1, 0, " missing token");
 494
 495                         params[4] = argv[arg];
 496                 } else
 497                         error(1, 0, "unknown param %s", argv[arg]);
 498         }
 499
 500         for (arg = 0; arg < 4; arg = arg + 2) {
 501                 /*  addr header */
 502                 addr_start = off;
 503                 addr = (void *)(data + off);
 504                 addr->rta_type = NLA_F_NESTED |
 505                         ((arg == 0) ? MPTCP_PM_ATTR_ADDR : MPTCP_PM_ATTR_ADDR_REMOTE);
 506                 addr->rta_len = RTA_LENGTH(0);
 507                 off += NLMSG_ALIGN(addr->rta_len);
 508
 509                 /*  addr data */
 510                 rta = (void *)(data + off);
 511                 if (inet_pton(AF_INET, params[arg], RTA_DATA(rta))) {
 512                         family = AF_INET;
 513                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
 514                         rta->rta_len = RTA_LENGTH(4);
 515                 } else if (inet_pton(AF_INET6, params[arg], RTA_DATA(rta))) {
 516                         family = AF_INET6;
 517                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
 518                         rta->rta_len = RTA_LENGTH(16);
 519                 } else
 520                         error(1, errno, "can't parse ip %s", params[arg]);
 521                 off += NLMSG_ALIGN(rta->rta_len);
 522
 523                 /* family */
 524                 rta = (void *)(data + off);
 525                 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
 526                 rta->rta_len = RTA_LENGTH(2);
 527                 memcpy(RTA_DATA(rta), &family, 2);
 528                 off += NLMSG_ALIGN(rta->rta_len);
 529
 530                 if (arg == 2) {
 531                         /*  port */
 532                         port = atoi(params[arg + 1]);
 533                         rta = (void *)(data + off);
 534                         rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
 535                         rta->rta_len = RTA_LENGTH(2);
 536                         memcpy(RTA_DATA(rta), &port, 2);
 537                         off += NLMSG_ALIGN(rta->rta_len);
 538                 }
 539
 540                 if (arg == 0) {
 541                         /* id */
 542                         id = atoi(params[arg + 1]);
 543                         rta = (void *)(data + off);
 544                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
 545                         rta->rta_len = RTA_LENGTH(1);
 546                         memcpy(RTA_DATA(rta), &id, 1);
 547                         off += NLMSG_ALIGN(rta->rta_len);
 548                 }
 549
 550                 addr->rta_len = off - addr_start;
 551         }
 552
 553         /* token */
 554         token = strtoul(params[4], NULL, 10);
 555         rta = (void *)(data + off);
 556         rta->rta_type = MPTCP_PM_ATTR_TOKEN;
 557         rta->rta_len = RTA_LENGTH(4);
 558         memcpy(RTA_DATA(rta), &token, 4);
 559         off += NLMSG_ALIGN(rta->rta_len);
 560
 561         do_nl_req(fd, nh, off, 0);
 562
 563         return 0;
 564 }
 565
 566 int remove_addr(int fd, int pm_family, int argc, char *argv[])
 567 {
 568         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 569                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 570                   1024];
 571         struct nlmsghdr *nh;
 572         struct rtattr *rta;
 573         u_int32_t token;
 574         u_int8_t id;
 575         int off = 0;
 576         int arg;
 577
 578         memset(data, 0, sizeof(data));
 579         nh = (void *)data;
 580         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_REMOVE,
 581                             MPTCP_PM_VER);
 582
 583         if (argc < 6)
 584                 syntax(argv);
 585
 586         for (arg = 2; arg < argc; arg++) {
 587                 if (!strcmp(argv[arg], "id")) {
 588                         if (++arg >= argc)
 589                                 error(1, 0, " missing id value");
 590
 591                         id = atoi(argv[arg]);
 592                         rta = (void *)(data + off);
 593                         rta->rta_type = MPTCP_PM_ATTR_LOC_ID;
 594                         rta->rta_len = RTA_LENGTH(1);
 595                         memcpy(RTA_DATA(rta), &id, 1);
 596                         off += NLMSG_ALIGN(rta->rta_len);
 597                 } else if (!strcmp(argv[arg], "token")) {
 598                         if (++arg >= argc)
 599                                 error(1, 0, " missing token value");
 600
 601                         token = strtoul(argv[arg], NULL, 10);
 602                         rta = (void *)(data + off);
 603                         rta->rta_type = MPTCP_PM_ATTR_TOKEN;
 604                         rta->rta_len = RTA_LENGTH(4);
 605                         memcpy(RTA_DATA(rta), &token, 4);
 606                         off += NLMSG_ALIGN(rta->rta_len);
 607                 } else
 608                         error(1, 0, "unknown keyword %s", argv[arg]);
 609         }
 610
 611         do_nl_req(fd, nh, off, 0);
 612         return 0;
 613 }
 614
 615 int announce_addr(int fd, int pm_family, int argc, char *argv[])
 616 {
 617         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 618                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 619                   1024];
 620         u_int32_t flags = MPTCP_PM_ADDR_FLAG_SIGNAL;
 621         u_int32_t token = UINT_MAX;
 622         struct rtattr *rta, *addr;
 623         u_int32_t id = UINT_MAX;
 624         struct nlmsghdr *nh;
 625         u_int16_t family;
 626         int addr_start;
 627         int off = 0;
 628         int arg;
 629
 630         memset(data, 0, sizeof(data));
 631         nh = (void *)data;
 632         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_ANNOUNCE,
 633                             MPTCP_PM_VER);
 634
 635         if (argc < 7)
 636                 syntax(argv);
 637
 638         /* local-ip header */
 639         addr_start = off;
 640         addr = (void *)(data + off);
 641         addr->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR;
 642         addr->rta_len = RTA_LENGTH(0);
 643         off += NLMSG_ALIGN(addr->rta_len);
 644
 645         /* local-ip data */
 646         /* record addr type */
 647         rta = (void *)(data + off);
 648         if (inet_pton(AF_INET, argv[2], RTA_DATA(rta))) {
 649                 family = AF_INET;
 650                 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
 651                 rta->rta_len = RTA_LENGTH(4);
 652         } else if (inet_pton(AF_INET6, argv[2], RTA_DATA(rta))) {
 653                 family = AF_INET6;
 654                 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
 655                 rta->rta_len = RTA_LENGTH(16);
 656         } else
 657                 error(1, errno, "can't parse ip %s", argv[2]);
 658         off += NLMSG_ALIGN(rta->rta_len);
 659
 660         /* addr family */
 661         rta = (void *)(data + off);
 662         rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
 663         rta->rta_len = RTA_LENGTH(2);
 664         memcpy(RTA_DATA(rta), &family, 2);
 665         off += NLMSG_ALIGN(rta->rta_len);
 666
 667         for (arg = 3; arg < argc; arg++) {
 668                 if (!strcmp(argv[arg], "id")) {
 669                         /* local-id */
 670                         if (++arg >= argc)
 671                                 error(1, 0, " missing id value");
 672
 673                         id = atoi(argv[arg]);
 674                         rta = (void *)(data + off);
 675                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
 676                         rta->rta_len = RTA_LENGTH(1);
 677                         memcpy(RTA_DATA(rta), &id, 1);
 678                         off += NLMSG_ALIGN(rta->rta_len);
 679                 } else if (!strcmp(argv[arg], "dev")) {
 680                         /* for the if_index */
 681                         int32_t ifindex;
 682
 683                         if (++arg >= argc)
 684                                 error(1, 0, " missing dev name");
 685
 686                         ifindex = if_nametoindex(argv[arg]);
 687                         if (!ifindex)
 688                                 error(1, errno, "unknown device %s", argv[arg]);
 689
 690                         rta = (void *)(data + off);
 691                         rta->rta_type = MPTCP_PM_ADDR_ATTR_IF_IDX;
 692                         rta->rta_len = RTA_LENGTH(4);
 693                         memcpy(RTA_DATA(rta), &ifindex, 4);
 694                         off += NLMSG_ALIGN(rta->rta_len);
 695                 } else if (!strcmp(argv[arg], "port")) {
 696                         /* local-port (optional) */
 697                         u_int16_t port;
 698
 699                         if (++arg >= argc)
 700                                 error(1, 0, " missing port value");
 701
 702                         port = atoi(argv[arg]);
 703                         rta = (void *)(data + off);
 704                         rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
 705                         rta->rta_len = RTA_LENGTH(2);
 706                         memcpy(RTA_DATA(rta), &port, 2);
 707                         off += NLMSG_ALIGN(rta->rta_len);
 708                 } else if (!strcmp(argv[arg], "token")) {
 709                         /* MPTCP connection token */
 710                         if (++arg >= argc)
 711                                 error(1, 0, " missing token value");
 712
 713                         token = strtoul(argv[arg], NULL, 10);
 714                 } else
 715                         error(1, 0, "unknown keyword %s", argv[arg]);
 716         }
 717
 718         /* addr flags */
 719         rta = (void *)(data + off);
 720         rta->rta_type = MPTCP_PM_ADDR_ATTR_FLAGS;
 721         rta->rta_len = RTA_LENGTH(4);
 722         memcpy(RTA_DATA(rta), &flags, 4);
 723         off += NLMSG_ALIGN(rta->rta_len);
 724
 725         addr->rta_len = off - addr_start;
 726
 727         if (id == UINT_MAX || token == UINT_MAX)
 728                 error(1, 0, " missing mandatory inputs");
 729
 730         /* token */
 731         rta = (void *)(data + off);
 732         rta->rta_type = MPTCP_PM_ATTR_TOKEN;
 733         rta->rta_len = RTA_LENGTH(4);
 734         memcpy(RTA_DATA(rta), &token, 4);
 735         off += NLMSG_ALIGN(rta->rta_len);
 736
 737         do_nl_req(fd, nh, off, 0);
 738
 739         return 0;
 740 }
 741
 742 int add_addr(int fd, int pm_family, int argc, char *argv[])
 743 {
 744         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 745                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 746                   1024];
 747         struct rtattr *rta, *nest;
 748         struct nlmsghdr *nh;
 749         u_int32_t flags = 0;
 750         u_int16_t family;
 751         int nest_start;
 752         u_int8_t id;
 753         int off = 0;
 754         int arg;
 755
 756         memset(data, 0, sizeof(data));
 757         nh = (void *)data;
 758         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_ADD_ADDR,
 759                             MPTCP_PM_VER);
 760
 761         if (argc < 3)
 762                 syntax(argv);
 763
 764         nest_start = off;
 765         nest = (void *)(data + off);
 766         nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR;
 767         nest->rta_len = RTA_LENGTH(0);
 768         off += NLMSG_ALIGN(nest->rta_len);
 769
 770         /* addr data */
 771         rta = (void *)(data + off);
 772         if (inet_pton(AF_INET, argv[2], RTA_DATA(rta))) {
 773                 family = AF_INET;
 774                 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
 775                 rta->rta_len = RTA_LENGTH(4);
 776         } else if (inet_pton(AF_INET6, argv[2], RTA_DATA(rta))) {
 777                 family = AF_INET6;
 778                 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
 779                 rta->rta_len = RTA_LENGTH(16);
 780         } else
 781                 error(1, errno, "can't parse ip %s", argv[2]);
 782         off += NLMSG_ALIGN(rta->rta_len);
 783
 784         /* family */
 785         rta = (void *)(data + off);
 786         rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
 787         rta->rta_len = RTA_LENGTH(2);
 788         memcpy(RTA_DATA(rta), &family, 2);
 789         off += NLMSG_ALIGN(rta->rta_len);
 790
 791         for (arg = 3; arg < argc; arg++) {
 792                 if (!strcmp(argv[arg], "flags")) {
 793                         char *tok, *str;
 794
 795                         /* flags */
 796                         if (++arg >= argc)
 797                                 error(1, 0, " missing flags value");
 798
 799                         /* do not support flag list yet */
 800                         for (str = argv[arg]; (tok = strtok(str, ","));
 801                              str = NULL) {
 802                                 if (!strcmp(tok, "subflow"))
 803                                         flags |= MPTCP_PM_ADDR_FLAG_SUBFLOW;
 804                                 else if (!strcmp(tok, "signal"))
 805                                         flags |= MPTCP_PM_ADDR_FLAG_SIGNAL;
 806                                 else if (!strcmp(tok, "backup"))
 807                                         flags |= MPTCP_PM_ADDR_FLAG_BACKUP;
 808                                 else if (!strcmp(tok, "fullmesh"))
 809                                         flags |= MPTCP_PM_ADDR_FLAG_FULLMESH;
 810                                 else
 811                                         error(1, errno,
 812                                               "unknown flag %s", argv[arg]);
 813                         }
 814
 815                         if (flags & MPTCP_PM_ADDR_FLAG_SIGNAL &&
 816                             flags & MPTCP_PM_ADDR_FLAG_FULLMESH) {
 817                                 error(1, errno, "error flag fullmesh");
 818                         }
 819
 820                         rta = (void *)(data + off);
 821                         rta->rta_type = MPTCP_PM_ADDR_ATTR_FLAGS;
 822                         rta->rta_len = RTA_LENGTH(4);
 823                         memcpy(RTA_DATA(rta), &flags, 4);
 824                         off += NLMSG_ALIGN(rta->rta_len);
 825                 } else if (!strcmp(argv[arg], "id")) {
 826                         if (++arg >= argc)
 827                                 error(1, 0, " missing id value");
 828
 829                         id = atoi(argv[arg]);
 830                         rta = (void *)(data + off);
 831                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
 832                         rta->rta_len = RTA_LENGTH(1);
 833                         memcpy(RTA_DATA(rta), &id, 1);
 834                         off += NLMSG_ALIGN(rta->rta_len);
 835                 } else if (!strcmp(argv[arg], "dev")) {
 836                         int32_t ifindex;
 837
 838                         if (++arg >= argc)
 839                                 error(1, 0, " missing dev name");
 840
 841                         ifindex = if_nametoindex(argv[arg]);
 842                         if (!ifindex)
 843                                 error(1, errno, "unknown device %s", argv[arg]);
 844
 845                         rta = (void *)(data + off);
 846                         rta->rta_type = MPTCP_PM_ADDR_ATTR_IF_IDX;
 847                         rta->rta_len = RTA_LENGTH(4);
 848                         memcpy(RTA_DATA(rta), &ifindex, 4);
 849                         off += NLMSG_ALIGN(rta->rta_len);
 850                 } else if (!strcmp(argv[arg], "port")) {
 851                         u_int16_t port;
 852
 853                         if (++arg >= argc)
 854                                 error(1, 0, " missing port value");
 855                         if (!(flags & MPTCP_PM_ADDR_FLAG_SIGNAL))
 856                                 error(1, 0, " flags must be signal when using port");
 857
 858                         port = atoi(argv[arg]);
 859                         rta = (void *)(data + off);
 860                         rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
 861                         rta->rta_len = RTA_LENGTH(2);
 862                         memcpy(RTA_DATA(rta), &port, 2);
 863                         off += NLMSG_ALIGN(rta->rta_len);
 864                 } else
 865                         error(1, 0, "unknown keyword %s", argv[arg]);
 866         }
 867         nest->rta_len = off - nest_start;
 868
 869         do_nl_req(fd, nh, off, 0);
 870         return 0;
 871 }
 872
 873 int del_addr(int fd, int pm_family, int argc, char *argv[])
 874 {
 875         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
 876                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
 877                   1024];
 878         struct rtattr *rta, *nest;
 879         struct nlmsghdr *nh;
 880         u_int16_t family;
 881         int nest_start;
 882         u_int8_t id;
 883         int off = 0;
 884
 885         memset(data, 0, sizeof(data));
 886         nh = (void *)data;
 887         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_DEL_ADDR,
 888                             MPTCP_PM_VER);
 889
 890         /* the only argument is the address id (nonzero) */
 891         if (argc != 3 && argc != 4)
 892                 syntax(argv);
 893
 894         id = atoi(argv[2]);
 895         /* zero id with the IP address */
 896         if (!id && argc != 4)
 897                 syntax(argv);
 898
 899         nest_start = off;
 900         nest = (void *)(data + off);
 901         nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR;
 902         nest->rta_len =  RTA_LENGTH(0);
 903         off += NLMSG_ALIGN(nest->rta_len);
 904
 905         /* build a dummy addr with only the ID set */
 906         rta = (void *)(data + off);
 907         rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
 908         rta->rta_len = RTA_LENGTH(1);
 909         memcpy(RTA_DATA(rta), &id, 1);
 910         off += NLMSG_ALIGN(rta->rta_len);
 911
 912         if (!id) {
 913                 /* addr data */
 914                 rta = (void *)(data + off);
 915                 if (inet_pton(AF_INET, argv[3], RTA_DATA(rta))) {
 916                         family = AF_INET;
 917                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
 918                         rta->rta_len = RTA_LENGTH(4);
 919                 } else if (inet_pton(AF_INET6, argv[3], RTA_DATA(rta))) {
 920                         family = AF_INET6;
 921                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
 922                         rta->rta_len = RTA_LENGTH(16);
 923                 } else {
 924                         error(1, errno, "can't parse ip %s", argv[3]);
 925                 }
 926                 off += NLMSG_ALIGN(rta->rta_len);
 927
 928                 /* family */
 929                 rta = (void *)(data + off);
 930                 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
 931                 rta->rta_len = RTA_LENGTH(2);
 932                 memcpy(RTA_DATA(rta), &family, 2);
 933                 off += NLMSG_ALIGN(rta->rta_len);
 934         }
 935         nest->rta_len = off - nest_start;
 936
 937         do_nl_req(fd, nh, off, 0);
 938         return 0;
 939 }
 940
 941 static void print_addr(struct rtattr *attrs, int len)
 942 {
 943         uint16_t family = 0;
 944         uint16_t port = 0;
 945         char str[1024];
 946         uint32_t flags;
 947         uint8_t id;
 948
 949         while (RTA_OK(attrs, len)) {
 950                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_FAMILY)
 951                         memcpy(&family, RTA_DATA(attrs), 2);
 952                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_PORT)
 953                         memcpy(&port, RTA_DATA(attrs), 2);
 954                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ADDR4) {
 955                         if (family != AF_INET)
 956                                 error(1, errno, "wrong IP (v4) for family %d",
 957                                       family);
 958                         inet_ntop(AF_INET, RTA_DATA(attrs), str, sizeof(str));
 959                         printf("%s", str);
 960                         if (port)
 961                                 printf(" %d", port);
 962                 }
 963                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ADDR6) {
 964                         if (family != AF_INET6)
 965                                 error(1, errno, "wrong IP (v6) for family %d",
 966                                       family);
 967                         inet_ntop(AF_INET6, RTA_DATA(attrs), str, sizeof(str));
 968                         printf("%s", str);
 969                         if (port)
 970                                 printf(" %d", port);
 971                 }
 972                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ID) {
 973                         memcpy(&id, RTA_DATA(attrs), 1);
 974                         printf("id %d ", id);
 975                 }
 976                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_FLAGS) {
 977                         memcpy(&flags, RTA_DATA(attrs), 4);
 978
 979                         printf("flags ");
 980                         if (flags & MPTCP_PM_ADDR_FLAG_SIGNAL) {
 981                                 printf("signal");
 982                                 flags &= ~MPTCP_PM_ADDR_FLAG_SIGNAL;
 983                                 if (flags)
 984                                         printf(",");
 985                         }
 986
 987                         if (flags & MPTCP_PM_ADDR_FLAG_SUBFLOW) {
 988                                 printf("subflow");
 989                                 flags &= ~MPTCP_PM_ADDR_FLAG_SUBFLOW;
 990                                 if (flags)
 991                                         printf(",");
 992                         }
 993
 994                         if (flags & MPTCP_PM_ADDR_FLAG_BACKUP) {
 995                                 printf("backup");
 996                                 flags &= ~MPTCP_PM_ADDR_FLAG_BACKUP;
 997                                 if (flags)
 998                                         printf(",");
 999                         }
1000
1001                         if (flags & MPTCP_PM_ADDR_FLAG_FULLMESH) {
1002                                 printf("fullmesh");
1003                                 flags &= ~MPTCP_PM_ADDR_FLAG_FULLMESH;
1004                                 if (flags)
1005                                         printf(",");
1006                         }
1007
1008                         if (flags & MPTCP_PM_ADDR_FLAG_IMPLICIT) {
1009                                 printf("implicit");
1010                                 flags &= ~MPTCP_PM_ADDR_FLAG_IMPLICIT;
1011                                 if (flags)
1012                                         printf(",");
1013                         }
1014
1015                         /* bump unknown flags, if any */
1016                         if (flags)
1017                                 printf("0x%x", flags);
1018                         printf(" ");
1019                 }
1020                 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_IF_IDX) {
1021                         char name[IF_NAMESIZE], *ret;
1022                         int32_t ifindex;
1023
1024                         memcpy(&ifindex, RTA_DATA(attrs), 4);
1025                         ret = if_indextoname(ifindex, name);
1026                         if (ret)
1027                                 printf("dev %s ", ret);
1028                         else
1029                                 printf("dev unknown/%d", ifindex);
1030                 }
1031
1032                 attrs = RTA_NEXT(attrs, len);
1033         }
1034         printf("\n");
1035 }
1036
1037 static void print_addrs(struct nlmsghdr *nh, int pm_family, int total_len)
1038 {
1039         struct rtattr *attrs;
1040
1041         for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) {
1042                 int len = nh->nlmsg_len;
1043
1044                 if (nh->nlmsg_type == NLMSG_DONE)
1045                         break;
1046                 if (nh->nlmsg_type == NLMSG_ERROR)
1047                         nl_error(nh);
1048                 if (nh->nlmsg_type != pm_family)
1049                         continue;
1050
1051                 len -= NLMSG_LENGTH(GENL_HDRLEN);
1052                 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) +
1053                                            GENL_HDRLEN);
1054                 while (RTA_OK(attrs, len)) {
1055                         if (attrs->rta_type ==
1056                             (MPTCP_PM_ATTR_ADDR | NLA_F_NESTED))
1057                                 print_addr((void *)RTA_DATA(attrs),
1058                                            attrs->rta_len);
1059                         attrs = RTA_NEXT(attrs, len);
1060                 }
1061         }
1062 }
1063
1064 int get_addr(int fd, int pm_family, int argc, char *argv[])
1065 {
1066         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
1067                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
1068                   1024];
1069         struct rtattr *rta, *nest;
1070         struct nlmsghdr *nh;
1071         int nest_start;
1072         u_int8_t id;
1073         int off = 0;
1074
1075         memset(data, 0, sizeof(data));
1076         nh = (void *)data;
1077         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR,
1078                             MPTCP_PM_VER);
1079
1080         /* the only argument is the address id */
1081         if (argc != 3)
1082                 syntax(argv);
1083
1084         id = atoi(argv[2]);
1085
1086         nest_start = off;
1087         nest = (void *)(data + off);
1088         nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR;
1089         nest->rta_len =  RTA_LENGTH(0);
1090         off += NLMSG_ALIGN(nest->rta_len);
1091
1092         /* build a dummy addr with only the ID set */
1093         rta = (void *)(data + off);
1094         rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
1095         rta->rta_len = RTA_LENGTH(1);
1096         memcpy(RTA_DATA(rta), &id, 1);
1097         off += NLMSG_ALIGN(rta->rta_len);
1098         nest->rta_len = off - nest_start;
1099
1100         print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data)));
1101         return 0;
1102 }
1103
1104 int dump_addrs(int fd, int pm_family, int argc, char *argv[])
1105 {
1106         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
1107                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
1108                   1024];
1109         pid_t pid = getpid();
1110         struct nlmsghdr *nh;
1111         int off = 0;
1112
1113         memset(data, 0, sizeof(data));
1114         nh = (void *)data;
1115         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR,
1116                             MPTCP_PM_VER);
1117         nh->nlmsg_flags |= NLM_F_DUMP;
1118         nh->nlmsg_seq = 1;
1119         nh->nlmsg_pid = pid;
1120         nh->nlmsg_len = off;
1121
1122         print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data)));
1123         return 0;
1124 }
1125
1126 int flush_addrs(int fd, int pm_family, int argc, char *argv[])
1127 {
1128         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
1129                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
1130                   1024];
1131         struct nlmsghdr *nh;
1132         int off = 0;
1133
1134         memset(data, 0, sizeof(data));
1135         nh = (void *)data;
1136         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_FLUSH_ADDRS,
1137                             MPTCP_PM_VER);
1138
1139         do_nl_req(fd, nh, off, 0);
1140         return 0;
1141 }
1142
1143 static void print_limits(struct nlmsghdr *nh, int pm_family, int total_len)
1144 {
1145         struct rtattr *attrs;
1146         uint32_t max;
1147
1148         for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) {
1149                 int len = nh->nlmsg_len;
1150
1151                 if (nh->nlmsg_type == NLMSG_DONE)
1152                         break;
1153                 if (nh->nlmsg_type == NLMSG_ERROR)
1154                         nl_error(nh);
1155                 if (nh->nlmsg_type != pm_family)
1156                         continue;
1157
1158                 len -= NLMSG_LENGTH(GENL_HDRLEN);
1159                 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) +
1160                                            GENL_HDRLEN);
1161                 while (RTA_OK(attrs, len)) {
1162                         int type = attrs->rta_type;
1163
1164                         if (type != MPTCP_PM_ATTR_RCV_ADD_ADDRS &&
1165                             type != MPTCP_PM_ATTR_SUBFLOWS)
1166                                 goto next;
1167
1168                         memcpy(&max, RTA_DATA(attrs), 4);
1169                         printf("%s %u\n", type == MPTCP_PM_ATTR_SUBFLOWS ?
1170                                           "subflows" : "accept", max);
1171
1172 next:
1173                         attrs = RTA_NEXT(attrs, len);
1174                 }
1175         }
1176 }
1177
1178 int get_set_limits(int fd, int pm_family, int argc, char *argv[])
1179 {
1180         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
1181                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
1182                   1024];
1183         uint32_t rcv_addr = 0, subflows = 0;
1184         int cmd, len = sizeof(data);
1185         struct nlmsghdr *nh;
1186         int off = 0;
1187
1188         /* limit */
1189         if (argc == 4) {
1190                 rcv_addr = atoi(argv[2]);
1191                 subflows = atoi(argv[3]);
1192                 cmd = MPTCP_PM_CMD_SET_LIMITS;
1193         } else {
1194                 cmd = MPTCP_PM_CMD_GET_LIMITS;
1195         }
1196
1197         memset(data, 0, sizeof(data));
1198         nh = (void *)data;
1199         off = init_genl_req(data, pm_family, cmd, MPTCP_PM_VER);
1200
1201         /* limit */
1202         if (cmd == MPTCP_PM_CMD_SET_LIMITS) {
1203                 struct rtattr *rta = (void *)(data + off);
1204
1205                 rta->rta_type = MPTCP_PM_ATTR_RCV_ADD_ADDRS;
1206                 rta->rta_len = RTA_LENGTH(4);
1207                 memcpy(RTA_DATA(rta), &rcv_addr, 4);
1208                 off += NLMSG_ALIGN(rta->rta_len);
1209
1210                 rta = (void *)(data + off);
1211                 rta->rta_type = MPTCP_PM_ATTR_SUBFLOWS;
1212                 rta->rta_len = RTA_LENGTH(4);
1213                 memcpy(RTA_DATA(rta), &subflows, 4);
1214                 off += NLMSG_ALIGN(rta->rta_len);
1215
1216                 /* do not expect a reply */
1217                 len = 0;
1218         }
1219
1220         len = do_nl_req(fd, nh, off, len);
1221         if (cmd == MPTCP_PM_CMD_GET_LIMITS)
1222                 print_limits(nh, pm_family, len);
1223         return 0;
1224 }
1225
1226 int add_listener(int argc, char *argv[])
1227 {
1228         struct sockaddr_storage addr;
1229         struct sockaddr_in6 *a6;
1230         struct sockaddr_in *a4;
1231         u_int16_t family;
1232         int enable = 1;
1233         int sock;
1234         int err;
1235
1236         if (argc < 4)
1237                 syntax(argv);
1238
1239         memset(&addr, 0, sizeof(struct sockaddr_storage));
1240         a4 = (struct sockaddr_in *)&addr;
1241         a6 = (struct sockaddr_in6 *)&addr;
1242
1243         if (inet_pton(AF_INET, argv[2], &a4->sin_addr)) {
1244                 family = AF_INET;
1245                 a4->sin_family = family;
1246                 a4->sin_port = htons(atoi(argv[3]));
1247         } else if (inet_pton(AF_INET6, argv[2], &a6->sin6_addr)) {
1248                 family = AF_INET6;
1249                 a6->sin6_family = family;
1250                 a6->sin6_port = htons(atoi(argv[3]));
1251         } else
1252                 error(1, errno, "can't parse ip %s", argv[2]);
1253
1254         sock = socket(family, SOCK_STREAM, IPPROTO_MPTCP);
1255         if (sock < 0)
1256                 error(1, errno, "can't create listener sock\n");
1257
1258         if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable))) {
1259                 close(sock);
1260                 error(1, errno, "can't set SO_REUSEADDR on listener sock\n");
1261         }
1262
1263         err = bind(sock, (struct sockaddr *)&addr,
1264                    ((family == AF_INET) ? sizeof(struct sockaddr_in) :
1265                     sizeof(struct sockaddr_in6)));
1266
1267         if (err == 0 && listen(sock, 30) == 0)
1268                 pause();
1269
1270         close(sock);
1271         return 0;
1272 }
1273
1274 int set_flags(int fd, int pm_family, int argc, char *argv[])
1275 {
1276         char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
1277                   NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
1278                   1024];
1279         struct rtattr *rta, *nest;
1280         struct nlmsghdr *nh;
1281         u_int32_t flags = 0;
1282         u_int32_t token = 0;
1283         u_int16_t rport = 0;
1284         u_int16_t family;
1285         void *rip = NULL;
1286         int nest_start;
1287         int use_id = 0;
1288         u_int8_t id;
1289         int off = 0;
1290         int arg = 2;
1291
1292         memset(data, 0, sizeof(data));
1293         nh = (void *)data;
1294         off = init_genl_req(data, pm_family, MPTCP_PM_CMD_SET_FLAGS,
1295                             MPTCP_PM_VER);
1296
1297         if (argc < 3)
1298                 syntax(argv);
1299
1300         nest_start = off;
1301         nest = (void *)(data + off);
1302         nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR;
1303         nest->rta_len = RTA_LENGTH(0);
1304         off += NLMSG_ALIGN(nest->rta_len);
1305
1306         if (!strcmp(argv[arg], "id")) {
1307                 if (++arg >= argc)
1308                         error(1, 0, " missing id value");
1309
1310                 use_id = 1;
1311                 id = atoi(argv[arg]);
1312                 rta = (void *)(data + off);
1313                 rta->rta_type = MPTCP_PM_ADDR_ATTR_ID;
1314                 rta->rta_len = RTA_LENGTH(1);
1315                 memcpy(RTA_DATA(rta), &id, 1);
1316                 off += NLMSG_ALIGN(rta->rta_len);
1317         } else {
1318                 /* addr data */
1319                 rta = (void *)(data + off);
1320                 if (inet_pton(AF_INET, argv[arg], RTA_DATA(rta))) {
1321                         family = AF_INET;
1322                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
1323                         rta->rta_len = RTA_LENGTH(4);
1324                 } else if (inet_pton(AF_INET6, argv[arg], RTA_DATA(rta))) {
1325                         family = AF_INET6;
1326                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
1327                         rta->rta_len = RTA_LENGTH(16);
1328                 } else {
1329                         error(1, errno, "can't parse ip %s", argv[arg]);
1330                 }
1331                 off += NLMSG_ALIGN(rta->rta_len);
1332
1333                 /* family */
1334                 rta = (void *)(data + off);
1335                 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
1336                 rta->rta_len = RTA_LENGTH(2);
1337                 memcpy(RTA_DATA(rta), &family, 2);
1338                 off += NLMSG_ALIGN(rta->rta_len);
1339         }
1340
1341         if (++arg >= argc)
1342                 error(1, 0, " missing flags keyword");
1343
1344         for (; arg < argc; arg++) {
1345                 if (!strcmp(argv[arg], "token")) {
1346                         if (++arg >= argc)
1347                                 error(1, 0, " missing token value");
1348
1349                         /* token */
1350                         token = strtoul(argv[arg], NULL, 10);
1351                 } else if (!strcmp(argv[arg], "flags")) {
1352                         char *tok, *str;
1353
1354                         /* flags */
1355                         if (++arg >= argc)
1356                                 error(1, 0, " missing flags value");
1357
1358                         for (str = argv[arg]; (tok = strtok(str, ","));
1359                              str = NULL) {
1360                                 if (!strcmp(tok, "backup"))
1361                                         flags |= MPTCP_PM_ADDR_FLAG_BACKUP;
1362                                 else if (!strcmp(tok, "fullmesh"))
1363                                         flags |= MPTCP_PM_ADDR_FLAG_FULLMESH;
1364                                 else if (strcmp(tok, "nobackup") &&
1365                                          strcmp(tok, "nofullmesh"))
1366                                         error(1, errno,
1367                                               "unknown flag %s", argv[arg]);
1368                         }
1369
1370                         rta = (void *)(data + off);
1371                         rta->rta_type = MPTCP_PM_ADDR_ATTR_FLAGS;
1372                         rta->rta_len = RTA_LENGTH(4);
1373                         memcpy(RTA_DATA(rta), &flags, 4);
1374                         off += NLMSG_ALIGN(rta->rta_len);
1375                 } else if (!strcmp(argv[arg], "port")) {
1376                         u_int16_t port;
1377
1378                         if (use_id)
1379                                 error(1, 0, " port can't be used with id");
1380
1381                         if (++arg >= argc)
1382                                 error(1, 0, " missing port value");
1383
1384                         port = atoi(argv[arg]);
1385                         rta = (void *)(data + off);
1386                         rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
1387                         rta->rta_len = RTA_LENGTH(2);
1388                         memcpy(RTA_DATA(rta), &port, 2);
1389                         off += NLMSG_ALIGN(rta->rta_len);
1390                 } else if (!strcmp(argv[arg], "rport")) {
1391                         if (++arg >= argc)
1392                                 error(1, 0, " missing remote port");
1393
1394                         rport = atoi(argv[arg]);
1395                 } else if (!strcmp(argv[arg], "rip")) {
1396                         if (++arg >= argc)
1397                                 error(1, 0, " missing remote ip");
1398
1399                         rip = argv[arg];
1400                 } else {
1401                         error(1, 0, "unknown keyword %s", argv[arg]);
1402                 }
1403         }
1404         nest->rta_len = off - nest_start;
1405
1406         /* token */
1407         if (token) {
1408                 rta = (void *)(data + off);
1409                 rta->rta_type = MPTCP_PM_ATTR_TOKEN;
1410                 rta->rta_len = RTA_LENGTH(4);
1411                 memcpy(RTA_DATA(rta), &token, 4);
1412                 off += NLMSG_ALIGN(rta->rta_len);
1413         }
1414
1415         /* remote addr/port */
1416         if (rip) {
1417                 nest_start = off;
1418                 nest = (void *)(data + off);
1419                 nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR_REMOTE;
1420                 nest->rta_len = RTA_LENGTH(0);
1421                 off += NLMSG_ALIGN(nest->rta_len);
1422
1423                 /* addr data */
1424                 rta = (void *)(data + off);
1425                 if (inet_pton(AF_INET, rip, RTA_DATA(rta))) {
1426                         family = AF_INET;
1427                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4;
1428                         rta->rta_len = RTA_LENGTH(4);
1429                 } else if (inet_pton(AF_INET6, rip, RTA_DATA(rta))) {
1430                         family = AF_INET6;
1431                         rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
1432                         rta->rta_len = RTA_LENGTH(16);
1433                 } else {
1434                         error(1, errno, "can't parse ip %s", (char *)rip);
1435                 }
1436                 off += NLMSG_ALIGN(rta->rta_len);
1437
1438                 /* family */
1439                 rta = (void *)(data + off);
1440                 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY;
1441                 rta->rta_len = RTA_LENGTH(2);
1442                 memcpy(RTA_DATA(rta), &family, 2);
1443                 off += NLMSG_ALIGN(rta->rta_len);
1444
1445                 if (rport) {
1446                         rta = (void *)(data + off);
1447                         rta->rta_type = MPTCP_PM_ADDR_ATTR_PORT;
1448                         rta->rta_len = RTA_LENGTH(2);
1449                         memcpy(RTA_DATA(rta), &rport, 2);
1450                         off += NLMSG_ALIGN(rta->rta_len);
1451                 }
1452
1453                 nest->rta_len = off - nest_start;
1454         }
1455
1456         do_nl_req(fd, nh, off, 0);
1457         return 0;
1458 }
1459
1460 int main(int argc, char *argv[])
1461 {
1462         int events_mcast_grp;
1463         int pm_family;
1464         int fd;
1465
1466         if (argc < 2)
1467                 syntax(argv);
1468
1469         fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
1470         if (fd == -1)
1471                 error(1, errno, "socket netlink");
1472
1473         resolve_mptcp_pm_netlink(fd, &pm_family, &events_mcast_grp);
1474
1475         if (!strcmp(argv[1], "add"))
1476                 return add_addr(fd, pm_family, argc, argv);
1477         else if (!strcmp(argv[1], "ann"))
1478                 return announce_addr(fd, pm_family, argc, argv);
1479         else if (!strcmp(argv[1], "rem"))
1480                 return remove_addr(fd, pm_family, argc, argv);
1481         else if (!strcmp(argv[1], "csf"))
1482                 return csf(fd, pm_family, argc, argv);
1483         else if (!strcmp(argv[1], "dsf"))
1484                 return dsf(fd, pm_family, argc, argv);
1485         else if (!strcmp(argv[1], "del"))
1486                 return del_addr(fd, pm_family, argc, argv);
1487         else if (!strcmp(argv[1], "flush"))
1488                 return flush_addrs(fd, pm_family, argc, argv);
1489         else if (!strcmp(argv[1], "get"))
1490                 return get_addr(fd, pm_family, argc, argv);
1491         else if (!strcmp(argv[1], "dump"))
1492                 return dump_addrs(fd, pm_family, argc, argv);
1493         else if (!strcmp(argv[1], "limits"))
1494                 return get_set_limits(fd, pm_family, argc, argv);
1495         else if (!strcmp(argv[1], "set"))
1496                 return set_flags(fd, pm_family, argc, argv);
1497         else if (!strcmp(argv[1], "events"))
1498                 return capture_events(fd, events_mcast_grp);
1499         else if (!strcmp(argv[1], "listen"))
1500                 return add_listener(argc, argv);
1501
1502         fprintf(stderr, "unknown sub-command: %s", argv[1]);
1503         syntax(argv);
1504         return 0;
1505 }