[thirdparty/iw.git] / nan.c

#include <net/if.h>
#include <errno.h>
#include <string.h>

#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include <netlink/msg.h>
#include <netlink/attr.h>

#include <ctype.h>
#include <inttypes.h>
#include "nl80211.h"
#include "iw.h"
#include "sha256.h"

SECTION(nan);

static int parse_bands(int argc, char **argv)
{
	int i = 0, bands = 0;

	for (i = 0; i < argc; i++) {
		if (!strcasecmp("2ghz", argv[i]))
			bands |= BIT(NL80211_BAND_2GHZ);
		else if (!strcasecmp("5ghz", argv[i]))
			bands |= BIT(NL80211_BAND_5GHZ);
		else
			return -EINVAL;
	}

	return bands;
}

static int handle_nan_start(struct nl80211_state *state,
			    struct nl_msg *msg, int argc, char **argv,
			    enum id_input id)
{
	int bands = 0;

	if (argc < 2)
		return -EINVAL;

	if (strcmp(argv[0], "pref") == 0) {
		argv++;
		argc--;
		NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
		argv++;
		argc--;
	} else {
		/* Master preference is mandatory */
		return -EINVAL;
	}

	if (argc > 1 && !strcmp(argv[0], "bands")) {
		argv++;
		argc--;

		bands = parse_bands(argc, argv);
		if (bands < 0)
			return bands;

		NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
	} else if (argc != 0)
		return -EINVAL;

	return 0;
nla_put_failure:
	return -ENOBUFS;
}
COMMAND(nan, start, "pref <pref> [bands [2GHz] [5GHz]]",
	NL80211_CMD_START_NAN, 0, CIB_WDEV, handle_nan_start, "");

static int handle_nan_stop(struct nl80211_state *state,
			   struct nl_msg *msg, int argc, char **argv,
			   enum id_input id)
{
	return 0;
}
COMMAND(nan, stop, "", NL80211_CMD_STOP_NAN, 0, CIB_WDEV, handle_nan_stop, "");

static int handle_nan_config(struct nl80211_state *state,
			     struct nl_msg *msg, int argc, char **argv,
			     enum id_input id)
{
	int bands = 0;

	if (argc < 2)
		return -EINVAL;

	if (strcmp(argv[0], "pref") == 0) {
		argv++;
		argc--;
		NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
		argv++;
		argc--;
	}

	if (argc > 1 && !strcmp(argv[0], "bands")) {
		argv++;
		argc--;

		bands = parse_bands(argc, argv);
		if (bands < 0)
			return bands;

		NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
		argv++;
		argc--;
	} else if (argc != 0)
		return -EINVAL;

	return 0;
nla_put_failure:
	return -ENOBUFS;
}
COMMAND(nan, config, "[pref <pref>] [bands [2GHz] [5GHz]]",
	NL80211_CMD_CHANGE_NAN_CONFIG, 0, CIB_WDEV, handle_nan_config, "");

static int handle_nan_rm_func(struct nl80211_state *state,
			      struct nl_msg *msg, int argc, char **argv,
			      enum id_input id)
{
	if (argc != 2)
		return -EINVAL;

	if (strcmp(argv[0], "cookie") == 0) {
		argv++;
		argc--;
		NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, atoi(argv[0]));
		argv++;
		argc--;
	}

	if (argc != 0)
		return -EINVAL;

	return 0;
nla_put_failure:
	return -ENOBUFS;
}
COMMAND(nan, rm_func, "cookie <cookie>", NL80211_CMD_DEL_NAN_FUNCTION, 0,
	CIB_WDEV, handle_nan_rm_func, "");

static int compute_service_id(const unsigned char *serv_name,
			      unsigned int len, unsigned char *res)
{
	size_t size = len;
	unsigned char md_value[32];
	int retcode = sha256(serv_name, size, md_value);

	if (retcode)
		return retcode;
	memcpy(res, md_value, 6);

	return 0;
}

