]>
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, ""); |