1 // SPDX-License-Identifier: GPL-2.0
10 #include <sys/socket.h>
11 #include <sys/types.h>
13 #include <arpa/inet.h>
16 #include <linux/rtnetlink.h>
17 #include <linux/genetlink.h>
19 #include "linux/mptcp.h"
22 #define MPTCP_PM_NAME "mptcp_pm"
25 static void syntax(char *argv[])
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");
37 static int init_genl_req(char *data, int family, int cmd, int version)
39 struct nlmsghdr *nh = (void *)data;
40 struct genlmsghdr *gh;
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));
48 gh = (void *)(data + off);
50 gh->version = version;
51 off += NLMSG_ALIGN(sizeof(*gh));
55 static void nl_error(struct nlmsghdr *nh)
57 struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh);
58 int len = nh->nlmsg_len - sizeof(*nh);
61 if (len < sizeof(struct nlmsgerr))
62 error(1, 0, "netlink error message truncated %d min %ld", len,
63 sizeof(struct nlmsgerr));
66 /* check messages from kernel */
67 struct rtattr *attrs = (struct rtattr *)NLMSG_DATA(nh);
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",
78 attrs = RTA_NEXT(attrs, len);
81 fprintf(stderr, "netlink error %d", err->error);
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)
88 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK };
95 ret = sendto(fd, data, len, 0, (void *)&nladdr, sizeof(nladdr));
97 error(1, errno, "send netlink: %uB != %uB\n", ret, len);
101 addr_len = sizeof(nladdr);
102 rem = ret = recvfrom(fd, data, max, 0, (void *)&nladdr, &addr_len);
104 error(1, errno, "recv netlink: %uB\n", ret);
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) {
114 error(1, 0, "bailing out due to netlink error[s]");
118 static int genl_parse_getfamily(struct nlmsghdr *nlh)
120 struct genlmsghdr *ghdr = NLMSG_DATA(nlh);
121 int len = nlh->nlmsg_len;
122 struct rtattr *attrs;
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);
128 len -= NLMSG_LENGTH(GENL_HDRLEN);
131 error(1, errno, "wrong controller message len %d\n", len);
133 if (ghdr->cmd != CTRL_CMD_NEWFAMILY)
134 error(1, errno, "Unknown controller command %d\n", ghdr->cmd);
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);
143 error(1, errno, "can't find CTRL_ATTR_FAMILY_ID attr");
147 static int resolve_mptcp_pm_netlink(int fd)
149 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
150 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
157 memset(data, 0, sizeof(data));
159 off = init_genl_req(data, GENL_ID_CTRL, CTRL_CMD_GETFAMILY, 0);
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);
168 do_nl_req(fd, nh, off, sizeof(data));
169 return genl_parse_getfamily((void *)data);
172 int add_addr(int fd, int pm_family, int argc, char *argv[])
174 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
175 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
177 struct rtattr *rta, *nest;
186 memset(data, 0, sizeof(data));
188 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_ADD_ADDR,
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);
201 rta = (void *)(data + off);
202 if (inet_pton(AF_INET, argv[2], RTA_DATA(rta))) {
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))) {
208 rta->rta_type = MPTCP_PM_ADDR_ATTR_ADDR6;
209 rta->rta_len = RTA_LENGTH(16);
211 error(1, errno, "can't parse ip %s", argv[2]);
212 off += NLMSG_ALIGN(rta->rta_len);
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);
221 for (arg = 3; arg < argc; arg++) {
222 if (!strcmp(argv[arg], "flags")) {
228 error(1, 0, " missing flags value");
230 /* do not support flag list yet */
231 for (str = argv[arg]; (tok = strtok(str, ","));
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;
241 "unknown flag %s", argv[arg]);
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")) {
251 error(1, 0, " missing id value");
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")) {
263 error(1, 0, " missing dev name");
265 ifindex = if_nametoindex(argv[arg]);
267 error(1, errno, "unknown device %s", argv[arg]);
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);
275 error(1, 0, "unknown keyword %s", argv[arg]);
277 nest->rta_len = off - nest_start;
279 do_nl_req(fd, nh, off, 0);
283 int del_addr(int fd, int pm_family, int argc, char *argv[])
285 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
286 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
288 struct rtattr *rta, *nest;
294 memset(data, 0, sizeof(data));
296 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_DEL_ADDR,
299 /* the only argument is the address id */
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);
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;
319 do_nl_req(fd, nh, off, 0);
323 static void print_addr(struct rtattr *attrs, int len)
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",
337 inet_ntop(AF_INET, RTA_DATA(attrs), str, sizeof(str));
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",
344 inet_ntop(AF_INET6, RTA_DATA(attrs), str, sizeof(str));
347 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_ID) {
348 memcpy(&id, RTA_DATA(attrs), 1);
349 printf("id %d ", id);
351 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_FLAGS) {
352 memcpy(&flags, RTA_DATA(attrs), 4);
355 if (flags & MPTCP_PM_ADDR_FLAG_SIGNAL) {
357 flags &= ~MPTCP_PM_ADDR_FLAG_SIGNAL;
362 if (flags & MPTCP_PM_ADDR_FLAG_SUBFLOW) {
364 flags &= ~MPTCP_PM_ADDR_FLAG_SUBFLOW;
369 if (flags & MPTCP_PM_ADDR_FLAG_BACKUP) {
371 flags &= ~MPTCP_PM_ADDR_FLAG_BACKUP;
376 /* bump unknown flags, if any */
378 printf("0x%x", flags);
381 if (attrs->rta_type == MPTCP_PM_ADDR_ATTR_IF_IDX) {
382 char name[IF_NAMESIZE], *ret;
385 memcpy(&ifindex, RTA_DATA(attrs), 4);
386 ret = if_indextoname(ifindex, name);
388 printf("dev %s ", ret);
390 printf("dev unknown/%d", ifindex);
393 attrs = RTA_NEXT(attrs, len);
398 static void print_addrs(struct nlmsghdr *nh, int pm_family, int total_len)
400 struct rtattr *attrs;
402 for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) {
403 int len = nh->nlmsg_len;
405 if (nh->nlmsg_type == NLMSG_DONE)
407 if (nh->nlmsg_type == NLMSG_ERROR)
409 if (nh->nlmsg_type != pm_family)
412 len -= NLMSG_LENGTH(GENL_HDRLEN);
413 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) +
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),
420 attrs = RTA_NEXT(attrs, len);
425 int get_addr(int fd, int pm_family, int argc, char *argv[])
427 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
428 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
430 struct rtattr *rta, *nest;
436 memset(data, 0, sizeof(data));
438 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR,
441 /* the only argument is the address id */
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);
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;
461 print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data)));
465 int dump_addrs(int fd, int pm_family, int argc, char *argv[])
467 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
468 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
470 pid_t pid = getpid();
474 memset(data, 0, sizeof(data));
476 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_GET_ADDR,
478 nh->nlmsg_flags |= NLM_F_DUMP;
483 print_addrs(nh, pm_family, do_nl_req(fd, nh, off, sizeof(data)));
487 int flush_addrs(int fd, int pm_family, int argc, char *argv[])
489 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
490 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
495 memset(data, 0, sizeof(data));
497 off = init_genl_req(data, pm_family, MPTCP_PM_CMD_FLUSH_ADDRS,
500 do_nl_req(fd, nh, off, 0);
504 static void print_limits(struct nlmsghdr *nh, int pm_family, int total_len)
506 struct rtattr *attrs;
509 for (; NLMSG_OK(nh, total_len); nh = NLMSG_NEXT(nh, total_len)) {
510 int len = nh->nlmsg_len;
512 if (nh->nlmsg_type == NLMSG_DONE)
514 if (nh->nlmsg_type == NLMSG_ERROR)
516 if (nh->nlmsg_type != pm_family)
519 len -= NLMSG_LENGTH(GENL_HDRLEN);
520 attrs = (struct rtattr *) ((char *) NLMSG_DATA(nh) +
522 while (RTA_OK(attrs, len)) {
523 int type = attrs->rta_type;
525 if (type != MPTCP_PM_ATTR_RCV_ADD_ADDRS &&
526 type != MPTCP_PM_ATTR_SUBFLOWS)
529 memcpy(&max, RTA_DATA(attrs), 4);
530 printf("%s %u\n", type == MPTCP_PM_ATTR_SUBFLOWS ?
531 "subflows" : "accept", max);
534 attrs = RTA_NEXT(attrs, len);
539 int get_set_limits(int fd, int pm_family, int argc, char *argv[])
541 char data[NLMSG_ALIGN(sizeof(struct nlmsghdr)) +
542 NLMSG_ALIGN(sizeof(struct genlmsghdr)) +
544 uint32_t rcv_addr = 0, subflows = 0;
545 int cmd, len = sizeof(data);
551 rcv_addr = atoi(argv[2]);
552 subflows = atoi(argv[3]);
553 cmd = MPTCP_PM_CMD_SET_LIMITS;
555 cmd = MPTCP_PM_CMD_GET_LIMITS;
558 memset(data, 0, sizeof(data));
560 off = init_genl_req(data, pm_family, cmd, MPTCP_PM_VER);
563 if (cmd == MPTCP_PM_CMD_SET_LIMITS) {
564 struct rtattr *rta = (void *)(data + off);
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);
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);
577 /* do not expect a reply */
581 len = do_nl_req(fd, nh, off, len);
582 if (cmd == MPTCP_PM_CMD_GET_LIMITS)
583 print_limits(nh, pm_family, len);
587 int main(int argc, char *argv[])
594 fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
596 error(1, errno, "socket netlink");
598 pm_family = resolve_mptcp_pm_netlink(fd);
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);
613 fprintf(stderr, "unknown sub-command: %s", argv[1]);