pm_nl_ctl.c

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