static int print_instance_id_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct nlattr *func[NL80211_NAN_FUNC_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);

	if (!tb[NL80211_ATTR_COOKIE]) {
		fprintf(stderr, "cookie is missing!\n");
		return NL_SKIP;
	}

	nla_parse_nested(func, NL80211_NAN_FUNC_ATTR_MAX,
			 tb[NL80211_ATTR_NAN_FUNC],
			 NULL);
	if (!func[NL80211_NAN_FUNC_INSTANCE_ID]) {
		fprintf(stderr, "instance id is missing!\n");
		return NL_SKIP;
	}

	printf("instance_id: %d, cookie: %" PRIu64 "\n",
	       nla_get_u8(func[NL80211_NAN_FUNC_INSTANCE_ID]),
	       nla_get_u64(tb[NL80211_ATTR_COOKIE]));

	return NL_SKIP;
}

static int parse_srf(char **argv, int argc, struct nl_msg *func_attrs)
{
	struct nl_msg *srf_attrs;
	int old_argc = argc;
	unsigned char mac_addr[ETH_ALEN];
	char *cur_mac, *sptr = NULL;

	srf_attrs = nlmsg_alloc();
	if (strcmp(argv[0], "include") == 0)
		NLA_PUT_FLAG(srf_attrs, NL80211_NAN_SRF_INCLUDE);
	else if (strcmp(argv[0], "exclude") != 0)
		return -EINVAL;

	argc--;
	argv++;
	if (strcmp(argv[0], "bf") == 0) {
		unsigned char *srf;
		size_t srf_len;
		__u8 bf_idx;
		int err;

		argc--;
		argv++;

		if (argc < 3)
			return -EINVAL;

		bf_idx = atoi(argv[0]);
		NLA_PUT_U8(srf_attrs, NL80211_NAN_SRF_BF_IDX, bf_idx);

		argc--;
		argv++;
		srf_len = atoi(argv[0]);
		if (srf_len == 0 || srf_len > NL80211_NAN_FUNC_SRF_MAX_LEN)
			return -EINVAL;

		argc--;
		argv++;
		srf = malloc(srf_len);
		if (!srf)
			return -ENOBUFS;

		memset(srf, 0, srf_len);
		cur_mac = strtok_r(argv[0], ";", &sptr);
		while (cur_mac) {
			if (mac_addr_a2n(mac_addr, cur_mac)) {
				printf("mac format error %s\n", cur_mac);
				free(srf);
				return -EINVAL;
			}

			nan_bf(bf_idx, srf, srf_len, mac_addr, ETH_ALEN);
			cur_mac = strtok_r(NULL, ";", &sptr);
		}

		err = nla_put(srf_attrs, NL80211_NAN_SRF_BF, srf_len, srf);
		free(srf);
		if (err)
			goto nla_put_failure;
		argv++;
		argc--;
	} else if  (strcmp(argv[0], "list") == 0) {
		struct nlattr *nl_macs = nla_nest_start(srf_attrs,
						NL80211_NAN_SRF_MAC_ADDRS);
		int i = 0;

		argc--;
		argv++;
		cur_mac = strtok_r(argv[0], ";", &sptr);
		while (cur_mac) {
			if (mac_addr_a2n(mac_addr, cur_mac))
				return -EINVAL;

			nla_put(srf_attrs, ++i, ETH_ALEN, mac_addr);
			cur_mac = strtok_r(NULL, ";", &sptr);
		}

		nla_nest_end(srf_attrs, nl_macs);
		argv++;
		argc--;
	} else {
		return -EINVAL;
	}

	nla_put_nested(func_attrs, NL80211_NAN_FUNC_SRF, srf_attrs);
	return old_argc - argc;
nla_put_failure:
	return -ENOBUFS;
}

