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