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 10 #include <sys/socket.h> 11 #include <sys/types.h> 12 13 #include <arpa/inet.h> 14 #include <net/if.h> 15 16 #include <linux/rtnetlink.h> 17 #include <linux/genetlink.h> 18 19 #include "linux/mptcp.h" 20 21 #ifndef MPTCP_PM_NAME 22 #define MPTCP_PM_NAME "mptcp_pm" 23 #endif 24 25 static void syntax(char *argv[]) 26 { 27 fprintf(stderr, "%s add|get|del|flush|dump|accept [<args>]\n", argv[0]); 28 fprintf(stderr, "\tadd [flags signal|subflow|backup] [id <nr>] [dev <name>] <ip>\n"); 29 fprintf(stderr, "\tdel <id>\n"); 30 fprintf(stderr, "\tget <id>\n"); 31 fprintf(stderr, "\tflush\n"); 32 fprintf(stderr, "\tdump\n"); 33 fprintf(stderr, "\tlimits [<rcv addr max> <subflow max>]\n"); 34 exit(0); 35 } 36 37 static int init_genl_req(char *data, int family, int cmd, int version) 38 { 39 struct nlmsghdr *nh = (void *)data; 40 struct genlmsghdr *gh; 41 int off = 0; 42 43 nh->nlmsg_type = family; 44 nh->nlmsg_flags = NLM_F_REQUEST; 45 nh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN); 46 off += NLMSG_ALIGN(sizeof(*nh)); 47 48 gh = (void *)(data + off); 49 gh->cmd = cmd; 50 gh->version = version; 51 off += NLMSG_ALIGN(sizeof(*gh)); 52 return off; 53 } 54 55 static void nl_error(struct nlmsghdr *nh) 56 { 57 struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh); 58 int len = nh->nlmsg_len - sizeof(*nh); 59 uint32_t off; 60 61 if (len < sizeof(struct nlmsgerr)) 62 error(1, 0, "netlink error message truncated %d min %ld", len, 63 sizeof(struct nlmsgerr)); 64 65 if (!err->error) { 66 /* check messages from kernel */ 67 struct rtattr *attrs = (struct rtattr *)NLMSG_DATA(nh); 68 69 while (RTA_OK(attrs, len)) { 70 if (attrs->rta_type == NLMSGERR_ATTR_MSG) 71 fprintf(stderr, "netlink ext ack msg: %s\n", 72 (char *)RTA_DATA(attrs)); 73 if (attrs->rta_type == NLMSGERR_ATTR_OFFS) { 74 memcpy(&off, RTA_DATA(attrs), 4); 75 fprintf(stderr, "netlink err off %d\n", 76 (int)off); 77 } 78 attrs = RTA_NEXT(attrs, len); 79 } 80 } else { 81 fprintf(stderr, "netlink error %d", err->error); 82 } 83 } 84 85 /* do a netlink command and, if max > 0, fetch the reply */ 86 static int do_nl_req(int fd, struct nlmsghdr *nh, int len, int max) 87 { 88 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; 89 socklen_t addr_len; 90 void *data = nh; 91 int rem, ret; 92 int err = 0; 93 94 nh->nlmsg_len = len; 95 ret = sendto(fd, data, len, 0, (void *)&nladdr, sizeof(nladdr)); 96 if (ret != len) 97 error(1, errno, "send netlink: %uB != %uB\n", ret, len); 98 if (max == 0) 99 return 0; 100 101 addr_len = sizeof(nladdr); 102 rem = ret = recvfrom(fd, data, max, 0, (void *)&nladdr, &addr_len); 103 if (ret < 0) 104 error(1, errno, "recv netlink: %uB\n", ret); 105 106 /* Beware: the NLMSG_NEXT macro updates the 'rem' argument */ 107 for (; NLMSG_OK(nh, rem); nh = NLMSG_NEXT(nh, rem)) { 108 if (nh->nlmsg_type == NLMSG_ERROR) { 109 nl_error(nh); 110 err = 1; 111 } 112 } 113 if (err) 114 error(1, 0, "bailing out due to netlink error[s]"); 115 return ret; 116 } 117 118 static int genl_parse_getfamily(struct nlmsghdr *nlh) 119 { 120 struct genlmsghdr *ghdr = NLMSG_DATA(nlh); 121 int len = nlh->nlmsg_len; 122 struct rtattr *attrs; 123 124 if (nlh->nlmsg_type != GENL_ID_CTRL) 125 error(1, errno, "Not a controller message, len=%d type=0x%x\n", 126 nlh->nlmsg_len, nlh->nlmsg_type); 127 128 len -= NLMSG_LENGTH(GENL_HDRLEN); 129 130 if (len < 0) 131 error(1, errno, "wrong controller message len %d\n", len); 132 133 if (ghdr->cmd != CTRL_CMD_NEWFAMILY) 134 error(1, errno, "Unknown controller command %d\n", ghdr->cmd); 135 136 attrs = (struct rtattr *) ((char *) ghdr + GENL_HDRLEN); 137 while (RTA_OK(attrs, len)) { 138 if (attrs->rta_type == CTRL_ATTR_FAMILY_ID) 139 return *(__u16 *)RTA_DATA(attrs); 140 attrs = RTA_NEXT(attrs, len); 141 } 142 143 error(1, errno, "can't find CTRL_ATTR_FAMILY_ID attr"); 144 return -1; 145 } 146 147 static int resolve_mptcp_pm_netlink(int fd) 148 { 149 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 150 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 151 1024]; 152 struct nlmsghdr *nh; 153 struct rtattr *rta; 154 int namelen; 155 int off = 0; 156 157 memset(data, 0, sizeof(data)); 158 nh = (void *)data; 159 off = init_genl_req(data, GENL_ID_CTRL, CTRL_CMD_GETFAMILY, 0); 160 161 rta = (void *)(data + off); 162 namelen = strlen(MPTCP_PM_NAME) + 1; 163 rta->rta_type = CTRL_ATTR_FAMILY_NAME; 164 rta->rta_len = RTA_LENGTH(namelen); 165 memcpy(RTA_DATA(rta), MPTCP_PM_NAME, namelen); 166 off += NLMSG_ALIGN(rta->rta_len); 167 168 do_nl_req(fd, nh, off, sizeof(data)); 169 return genl_parse_getfamily((void *)data); 170 } 171 172 int add_addr(int fd, int pm_family, int argc, char *argv[]) 173 { 174 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 175 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 176 1024]; 177 struct rtattr *rta, *nest; 178 struct nlmsghdr *nh; 179 u_int16_t family; 180 u_int32_t flags; 181 int nest_start; 182 u_int8_t id; 183 int off = 0; 184 int arg; 185 186 memset(data, 0, sizeof(data)); 187 nh = (void *)data; 188 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_ADD_ADDR, 189 MPTCP_PM_VER); 190 191 if (argc < 3) 192 syntax(argv); 193 194 nest_start = off; 195 nest = (void *)(data + off); 196 nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR; 197 nest->rta_len = RTA_LENGTH(0); 198 off += NLMSG_ALIGN(nest->rta_len); 199 200 /* addr data */ 201 rta = (void *)(data + off); 202 if (inet_pton(AF_INET, argv[2], RTA_DATA(rta))) { 203 family = AF_INET; 204 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR4; 205 rta->rta_len = RTA_LENGTH(4); 206 } else if (inet_pton(AF_INET6, argv[2], RTA_DATA(rta))) { 207 family = AF_INET6; 208 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6; 209 rta->rta_len = RTA_LENGTH(16); 210 } else 211 error(1, errno, "can't parse ip %s", argv[2]); 212 off += NLMSG_ALIGN(rta->rta_len); 213 214 /* family */ 215 rta = (void *)(data + off); 216 rta->rta_type = MPTCP_PM_ADDR_ATTR_FAMILY; 217 rta->rta_len = RTA_LENGTH(2); 218 memcpy(RTA_DATA(rta), &family, 2); 219 off += NLMSG_ALIGN(rta->rta_len); 220 221 for (arg = 3; arg < argc; arg++) { 222 if (!strcmp(argv[arg], "flags")) { 223 char *tok, *str; 224 225 /* flags */ 226 flags = 0; 227 if (++arg >= argc) 228 error(1, 0, " missing flags value"); 229 230 /* do not support flag list yet */ 231 for (str = argv[arg]; (tok = strtok(str, ",")); 232 str = NULL) { 233 if (!strcmp(tok, "subflow")) 234 flags |= MPTCP_PM_ADDR_FLAG_SUBFLOW; 235 else if (!strcmp(tok, "signal")) 236 flags |= MPTCP_PM_ADDR_FLAG_SIGNAL; 237 else if (!strcmp(tok, "backup")) 238 flags |= MPTCP_PM_ADDR_FLAG_BACKUP; 239 else 240 error(1, errno, 241 "unknown flag %s", argv[arg]); 242 } 243 244 rta = (void *)(data + off); 245 rta->rta_type = MPTCP_PM_ADDR_ATTR_FLAGS; 246 rta->rta_len = RTA_LENGTH(4); 247 memcpy(RTA_DATA(rta), &flags, 4); 248 off += NLMSG_ALIGN(rta->rta_len); 249 } else if (!strcmp(argv[arg], "id")) { 250 if (++arg >= argc) 251 error(1, 0, " missing id value"); 252 253 id = atoi(argv[arg]); 254 rta = (void *)(data + off); 255 rta->rta_type = MPTCP_PM_ADDR_ATTR_ID; 256 rta->rta_len = RTA_LENGTH(1); 257 memcpy(RTA_DATA(rta), &id, 1); 258 off += NLMSG_ALIGN(rta->rta_len); 259 } else if (!strcmp(argv[arg], "dev")) { 260 int32_t ifindex; 261 262 if (++arg >= argc) 263 error(1, 0, " missing dev name"); 264 265 ifindex = if_nametoindex(argv[arg]); 266 if (!ifindex) 267 error(1, errno, "unknown device %s", argv[arg]); 268 269 rta = (void *)(data + off); 270 rta->rta_type = MPTCP_PM_ADDR_ATTR_IF_IDX; 271 rta->rta_len = RTA_LENGTH(4); 272 memcpy(RTA_DATA(rta), &ifindex, 4); 273 off += NLMSG_ALIGN(rta->rta_len); 274 } else 275 error(1, 0, "unknown keyword %s", argv[arg]); 276 } 277 nest->rta_len = off - nest_start; 278 279 do_nl_req(fd, nh, off, 0); 280 return 0; 281 } 282 283 int del_addr(int fd, int pm_family, int argc, char *argv[]) 284 { 285 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 286 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 287 1024]; 288 struct rtattr *rta, *nest; 289 struct nlmsghdr *nh; 290 int nest_start; 291 u_int8_t id; 292 int off = 0; 293 294 memset(data, 0, sizeof(data)); 295 nh = (void *)data; 296 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_DEL_ADDR, 297 MPTCP_PM_VER); 298 299 /* the only argument is the address id */ 300 if (argc != 3) 301 syntax(argv); 302 303 id = atoi(argv[2]); 304 305 nest_start = off; 306 nest = (void *)(data + off); 307 nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR; 308 nest->rta_len = RTA_LENGTH(0); 309 off += NLMSG_ALIGN(nest->rta_len); 310 311 /* build a dummy addr with only the ID set */ 312 rta = (void *)(data + off); 313 rta->rta_type = MPTCP_PM_ADDR_ATTR_ID; 314 rta->rta_len = RTA_LENGTH(1); 315 memcpy(RTA_DATA(rta), &id, 1); 316 off += NLMSG_ALIGN(rta->rta_len); 317 nest->rta_len = off - nest_start; 318 319 do_nl_req(fd, nh, off, 0); 320 return 0; 321 } 322 323 static void print_addr(struct rtattr *attrs, int len) 324 { 325 uint16_t family = 0; 326 char str[1024]; 327 uint32_t flags; 328 uint8_t id; 329 330 while (RTA_OK(attrs, len)) { 331 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_FAMILY) 332 memcpy(&family, RTA_DATA(attrs), 2); 333 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ADDR4) { 334 if (family != AF_INET) 335 error(1, errno, "wrong IP (v4) for family %d", 336 family); 337 inet_ntop(AF_INET, RTA_DATA(attrs), str, sizeof(str)); 338 printf("%s", str); 339 } 340 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ADDR6) { 341 if (family != AF_INET6) 342 error(1, errno, "wrong IP (v6) for family %d", 343 family); 344 inet_ntop(AF_INET6, RTA_DATA(attrs), str, sizeof(str)); 345 printf("%s", str); 346 } 347 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ID) { 348 memcpy(&id, RTA_DATA(attrs), 1); 349 printf("id %d ", id); 350 } 351 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_FLAGS) { 352 memcpy(&flags, RTA_DATA(attrs), 4); 353 354 printf("flags "); 355 if (flags & MPTCP_PM_ADDR_FLAG_SIGNAL) { 356 printf("signal"); 357 flags &= ~MPTCP_PM_ADDR_FLAG_SIGNAL; 358 if (flags) 359 printf(","); 360 } 361 362 if (flags & MPTCP_PM_ADDR_FLAG_SUBFLOW) { 363 printf("subflow"); 364 flags &= ~MPTCP_PM_ADDR_FLAG_SUBFLOW; 365 if (flags) 366 printf(","); 367 } 368 369 if (flags & MPTCP_PM_ADDR_FLAG_BACKUP) { 370 printf("backup"); 371 flags &= ~MPTCP_PM_ADDR_FLAG_BACKUP; 372 if (flags) 373 printf(","); 374 } 375 376 /* bump unknown flags, if any */ 377 if (flags) 378 printf("0x%x", flags); 379 printf(" "); 380 } 381 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_IF_IDX) { 382 char name[IF_NAMESIZE], *ret; 383 int32_t ifindex; 384 385 memcpy(&ifindex, RTA_DATA(attrs), 4); 386 ret = if_indextoname(ifindex, name); 387 if (ret) 388 printf("dev %s ", ret); 389 else 390 printf("dev unknown/%d", ifindex); 391 } 392 393 attrs = RTA_NEXT(attrs, len); 394 } 395 printf("\n"); 396 } 397 398 static void print_addrs(struct nlmsghdr *nh, int pm_family, int total_len) 399 { 400 struct rtattr *attrs; 401 402 for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) { 403 int len = nh->nlmsg_len; 404 405 if (nh->nlmsg_type == NLMSG_DONE) 406 break; 407 if (nh->nlmsg_type == NLMSG_ERROR) 408 nl_error(nh); 409 if (nh->nlmsg_type != pm_family) 410 continue; 411 412 len -= NLMSG_LENGTH(GENL_HDRLEN); 413 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) + 414 GENL_HDRLEN); 415 while (RTA_OK(attrs, len)) { 416 if (attrs->rta_type == 417 (MPTCP_PM_ATTR_ADDR | NLA_F_NESTED)) 418 print_addr((void *)RTA_DATA(attrs), 419 attrs->rta_len); 420 attrs = RTA_NEXT(attrs, len); 421 } 422 } 423 } 424 425 int get_addr(int fd, int pm_family, int argc, char *argv[]) 426 { 427 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 428 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 429 1024]; 430 struct rtattr *rta, *nest; 431 struct nlmsghdr *nh; 432 int nest_start; 433 u_int8_t id; 434 int off = 0; 435 436 memset(data, 0, sizeof(data)); 437 nh = (void *)data; 438 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR, 439 MPTCP_PM_VER); 440 441 /* the only argument is the address id */ 442 if (argc != 3) 443 syntax(argv); 444 445 id = atoi(argv[2]); 446 447 nest_start = off; 448 nest = (void *)(data + off); 449 nest->rta_type = NLA_F_NESTED | MPTCP_PM_ATTR_ADDR; 450 nest->rta_len = RTA_LENGTH(0); 451 off += NLMSG_ALIGN(nest->rta_len); 452 453 /* build a dummy addr with only the ID set */ 454 rta = (void *)(data + off); 455 rta->rta_type = MPTCP_PM_ADDR_ATTR_ID; 456 rta->rta_len = RTA_LENGTH(1); 457 memcpy(RTA_DATA(rta), &id, 1); 458 off += NLMSG_ALIGN(rta->rta_len); 459 nest->rta_len = off - nest_start; 460 461 print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data))); 462 return 0; 463 } 464 465 int dump_addrs(int fd, int pm_family, int argc, char *argv[]) 466 { 467 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 468 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 469 1024]; 470 pid_t pid = getpid(); 471 struct nlmsghdr *nh; 472 int off = 0; 473 474 memset(data, 0, sizeof(data)); 475 nh = (void *)data; 476 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR, 477 MPTCP_PM_VER); 478 nh->nlmsg_flags |= NLM_F_DUMP; 479 nh->nlmsg_seq = 1; 480 nh->nlmsg_pid = pid; 481 nh->nlmsg_len = off; 482 483 print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data))); 484 return 0; 485 } 486 487 int flush_addrs(int fd, int pm_family, int argc, char *argv[]) 488 { 489 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 490 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 491 1024]; 492 struct nlmsghdr *nh; 493 int off = 0; 494 495 memset(data, 0, sizeof(data)); 496 nh = (void *)data; 497 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_FLUSH_ADDRS, 498 MPTCP_PM_VER); 499 500 do_nl_req(fd, nh, off, 0); 501 return 0; 502 } 503 504 static void print_limits(struct nlmsghdr *nh, int pm_family, int total_len) 505 { 506 struct rtattr *attrs; 507 uint32_t max; 508 509 for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) { 510 int len = nh->nlmsg_len; 511 512 if (nh->nlmsg_type == NLMSG_DONE) 513 break; 514 if (nh->nlmsg_type == NLMSG_ERROR) 515 nl_error(nh); 516 if (nh->nlmsg_type != pm_family) 517 continue; 518 519 len -= NLMSG_LENGTH(GENL_HDRLEN); 520 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) + 521 GENL_HDRLEN); 522 while (RTA_OK(attrs, len)) { 523 int type = attrs->rta_type; 524 525 if (type != MPTCP_PM_ATTR_RCV_ADD_ADDRS && 526 type != MPTCP_PM_ATTR_SUBFLOWS) 527 goto next; 528 529 memcpy(&max, RTA_DATA(attrs), 4); 530 printf("%s %u\n", type == MPTCP_PM_ATTR_SUBFLOWS ? 531 "subflows" : "accept", max); 532 533 next: 534 attrs = RTA_NEXT(attrs, len); 535 } 536 } 537 } 538 539 int get_set_limits(int fd, int pm_family, int argc, char *argv[]) 540 { 541 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) + 542 NLMSG_ALIGN(sizeof(struct genlmsghdr)) + 543 1024]; 544 uint32_t rcv_addr = 0, subflows = 0; 545 int cmd, len = sizeof(data); 546 struct nlmsghdr *nh; 547 int off = 0; 548 549 /* limit */ 550 if (argc == 4) { 551 rcv_addr = atoi(argv[2]); 552 subflows = atoi(argv[3]); 553 cmd = MPTCP_PM_CMD_SET_LIMITS; 554 } else { 555 cmd = MPTCP_PM_CMD_GET_LIMITS; 556 } 557 558 memset(data, 0, sizeof(data)); 559 nh = (void *)data; 560 off = init_genl_req(data, pm_family, cmd, MPTCP_PM_VER); 561 562 /* limit */ 563 if (cmd == MPTCP_PM_CMD_SET_LIMITS) { 564 struct rtattr *rta = (void *)(data + off); 565 566 rta->rta_type = MPTCP_PM_ATTR_RCV_ADD_ADDRS; 567 rta->rta_len = RTA_LENGTH(4); 568 memcpy(RTA_DATA(rta), &rcv_addr, 4); 569 off += NLMSG_ALIGN(rta->rta_len); 570 571 rta = (void *)(data + off); 572 rta->rta_type = MPTCP_PM_ATTR_SUBFLOWS; 573 rta->rta_len = RTA_LENGTH(4); 574 memcpy(RTA_DATA(rta), &subflows, 4); 575 off += NLMSG_ALIGN(rta->rta_len); 576 577 /* do not expect a reply */ 578 len = 0; 579 } 580 581 len = do_nl_req(fd, nh, off, len); 582 if (cmd == MPTCP_PM_CMD_GET_LIMITS) 583 print_limits(nh, pm_family, len); 584 return 0; 585 } 586 587 int main(int argc, char *argv[]) 588 { 589 int fd, pm_family; 590 591 if (argc < 2) 592 syntax(argv); 593 594 fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC); 595 if (fd == -1) 596 error(1, errno, "socket netlink"); 597 598 pm_family = resolve_mptcp_pm_netlink(fd); 599 600 if (!strcmp(argv[1], "add")) 601 return add_addr(fd, pm_family, argc, argv); 602 else if (!strcmp(argv[1], "del")) 603 return del_addr(fd, pm_family, argc, argv); 604 else if (!strcmp(argv[1], "flush")) 605 return flush_addrs(fd, pm_family, argc, argv); 606 else if (!strcmp(argv[1], "get")) 607 return get_addr(fd, pm_family, argc, argv); 608 else if (!strcmp(argv[1], "dump")) 609 return dump_addrs(fd, pm_family, argc, argv); 610 else if (!strcmp(argv[1], "limits")) 611 return get_set_limits(fd, pm_family, argc, argv); 612 613 fprintf(stderr, "unknown sub-command: %s", argv[1]); 614 syntax(argv); 615 return 0; 616 } 617