static void parse_match_filter(char *filter, struct nl_msg *func_attrs, int tx)
{
	struct nlattr *nl_filt;
	char *cur_filt, *sptr = NULL;
	int i = 0;

	if (tx)
		nl_filt = nla_nest_start(func_attrs,
					 NL80211_NAN_FUNC_TX_MATCH_FILTER);
	else
		nl_filt = nla_nest_start(func_attrs,
					 NL80211_NAN_FUNC_RX_MATCH_FILTER);

	cur_filt = strtok_r(filter, ":", &sptr);
	while (cur_filt) {
		if (strcmp(cur_filt, "*") != 0)
			nla_put(func_attrs, ++i, strlen(cur_filt), cur_filt);
		else
			nla_put(func_attrs, ++i, 0, NULL);

		cur_filt = strtok_r(NULL, ":", &sptr);
	}

	nla_nest_end(func_attrs, nl_filt);
}

static int handle_nan_add_func(struct nl80211_state *state,
			       struct nl_msg *msg, int argc, char **argv,
			       enum id_input id)
{
	struct nl_msg *func_attrs = NULL;
	int err = 0;
	__u8 type;

	func_attrs = nlmsg_alloc();
	if (!func_attrs) {
		err = -ENOBUFS;
		goto out;
	}

	if (argc > 1 && strcmp(argv[0], "type") == 0) {
		argv++;
		argc--;
		if (strcmp(argv[0], "publish") == 0)
			type = NL80211_NAN_FUNC_PUBLISH;
		else if (strcmp(argv[0], "subscribe") == 0)
			type = NL80211_NAN_FUNC_SUBSCRIBE;
		else if (strcmp(argv[0], "followup") == 0)
			type = NL80211_NAN_FUNC_FOLLOW_UP;
		else
			return -EINVAL;
		argv++;
		argc--;

		NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_TYPE, type);
	} else {
		return -EINVAL;
	}

	if (type == NL80211_NAN_FUNC_SUBSCRIBE) {
		if (argc > 1 && strcmp(argv[0], "active") == 0) {
			argv++;
			argc--;
			NLA_PUT_FLAG(func_attrs,
				     NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE);
		}
	}

	if (type == NL80211_NAN_FUNC_PUBLISH) {
		__u8 publish_type = 0;

		if (argc > 1 && strcmp(argv[0], "solicited") == 0) {
			argv++;
			argc--;
			publish_type |= NL80211_NAN_SOLICITED_PUBLISH;
		}

		if (argc > 1 && strcmp(argv[0], "unsolicited") == 0) {
			argv++;
			argc--;
			publish_type |= NL80211_NAN_UNSOLICITED_PUBLISH;
		}

		NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_PUBLISH_TYPE,
			   publish_type);

		/* only allow for solicited publish */
		if (argc > 1 && strcmp(argv[0], "bcast") == 0) {
			argv++;
			argc--;
			if (!(publish_type & NL80211_NAN_SOLICITED_PUBLISH))
				return -EINVAL;

			NLA_PUT_FLAG(func_attrs,
				     NL80211_NAN_FUNC_PUBLISH_BCAST);
		}
	}

	if (argc > 1 && strcmp(argv[0], "close_range") == 0) {
		argv++;
		argc--;
		NLA_PUT_FLAG(func_attrs, NL80211_NAN_FUNC_CLOSE_RANGE);
	}

	if (argc > 1 && strcmp(argv[0], "name") == 0) {
		unsigned char serv_id_c[6] = {0};
		__u64 service_id;

		argv++;
		argc--;
		compute_service_id((const unsigned char *)argv[0],
				   strlen(argv[0]), serv_id_c);
		service_id = (__u64)serv_id_c[0] << 0  |
			     (__u64)serv_id_c[1] << 8  |
			     (__u64)serv_id_c[2] << 16 |
			     (__u64)serv_id_c[3] << 24 |
			     (__u64)serv_id_c[4] << 32 |
			     (__u64)serv_id_c[5] << 40;

		NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_ID, 6,
			&service_id);
		argv++;
		argc--;
	} else {
		return -EINVAL;
	}

	if (argc > 1 && strcmp(argv[0], "info") == 0) {
		argv++;
		argc--;
		NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_INFO,
			strlen(argv[0]), argv[0]);
		argv++;
		argc--;
	}

	if (type == NL80211_NAN_FUNC_FOLLOW_UP) {
		if (argc > 1 && strcmp(argv[0], "flw_up_id") == 0) {
			argv++;
			argc--;
			NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_ID,
				   atoi(argv[0]));
			argv++;
			argc--;
		}

		if (argc > 1 && strcmp(argv[0], "flw_up_req_id") == 0) {
			argv++;
			argc--;
			NLA_PUT_U8(func_attrs,
				   NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID,
				   atoi(argv[0]));
			argv++;
			argc--;
		}

		if (argc > 1 && strcmp(argv[0], "flw_up_dest") == 0) {
			unsigned char addr[6];

			argv++;
			argc--;
			if (mac_addr_a2n(addr, argv[0]))
				goto nla_put_failure;
			nla_put(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_DEST,
				ETH_ALEN, addr);
			argv++;
			argc--;
		}
	}

	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
	    argc > 1 && strcmp(argv[0], "ttl") == 0) {
		argv++;
		argc--;
		NLA_PUT_U32(func_attrs, NL80211_NAN_FUNC_TTL, atoi(argv[0]));
		argv++;
		argc--;
	}

	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
	    argc >= 4 && strcmp(argv[0], "srf") == 0) {
		int res;

		argv++;
		argc--;
		res = parse_srf(argv, argc, func_attrs);
		if (res < 0)
			return -EINVAL;

		argc -= res;
		argv += res;
	}

	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
	    argc > 1 && strcmp(argv[0], "rx_filter") == 0) {
		argv++;
		argc--;
		parse_match_filter(argv[0], func_attrs, 0);

		argv++;
		argc--;
	}

	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
	    argc > 1 && strcmp(argv[0], "tx_filter") == 0) {
		argv++;
		argc--;
		parse_match_filter(argv[0], func_attrs, 1);

		argv++;
		argc--;
	}

	if (argc != 0)
		return -EINVAL;

	nla_put_nested(msg, NL80211_ATTR_NAN_FUNC, func_attrs);
	register_handler(print_instance_id_handler, NULL);

	return err;
