[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 <openssl/evp.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;

	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;

		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);
				return -EINVAL;
			}

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

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