]>
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; | |
207 | ||
208 | argc--; | |
209 | argv++; | |
210 | ||
211 | if (argc < 3) | |
212 | return -EINVAL; | |
213 | ||
214 | bf_idx = atoi(argv[0]); | |
215 | NLA_PUT_U8(srf_attrs, NL80211_NAN_SRF_BF_IDX, bf_idx); | |
216 | ||
217 | argc--; | |
218 | argv++; | |
219 | srf_len = atoi(argv[0]); | |
220 | if (srf_len == 0 || srf_len > NL80211_NAN_FUNC_SRF_MAX_LEN) | |
221 | return -EINVAL; | |
222 | ||
223 | argc--; | |
224 | argv++; | |
225 | srf = malloc(srf_len); | |
226 | if (!srf) | |
227 | return -ENOBUFS; | |
228 | ||
229 | memset(srf, 0, srf_len); | |
230 | cur_mac = strtok_r(argv[0], ";", &sptr); | |
231 | while (cur_mac) { | |
232 | if (mac_addr_a2n(mac_addr, cur_mac)) { | |
233 | printf("mac format error %s\n", cur_mac); | |
234 | return -EINVAL; | |
235 | } | |
236 | ||
237 | nan_bf(bf_idx, srf, srf_len, mac_addr, ETH_ALEN); | |
238 | cur_mac = strtok_r(NULL, ";", &sptr); | |
239 | } | |
240 | ||
241 | NLA_PUT(srf_attrs, NL80211_NAN_SRF_BF, srf_len, srf); | |
242 | argv++; | |
243 | argc--; | |
244 | } else if (strcmp(argv[0], "list") == 0) { | |
245 | struct nlattr *nl_macs = nla_nest_start(srf_attrs, | |
246 | NL80211_NAN_SRF_MAC_ADDRS); | |
247 | int i = 0; | |
248 | ||
249 | argc--; | |
250 | argv++; | |
251 | cur_mac = strtok_r(argv[0], ";", &sptr); | |
252 | while (cur_mac) { | |
253 | if (mac_addr_a2n(mac_addr, cur_mac)) | |
254 | return -EINVAL; | |
255 | ||
256 | nla_put(srf_attrs, ++i, ETH_ALEN, mac_addr); | |
257 | cur_mac = strtok_r(NULL, ";", &sptr); | |
258 | } | |
259 | ||
260 | nla_nest_end(srf_attrs, nl_macs); | |
261 | argv++; | |
262 | argc--; | |
263 | } else { | |
264 | return -EINVAL; | |
265 | } | |
266 | ||
267 | nla_put_nested(func_attrs, NL80211_NAN_FUNC_SRF, srf_attrs); | |
268 | return old_argc - argc; | |
269 | nla_put_failure: | |
270 | return -ENOBUFS; | |
271 | } | |
272 | ||
273 | static void parse_match_filter(char *filter, struct nl_msg *func_attrs, int tx) | |
274 | { | |
275 | struct nlattr *nl_filt; | |
ed3552e5 | 276 | char *cur_filt, *sptr = NULL; |
63d6aac5 RZ |
277 | int i = 0; |
278 | ||
279 | if (tx) | |
280 | nl_filt = nla_nest_start(func_attrs, | |
281 | NL80211_NAN_FUNC_TX_MATCH_FILTER); | |
282 | else | |
283 | nl_filt = nla_nest_start(func_attrs, | |
284 | NL80211_NAN_FUNC_RX_MATCH_FILTER); | |
285 | ||
286 | cur_filt = strtok_r(filter, ":", &sptr); | |
287 | while (cur_filt) { | |
288 | if (strcmp(cur_filt, "*") != 0) | |
289 | nla_put(func_attrs, ++i, strlen(cur_filt), cur_filt); | |
290 | else | |
291 | nla_put(func_attrs, ++i, 0, NULL); | |
292 | ||
293 | cur_filt = strtok_r(NULL, ":", &sptr); | |
294 | } | |
295 | ||
296 | nla_nest_end(func_attrs, nl_filt); | |
297 | } | |
298 | ||
299 | static int handle_nan_add_func(struct nl80211_state *state, | |
300 | struct nl_msg *msg, int argc, char **argv, | |
301 | enum id_input id) | |
302 | { | |
303 | struct nl_msg *func_attrs = NULL; | |
304 | int err = 0; | |
305 | __u8 type; | |
306 | ||
307 | func_attrs = nlmsg_alloc(); | |
308 | if (!func_attrs) { | |
309 | err = -ENOBUFS; | |
310 | goto out; | |
311 | } | |
312 | ||
313 | if (argc > 1 && strcmp(argv[0], "type") == 0) { | |
314 | argv++; | |
315 | argc--; | |
316 | if (strcmp(argv[0], "publish") == 0) | |
317 | type = NL80211_NAN_FUNC_PUBLISH; | |
318 | else if (strcmp(argv[0], "subscribe") == 0) | |
319 | type = NL80211_NAN_FUNC_SUBSCRIBE; | |
320 | else if (strcmp(argv[0], "followup") == 0) | |
321 | type = NL80211_NAN_FUNC_FOLLOW_UP; | |
322 | else | |
323 | return -EINVAL; | |
324 | argv++; | |
325 | argc--; | |
326 | ||
327 | NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_TYPE, type); | |
328 | } else { | |
329 | return -EINVAL; | |
330 | } | |
331 | ||
332 | if (type == NL80211_NAN_FUNC_SUBSCRIBE) { | |
333 | if (argc > 1 && strcmp(argv[0], "active") == 0) { | |
334 | argv++; | |
335 | argc--; | |
336 | NLA_PUT_FLAG(func_attrs, | |
337 | NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE); | |
338 | } | |
339 | } | |
340 | ||
341 | if (type == NL80211_NAN_FUNC_PUBLISH) { | |
342 | __u8 publish_type = 0; | |
343 | ||
344 | if (argc > 1 && strcmp(argv[0], "solicited") == 0) { | |
345 | argv++; | |
346 | argc--; | |
347 | publish_type |= NL80211_NAN_SOLICITED_PUBLISH; | |
348 | } | |
349 | ||
350 | if (argc > 1 && strcmp(argv[0], "unsolicited") == 0) { | |
351 | argv++; | |
352 | argc--; | |
353 | publish_type |= NL80211_NAN_UNSOLICITED_PUBLISH; | |
354 | } | |
355 | ||
356 | NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_PUBLISH_TYPE, | |
357 | publish_type); | |
358 | ||
359 | /* only allow for solicited publish */ | |
360 | if (argc > 1 && strcmp(argv[0], "bcast") == 0) { | |
361 | argv++; | |
362 | argc--; | |
363 | if (!(publish_type & NL80211_NAN_SOLICITED_PUBLISH)) | |
364 | return -EINVAL; | |
365 | ||
366 | NLA_PUT_FLAG(func_attrs, | |
367 | NL80211_NAN_FUNC_PUBLISH_BCAST); | |
368 | } | |
369 | } | |
370 | ||
371 | if (argc > 1 && strcmp(argv[0], "close_range") == 0) { | |
372 | argv++; | |
373 | argc--; | |
374 | NLA_PUT_FLAG(func_attrs, NL80211_NAN_FUNC_CLOSE_RANGE); | |
375 | } | |
376 | ||
377 | if (argc > 1 && strcmp(argv[0], "name") == 0) { | |
378 | unsigned char serv_id_c[6] = {0}; | |
379 | __u64 service_id; | |
380 | ||
381 | argv++; | |
382 | argc--; | |
383 | compute_service_id((const unsigned char *)argv[0], | |
384 | strlen(argv[0]), serv_id_c); | |
385 | service_id = (__u64)serv_id_c[0] << 0 | | |
386 | (__u64)serv_id_c[1] << 8 | | |
387 | (__u64)serv_id_c[2] << 16 | | |
388 | (__u64)serv_id_c[3] << 24 | | |
389 | (__u64)serv_id_c[4] << 32 | | |
390 | (__u64)serv_id_c[5] << 40; | |
391 | ||
392 | NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_ID, 6, | |
393 | &service_id); | |
394 | argv++; | |
395 | argc--; | |
396 | } else { | |
397 | return -EINVAL; | |
398 | } | |
399 | ||
400 | if (argc > 1 && strcmp(argv[0], "info") == 0) { | |
401 | argv++; | |
402 | argc--; | |
403 | NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_INFO, | |
404 | strlen(argv[0]), argv[0]); | |
405 | argv++; | |
406 | argc--; | |
407 | } | |
408 | ||
409 | if (type == NL80211_NAN_FUNC_FOLLOW_UP) { | |
410 | if (argc > 1 && strcmp(argv[0], "flw_up_id") == 0) { | |
411 | argv++; | |
412 | argc--; | |
413 | NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_ID, | |
414 | atoi(argv[0])); | |
415 | argv++; | |
416 | argc--; | |
417 | } | |
418 | ||
419 | if (argc > 1 && strcmp(argv[0], "flw_up_req_id") == 0) { | |
420 | argv++; | |
421 | argc--; | |
422 | NLA_PUT_U8(func_attrs, | |
423 | NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID, | |
424 | atoi(argv[0])); | |
425 | argv++; | |
426 | argc--; | |
427 | } | |
428 | ||
429 | if (argc > 1 && strcmp(argv[0], "flw_up_dest") == 0) { | |
430 | unsigned char addr[6]; | |
431 | ||
432 | argv++; | |
433 | argc--; | |
434 | if (mac_addr_a2n(addr, argv[0])) | |
435 | goto nla_put_failure; | |
436 | nla_put(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_DEST, | |
437 | ETH_ALEN, addr); | |
438 | argv++; | |
439 | argc--; | |
440 | } | |
441 | } | |
442 | ||
443 | if (type != NL80211_NAN_FUNC_FOLLOW_UP && | |
444 | argc > 1 && strcmp(argv[0], "ttl") == 0) { | |
445 | argv++; | |
446 | argc--; | |
447 | NLA_PUT_U32(func_attrs, NL80211_NAN_FUNC_TTL, atoi(argv[0])); | |
448 | argv++; | |
449 | argc--; | |
450 | } | |
451 | ||
452 | if (type != NL80211_NAN_FUNC_FOLLOW_UP && | |
453 | argc >= 4 && strcmp(argv[0], "srf") == 0) { | |
454 | int res; | |
455 | ||
456 | argv++; | |
457 | argc--; | |
458 | res = parse_srf(argv, argc, func_attrs); | |
459 | if (res < 0) | |
460 | return -EINVAL; | |
461 | ||
462 | argc -= res; | |
463 | argv += res; | |
464 | } | |
465 | ||
466 | if (type != NL80211_NAN_FUNC_FOLLOW_UP && | |
467 | argc > 1 && strcmp(argv[0], "rx_filter") == 0) { | |
468 | argv++; | |
469 | argc--; | |
470 | parse_match_filter(argv[0], func_attrs, 0); | |
471 | ||
472 | argv++; | |
473 | argc--; | |
474 | } | |
475 | ||
476 | if (type != NL80211_NAN_FUNC_FOLLOW_UP && | |
477 | argc > 1 && strcmp(argv[0], "tx_filter") == 0) { | |
478 | argv++; | |
479 | argc--; | |
480 | parse_match_filter(argv[0], func_attrs, 1); | |
481 | ||
482 | argv++; | |
483 | argc--; | |
484 | } | |
485 | ||
486 | if (argc != 0) | |
487 | return -EINVAL; | |
488 | ||
489 | nla_put_nested(msg, NL80211_ATTR_NAN_FUNC, func_attrs); | |
490 | register_handler(print_instance_id_handler, NULL); | |
491 | ||
492 | return err; | |
493 | nla_put_failure: | |
494 | return -ENOBUFS; | |
495 | out: | |
496 | return err; | |
497 | } | |
498 | COMMAND(nan, add_func, | |
499 | "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...>]", | |
500 | NL80211_CMD_ADD_NAN_FUNCTION, 0, CIB_WDEV, | |
501 | handle_nan_add_func, ""); |