nla_put_failure:
	return -ENOBUFS;
out:
	return err;
}
COMMAND(nan, add_func,
	"type <publish|subscribe|followup> [active] [solicited] [unsolicited] [bcast] [close_range] name <name> [info <info>] [flw_up_id <id> flw_up_req_id <id> flw_up_dest <mac>] [ttl <ttl>] [srf <include|exclude> <bf|list> [bf_idx] [bf_len] <mac1;mac2...>] [rx_filter <str1:str2...>] [tx_filter <str1:str2...>]",
	NL80211_CMD_ADD_NAN_FUNCTION, 0, CIB_WDEV,
	handle_nan_add_func, "");
Commit	Line	Data
63d6aac5 RZ	1	#include <net/if.h>
	2	#include <errno.h>
	3	#include <string.h>
	4
	5	#include <netlink/genl/genl.h>
	6	#include <netlink/genl/family.h>
	7	#include <netlink/genl/ctrl.h>
	8	#include <netlink/msg.h>
	9	#include <netlink/attr.h>
	10
63d6aac5 RZ	11	#include <ctype.h>
	12	#include <inttypes.h>
	13	#include "nl80211.h"
	14	#include "iw.h"
	15	#include "sha256.h"
	16
	17	SECTION(nan);
	18
	19	static int parse_bands(int argc, char **argv)
	20	{
	21	int i = 0, bands = 0;
	22
	23	for (i = 0; i < argc; i++) {
	24	if (!strcasecmp("2ghz", argv[i]))
	25	bands \|= BIT(NL80211_BAND_2GHZ);
	26	else if (!strcasecmp("5ghz", argv[i]))
	27	bands \|= BIT(NL80211_BAND_5GHZ);
	28	else
	29	return -EINVAL;
	30	}
	31
	32	return bands;
	33	}
	34
	35	static int handle_nan_start(struct nl80211_state *state,
	36	struct nl_msg msg, int argc, char *argv,
	37	enum id_input id)
	38	{
	39	int bands = 0;
	40
	41	if (argc < 2)
	42	return -EINVAL;
	43
	44	if (strcmp(argv[0], "pref") == 0) {
	45	argv++;
	46	argc--;
	47	NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
	48	argv++;
	49	argc--;
	50	} else {
	51	/* Master preference is mandatory */
	52	return -EINVAL;
	53	}
	54
	55	if (argc > 1 && !strcmp(argv[0], "bands")) {
	56	argv++;
	57	argc--;
	58
	59	bands = parse_bands(argc, argv);
	60	if (bands < 0)
	61	return bands;
	62
	63	NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
	64	} else if (argc != 0)
	65	return -EINVAL;
	66
	67	return 0;
	68	nla_put_failure:
	69	return -ENOBUFS;
	70	}
	71	COMMAND(nan, start, "pref <pref> [bands [2GHz] [5GHz]]",
	72	NL80211_CMD_START_NAN, 0, CIB_WDEV, handle_nan_start, "");
	73
	74	static int handle_nan_stop(struct nl80211_state *state,
75	struct nl_msg msg, int argc, char *argv,
76	enum id_input id)
77	{
78	return 0;
79	}
80	COMMAND(nan, stop, "", NL80211_CMD_STOP_NAN, 0, CIB_WDEV, handle_nan_stop, "");
81
82	static int handle_nan_config(struct nl80211_state *state,
83	struct nl_msg msg, int argc, char *argv,
84	enum id_input id)
85	{
86	int bands = 0;
87
88	if (argc < 2)
89	return -EINVAL;
90
91	if (strcmp(argv[0], "pref") == 0) {
92	argv++;
93	argc--;
94	NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
95	argv++;
96	argc--;
97	}
98
99	if (argc > 1 && !strcmp(argv[0], "bands")) {
100	argv++;
101	argc--;
102
103	bands = parse_bands(argc, argv);
104	if (bands < 0)
105	return bands;
106
107	NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
108	argv++;
109	argc--;
110	} else if (argc != 0)
111	return -EINVAL;
112
113	return 0;
114	nla_put_failure:
115	return -ENOBUFS;
116	}
117	COMMAND(nan, config, "[pref <pref>] [bands [2GHz] [5GHz]]",
118	NL80211_CMD_CHANGE_NAN_CONFIG, 0, CIB_WDEV, handle_nan_config, "");
119
120	static int handle_nan_rm_func(struct nl80211_state *state,
121	struct nl_msg msg, int argc, char *argv,
122	enum id_input id)
123	{
124	if (argc != 2)
125	return -EINVAL;
126
127	if (strcmp(argv[0], "cookie") == 0) {
128	argv++;
129	argc--;
130	NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, atoi(argv[0]));
131	argv++;
132	argc--;
133	}
134
135	if (argc != 0)
136	return -EINVAL;
137
138	return 0;
139	nla_put_failure:
140	return -ENOBUFS;
141	}
142	COMMAND(nan, rm_func, "cookie <cookie>", NL80211_CMD_DEL_NAN_FUNCTION, 0,
143	CIB_WDEV, handle_nan_rm_func, "");
144
145	static int compute_service_id(const unsigned char *serv_name,
146	unsigned int len, unsigned char *res)
147	{
148	size_t size = len;
149	unsigned char md_value[32];
150	int retcode = sha256(serv_name, size, md_value);
151
152	if (retcode)
153	return retcode;
154	memcpy(res, md_value, 6);
155
156	return 0;
157	}
158
159	static int print_instance_id_handler(struct nl_msg msg, void arg)
160	{
161	struct nlattr *tb[NL80211_ATTR_MAX + 1];
162	struct nlattr *func[NL80211_NAN_FUNC_ATTR_MAX + 1];
163	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
164
165	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
166	genlmsg_attrlen(gnlh, 0), NULL);
167
168	if (!tb[NL80211_ATTR_COOKIE]) {
169	fprintf(stderr, "cookie is missing!\n");
170	return NL_SKIP;
171	}
172
173	nla_parse_nested(func, NL80211_NAN_FUNC_ATTR_MAX,
174	tb[NL80211_ATTR_NAN_FUNC],
175	NULL);
176	if (!func[NL80211_NAN_FUNC_INSTANCE_ID]) {
177	fprintf(stderr, "instance id is missing!\n");
178	return NL_SKIP;
179	}
180
181	printf("instance_id: %d, cookie: %" PRIu64 "\n",
182	nla_get_u8(func[NL80211_NAN_FUNC_INSTANCE_ID]),
183	nla_get_u64(tb[NL80211_ATTR_COOKIE]));
184
185	return NL_SKIP;
186	}
187
188	static int parse_srf(char *argv, int argc, struct nl_msg func_attrs)
189	{
190	struct nl_msg *srf_attrs;
191	int old_argc = argc;
192	unsigned char mac_addr[ETH_ALEN];
ed3552e5	193	char cur_mac, sptr = NULL;
63d6aac5 RZ	194
	195	srf_attrs = nlmsg_alloc();
	196	if (strcmp(argv[0], "include") == 0)
	197	NLA_PUT_FLAG(srf_attrs, NL80211_NAN_SRF_INCLUDE);
	198	else if (strcmp(argv[0], "exclude") != 0)
	199	return -EINVAL;
	200
	201	argc--;
	202	argv++;
	203	if (strcmp(argv[0], "bf") == 0) {
	204	unsigned char *srf;
	205	size_t srf_len;
	206	__u8 bf_idx;
4fadb60a	207	int err;
63d6aac5 RZ	208
	209	argc--;
	210	argv++;
	211
	212	if (argc < 3)
	213	return -EINVAL;
	214
	215	bf_idx = atoi(argv[0]);
	216	NLA_PUT_U8(srf_attrs, NL80211_NAN_SRF_BF_IDX, bf_idx);
	217
	218	argc--;
	219	argv++;
	220	srf_len = atoi(argv[0]);
	221	if (srf_len == 0 \|\| srf_len > NL80211_NAN_FUNC_SRF_MAX_LEN)
	222	return -EINVAL;
	223
	224	argc--;
	225	argv++;
	226	srf = malloc(srf_len);
	227	if (!srf)
	228	return -ENOBUFS;
	229
	230	memset(srf, 0, srf_len);
	231	cur_mac = strtok_r(argv[0], ";", &sptr);
	232	while (cur_mac) {
	233	if (mac_addr_a2n(mac_addr, cur_mac)) {
	234	printf("mac format error %s\n", cur_mac);
4fadb60a	235	free(srf);
63d6aac5 RZ	236	return -EINVAL;
	237	}
	238
	239	nan_bf(bf_idx, srf, srf_len, mac_addr, ETH_ALEN);
	240	cur_mac = strtok_r(NULL, ";", &sptr);
	241	}
	242
4fadb60a JB	243	err = nla_put(srf_attrs, NL80211_NAN_SRF_BF, srf_len, srf);
	244	free(srf);
	245	if (err)
	246	goto nla_put_failure;
63d6aac5 RZ	247	argv++;
	248	argc--;
	249	} else if (strcmp(argv[0], "list") == 0) {
	250	struct nlattr *nl_macs = nla_nest_start(srf_attrs,
	251	NL80211_NAN_SRF_MAC_ADDRS);
	252	int i = 0;
	253
	254	argc--;
	255	argv++;
	256	cur_mac = strtok_r(argv[0], ";", &sptr);
	257	while (cur_mac) {
	258	if (mac_addr_a2n(mac_addr, cur_mac))
	259	return -EINVAL;
	260
	261	nla_put(srf_attrs, ++i, ETH_ALEN, mac_addr);
	262	cur_mac = strtok_r(NULL, ";", &sptr);
	263	}
	264
	265	nla_nest_end(srf_attrs, nl_macs);
	266	argv++;
	267	argc--;
	268	} else {
	269	return -EINVAL;
	270	}
	271
	272	nla_put_nested(func_attrs, NL80211_NAN_FUNC_SRF, srf_attrs);
	273	return old_argc - argc;
	274	nla_put_failure:
	275	return -ENOBUFS;
	276	}
	277
	278	static void parse_match_filter(char filter, struct nl_msg func_attrs, int tx)
	279	{
	280	struct nlattr *nl_filt;
ed3552e5	281	char cur_filt, sptr = NULL;
63d6aac5 RZ	282	int i = 0;
	283
	284	if (tx)
	285	nl_filt = nla_nest_start(func_attrs,
	286	NL80211_NAN_FUNC_TX_MATCH_FILTER);
	287	else
	288	nl_filt = nla_nest_start(func_attrs,
	289	NL80211_NAN_FUNC_RX_MATCH_FILTER);
	290
	291	cur_filt = strtok_r(filter, ":", &sptr);
	292	while (cur_filt) {
	293	if (strcmp(cur_filt, "*") != 0)
	294	nla_put(func_attrs, ++i, strlen(cur_filt), cur_filt);
	295	else
	296	nla_put(func_attrs, ++i, 0, NULL);
	297
	298	cur_filt = strtok_r(NULL, ":", &sptr);
	299	}
	300
	301	nla_nest_end(func_attrs, nl_filt);
	302	}
	303
	304	static int handle_nan_add_func(struct nl80211_state *state,
	305	struct nl_msg msg, int argc, char *argv,
	306	enum id_input id)
	307	{
	308	struct nl_msg *func_attrs = NULL;
	309	int err = 0;
	310	__u8 type;
	311
	312	func_attrs = nlmsg_alloc();
	313	if (!func_attrs) {
	314	err = -ENOBUFS;
	315	goto out;
	316	}
	317
	318	if (argc > 1 && strcmp(argv[0], "type") == 0) {
	319	argv++;
	320	argc--;
	321	if (strcmp(argv[0], "publish") == 0)
	322	type = NL80211_NAN_FUNC_PUBLISH;
	323	else if (strcmp(argv[0], "subscribe") == 0)
	324	type = NL80211_NAN_FUNC_SUBSCRIBE;
	325	else if (strcmp(argv[0], "followup") == 0)
	326	type = NL80211_NAN_FUNC_FOLLOW_UP;
	327	else
	328	return -EINVAL;
	329	argv++;
	330	argc--;
	331
	332	NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_TYPE, type);
	333	} else {
	334	return -EINVAL;
	335	}
	336
	337	if (type == NL80211_NAN_FUNC_SUBSCRIBE) {
	338	if (argc > 1 && strcmp(argv[0], "active") == 0) {
	339	argv++;
	340	argc--;
	341	NLA_PUT_FLAG(func_attrs,
	342	NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE);
	343	}
	344	}
	345
