Merge branch 'vendor/MDOCML'
[dragonfly.git] / contrib / hostapd / src / p2p / p2p_sd.c
1 /*
2  * Wi-Fi Direct - P2P service discovery
3  * Copyright (c) 2009, Atheros Communications
4  *
5  * This software may be distributed under the terms of the BSD license.
6  * See README for more details.
7  */
8
9 #include "includes.h"
10
11 #include "common.h"
12 #include "common/ieee802_11_defs.h"
13 #include "common/gas.h"
14 #include "p2p_i.h"
15 #include "p2p.h"
16
17
18 #ifdef CONFIG_WIFI_DISPLAY
19 static int wfd_wsd_supported(struct wpabuf *wfd)
20 {
21         const u8 *pos, *end;
22         u8 subelem;
23         u16 len;
24
25         if (wfd == NULL)
26                 return 0;
27
28         pos = wpabuf_head(wfd);
29         end = pos + wpabuf_len(wfd);
30
31         while (pos + 3 <= end) {
32                 subelem = *pos++;
33                 len = WPA_GET_BE16(pos);
34                 pos += 2;
35                 if (pos + len > end)
36                         break;
37
38                 if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
39                         u16 info = WPA_GET_BE16(pos);
40                         return !!(info & 0x0040);
41                 }
42
43                 pos += len;
44         }
45
46         return 0;
47 }
48 #endif /* CONFIG_WIFI_DISPLAY */
49
50 struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
51                                          struct p2p_device *dev)
52 {
53         struct p2p_sd_query *q;
54         int wsd = 0;
55
56         if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
57                 return NULL; /* peer does not support SD */
58 #ifdef CONFIG_WIFI_DISPLAY
59         if (wfd_wsd_supported(dev->info.wfd_subelems))
60                 wsd = 1;
61 #endif /* CONFIG_WIFI_DISPLAY */
62
63         for (q = p2p->sd_queries; q; q = q->next) {
64                 /* Use WSD only if the peer indicates support or it */
65                 if (q->wsd && !wsd)
66                         continue;
67                 if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
68                         return q;
69                 if (!q->for_all_peers &&
70                     os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
71                     0)
72                         return q;
73         }
74
75         return NULL;
76 }
77
78
79 static int p2p_unlink_sd_query(struct p2p_data *p2p,
80                                struct p2p_sd_query *query)
81 {
82         struct p2p_sd_query *q, *prev;
83         q = p2p->sd_queries;
84         prev = NULL;
85         while (q) {
86                 if (q == query) {
87                         if (prev)
88                                 prev->next = q->next;
89                         else
90                                 p2p->sd_queries = q->next;
91                         if (p2p->sd_query == query)
92                                 p2p->sd_query = NULL;
93                         return 1;
94                 }
95                 prev = q;
96                 q = q->next;
97         }
98         return 0;
99 }
100
101
102 static void p2p_free_sd_query(struct p2p_sd_query *q)
103 {
104         if (q == NULL)
105                 return;
106         wpabuf_free(q->tlvs);
107         os_free(q);
108 }
109
110
111 void p2p_free_sd_queries(struct p2p_data *p2p)
112 {
113         struct p2p_sd_query *q, *prev;
114         q = p2p->sd_queries;
115         p2p->sd_queries = NULL;
116         while (q) {
117                 prev = q;
118                 q = q->next;
119                 p2p_free_sd_query(prev);
120         }
121 }
122
123
124 static struct wpabuf * p2p_build_sd_query(u16 update_indic,
125                                           struct wpabuf *tlvs)
126 {
127         struct wpabuf *buf;
128         u8 *len_pos;
129
130         buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
131         if (buf == NULL)
132                 return NULL;
133
134         /* ANQP Query Request Frame */
135         len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
136         wpabuf_put_be24(buf, OUI_WFA);
137         wpabuf_put_u8(buf, P2P_OUI_TYPE);
138         wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
139         wpabuf_put_buf(buf, tlvs);
140         gas_anqp_set_element_len(buf, len_pos);
141
142         gas_anqp_set_len(buf);
143
144         return buf;
145 }
146
147
148 static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
149                                       u8 dialog_token, int freq)
150 {
151         struct wpabuf *req;
152
153         req = gas_build_comeback_req(dialog_token);
154         if (req == NULL)
155                 return;
156
157         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
158         if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
159                             wpabuf_head(req), wpabuf_len(req), 200) < 0)
160                 p2p_dbg(p2p, "Failed to send Action frame");
161
162         wpabuf_free(req);
163 }
164
165
166 static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
167                                              u16 comeback_delay,
168                                              u16 update_indic,
169                                              const struct wpabuf *tlvs)
170 {
171         struct wpabuf *buf;
172         u8 *len_pos;
173
174         buf = gas_anqp_build_initial_resp(dialog_token, status_code,
175                                           comeback_delay,
176                                           100 + (tlvs ? wpabuf_len(tlvs) : 0));
177         if (buf == NULL)
178                 return NULL;
179
180         if (tlvs) {
181                 /* ANQP Query Response Frame */
182                 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
183                 wpabuf_put_be24(buf, OUI_WFA);
184                 wpabuf_put_u8(buf, P2P_OUI_TYPE);
185                  /* Service Update Indicator */
186                 wpabuf_put_le16(buf, update_indic);
187                 wpabuf_put_buf(buf, tlvs);
188                 gas_anqp_set_element_len(buf, len_pos);
189         }
190
191         gas_anqp_set_len(buf);
192
193         return buf;
194 }
195
196
197 static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
198                                                    u16 status_code,
199                                                    u16 update_indic,
200                                                    const u8 *data, size_t len,
201                                                    u8 frag_id, u8 more,
202                                                    u16 total_len)
203 {
204         struct wpabuf *buf;
205
206         buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
207                                            more, 0, 100 + len);
208         if (buf == NULL)
209                 return NULL;
210
211         if (frag_id == 0) {
212                 /* ANQP Query Response Frame */
213                 wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
214                 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
215                 wpabuf_put_be24(buf, OUI_WFA);
216                 wpabuf_put_u8(buf, P2P_OUI_TYPE);
217                 /* Service Update Indicator */
218                 wpabuf_put_le16(buf, update_indic);
219         }
220
221         wpabuf_put_data(buf, data, len);
222         gas_anqp_set_len(buf);
223
224         return buf;
225 }
226
227
228 int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
229 {
230         struct wpabuf *req;
231         int ret = 0;
232         struct p2p_sd_query *query;
233         int freq;
234
235         freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
236         if (freq <= 0) {
237                 p2p_dbg(p2p, "No Listen/Operating frequency known for the peer "
238                         MACSTR " to send SD Request",
239                         MAC2STR(dev->info.p2p_device_addr));
240                 return -1;
241         }
242
243         query = p2p_pending_sd_req(p2p, dev);
244         if (query == NULL)
245                 return -1;
246
247         p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
248                 MAC2STR(dev->info.p2p_device_addr));
249
250         req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
251         if (req == NULL)
252                 return -1;
253
254         p2p->sd_peer = dev;
255         p2p->sd_query = query;
256         p2p->pending_action_state = P2P_PENDING_SD;
257
258         if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
259                             p2p->cfg->dev_addr, dev->info.p2p_device_addr,
260                             wpabuf_head(req), wpabuf_len(req), 5000) < 0) {
261                 p2p_dbg(p2p, "Failed to send Action frame");
262                 ret = -1;
263         }
264
265         wpabuf_free(req);
266
267         return ret;
268 }
269
270
271 void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
272                             const u8 *data, size_t len, int rx_freq)
273 {
274         const u8 *pos = data;
275         const u8 *end = data + len;
276         const u8 *next;
277         u8 dialog_token;
278         u16 slen;
279         int freq;
280         u16 update_indic;
281
282
283         if (p2p->cfg->sd_request == NULL)
284                 return;
285
286         if (rx_freq > 0)
287                 freq = rx_freq;
288         else
289                 freq = p2p_channel_to_freq(p2p->cfg->reg_class,
290                                            p2p->cfg->channel);
291         if (freq < 0)
292                 return;
293
294         if (len < 1 + 2)
295                 return;
296
297         dialog_token = *pos++;
298         p2p_dbg(p2p, "GAS Initial Request from " MACSTR
299                 " (dialog token %u, freq %d)",
300                 MAC2STR(sa), dialog_token, rx_freq);
301
302         if (*pos != WLAN_EID_ADV_PROTO) {
303                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
304                 return;
305         }
306         pos++;
307
308         slen = *pos++;
309         next = pos + slen;
310         if (next > end || slen < 2) {
311                 p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
312                 return;
313         }
314         pos++; /* skip QueryRespLenLimit and PAME-BI */
315
316         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
317                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
318                         *pos);
319                 return;
320         }
321
322         pos = next;
323         /* Query Request */
324         if (pos + 2 > end)
325                 return;
326         slen = WPA_GET_LE16(pos);
327         pos += 2;
328         if (pos + slen > end)
329                 return;
330         end = pos + slen;
331
332         /* ANQP Query Request */
333         if (pos + 4 > end)
334                 return;
335         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
336                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
337                 return;
338         }
339         pos += 2;
340
341         slen = WPA_GET_LE16(pos);
342         pos += 2;
343         if (pos + slen > end || slen < 3 + 1) {
344                 p2p_dbg(p2p, "Invalid ANQP Query Request length");
345                 return;
346         }
347
348         if (WPA_GET_BE24(pos) != OUI_WFA) {
349                 p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
350                 return;
351         }
352         pos += 3;
353
354         if (*pos != P2P_OUI_TYPE) {
355                 p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
356                 return;
357         }
358         pos++;
359
360         if (pos + 2 > end)
361                 return;
362         update_indic = WPA_GET_LE16(pos);
363         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
364         pos += 2;
365
366         p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
367                              update_indic, pos, end - pos);
368         /* the response will be indicated with a call to p2p_sd_response() */
369 }
370
371
372 void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
373                      u8 dialog_token, const struct wpabuf *resp_tlvs)
374 {
375         struct wpabuf *resp;
376
377         /* TODO: fix the length limit to match with the maximum frame length */
378         if (wpabuf_len(resp_tlvs) > 1400) {
379                 p2p_dbg(p2p, "SD response long enough to require fragmentation");
380                 if (p2p->sd_resp) {
381                         /*
382                          * TODO: Could consider storing the fragmented response
383                          * separately for each peer to avoid having to drop old
384                          * one if there is more than one pending SD query.
385                          * Though, that would eat more memory, so there are
386                          * also benefits to just using a single buffer.
387                          */
388                         p2p_dbg(p2p, "Drop previous SD response");
389                         wpabuf_free(p2p->sd_resp);
390                 }
391                 p2p->sd_resp = wpabuf_dup(resp_tlvs);
392                 if (p2p->sd_resp == NULL) {
393                         p2p_err(p2p, "Failed to allocate SD response fragmentation area");
394                         return;
395                 }
396                 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
397                 p2p->sd_resp_dialog_token = dialog_token;
398                 p2p->sd_resp_pos = 0;
399                 p2p->sd_frag_id = 0;
400                 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
401                                              1, p2p->srv_update_indic, NULL);
402         } else {
403                 p2p_dbg(p2p, "SD response fits in initial response");
404                 resp = p2p_build_sd_response(dialog_token,
405                                              WLAN_STATUS_SUCCESS, 0,
406                                              p2p->srv_update_indic, resp_tlvs);
407         }
408         if (resp == NULL)
409                 return;
410
411         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
412         if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
413                             p2p->cfg->dev_addr,
414                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
415                 p2p_dbg(p2p, "Failed to send Action frame");
416
417         wpabuf_free(resp);
418 }
419
420
421 void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
422                              const u8 *data, size_t len, int rx_freq)
423 {
424         const u8 *pos = data;
425         const u8 *end = data + len;
426         const u8 *next;
427         u8 dialog_token;
428         u16 status_code;
429         u16 comeback_delay;
430         u16 slen;
431         u16 update_indic;
432
433         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
434             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
435                 p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
436                         MACSTR, MAC2STR(sa));
437                 return;
438         }
439         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
440         p2p_clear_timeout(p2p);
441
442         p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
443                 MAC2STR(sa), (int) len);
444
445         if (len < 5 + 2) {
446                 p2p_dbg(p2p, "Too short GAS Initial Response frame");
447                 return;
448         }
449
450         dialog_token = *pos++;
451         /* TODO: check dialog_token match */
452         status_code = WPA_GET_LE16(pos);
453         pos += 2;
454         comeback_delay = WPA_GET_LE16(pos);
455         pos += 2;
456         p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
457                 dialog_token, status_code, comeback_delay);
458         if (status_code) {
459                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
460                         status_code);
461                 return;
462         }
463
464         if (*pos != WLAN_EID_ADV_PROTO) {
465                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
466                 return;
467         }
468         pos++;
469
470         slen = *pos++;
471         next = pos + slen;
472         if (next > end || slen < 2) {
473                 p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
474                 return;
475         }
476         pos++; /* skip QueryRespLenLimit and PAME-BI */
477
478         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
479                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
480                         *pos);
481                 return;
482         }
483
484         pos = next;
485         /* Query Response */
486         if (pos + 2 > end) {
487                 p2p_dbg(p2p, "Too short Query Response");
488                 return;
489         }
490         slen = WPA_GET_LE16(pos);
491         pos += 2;
492         p2p_dbg(p2p, "Query Response Length: %d", slen);
493         if (pos + slen > end) {
494                 p2p_dbg(p2p, "Not enough Query Response data");
495                 return;
496         }
497         end = pos + slen;
498
499         if (comeback_delay) {
500                 p2p_dbg(p2p, "Fragmented response - request fragments");
501                 if (p2p->sd_rx_resp) {
502                         p2p_dbg(p2p, "Drop old SD reassembly buffer");
503                         wpabuf_free(p2p->sd_rx_resp);
504                         p2p->sd_rx_resp = NULL;
505                 }
506                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
507                 return;
508         }
509
510         /* ANQP Query Response */
511         if (pos + 4 > end)
512                 return;
513         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
514                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
515                 return;
516         }
517         pos += 2;
518
519         slen = WPA_GET_LE16(pos);
520         pos += 2;
521         if (pos + slen > end || slen < 3 + 1) {
522                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
523                 return;
524         }
525
526         if (WPA_GET_BE24(pos) != OUI_WFA) {
527                 p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
528                 return;
529         }
530         pos += 3;
531
532         if (*pos != P2P_OUI_TYPE) {
533                 p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
534                 return;
535         }
536         pos++;
537
538         if (pos + 2 > end)
539                 return;
540         update_indic = WPA_GET_LE16(pos);
541         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
542         pos += 2;
543
544         p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
545         p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
546         p2p->sd_peer = NULL;
547
548         if (p2p->sd_query) {
549                 if (!p2p->sd_query->for_all_peers) {
550                         struct p2p_sd_query *q;
551                         p2p_dbg(p2p, "Remove completed SD query %p",
552                                 p2p->sd_query);
553                         q = p2p->sd_query;
554                         p2p_unlink_sd_query(p2p, p2p->sd_query);
555                         p2p_free_sd_query(q);
556                 }
557                 p2p->sd_query = NULL;
558         }
559
560         if (p2p->cfg->sd_response)
561                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
562                                       pos, end - pos);
563         p2p_continue_find(p2p);
564 }
565
566
567 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
568                              const u8 *data, size_t len, int rx_freq)
569 {
570         struct wpabuf *resp;
571         u8 dialog_token;
572         size_t frag_len;
573         int more = 0;
574
575         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
576         if (len < 1)
577                 return;
578         dialog_token = *data;
579         p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
580         if (dialog_token != p2p->sd_resp_dialog_token) {
581                 p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
582                         dialog_token);
583                 return;
584         }
585
586         if (p2p->sd_resp == NULL) {
587                 p2p_dbg(p2p, "No pending SD response fragment available");
588                 return;
589         }
590         if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
591                 p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
592                         MAC2STR(sa));
593                 return;
594         }
595
596         frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
597         if (frag_len > 1400) {
598                 frag_len = 1400;
599                 more = 1;
600         }
601         resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
602                                            p2p->srv_update_indic,
603                                            wpabuf_head_u8(p2p->sd_resp) +
604                                            p2p->sd_resp_pos, frag_len,
605                                            p2p->sd_frag_id, more,
606                                            wpabuf_len(p2p->sd_resp));
607         if (resp == NULL)
608                 return;
609         p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
610                 p2p->sd_frag_id, more, (int) frag_len);
611         p2p->sd_frag_id++;
612         p2p->sd_resp_pos += frag_len;
613
614         if (more) {
615                 p2p_dbg(p2p, "%d more bytes remain to be sent",
616                         (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
617         } else {
618                 p2p_dbg(p2p, "All fragments of SD response sent");
619                 wpabuf_free(p2p->sd_resp);
620                 p2p->sd_resp = NULL;
621         }
622
623         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
624         if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
625                             p2p->cfg->dev_addr,
626                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
627                 p2p_dbg(p2p, "Failed to send Action frame");
628
629         wpabuf_free(resp);
630 }
631
632
633 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
634                               const u8 *data, size_t len, int rx_freq)
635 {
636         const u8 *pos = data;
637         const u8 *end = data + len;
638         const u8 *next;
639         u8 dialog_token;
640         u16 status_code;
641         u8 frag_id;
642         u8 more_frags;
643         u16 comeback_delay;
644         u16 slen;
645
646         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
647
648         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
649             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
650                 p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
651                         MACSTR, MAC2STR(sa));
652                 return;
653         }
654         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
655         p2p_clear_timeout(p2p);
656
657         p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
658                 MAC2STR(sa), (int) len);
659
660         if (len < 6 + 2) {
661                 p2p_dbg(p2p, "Too short GAS Comeback Response frame");
662                 return;
663         }
664
665         dialog_token = *pos++;
666         /* TODO: check dialog_token match */
667         status_code = WPA_GET_LE16(pos);
668         pos += 2;
669         frag_id = *pos & 0x7f;
670         more_frags = (*pos & 0x80) >> 7;
671         pos++;
672         comeback_delay = WPA_GET_LE16(pos);
673         pos += 2;
674         p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
675                 "comeback_delay=%u",
676                 dialog_token, status_code, frag_id, more_frags,
677                 comeback_delay);
678         /* TODO: check frag_id match */
679         if (status_code) {
680                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
681                         status_code);
682                 return;
683         }
684
685         if (*pos != WLAN_EID_ADV_PROTO) {
686                 p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
687                         *pos);
688                 return;
689         }
690         pos++;
691
692         slen = *pos++;
693         next = pos + slen;
694         if (next > end || slen < 2) {
695                 p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
696                 return;
697         }
698         pos++; /* skip QueryRespLenLimit and PAME-BI */
699
700         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
701                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
702                         *pos);
703                 return;
704         }
705
706         pos = next;
707         /* Query Response */
708         if (pos + 2 > end) {
709                 p2p_dbg(p2p, "Too short Query Response");
710                 return;
711         }
712         slen = WPA_GET_LE16(pos);
713         pos += 2;
714         p2p_dbg(p2p, "Query Response Length: %d", slen);
715         if (pos + slen > end) {
716                 p2p_dbg(p2p, "Not enough Query Response data");
717                 return;
718         }
719         if (slen == 0) {
720                 p2p_dbg(p2p, "No Query Response data");
721                 return;
722         }
723         end = pos + slen;
724
725         if (p2p->sd_rx_resp) {
726                  /*
727                   * ANQP header is only included in the first fragment; rest of
728                   * the fragments start with continue TLVs.
729                   */
730                 goto skip_nqp_header;
731         }
732
733         /* ANQP Query Response */
734         if (pos + 4 > end)
735                 return;
736         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
737                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
738                 return;
739         }
740         pos += 2;
741
742         slen = WPA_GET_LE16(pos);
743         pos += 2;
744         p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
745         if (slen < 3 + 1) {
746                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
747                 return;
748         }
749         if (pos + 4 > end)
750                 return;
751
752         if (WPA_GET_BE24(pos) != OUI_WFA) {
753                 p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
754                 return;
755         }
756         pos += 3;
757
758         if (*pos != P2P_OUI_TYPE) {
759                 p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
760                 return;
761         }
762         pos++;
763
764         if (pos + 2 > end)
765                 return;
766         p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
767         p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
768         pos += 2;
769
770 skip_nqp_header:
771         if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
772                 return;
773         wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
774         p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
775                 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
776
777         if (more_frags) {
778                 p2p_dbg(p2p, "More fragments remains");
779                 /* TODO: what would be a good size limit? */
780                 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
781                         wpabuf_free(p2p->sd_rx_resp);
782                         p2p->sd_rx_resp = NULL;
783                         p2p_dbg(p2p, "Too long SD response - drop it");
784                         return;
785                 }
786                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
787                 return;
788         }
789
790         p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
791         p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
792         p2p->sd_peer = NULL;
793
794         if (p2p->sd_query) {
795                 if (!p2p->sd_query->for_all_peers) {
796                         struct p2p_sd_query *q;
797                         p2p_dbg(p2p, "Remove completed SD query %p",
798                                 p2p->sd_query);
799                         q = p2p->sd_query;
800                         p2p_unlink_sd_query(p2p, p2p->sd_query);
801                         p2p_free_sd_query(q);
802                 }
803                 p2p->sd_query = NULL;
804         }
805
806         if (p2p->cfg->sd_response)
807                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
808                                       p2p->sd_rx_update_indic,
809                                       wpabuf_head(p2p->sd_rx_resp),
810                                       wpabuf_len(p2p->sd_rx_resp));
811         wpabuf_free(p2p->sd_rx_resp);
812         p2p->sd_rx_resp = NULL;
813
814         p2p_continue_find(p2p);
815 }
816
817
818 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
819                       const struct wpabuf *tlvs)
820 {
821         struct p2p_sd_query *q;
822
823         q = os_zalloc(sizeof(*q));
824         if (q == NULL)
825                 return NULL;
826
827         if (dst)
828                 os_memcpy(q->peer, dst, ETH_ALEN);
829         else
830                 q->for_all_peers = 1;
831
832         q->tlvs = wpabuf_dup(tlvs);
833         if (q->tlvs == NULL) {
834                 p2p_free_sd_query(q);
835                 return NULL;
836         }
837
838         q->next = p2p->sd_queries;
839         p2p->sd_queries = q;
840         p2p_dbg(p2p, "Added SD Query %p", q);
841
842         if (dst == NULL) {
843                 struct p2p_device *dev;
844                 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
845                         dev->flags &= ~P2P_DEV_SD_INFO;
846         }
847
848         return q;
849 }
850
851
852 #ifdef CONFIG_WIFI_DISPLAY
853 void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
854                           const struct wpabuf *tlvs)
855 {
856         struct p2p_sd_query *q;
857         q = p2p_sd_request(p2p, dst, tlvs);
858         if (q)
859                 q->wsd = 1;
860         return q;
861 }
862 #endif /* CONFIG_WIFI_DISPLAY */
863
864
865 void p2p_sd_service_update(struct p2p_data *p2p)
866 {
867         p2p->srv_update_indic++;
868 }
869
870
871 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
872 {
873         if (p2p_unlink_sd_query(p2p, req)) {
874                 p2p_dbg(p2p, "Cancel pending SD query %p", req);
875                 p2p_free_sd_query(req);
876                 return 0;
877         }
878         return -1;
879 }