1 /*
2 * NAN Discovery Engine
3 * Copyright (c) 2024, Qualcomm Innovation Center, Inc.
4 *
5 * This software may be distributed under the terms of the BSD license.
6 * See README for more details.
7 */
8
9 #include "utils/includes.h"
10
11 #include "utils/common.h"
12 #include "utils/eloop.h"
13 #include "crypto/crypto.h"
14 #include "crypto/sha256.h"
15 #include "ieee802_11_defs.h"
16 #include "nan.h"
17 #include "nan_de.h"
18
19 static const u8 nan_network_id[ETH_ALEN] =
20 { 0x51, 0x6f, 0x9a, 0x01, 0x00, 0x00 };
21
22 enum nan_de_service_type {
23 NAN_DE_PUBLISH,
24 NAN_DE_SUBSCRIBE,
25 };
26
27 static const u8 p2p_network_id[ETH_ALEN] =
28 { 0x51, 0x6f, 0x9a, 0x02, 0x00, 0x00 };
29
30 static const u8 wildcard_bssid[ETH_ALEN] =
31 { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
32
33 struct nan_de_service {
34 int id;
35 enum nan_de_service_type type;
36 char *service_name;
37 u8 service_id[NAN_SERVICE_ID_LEN];
38 struct nan_publish_params publish;
39 struct nan_subscribe_params subscribe;
40 enum nan_service_protocol_type srv_proto_type;
41 struct wpabuf *ssi;
42 struct wpabuf *elems;
43 struct os_reltime time_started;
44 struct os_reltime end_time;
45 struct os_reltime last_multicast;
46 struct os_reltime first_discovered;
47 bool needs_fsd;
48 unsigned int freq;
49 unsigned int default_freq;
50 int *freq_list;
51 u8 a3[ETH_ALEN];
52 bool a3_set;
53
54 /* pauseState information for Publish function */
55 struct os_reltime pause_state_end;
56 u8 sel_peer_id;
57 u8 sel_peer_addr[ETH_ALEN];
58
59 /* Publish state - channel iteration */
60 bool in_multi_chan;
61 bool first_multi_chan;
62 int multi_chan_idx; /* index to freq_list[] */
63 struct os_reltime next_publish_state;
64 struct os_reltime next_publish_chan;
65 unsigned int next_publish_duration;
66 bool is_p2p;
67 };
68
69 struct nan_de {
70 u8 nmi[ETH_ALEN];
71 bool offload;
72 bool ap;
73 unsigned int max_listen;
74 struct nan_callbacks cb;
75
76 struct nan_de_service *service[NAN_DE_MAX_SERVICE];
77 unsigned int num_service;
78
79 int next_handle;
80
81 unsigned int ext_listen_freq;
82 unsigned int listen_freq;
83 unsigned int tx_wait_status_freq;
84 unsigned int tx_wait_end_freq;
85 };
86
87
nan_de_is_nan_network_id(const u8 * addr)88 bool nan_de_is_nan_network_id(const u8 *addr)
89 {
90 return ether_addr_equal(addr, nan_network_id);
91 }
92
93
nan_de_is_p2p_network_id(const u8 * addr)94 bool nan_de_is_p2p_network_id(const u8 *addr)
95 {
96 return ether_addr_equal(addr, p2p_network_id);
97 }
98
99
nan_de_init(const u8 * nmi,bool offload,bool ap,unsigned int max_listen,const struct nan_callbacks * cb)100 struct nan_de * nan_de_init(const u8 *nmi, bool offload, bool ap,
101 unsigned int max_listen,
102 const struct nan_callbacks *cb)
103 {
104 struct nan_de *de;
105
106 de = os_zalloc(sizeof(*de));
107 if (!de)
108 return NULL;
109
110 os_memcpy(de->nmi, nmi, ETH_ALEN);
111 de->offload = offload;
112 de->ap = ap;
113 de->max_listen = max_listen ? max_listen : 1000;
114 os_memcpy(&de->cb, cb, sizeof(*cb));
115
116 return de;
117 }
118
119
nan_de_service_free(struct nan_de_service * srv)120 static void nan_de_service_free(struct nan_de_service *srv)
121 {
122 os_free(srv->service_name);
123 wpabuf_free(srv->ssi);
124 wpabuf_free(srv->elems);
125 os_free(srv->freq_list);
126 os_free(srv);
127 }
128
129
nan_de_service_deinit(struct nan_de * de,struct nan_de_service * srv,enum nan_de_reason reason)130 static void nan_de_service_deinit(struct nan_de *de, struct nan_de_service *srv,
131 enum nan_de_reason reason)
132 {
133 if (!srv)
134 return;
135 if (srv->type == NAN_DE_PUBLISH && de->cb.publish_terminated)
136 de->cb.publish_terminated(de->cb.ctx, srv->id, reason);
137 if (srv->type == NAN_DE_SUBSCRIBE && de->cb.subscribe_terminated)
138 de->cb.subscribe_terminated(de->cb.ctx, srv->id, reason);
139 nan_de_service_free(srv);
140 }
141
142
nan_de_clear_pending(struct nan_de * de)143 static void nan_de_clear_pending(struct nan_de *de)
144 {
145 de->listen_freq = 0;
146 de->tx_wait_status_freq = 0;
147 de->tx_wait_end_freq = 0;
148 }
149
150
nan_de_flush(struct nan_de * de)151 void nan_de_flush(struct nan_de *de)
152 {
153 unsigned int i;
154
155 if (!de)
156 return;
157
158 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) {
159 nan_de_service_deinit(de, de->service[i],
160 NAN_DE_REASON_USER_REQUEST);
161 de->service[i] = NULL;
162 }
163
164 de->num_service = 0;
165 nan_de_clear_pending(de);
166 }
167
168
nan_de_pause_state(struct nan_de_service * srv,const u8 * peer_addr,u8 peer_id)169 static void nan_de_pause_state(struct nan_de_service *srv, const u8 *peer_addr,
170 u8 peer_id)
171 {
172 wpa_printf(MSG_DEBUG, "NAN: Start pauseState");
173 os_get_reltime(&srv->pause_state_end);
174 srv->pause_state_end.sec += 60;
175 if (os_reltime_initialized(&srv->end_time) &&
176 os_reltime_before(&srv->end_time, &srv->pause_state_end))
177 srv->pause_state_end = srv->end_time;
178 os_memcpy(srv->sel_peer_addr, peer_addr, ETH_ALEN);
179 srv->sel_peer_id = peer_id;
180 }
181
182
nan_de_unpause_state(struct nan_de_service * srv)183 static void nan_de_unpause_state(struct nan_de_service *srv)
184 {
185 wpa_printf(MSG_DEBUG, "NAN: Stop pauseState");
186 srv->pause_state_end.sec = 0;
187 srv->pause_state_end.usec = 0;
188 os_memset(srv->sel_peer_addr, 0, ETH_ALEN);
189 srv->sel_peer_id = 0;
190 }
191
192
nan_de_alloc_sdf(size_t len)193 static struct wpabuf * nan_de_alloc_sdf(size_t len)
194 {
195 struct wpabuf *buf;
196
197 buf = wpabuf_alloc(2 + 4 + len);
198 if (buf) {
199 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
200 wpabuf_put_u8(buf, WLAN_PA_VENDOR_SPECIFIC);
201 wpabuf_put_be32(buf, NAN_SDF_VENDOR_TYPE);
202 }
203
204 return buf;
205 }
206
207
nan_de_tx(struct nan_de * de,unsigned int freq,unsigned int wait_time,const u8 * dst,const u8 * src,const u8 * bssid,const struct wpabuf * buf)208 static int nan_de_tx(struct nan_de *de, unsigned int freq,
209 unsigned int wait_time,
210 const u8 *dst, const u8 *src, const u8 *bssid,
211 const struct wpabuf *buf)
212 {
213 int res;
214
215 if (!de->cb.tx)
216 return -1;
217
218 res = de->cb.tx(de->cb.ctx, freq, wait_time, dst, src, bssid, buf);
219 if (res < 0)
220 return res;
221
222 de->tx_wait_status_freq = freq;
223 de->tx_wait_end_freq = wait_time ? freq : 0;
224
225 return res;
226 }
227
228
nan_de_tx_sdf(struct nan_de * de,struct nan_de_service * srv,unsigned int wait_time,enum nan_service_control_type type,const u8 * dst,const u8 * a3,u8 req_instance_id,const struct wpabuf * ssi)229 static void nan_de_tx_sdf(struct nan_de *de, struct nan_de_service *srv,
230 unsigned int wait_time,
231 enum nan_service_control_type type,
232 const u8 *dst, const u8 *a3, u8 req_instance_id,
233 const struct wpabuf *ssi)
234 {
235 struct wpabuf *buf;
236 size_t len = 0, sda_len, sdea_len;
237 u8 ctrl = type;
238 u16 sdea_ctrl = 0;
239
240 /* Service Descriptor attribute */
241 sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1;
242 len += NAN_ATTR_HDR_LEN + sda_len;
243
244 /* Service Descriptor Extension attribute */
245 sdea_len = 1 + 2;
246 if (ssi)
247 sdea_len += 2 + 4 + wpabuf_len(ssi);
248 len += NAN_ATTR_HDR_LEN + sdea_len;
249
250 /* Element Container attribute */
251 if (srv->elems)
252 len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems);
253
254 buf = nan_de_alloc_sdf(len);
255 if (!buf)
256 return;
257
258 /* Service Descriptor attribute */
259 wpabuf_put_u8(buf, NAN_ATTR_SDA);
260 wpabuf_put_le16(buf, sda_len);
261 wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN);
262 wpabuf_put_u8(buf, srv->id); /* Instance ID */
263 wpabuf_put_u8(buf, req_instance_id); /* Requestor Instance ID */
264 wpabuf_put_u8(buf, ctrl);
265
266 /* Service Descriptor Extension attribute */
267 if (srv->type == NAN_DE_PUBLISH || ssi) {
268 wpabuf_put_u8(buf, NAN_ATTR_SDEA);
269 wpabuf_put_le16(buf, sdea_len);
270 wpabuf_put_u8(buf, srv->id); /* Instance ID */
271 if (srv->type == NAN_DE_PUBLISH) {
272 if (srv->publish.fsd)
273 sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ;
274 if (srv->publish.fsd_gas)
275 sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS;
276 }
277 wpabuf_put_le16(buf, sdea_ctrl);
278 if (ssi) {
279 wpabuf_put_le16(buf, 4 + wpabuf_len(ssi));
280 wpabuf_put_be24(buf, OUI_WFA);
281 wpabuf_put_u8(buf, srv->srv_proto_type);
282 wpabuf_put_buf(buf, ssi);
283 }
284 }
285
286 /* Element Container attribute */
287 if (srv->elems) {
288 wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER);
289 wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems));
290 wpabuf_put_u8(buf, 0); /* Map ID */
291 wpabuf_put_buf(buf, srv->elems);
292 }
293
294 nan_de_tx(de, srv->freq, wait_time, dst, de->nmi, a3, buf);
295 wpabuf_free(buf);
296 }
297
298
nan_de_time_to_next_chan_change(struct nan_de_service * srv)299 static int nan_de_time_to_next_chan_change(struct nan_de_service *srv)
300 {
301 struct os_reltime tmp, diff, now;
302
303 if (os_reltime_before(&srv->next_publish_state,
304 &srv->next_publish_chan))
305 tmp = srv->next_publish_state;
306 else if (srv->in_multi_chan)
307 tmp = srv->next_publish_chan;
308 else
309 tmp = srv->next_publish_state;
310
311 os_get_reltime(&now);
312 os_reltime_sub(&tmp, &now, &diff);
313 return os_reltime_in_ms(&diff);
314 }
315
316
nan_de_set_publish_times(struct nan_de_service * srv)317 static void nan_de_set_publish_times(struct nan_de_service *srv)
318 {
319 os_get_reltime(&srv->next_publish_state);
320 srv->next_publish_chan = srv->next_publish_state;
321 /* Swap single/multi channel state in N * 100 TU */
322 os_reltime_add_ms(&srv->next_publish_state,
323 srv->next_publish_duration * 1024 / 1000);
324
325 /* Swap channel in multi channel state after 150 ms */
326 os_reltime_add_ms(&srv->next_publish_chan, 150);
327 }
328
329
nan_de_check_chan_change(struct nan_de_service * srv)330 static void nan_de_check_chan_change(struct nan_de_service *srv)
331 {
332 if (srv->next_publish_duration) {
333 /* Update end times for the first operation of the publish
334 * iteration */
335 nan_de_set_publish_times(srv);
336 srv->next_publish_duration = 0;
337 } else if (srv->in_multi_chan) {
338 if (!os_reltime_initialized(&srv->pause_state_end)) {
339 srv->multi_chan_idx++;
340 if (srv->freq_list[srv->multi_chan_idx] == 0)
341 srv->multi_chan_idx = 0;
342 srv->freq = srv->freq_list[srv->multi_chan_idx];
343 wpa_printf(MSG_DEBUG,
344 "NAN: Publish multi-channel change to %u MHz",
345 srv->freq);
346 }
347 os_get_reltime(&srv->next_publish_chan);
348 os_reltime_add_ms(&srv->next_publish_chan, 150);
349 }
350 }
351
352
nan_de_tx_multicast(struct nan_de * de,struct nan_de_service * srv,u8 req_instance_id)353 static void nan_de_tx_multicast(struct nan_de *de, struct nan_de_service *srv,
354 u8 req_instance_id)
355 {
356 enum nan_service_control_type type;
357 unsigned int wait_time = 100;
358 const u8 *network_id;
359 const u8 *bssid;
360
361 if (srv->type == NAN_DE_PUBLISH) {
362 int ms;
363
364 type = NAN_SRV_CTRL_PUBLISH;
365
366 nan_de_check_chan_change(srv);
367 ms = nan_de_time_to_next_chan_change(srv);
368 if (ms < 100)
369 ms = 100;
370 wait_time = ms;
371 } else if (srv->type == NAN_DE_SUBSCRIBE) {
372 type = NAN_SRV_CTRL_SUBSCRIBE;
373 } else {
374 return;
375 }
376
377 if (srv->is_p2p) {
378 network_id = p2p_network_id;
379 bssid = wildcard_bssid;
380 } else {
381 network_id = nan_network_id;
382 bssid = nan_network_id;
383 }
384
385 nan_de_tx_sdf(de, srv, wait_time, type, network_id, bssid,
386 req_instance_id, srv->ssi);
387 os_get_reltime(&srv->last_multicast);
388 }
389
390
nan_de_add_srv(struct nan_de * de,struct nan_de_service * srv)391 static void nan_de_add_srv(struct nan_de *de, struct nan_de_service *srv)
392 {
393 int ttl;
394
395 os_get_reltime(&srv->time_started);
396 ttl = srv->type == NAN_DE_PUBLISH ? srv->publish.ttl :
397 srv->subscribe.ttl;
398 if (ttl) {
399 srv->end_time = srv->time_started;
400 srv->end_time.sec += ttl;
401 }
402
403 de->service[srv->id - 1] = srv;
404 de->num_service++;
405 }
406
407
nan_de_del_srv(struct nan_de * de,struct nan_de_service * srv,enum nan_de_reason reason)408 static void nan_de_del_srv(struct nan_de *de, struct nan_de_service *srv,
409 enum nan_de_reason reason)
410 {
411 de->service[srv->id - 1] = NULL;
412 nan_de_service_deinit(de, srv, reason);
413 de->num_service--;
414 if (de->num_service == 0)
415 nan_de_clear_pending(de);
416 }
417
418
nan_de_srv_expired(struct nan_de_service * srv,struct os_reltime * now)419 static bool nan_de_srv_expired(struct nan_de_service *srv,
420 struct os_reltime *now)
421 {
422 if (os_reltime_initialized(&srv->end_time))
423 return os_reltime_before(&srv->end_time, now);
424
425 if (srv->type == NAN_DE_PUBLISH) {
426 /* Time out after one transmission (and wait for FSD) */
427 if (!os_reltime_initialized(&srv->last_multicast))
428 return false;
429 if (!srv->publish.fsd)
430 return true;
431 }
432
433 if (srv->type == NAN_DE_SUBSCRIBE) {
434 /* Time out after first DiscoveryResult event (and wait for
435 * FSD) */
436 if (!os_reltime_initialized(&srv->first_discovered))
437 return false;
438 if (!srv->needs_fsd)
439 return true;
440 }
441
442 return false;
443 }
444
445
nan_de_next_multicast(struct nan_de * de,struct nan_de_service * srv,struct os_reltime * now)446 static int nan_de_next_multicast(struct nan_de *de, struct nan_de_service *srv,
447 struct os_reltime *now)
448 {
449 unsigned int period;
450 struct os_reltime next, diff;
451
452 if (srv->type == NAN_DE_PUBLISH && !srv->publish.unsolicited)
453 return -1;
454 if (srv->type == NAN_DE_SUBSCRIBE && !srv->subscribe.active)
455 return -1;
456
457 if (!os_reltime_initialized(&srv->last_multicast))
458 return 0;
459
460 if (srv->type == NAN_DE_PUBLISH && srv->publish.ttl == 0)
461 return -1;
462
463 if (srv->type == NAN_DE_PUBLISH &&
464 os_reltime_initialized(&srv->pause_state_end))
465 return -1;
466
467 period = srv->type == NAN_DE_PUBLISH ?
468 srv->publish.announcement_period :
469 srv->subscribe.query_period;
470 if (period == 0)
471 period = 100;
472 next = srv->last_multicast;
473 os_reltime_add_ms(&next, period);
474
475 if (srv->type == NAN_DE_PUBLISH) {
476 if (!de->tx_wait_end_freq && srv->publish.unsolicited &&
477 os_reltime_before(&next, now))
478 return 0;
479 next = srv->next_publish_state;
480 }
481
482 if (os_reltime_before(&next, now))
483 return 0;
484
485 os_reltime_sub(&next, now, &diff);
486 return os_reltime_in_ms(&diff);
487 }
488
489
nan_de_srv_time_to_next(struct nan_de * de,struct nan_de_service * srv,struct os_reltime * now)490 static int nan_de_srv_time_to_next(struct nan_de *de,
491 struct nan_de_service *srv,
492 struct os_reltime *now)
493 {
494 struct os_reltime diff;
495 int next = -1, tmp;
496
497 if (os_reltime_initialized(&srv->end_time)) {
498 os_reltime_sub(&srv->end_time, now, &diff);
499 tmp = os_reltime_in_ms(&diff);
500 if (next == -1 || tmp < next)
501 next = tmp;
502 }
503
504 if (srv->type == NAN_DE_PUBLISH &&
505 srv->publish.fsd &&
506 os_reltime_initialized(&srv->pause_state_end)) {
507 os_reltime_sub(&srv->pause_state_end, now, &diff);
508 tmp = os_reltime_in_ms(&diff);
509 if (next == -1 || tmp < next)
510 next = tmp;
511 return next;
512 }
513
514 tmp = nan_de_next_multicast(de, srv, now);
515 if (tmp >= 0 && (next == -1 || tmp < next))
516 next = tmp;
517
518 if (srv->type == NAN_DE_PUBLISH &&
519 os_reltime_initialized(&srv->last_multicast)) {
520 /* Time out after one transmission (and wait for FSD) */
521 tmp = srv->publish.fsd ? 1000 : 100;
522 if (next == -1 || tmp < next)
523 next = tmp;
524 }
525
526 if (srv->type == NAN_DE_SUBSCRIBE &&
527 os_reltime_initialized(&srv->first_discovered)) {
528 /* Time out after first DiscoveryResult event (and wait for
529 * FSD) */
530 tmp = srv->needs_fsd ? 1000 : 100;
531 if (next == -1 || tmp < next)
532 next = tmp;
533 }
534
535 if (os_reltime_initialized(&srv->next_publish_state)) {
536 os_reltime_sub(&srv->next_publish_state, now, &diff);
537 if (diff.sec < 0 || (diff.sec == 0 && diff.usec < 0))
538 tmp = 0;
539 else
540 tmp = os_reltime_in_ms(&diff);
541 if (next == -1 || tmp < next)
542 next = tmp;
543 }
544
545 return next;
546 }
547
548
nan_de_start_new_publish_state(struct nan_de_service * srv,bool force_single)549 static void nan_de_start_new_publish_state(struct nan_de_service *srv,
550 bool force_single)
551 {
552 unsigned int n;
553
554 if (force_single || !srv->freq_list || srv->freq_list[0] == 0)
555 srv->in_multi_chan = false;
556 else
557 srv->in_multi_chan = !srv->in_multi_chan;
558
559 /* Use hardcoded Nmin=5 and Nmax=10 and pick a random N from that range.
560 * Use same values for M. */
561 n = 5 + os_random() % 5;
562 srv->next_publish_duration = n * 100;
563
564 nan_de_set_publish_times(srv);
565
566 if (os_reltime_initialized(&srv->pause_state_end))
567 return;
568
569 if (srv->in_multi_chan && srv->freq_list && srv->freq_list[0]) {
570 if (!srv->first_multi_chan)
571 srv->multi_chan_idx++;
572 if (srv->freq_list[srv->multi_chan_idx] == 0)
573 srv->multi_chan_idx = 0;
574 srv->first_multi_chan = false;
575 srv->freq = srv->freq_list[srv->multi_chan_idx];
576 } else {
577 srv->freq = srv->default_freq;
578 }
579
580 wpa_printf(MSG_DEBUG,
581 "NAN: Publish in %s channel state for %u TU; starting with %u MHz",
582 srv->in_multi_chan ? "multi" : "single", n * 100, srv->freq);
583 }
584
585
nan_de_timer(void * eloop_ctx,void * timeout_ctx)586 static void nan_de_timer(void *eloop_ctx, void *timeout_ctx)
587 {
588 struct nan_de *de = eloop_ctx;
589 unsigned int i;
590 int next = -1;
591 bool started = false;
592 struct os_reltime now;
593
594 os_get_reltime(&now);
595
596 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) {
597 struct nan_de_service *srv = de->service[i];
598 int srv_next;
599
600 if (!srv)
601 continue;
602
603 if (nan_de_srv_expired(srv, &now)) {
604 wpa_printf(MSG_DEBUG, "NAN: Service id %d expired",
605 srv->id);
606 nan_de_del_srv(de, srv, NAN_DE_REASON_TIMEOUT);
607 continue;
608 }
609
610 if (os_reltime_initialized(&srv->next_publish_state) &&
611 os_reltime_before(&srv->next_publish_state, &now))
612 nan_de_start_new_publish_state(srv, false);
613
614 if (srv->type == NAN_DE_PUBLISH &&
615 os_reltime_initialized(&srv->pause_state_end) &&
616 (os_reltime_before(&srv->pause_state_end, &now)))
617 nan_de_unpause_state(srv);
618
619 srv_next = nan_de_srv_time_to_next(de, srv, &now);
620 if (srv_next >= 0 && (next == -1 || srv_next < next))
621 next = srv_next;
622
623 if (srv->type == NAN_DE_PUBLISH &&
624 srv->publish.fsd &&
625 os_reltime_initialized(&srv->pause_state_end) &&
626 de->tx_wait_end_freq == 0 &&
627 de->listen_freq == 0 && de->ext_listen_freq == 0) {
628 struct os_reltime diff;
629 int duration;
630
631 os_reltime_sub(&srv->pause_state_end, &now, &diff);
632 duration = os_reltime_in_ms(&diff);
633 if (duration < 0)
634 continue;
635 if ((unsigned int) duration > de->max_listen)
636 duration = de->max_listen;
637 if (de->cb.listen(de->cb.ctx, srv->freq, duration) ==
638 0) {
639 wpa_printf(MSG_DEBUG,
640 "NAN: Publisher in pauseState - started listen on %u MHz",
641 srv->freq);
642 de->listen_freq = srv->freq;
643 return;
644 }
645 }
646
647 if (srv_next == 0 && !started && !de->offload &&
648 de->listen_freq == 0 && de->ext_listen_freq == 0 &&
649 de->tx_wait_end_freq == 0 &&
650 nan_de_next_multicast(de, srv, &now) == 0) {
651 started = true;
652 nan_de_tx_multicast(de, srv, 0);
653 }
654
655 if (!started && !de->offload && de->cb.listen &&
656 de->listen_freq == 0 && de->ext_listen_freq == 0 &&
657 de->tx_wait_end_freq == 0 &&
658 ((srv->type == NAN_DE_PUBLISH &&
659 !srv->publish.unsolicited && srv->publish.solicited) ||
660 (srv->type == NAN_DE_SUBSCRIBE &&
661 !srv->subscribe.active))) {
662 int duration = 1000;
663
664 if (srv->type == NAN_DE_PUBLISH) {
665 nan_de_check_chan_change(srv);
666 duration = nan_de_time_to_next_chan_change(srv);
667 if (duration < 150)
668 duration = 150;
669 }
670
671 started = true;
672 if (de->cb.listen(de->cb.ctx, srv->freq, duration) == 0)
673 de->listen_freq = srv->freq;
674 }
675
676 }
677
678 if (next < 0)
679 return;
680
681 if (next == 0)
682 next = 1;
683
684 eloop_register_timeout(next / 1000, (next % 1000) * 1000, nan_de_timer,
685 de, NULL);
686 }
687
688
nan_de_run_timer(struct nan_de * de)689 static void nan_de_run_timer(struct nan_de *de)
690 {
691 eloop_cancel_timeout(nan_de_timer, de, NULL);
692 eloop_register_timeout(0, 0, nan_de_timer, de, NULL);
693 }
694
695
nan_de_deinit(struct nan_de * de)696 void nan_de_deinit(struct nan_de *de)
697 {
698 eloop_cancel_timeout(nan_de_timer, de, NULL);
699 nan_de_flush(de);
700 os_free(de);
701 }
702
703
nan_de_listen_started(struct nan_de * de,unsigned int freq,unsigned int duration)704 void nan_de_listen_started(struct nan_de *de, unsigned int freq,
705 unsigned int duration)
706 {
707 if (freq != de->listen_freq)
708 de->ext_listen_freq = freq;
709 }
710
711
nan_de_listen_ended(struct nan_de * de,unsigned int freq)712 void nan_de_listen_ended(struct nan_de *de, unsigned int freq)
713 {
714 if (freq == de->ext_listen_freq)
715 de->ext_listen_freq = 0;
716
717 if (freq == de->listen_freq) {
718 de->listen_freq = 0;
719 nan_de_run_timer(de);
720 }
721 }
722
723
nan_de_tx_status(struct nan_de * de,unsigned int freq,const u8 * dst)724 void nan_de_tx_status(struct nan_de *de, unsigned int freq, const u8 *dst)
725 {
726 if (freq == de->tx_wait_status_freq)
727 de->tx_wait_status_freq = 0;
728 }
729
730
nan_de_tx_wait_ended(struct nan_de * de)731 void nan_de_tx_wait_ended(struct nan_de *de)
732 {
733 if (de->tx_wait_end_freq)
734 wpa_printf(MSG_DEBUG,
735 "NAN: TX wait for response ended (freq=%u)",
736 de->tx_wait_end_freq);
737 de->tx_wait_end_freq = 0;
738 nan_de_run_timer(de);
739 }
740
741
742 static const u8 *
nan_de_get_attr(const u8 * buf,size_t len,enum nan_attr_id id,unsigned int skip)743 nan_de_get_attr(const u8 *buf, size_t len, enum nan_attr_id id,
744 unsigned int skip)
745 {
746 const u8 *pos = buf, *end = buf + len;
747
748 while (end - pos >= NAN_ATTR_HDR_LEN) {
749 const u8 *attr = pos;
750 u8 attr_id;
751 u16 attr_len;
752
753 attr_id = *pos++;
754 attr_len = WPA_GET_LE16(pos);
755 pos += 2;
756 if (attr_len > end - pos) {
757 wpa_printf(MSG_DEBUG,
758 "NAN: Truncated attribute %u (len %u; left %zu)",
759 attr_id, attr_len, end - pos);
760 break;
761 }
762
763 if (attr_id == id) {
764 if (skip == 0)
765 return attr;
766 skip--;
767 }
768
769 pos += attr_len;
770 }
771
772 return NULL;
773 }
774
775
nan_de_get_sdea(const u8 * buf,size_t len,u8 instance_id,u16 * sdea_control,enum nan_service_protocol_type * srv_proto_type,const u8 ** ssi,size_t * ssi_len)776 static void nan_de_get_sdea(const u8 *buf, size_t len, u8 instance_id,
777 u16 *sdea_control,
778 enum nan_service_protocol_type *srv_proto_type,
779 const u8 **ssi, size_t *ssi_len)
780 {
781 unsigned int skip;
782 const u8 *sdea, *end;
783 u16 sdea_len;
784
785 for (skip = 0; ; skip++) {
786 sdea = nan_de_get_attr(buf, len, NAN_ATTR_SDEA, skip);
787 if (!sdea)
788 break;
789
790 sdea++;
791 sdea_len = WPA_GET_LE16(sdea);
792 sdea += 2;
793 if (sdea_len < 1 + 2)
794 continue;
795 end = sdea + sdea_len;
796
797 if (instance_id != *sdea++)
798 continue; /* Mismatching Instance ID */
799
800 *sdea_control = WPA_GET_LE16(sdea);
801 sdea += 2;
802
803 if (*sdea_control & NAN_SDEA_CTRL_RANGE_LIMIT) {
804 if (end - sdea < 4)
805 continue;
806 sdea += 4;
807 }
808
809 if (*sdea_control & NAN_SDEA_CTRL_SRV_UPD_INDIC) {
810 if (end - sdea < 1)
811 continue;
812 sdea++;
813 }
814
815 if (end - sdea >= 2) {
816 u16 srv_info_len;
817
818 srv_info_len = WPA_GET_LE16(sdea);
819 sdea += 2;
820
821 if (srv_info_len > end - sdea)
822 continue;
823
824 if (srv_info_len >= 4 &&
825 WPA_GET_BE24(sdea) == OUI_WFA) {
826 *srv_proto_type = sdea[3];
827 *ssi = sdea + 4;
828 *ssi_len = srv_info_len - 4;
829 }
830 }
831 }
832 }
833
834
nan_de_process_elem_container(struct nan_de * de,const u8 * buf,size_t len,const u8 * peer_addr,unsigned int freq,bool p2p)835 static void nan_de_process_elem_container(struct nan_de *de, const u8 *buf,
836 size_t len, const u8 *peer_addr,
837 unsigned int freq, bool p2p)
838 {
839 const u8 *elem;
840 u16 elem_len;
841
842 elem = nan_de_get_attr(buf, len, NAN_ATTR_ELEM_CONTAINER, 0);
843 if (!elem)
844 return;
845
846 elem++;
847 elem_len = WPA_GET_LE16(elem);
848 elem += 2;
849 /* Skip the attribute if there is not enough froom for an element. */
850 if (elem_len < 1 + 2)
851 return;
852
853 /* Skip Map ID */
854 elem++;
855 elem_len--;
856
857 if (p2p && de->cb.process_p2p_usd_elems)
858 de->cb.process_p2p_usd_elems(de->cb.ctx, elem, elem_len,
859 peer_addr, freq);
860 }
861
862
nan_de_rx_publish(struct nan_de * de,struct nan_de_service * srv,const u8 * peer_addr,const u8 * a3,u8 instance_id,u8 req_instance_id,u16 sdea_control,enum nan_service_protocol_type srv_proto_type,const u8 * ssi,size_t ssi_len)863 static void nan_de_rx_publish(struct nan_de *de, struct nan_de_service *srv,
864 const u8 *peer_addr, const u8 *a3, u8 instance_id,
865 u8 req_instance_id, u16 sdea_control,
866 enum nan_service_protocol_type srv_proto_type,
867 const u8 *ssi, size_t ssi_len)
868 {
869 /* Subscribe function processing of a receive Publish message */
870 if (!os_reltime_initialized(&srv->first_discovered)) {
871 os_get_reltime(&srv->first_discovered);
872 srv->needs_fsd = sdea_control & NAN_SDEA_CTRL_FSD_REQ;
873 nan_de_run_timer(de);
874 }
875
876 if (!de->offload && srv->subscribe.active && req_instance_id == 0) {
877 /* Active subscriber replies with a Subscribe message if it
878 * received a matching unsolicited Publish message. */
879 nan_de_tx_multicast(de, srv, instance_id);
880 }
881
882 if (!de->offload && !srv->subscribe.active && req_instance_id == 0) {
883 /* Passive subscriber replies with a Follow-up message without
884 * Service Specific Info field if it received a matching
885 * unsolicited Publish message. */
886 nan_de_transmit(de, srv->id, NULL, NULL, peer_addr,
887 instance_id);
888 }
889
890 if (de->cb.discovery_result)
891 de->cb.discovery_result(
892 de->cb.ctx, srv->id, srv_proto_type,
893 ssi, ssi_len, instance_id,
894 peer_addr,
895 sdea_control & NAN_SDEA_CTRL_FSD_REQ,
896 sdea_control & NAN_SDEA_CTRL_FSD_GAS);
897 }
898
899
nan_de_filter_match(struct nan_de_service * srv,const u8 * matching_filter,size_t matching_filter_len)900 static bool nan_de_filter_match(struct nan_de_service *srv,
901 const u8 *matching_filter,
902 size_t matching_filter_len)
903 {
904 const u8 *pos, *end;
905
906 /* Since we do not currently support matching_filter_rx values for the
907 * local Publish function, any matching filter with at least one
908 * <length,value> pair with length larger than zero implies a mismatch.
909 */
910
911 if (!matching_filter)
912 return true;
913
914 pos = matching_filter;
915 end = matching_filter + matching_filter_len;
916
917 while (pos < end) {
918 u8 len;
919
920 len = *pos++;
921 if (len > end - pos)
922 break;
923 if (len) {
924 /* A non-empty Matching Filter entry: no match since
925 * there is no local matching_filter_rx. */
926 return false;
927 }
928 }
929
930 return true;
931 }
932
933
nan_de_rx_subscribe(struct nan_de * de,struct nan_de_service * srv,const u8 * peer_addr,const u8 * a3,u8 instance_id,const u8 * matching_filter,size_t matching_filter_len,enum nan_service_protocol_type srv_proto_type,const u8 * ssi,size_t ssi_len)934 static void nan_de_rx_subscribe(struct nan_de *de, struct nan_de_service *srv,
935 const u8 *peer_addr, const u8 *a3,
936 u8 instance_id,
937 const u8 *matching_filter,
938 size_t matching_filter_len,
939 enum nan_service_protocol_type srv_proto_type,
940 const u8 *ssi, size_t ssi_len)
941 {
942 struct wpabuf *buf;
943 size_t len = 0, sda_len, sdea_len;
944 u8 ctrl = 0;
945 u16 sdea_ctrl = 0;
946 const u8 *network_id;
947
948 /* Publish function processing of a receive Subscribe message */
949
950 if (!nan_de_filter_match(srv, matching_filter, matching_filter_len))
951 return;
952
953 if (!srv->publish.solicited)
954 return;
955
956 if (os_reltime_initialized(&srv->pause_state_end) &&
957 (!ether_addr_equal(peer_addr, srv->sel_peer_addr) ||
958 instance_id != srv->sel_peer_id)) {
959 wpa_printf(MSG_DEBUG,
960 "NAN: In pauseState - ignore Subscribe message from another subscriber");
961 return;
962 }
963
964 if (de->offload)
965 goto offload;
966
967 /* Reply with a solicited Publish message */
968 /* Service Descriptor attribute */
969 sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1;
970 len += NAN_ATTR_HDR_LEN + sda_len;
971
972 /* Service Descriptor Extension attribute */
973 sdea_len = 1 + 2;
974 if (srv->ssi)
975 sdea_len += 2 + 4 + wpabuf_len(srv->ssi);
976 len += NAN_ATTR_HDR_LEN + sdea_len;
977
978 /* Element Container attribute */
979 if (srv->elems)
980 len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems);
981
982 buf = nan_de_alloc_sdf(len);
983 if (!buf)
984 return;
985
986 /* Service Descriptor attribute */
987 wpabuf_put_u8(buf, NAN_ATTR_SDA);
988 wpabuf_put_le16(buf, sda_len);
989 wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN);
990 wpabuf_put_u8(buf, srv->id); /* Instance ID */
991 wpabuf_put_u8(buf, instance_id); /* Requestor Instance ID */
992 ctrl |= NAN_SRV_CTRL_PUBLISH;
993 wpabuf_put_u8(buf, ctrl);
994
995 /* Service Descriptor Extension attribute */
996 if (srv->type == NAN_DE_PUBLISH || srv->ssi) {
997 wpabuf_put_u8(buf, NAN_ATTR_SDEA);
998 wpabuf_put_le16(buf, sdea_len);
999 wpabuf_put_u8(buf, srv->id); /* Instance ID */
1000 if (srv->type == NAN_DE_PUBLISH) {
1001 if (srv->publish.fsd)
1002 sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ;
1003 if (srv->publish.fsd_gas)
1004 sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS;
1005 }
1006 wpabuf_put_le16(buf, sdea_ctrl);
1007 if (srv->ssi) {
1008 wpabuf_put_le16(buf, 4 + wpabuf_len(srv->ssi));
1009 wpabuf_put_be24(buf, OUI_WFA);
1010 wpabuf_put_u8(buf, srv->srv_proto_type);
1011 wpabuf_put_buf(buf, srv->ssi);
1012 }
1013 }
1014
1015 /* Element Container attribute */
1016 if (srv->elems) {
1017 wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER);
1018 wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems));
1019 wpabuf_put_u8(buf, 0); /* Map ID */
1020 wpabuf_put_buf(buf, srv->elems);
1021 }
1022
1023 if (srv->is_p2p)
1024 network_id = p2p_network_id;
1025 else
1026 network_id = nan_network_id;
1027
1028 if (srv->publish.solicited_multicast || !a3)
1029 a3 = network_id;
1030 else if (srv->is_p2p)
1031 a3 = de->nmi;
1032
1033 nan_de_tx(de, srv->freq, 100,
1034 srv->publish.solicited_multicast ? network_id : peer_addr,
1035 de->nmi, a3, buf);
1036 wpabuf_free(buf);
1037
1038 if (!srv->is_p2p)
1039 nan_de_pause_state(srv, peer_addr, instance_id);
1040
1041 offload:
1042 if (!srv->publish.disable_events && de->cb.replied)
1043 de->cb.replied(de->cb.ctx, srv->id, peer_addr, instance_id,
1044 srv_proto_type, ssi, ssi_len);
1045 }
1046
1047
nan_de_rx_follow_up(struct nan_de * de,struct nan_de_service * srv,const u8 * peer_addr,const u8 * a3,u8 instance_id,const u8 * ssi,size_t ssi_len)1048 static void nan_de_rx_follow_up(struct nan_de *de, struct nan_de_service *srv,
1049 const u8 *peer_addr, const u8 *a3,
1050 u8 instance_id, const u8 *ssi, size_t ssi_len)
1051 {
1052 /* Follow-up function processing of a receive Follow-up message for a
1053 * Subscribe or Publish instance */
1054
1055 if (srv->type == NAN_DE_PUBLISH &&
1056 os_reltime_initialized(&srv->pause_state_end) &&
1057 (!ether_addr_equal(peer_addr, srv->sel_peer_addr) ||
1058 instance_id != srv->sel_peer_id ||
1059 !ssi)) {
1060 wpa_printf(MSG_DEBUG,
1061 "NAN: In pauseState - ignore Follow-up message from another subscriber or without ssi");
1062 return;
1063 }
1064
1065 if (srv->type == NAN_DE_PUBLISH && !ssi)
1066 nan_de_pause_state(srv, peer_addr, instance_id);
1067
1068 os_memcpy(srv->a3, a3, ETH_ALEN);
1069 srv->a3_set = true;
1070
1071 if (de->cb.receive)
1072 de->cb.receive(de->cb.ctx, srv->id, instance_id, ssi, ssi_len,
1073 peer_addr);
1074 }
1075
1076
nan_de_rx_sda(struct nan_de * de,const u8 * peer_addr,const u8 * a3,unsigned int freq,const u8 * buf,size_t len,const u8 * sda,size_t sda_len)1077 static void nan_de_rx_sda(struct nan_de *de, const u8 *peer_addr, const u8 *a3,
1078 unsigned int freq, const u8 *buf, size_t len,
1079 const u8 *sda, size_t sda_len)
1080 {
1081 const u8 *service_id;
1082 u8 instance_id, req_instance_id, ctrl;
1083 u16 sdea_control = 0;
1084 unsigned int i;
1085 enum nan_service_control_type type = 0;
1086 enum nan_service_protocol_type srv_proto_type = 0;
1087 const u8 *ssi = NULL;
1088 size_t ssi_len = 0;
1089 bool first = true;
1090 const u8 *end;
1091 const u8 *matching_filter = NULL;
1092 size_t matching_filter_len = 0;
1093
1094 if (sda_len < NAN_SERVICE_ID_LEN + 1 + 1 + 1)
1095 return;
1096 end = sda + sda_len;
1097
1098 service_id = sda;
1099 sda += NAN_SERVICE_ID_LEN;
1100 instance_id = *sda++;
1101 req_instance_id = *sda++;
1102 ctrl = *sda;
1103 type = ctrl & NAN_SRV_CTRL_TYPE_MASK;
1104 wpa_printf(MSG_DEBUG,
1105 "NAN: SDA - Service ID %02x%02x%02x%02x%02x%02x Instance ID %u Requestor Instance ID %u Service Control 0x%x (Service Control Type %u)",
1106 MAC2STR(service_id), instance_id, req_instance_id,
1107 ctrl, type);
1108 if (type != NAN_SRV_CTRL_PUBLISH &&
1109 type != NAN_SRV_CTRL_SUBSCRIBE &&
1110 type != NAN_SRV_CTRL_FOLLOW_UP) {
1111 wpa_printf(MSG_DEBUG,
1112 "NAN: Discard SDF with unknown Service Control Type %u",
1113 type);
1114 return;
1115 }
1116
1117 if (ctrl & NAN_SRV_CTRL_BINDING_BITMAP) {
1118 if (end - sda < 2)
1119 return;
1120 sda += 2;
1121 }
1122
1123 if (ctrl & NAN_SRV_CTRL_MATCHING_FILTER) {
1124 u8 flen;
1125
1126 if (end - sda < 1)
1127 return;
1128 flen = *sda++;
1129 if (end - sda < flen)
1130 return;
1131 matching_filter = sda;
1132 matching_filter_len = flen;
1133 sda += flen;
1134 }
1135
1136 if (ctrl & NAN_SRV_CTRL_RESP_FILTER) {
1137 u8 flen;
1138
1139 if (end - sda < 1)
1140 return;
1141 flen = *sda++;
1142 if (end - sda < flen)
1143 return;
1144 sda += flen;
1145 }
1146
1147 if (ctrl & NAN_SRV_CTRL_SRV_INFO) {
1148 u8 flen;
1149
1150 if (end - sda < 1)
1151 return;
1152 flen = *sda++;
1153 if (end - sda < flen)
1154 return;
1155 if (flen >= 4 && WPA_GET_BE24(sda) == OUI_WFA) {
1156 srv_proto_type = sda[3];
1157 ssi = sda + 4;
1158 ssi_len = flen - 4;
1159 wpa_printf(MSG_DEBUG, "NAN: Service Protocol Type %d",
1160 srv_proto_type);
1161 wpa_hexdump(MSG_MSGDUMP, "NAN: ssi", ssi, ssi_len);
1162 }
1163 sda += flen;
1164 }
1165
1166 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) {
1167 struct nan_de_service *srv = de->service[i];
1168
1169 if (!srv)
1170 continue;
1171 if (os_memcmp(srv->service_id, service_id,
1172 NAN_SERVICE_ID_LEN) != 0)
1173 continue;
1174 if (type == NAN_SRV_CTRL_PUBLISH) {
1175 if (srv->type == NAN_DE_PUBLISH)
1176 continue;
1177 if (req_instance_id && srv->id != req_instance_id)
1178 continue;
1179 }
1180 if (type == NAN_SRV_CTRL_SUBSCRIBE &&
1181 srv->type == NAN_DE_SUBSCRIBE)
1182 continue;
1183 wpa_printf(MSG_DEBUG, "NAN: Received SDF matches service ID %u",
1184 i + 1);
1185
1186 if (first) {
1187 first = false;
1188 nan_de_get_sdea(buf, len, instance_id, &sdea_control,
1189 &srv_proto_type, &ssi, &ssi_len);
1190
1191 if (ssi) {
1192 wpa_printf(MSG_DEBUG,
1193 "NAN: Service Protocol Type %d",
1194 srv_proto_type);
1195 wpa_hexdump(MSG_MSGDUMP, "NAN: ssi",
1196 ssi, ssi_len);
1197 }
1198 nan_de_process_elem_container(de, buf, len, peer_addr,
1199 freq, srv->is_p2p);
1200 }
1201
1202 switch (type) {
1203 case NAN_SRV_CTRL_PUBLISH:
1204 nan_de_rx_publish(de, srv, peer_addr, a3, instance_id,
1205 req_instance_id,
1206 sdea_control, srv_proto_type,
1207 ssi, ssi_len);
1208 break;
1209 case NAN_SRV_CTRL_SUBSCRIBE:
1210 nan_de_rx_subscribe(de, srv, peer_addr, a3, instance_id,
1211 matching_filter,
1212 matching_filter_len,
1213 srv_proto_type,
1214 ssi, ssi_len);
1215 break;
1216 case NAN_SRV_CTRL_FOLLOW_UP:
1217 nan_de_rx_follow_up(de, srv, peer_addr, a3, instance_id,
1218 ssi, ssi_len);
1219 break;
1220 }
1221 }
1222 }
1223
1224
nan_de_rx_sdf(struct nan_de * de,const u8 * peer_addr,const u8 * a3,unsigned int freq,const u8 * buf,size_t len)1225 void nan_de_rx_sdf(struct nan_de *de, const u8 *peer_addr, const u8 *a3,
1226 unsigned int freq, const u8 *buf, size_t len)
1227 {
1228 const u8 *sda;
1229 u16 sda_len;
1230 unsigned int skip;
1231
1232 if (!de->num_service)
1233 return;
1234
1235 wpa_printf(MSG_DEBUG, "NAN: RX SDF from " MACSTR " freq=%u len=%zu",
1236 MAC2STR(peer_addr), freq, len);
1237
1238 wpa_hexdump(MSG_MSGDUMP, "NAN: SDF payload", buf, len);
1239
1240 for (skip = 0; ; skip++) {
1241 sda = nan_de_get_attr(buf, len, NAN_ATTR_SDA, skip);
1242 if (!sda)
1243 break;
1244
1245 sda++;
1246 sda_len = WPA_GET_LE16(sda);
1247 sda += 2;
1248 nan_de_rx_sda(de, peer_addr, a3, freq, buf, len, sda, sda_len);
1249 }
1250 }
1251
1252
nan_de_get_handle(struct nan_de * de)1253 static int nan_de_get_handle(struct nan_de *de)
1254 {
1255 int i = de->next_handle;
1256
1257 if (de->num_service >= NAN_DE_MAX_SERVICE)
1258 goto fail;
1259
1260 do {
1261 if (!de->service[i]) {
1262 de->next_handle = (i + 1) % NAN_DE_MAX_SERVICE;
1263 return i + 1;
1264 }
1265 i = (i + 1) % NAN_DE_MAX_SERVICE;
1266 } while (i != de->next_handle);
1267
1268 fail:
1269 wpa_printf(MSG_DEBUG, "NAN: No more room for a new service");
1270 return -1;
1271 }
1272
1273
nan_de_derive_service_id(struct nan_de_service * srv)1274 static int nan_de_derive_service_id(struct nan_de_service *srv)
1275 {
1276 u8 hash[SHA256_MAC_LEN];
1277 char *name, *pos;
1278 int ret;
1279 const u8 *addr[1];
1280 size_t len[1];
1281
1282 name = os_strdup(srv->service_name);
1283 if (!name)
1284 return -1;
1285 pos = name;
1286 while (*pos) {
1287 *pos = tolower(*pos);
1288 pos++;
1289 }
1290
1291 addr[0] = (u8 *) name;
1292 len[0] = os_strlen(name);
1293 ret = sha256_vector(1, addr, len, hash);
1294 os_free(name);
1295 if (ret == 0)
1296 os_memcpy(srv->service_id, hash, NAN_SERVICE_ID_LEN);
1297
1298 return ret;
1299 }
1300
1301
nan_de_get_service_id(struct nan_de * de,int id)1302 const u8 * nan_de_get_service_id(struct nan_de *de, int id)
1303 {
1304 struct nan_de_service *srv;
1305
1306 if (id < 1 || id > NAN_DE_MAX_SERVICE)
1307 return NULL;
1308 srv = de->service[id - 1];
1309 if (!srv)
1310 return NULL;
1311 return srv->service_id;
1312 }
1313
1314
nan_de_publish(struct nan_de * de,const char * service_name,enum nan_service_protocol_type srv_proto_type,const struct wpabuf * ssi,const struct wpabuf * elems,struct nan_publish_params * params,bool p2p)1315 int nan_de_publish(struct nan_de *de, const char *service_name,
1316 enum nan_service_protocol_type srv_proto_type,
1317 const struct wpabuf *ssi, const struct wpabuf *elems,
1318 struct nan_publish_params *params, bool p2p)
1319 {
1320 int publish_id;
1321 struct nan_de_service *srv;
1322
1323 if (!service_name) {
1324 wpa_printf(MSG_DEBUG, "NAN: Publish() - no service_name");
1325 return -1;
1326 }
1327
1328 if (!params->unsolicited && !params->solicited) {
1329 wpa_printf(MSG_INFO,
1330 "NAN: Publish() - both unsolicited and solicited disabled is invalid");
1331 return -1;
1332 }
1333
1334 publish_id = nan_de_get_handle(de);
1335 if (publish_id < 1)
1336 return -1;
1337
1338 srv = os_zalloc(sizeof(*srv));
1339 if (!srv)
1340 return -1;
1341 srv->type = NAN_DE_PUBLISH;
1342 srv->freq = srv->default_freq = params->freq;
1343 srv->service_name = os_strdup(service_name);
1344 if (!srv->service_name)
1345 goto fail;
1346 if (nan_de_derive_service_id(srv) < 0)
1347 goto fail;
1348 os_memcpy(&srv->publish, params, sizeof(*params));
1349
1350 if (params->freq_list) {
1351 size_t len;
1352
1353 len = (int_array_len(params->freq_list) + 1) * sizeof(int);
1354 srv->freq_list = os_memdup(params->freq_list, len);
1355 if (!srv->freq_list)
1356 goto fail;
1357 }
1358 srv->publish.freq_list = NULL;
1359
1360 srv->srv_proto_type = srv_proto_type;
1361 if (ssi) {
1362 srv->ssi = wpabuf_dup(ssi);
1363 if (!srv->ssi)
1364 goto fail;
1365 }
1366 if (elems) {
1367 srv->elems = wpabuf_dup(elems);
1368 if (!srv->elems)
1369 goto fail;
1370 }
1371
1372 /* Prepare for single and multi-channel states; starting with
1373 * single channel */
1374 srv->first_multi_chan = true;
1375 nan_de_start_new_publish_state(srv, true);
1376
1377 wpa_printf(MSG_DEBUG, "NAN: Assigned new publish handle %d for %s",
1378 publish_id, service_name);
1379 srv->id = publish_id;
1380 srv->is_p2p = p2p;
1381 nan_de_add_srv(de, srv);
1382 nan_de_run_timer(de);
1383 return publish_id;
1384 fail:
1385 nan_de_service_free(srv);
1386 return -1;
1387 }
1388
1389
nan_de_cancel_publish(struct nan_de * de,int publish_id)1390 void nan_de_cancel_publish(struct nan_de *de, int publish_id)
1391 {
1392 struct nan_de_service *srv;
1393
1394 wpa_printf(MSG_DEBUG, "NAN: CancelPublish(publish_id=%d)", publish_id);
1395
1396 if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE)
1397 return;
1398 srv = de->service[publish_id - 1];
1399 if (!srv || srv->type != NAN_DE_PUBLISH)
1400 return;
1401 nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST);
1402 }
1403
1404
nan_de_update_publish(struct nan_de * de,int publish_id,const struct wpabuf * ssi)1405 int nan_de_update_publish(struct nan_de *de, int publish_id,
1406 const struct wpabuf *ssi)
1407 {
1408 struct nan_de_service *srv;
1409
1410 wpa_printf(MSG_DEBUG, "NAN: UpdatePublish(publish_id=%d)", publish_id);
1411
1412 if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE)
1413 return -1;
1414 srv = de->service[publish_id - 1];
1415 if (!srv || srv->type != NAN_DE_PUBLISH)
1416 return -1;
1417
1418 wpabuf_free(srv->ssi);
1419 srv->ssi = NULL;
1420 if (!ssi)
1421 return 0;
1422 srv->ssi = wpabuf_dup(ssi);
1423 if (!srv->ssi)
1424 return -1;
1425 return 0;
1426 }
1427
1428
nan_de_unpause_publish(struct nan_de * de,int publish_id,u8 peer_instance_id,const u8 * peer_addr)1429 int nan_de_unpause_publish(struct nan_de *de, int publish_id,
1430 u8 peer_instance_id, const u8 *peer_addr)
1431 {
1432 struct nan_de_service *srv;
1433
1434 wpa_printf(MSG_DEBUG,
1435 "NAN: UnpausePublish(publish_id=%d, peer_instance_id=%d peer_addr="
1436 MACSTR ")",
1437 publish_id, peer_instance_id, MAC2STR(peer_addr));
1438
1439 if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE)
1440 return -1;
1441 srv = de->service[publish_id - 1];
1442 if (!srv || srv->type != NAN_DE_PUBLISH)
1443 return -1;
1444
1445 if (srv->sel_peer_id != peer_instance_id ||
1446 !ether_addr_equal(peer_addr, srv->sel_peer_addr) ||
1447 !os_reltime_initialized(&srv->pause_state_end))
1448 return -1;
1449
1450 nan_de_unpause_state(srv);
1451 return 0;
1452 }
1453
1454
nan_de_subscribe(struct nan_de * de,const char * service_name,enum nan_service_protocol_type srv_proto_type,const struct wpabuf * ssi,const struct wpabuf * elems,struct nan_subscribe_params * params,bool p2p)1455 int nan_de_subscribe(struct nan_de *de, const char *service_name,
1456 enum nan_service_protocol_type srv_proto_type,
1457 const struct wpabuf *ssi, const struct wpabuf *elems,
1458 struct nan_subscribe_params *params, bool p2p)
1459 {
1460 int subscribe_id;
1461 struct nan_de_service *srv;
1462
1463 if (!service_name) {
1464 wpa_printf(MSG_DEBUG, "NAN: Subscribe() - no service_name");
1465 return -1;
1466 }
1467
1468 subscribe_id = nan_de_get_handle(de);
1469 if (subscribe_id < 1)
1470 return -1;
1471
1472 srv = os_zalloc(sizeof(*srv));
1473 if (!srv)
1474 return -1;
1475 srv->type = NAN_DE_SUBSCRIBE;
1476 srv->freq = params->freq;
1477 srv->service_name = os_strdup(service_name);
1478 if (!srv->service_name)
1479 goto fail;
1480 if (nan_de_derive_service_id(srv) < 0)
1481 goto fail;
1482 os_memcpy(&srv->subscribe, params, sizeof(*params));
1483
1484 if (params->freq_list) {
1485 size_t len;
1486
1487 len = (int_array_len(params->freq_list) + 1) * sizeof(int);
1488 srv->freq_list = os_memdup(params->freq_list, len);
1489 if (!srv->freq_list)
1490 goto fail;
1491 }
1492 srv->subscribe.freq_list = NULL;
1493
1494 srv->srv_proto_type = srv_proto_type;
1495 if (ssi) {
1496 srv->ssi = wpabuf_dup(ssi);
1497 if (!srv->ssi)
1498 goto fail;
1499 }
1500 if (elems) {
1501 srv->elems = wpabuf_dup(elems);
1502 if (!srv->elems)
1503 goto fail;
1504 }
1505
1506 wpa_printf(MSG_DEBUG, "NAN: Assigned new subscribe handle %d for %s",
1507 subscribe_id, service_name);
1508 srv->id = subscribe_id;
1509 srv->is_p2p = p2p;
1510 nan_de_add_srv(de, srv);
1511 nan_de_run_timer(de);
1512 return subscribe_id;
1513 fail:
1514 nan_de_service_free(srv);
1515 return -1;
1516 }
1517
1518
nan_de_cancel_subscribe(struct nan_de * de,int subscribe_id)1519 void nan_de_cancel_subscribe(struct nan_de *de, int subscribe_id)
1520 {
1521 struct nan_de_service *srv;
1522
1523 if (subscribe_id < 1 || subscribe_id > NAN_DE_MAX_SERVICE)
1524 return;
1525 srv = de->service[subscribe_id - 1];
1526 if (!srv || srv->type != NAN_DE_SUBSCRIBE)
1527 return;
1528 nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST);
1529 }
1530
1531
nan_de_transmit(struct nan_de * de,int handle,const struct wpabuf * ssi,const struct wpabuf * elems,const u8 * peer_addr,u8 req_instance_id)1532 int nan_de_transmit(struct nan_de *de, int handle,
1533 const struct wpabuf *ssi, const struct wpabuf *elems,
1534 const u8 *peer_addr, u8 req_instance_id)
1535 {
1536 struct nan_de_service *srv;
1537 const u8 *a3;
1538 const u8 *network_id;
1539
1540 if (handle < 1 || handle > NAN_DE_MAX_SERVICE)
1541 return -1;
1542
1543 srv = de->service[handle - 1];
1544 if (!srv)
1545 return -1;
1546
1547 if (srv->is_p2p)
1548 network_id = p2p_network_id;
1549 else
1550 network_id = nan_network_id;
1551
1552 if (srv->a3_set)
1553 a3 = srv->a3;
1554 else
1555 a3 = network_id;
1556 nan_de_tx_sdf(de, srv, 100, NAN_SRV_CTRL_FOLLOW_UP,
1557 peer_addr, a3, req_instance_id, ssi);
1558
1559 return 0;
1560 }
1561