346	if (type == NL80211_NAN_FUNC_PUBLISH) {
347	__u8 publish_type = 0;
348
349	if (argc > 1 && strcmp(argv[0], "solicited") == 0) {
350	argv++;
351	argc--;
352	publish_type \|= NL80211_NAN_SOLICITED_PUBLISH;
353	}
354
355	if (argc > 1 && strcmp(argv[0], "unsolicited") == 0) {
356	argv++;
357	argc--;
358	publish_type \|= NL80211_NAN_UNSOLICITED_PUBLISH;
359	}
360
361	NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_PUBLISH_TYPE,
362	publish_type);
363
364	/* only allow for solicited publish */
365	if (argc > 1 && strcmp(argv[0], "bcast") == 0) {
366	argv++;
367	argc--;
368	if (!(publish_type & NL80211_NAN_SOLICITED_PUBLISH))
369	return -EINVAL;
370
371	NLA_PUT_FLAG(func_attrs,
372	NL80211_NAN_FUNC_PUBLISH_BCAST);
373	}
374	}
375
376	if (argc > 1 && strcmp(argv[0], "close_range") == 0) {
377	argv++;
378	argc--;
379	NLA_PUT_FLAG(func_attrs, NL80211_NAN_FUNC_CLOSE_RANGE);
380	}
381
382	if (argc > 1 && strcmp(argv[0], "name") == 0) {
383	unsigned char serv_id_c[6] = {0};
384	__u64 service_id;
385
386	argv++;
387	argc--;
388	compute_service_id((const unsigned char *)argv[0],
389	strlen(argv[0]), serv_id_c);
390	service_id = (__u64)serv_id_c[0] << 0 \|
391	(__u64)serv_id_c[1] << 8 \|
392	(__u64)serv_id_c[2] << 16 \|
393	(__u64)serv_id_c[3] << 24 \|
394	(__u64)serv_id_c[4] << 32 \|
395	(__u64)serv_id_c[5] << 40;
396
397	NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_ID, 6,
398	&service_id);
399	argv++;
400	argc--;
401	} else {
402	return -EINVAL;
403	}
404
405	if (argc > 1 && strcmp(argv[0], "info") == 0) {
406	argv++;
407	argc--;
408	NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_INFO,
409	strlen(argv[0]), argv[0]);
410	argv++;
411	argc--;
412	}
413
414	if (type == NL80211_NAN_FUNC_FOLLOW_UP) {
415	if (argc > 1 && strcmp(argv[0], "flw_up_id") == 0) {
416	argv++;
417	argc--;
418	NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_ID,
419	atoi(argv[0]));
420	argv++;
421	argc--;
422	}
423
424	if (argc > 1 && strcmp(argv[0], "flw_up_req_id") == 0) {
425	argv++;
426	argc--;
427	NLA_PUT_U8(func_attrs,
428	NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID,
429	atoi(argv[0]));
430	argv++;
431	argc--;
432	}
433
434	if (argc > 1 && strcmp(argv[0], "flw_up_dest") == 0) {
435	unsigned char addr[6];
436
437	argv++;
438	argc--;
439	if (mac_addr_a2n(addr, argv[0]))
440	goto nla_put_failure;
441	nla_put(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_DEST,
442	ETH_ALEN, addr);
443	argv++;
444	argc--;
445	}
446	}
447
448	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
449	argc > 1 && strcmp(argv[0], "ttl") == 0) {
450	argv++;
451	argc--;
452	NLA_PUT_U32(func_attrs, NL80211_NAN_FUNC_TTL, atoi(argv[0]));
453	argv++;
454	argc--;
455	}
456
457	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
458	argc >= 4 && strcmp(argv[0], "srf") == 0) {
459	int res;
460
461	argv++;
462	argc--;
463	res = parse_srf(argv, argc, func_attrs);
464	if (res < 0)
465	return -EINVAL;
466
467	argc -= res;
468	argv += res;
469	}
470
471	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
472	argc > 1 && strcmp(argv[0], "rx_filter") == 0) {
473	argv++;
474	argc--;
475	parse_match_filter(argv[0], func_attrs, 0);
476
477	argv++;
478	argc--;
479	}
480
481	if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
482	argc > 1 && strcmp(argv[0], "tx_filter") == 0) {
483	argv++;
484	argc--;
485	parse_match_filter(argv[0], func_attrs, 1);
486
487	argv++;
488	argc--;
489	}
490
491	if (argc != 0)
492	return -EINVAL;
493
494	nla_put_nested(msg, NL80211_ATTR_NAN_FUNC, func_attrs);
495	register_handler(print_instance_id_handler, NULL);
496
497	return err;
498	nla_put_failure:
499	return -ENOBUFS;
500	out:
501	return err;
502	}
503	COMMAND(nan, add_func,
504	"type <publish\|subscribe\|followup> [active] [solicited] [unsolicited] [bcast] [close_range] name <name> [info <info>] [flw_up_id <id> flw_up_req_id <id> flw_up_dest <mac>] [ttl <ttl>] [srf <include\|exclude> <bf\|list> [bf_idx] [bf_len] <mac1;mac2...>] [rx_filter <str1:str2...>] [tx_filter <str1:str2...>]",
505	NL80211_CMD_ADD_NAN_FUNCTION, 0, CIB_WDEV,
506	handle_nan_add_func, "");