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 <ctype.h>
12 #include <inttypes.h>
13 #include "nl80211.h"
14 #include "iw.h"
15 #include "sha256.h"
16
17 SECTION(nan);
18
parse_bands(int argc,char ** argv)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
handle_nan_start(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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
handle_nan_stop(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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
handle_nan_config(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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
handle_nan_rm_func(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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
compute_service_id(const unsigned char * serv_name,unsigned int len,unsigned char * res)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
print_instance_id_handler(struct nl_msg * msg,void * arg)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
parse_srf(char ** argv,int argc,struct nl_msg * func_attrs)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];
193 char *cur_mac, *sptr = NULL;
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;
207 int err;
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 free(srf);
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
243 err = nla_put(srf_attrs, NL80211_NAN_SRF_BF, srf_len, srf);
244 free(srf);
245 if (err)
246 goto nla_put_failure;
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
parse_match_filter(char * filter,struct nl_msg * func_attrs,int tx)278 static void parse_match_filter(char *filter, struct nl_msg *func_attrs, int tx)
279 {
280 struct nlattr *nl_filt;
281 char *cur_filt, *sptr = NULL;
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
handle_nan_add_func(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)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, "");
507