2 * Wi-Fi Direct - P2P service discovery
3 * Copyright (c) 2009, Atheros Communications
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License version 2 as
7 * published by the Free Software Foundation.
9 * Alternatively, this software may be distributed under the terms of BSD
12 * See README and COPYING for more details.
18 #include "common/ieee802_11_defs.h"
23 struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
24 struct p2p_device *dev)
26 struct p2p_sd_query *q;
28 if (!(dev->dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
29 return 0; /* peer does not support SD */
31 for (q = p2p->sd_queries; q; q = q->next) {
32 if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
34 if (!q->for_all_peers &&
35 os_memcmp(q->peer, dev->p2p_device_addr, ETH_ALEN) == 0)
43 static int p2p_unlink_sd_query(struct p2p_data *p2p,
44 struct p2p_sd_query *query)
46 struct p2p_sd_query *q, *prev;
54 p2p->sd_queries = q->next;
55 if (p2p->sd_query == query)
66 static void p2p_free_sd_query(struct p2p_sd_query *q)
75 void p2p_free_sd_queries(struct p2p_data *p2p)
77 struct p2p_sd_query *q, *prev;
79 p2p->sd_queries = NULL;
83 p2p_free_sd_query(prev);
88 static struct wpabuf * p2p_build_sd_query(u16 update_indic,
92 u8 *len_pos, *len_pos2;
94 buf = wpabuf_alloc(1000 + wpabuf_len(tlvs));
98 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
99 wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_REQ);
100 wpabuf_put_u8(buf, 0); /* Dialog Token */
102 /* Advertisement Protocol IE */
103 wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
104 wpabuf_put_u8(buf, 2); /* Length */
105 wpabuf_put_u8(buf, 0); /* QueryRespLenLimit | PAME-BI */
106 wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */
109 len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */
111 /* NQP Query Request Frame */
112 wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
113 len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
114 wpabuf_put_be24(buf, OUI_WFA);
115 wpabuf_put_u8(buf, P2P_OUI_TYPE);
116 wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
117 wpabuf_put_buf(buf, tlvs);
119 WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
120 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
126 static struct wpabuf * p2p_build_gas_comeback_req(u8 dialog_token)
130 buf = wpabuf_alloc(3);
134 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
135 wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_REQ);
136 wpabuf_put_u8(buf, dialog_token);
142 static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
143 u8 dialog_token, int freq)
147 req = p2p_build_gas_comeback_req(dialog_token);
151 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
152 if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
153 dst, p2p->cfg->dev_addr, dst,
154 wpabuf_head(req), wpabuf_len(req), 200) < 0)
155 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
156 "P2P: Failed to send Action frame");
162 static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
165 const struct wpabuf *tlvs)
168 u8 *len_pos, *len_pos2;
170 buf = wpabuf_alloc(1000 + (tlvs ? wpabuf_len(tlvs) : 0));
174 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
175 wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP);
176 wpabuf_put_u8(buf, dialog_token);
177 wpabuf_put_le16(buf, status_code);
178 wpabuf_put_le16(buf, comeback_delay);
180 /* Advertisement Protocol IE */
181 wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
182 wpabuf_put_u8(buf, 2); /* Length */
183 wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */
184 wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */
187 len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */
190 /* NQP Query Response Frame */
191 wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
192 len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
193 wpabuf_put_be24(buf, OUI_WFA);
194 wpabuf_put_u8(buf, P2P_OUI_TYPE);
195 /* Service Update Indicator */
196 wpabuf_put_le16(buf, update_indic);
197 wpabuf_put_buf(buf, tlvs);
199 WPA_PUT_LE16(len_pos2,
200 (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
203 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
209 static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
212 const u8 *data, size_t len,
219 buf = wpabuf_alloc(1000 + len);
223 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
224 wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_RESP);
225 wpabuf_put_u8(buf, dialog_token);
226 wpabuf_put_le16(buf, status_code);
227 wpabuf_put_u8(buf, frag_id | (more ? 0x80 : 0));
228 wpabuf_put_le16(buf, 0); /* Comeback Delay */
230 /* Advertisement Protocol IE */
231 wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
232 wpabuf_put_u8(buf, 2); /* Length */
233 wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */
234 wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */
237 len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */
240 /* NQP Query Response Frame */
241 wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
242 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
243 wpabuf_put_be24(buf, OUI_WFA);
244 wpabuf_put_u8(buf, P2P_OUI_TYPE);
245 /* Service Update Indicator */
246 wpabuf_put_le16(buf, update_indic);
249 wpabuf_put_data(buf, data, len);
251 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
257 int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
261 struct p2p_sd_query *query;
264 freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
266 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
267 "P2P: No Listen/Operating frequency known for the "
268 "peer " MACSTR " to send SD Request",
269 MAC2STR(dev->p2p_device_addr));
273 query = p2p_pending_sd_req(p2p, dev);
277 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
278 "P2P: Start Service Discovery with " MACSTR,
279 MAC2STR(dev->p2p_device_addr));
281 req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
286 p2p->sd_query = query;
287 p2p->pending_action_state = P2P_PENDING_SD;
289 if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
290 dev->p2p_device_addr, p2p->cfg->dev_addr,
291 dev->p2p_device_addr,
292 wpabuf_head(req), wpabuf_len(req), 5000) < 0)
294 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
295 "P2P: Failed to send Action frame");
305 void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
306 const u8 *data, size_t len, int rx_freq)
308 const u8 *pos = data;
309 const u8 *end = data + len;
317 if (p2p->cfg->sd_request == NULL)
323 freq = p2p_channel_to_freq(p2p->cfg->country,
332 dialog_token = *pos++;
333 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
334 "P2P: GAS Initial Request from " MACSTR " (dialog token %u, "
336 MAC2STR(sa), dialog_token, rx_freq);
338 if (*pos != WLAN_EID_ADV_PROTO) {
339 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
340 "P2P: Unexpected IE in GAS Initial Request: %u", *pos);
347 if (next > end || slen < 2) {
348 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
349 "P2P: Invalid IE in GAS Initial Request");
352 pos++; /* skip QueryRespLenLimit and PAME-BI */
354 if (*pos != NATIVE_QUERY_PROTOCOL) {
355 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
356 "P2P: Unsupported GAS advertisement protocol id %u",
365 slen = WPA_GET_LE16(pos);
367 if (pos + slen > end)
371 /* NQP Query Request */
374 if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
375 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
376 "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
381 slen = WPA_GET_LE16(pos);
383 if (pos + slen > end || slen < 3 + 1) {
384 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
385 "P2P: Invalid NQP Query Request length");
389 if (WPA_GET_BE24(pos) != OUI_WFA) {
390 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
391 "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
396 if (*pos != P2P_OUI_TYPE) {
397 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
398 "P2P: Unsupported NQP vendor type %u", *pos);
405 update_indic = WPA_GET_LE16(pos);
406 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
407 "P2P: Service Update Indicator: %u", update_indic);
410 p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
411 update_indic, pos, end - pos);
412 /* the response will be indicated with a call to p2p_sd_response() */
416 void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
417 u8 dialog_token, const struct wpabuf *resp_tlvs)
421 /* TODO: fix the length limit to match with the maximum frame length */
422 if (wpabuf_len(resp_tlvs) > 1400) {
423 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long "
424 "enough to require fragmentation");
427 * TODO: Could consider storing the fragmented response
428 * separately for each peer to avoid having to drop old
429 * one if there is more than one pending SD query.
430 * Though, that would eat more memory, so there are
431 * also benefits to just using a single buffer.
433 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
434 "previous SD response");
435 wpabuf_free(p2p->sd_resp);
437 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
438 p2p->sd_resp_dialog_token = dialog_token;
439 p2p->sd_resp = wpabuf_dup(resp_tlvs);
440 p2p->sd_resp_pos = 0;
442 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
443 1, p2p->srv_update_indic, NULL);
445 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits "
446 "in initial response");
447 resp = p2p_build_sd_response(dialog_token,
448 WLAN_STATUS_SUCCESS, 0,
449 p2p->srv_update_indic, resp_tlvs);
454 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
455 if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
456 dst, p2p->cfg->dev_addr, p2p->cfg->dev_addr,
457 wpabuf_head(resp), wpabuf_len(resp), 200) <
459 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
460 "P2P: Failed to send Action frame");
466 void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
467 const u8 *data, size_t len, int rx_freq)
469 const u8 *pos = data;
470 const u8 *end = data + len;
478 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
479 os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) {
480 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
481 "P2P: Ignore unexpected GAS Initial Response from "
482 MACSTR, MAC2STR(sa));
485 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
486 p2p_clear_timeout(p2p);
488 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
489 "P2P: Received GAS Initial Response from " MACSTR " (len=%d)",
490 MAC2STR(sa), (int) len);
493 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
494 "P2P: Too short GAS Initial Response frame");
498 dialog_token = *pos++;
499 /* TODO: check dialog_token match */
500 status_code = WPA_GET_LE16(pos);
502 comeback_delay = WPA_GET_LE16(pos);
504 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
505 "P2P: dialog_token=%u status_code=%u comeback_delay=%u",
506 dialog_token, status_code, comeback_delay);
508 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
509 "P2P: Service Discovery failed: status code %u",
514 if (*pos != WLAN_EID_ADV_PROTO) {
515 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
516 "P2P: Unexpected IE in GAS Initial Response: %u",
524 if (next > end || slen < 2) {
525 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
526 "P2P: Invalid IE in GAS Initial Response");
529 pos++; /* skip QueryRespLenLimit and PAME-BI */
531 if (*pos != NATIVE_QUERY_PROTOCOL) {
532 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
533 "P2P: Unsupported GAS advertisement protocol id %u",
541 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
545 slen = WPA_GET_LE16(pos);
547 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
549 if (pos + slen > end) {
550 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
556 if (comeback_delay) {
557 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
558 "response - request fragments");
559 if (p2p->sd_rx_resp) {
560 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
561 "old SD reassembly buffer");
562 wpabuf_free(p2p->sd_rx_resp);
563 p2p->sd_rx_resp = NULL;
565 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
569 /* NQP Query Response */
572 if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
573 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
574 "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
579 slen = WPA_GET_LE16(pos);
581 if (pos + slen > end || slen < 3 + 1) {
582 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
583 "P2P: Invalid NQP Query Response length");
587 if (WPA_GET_BE24(pos) != OUI_WFA) {
588 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
589 "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
594 if (*pos != P2P_OUI_TYPE) {
595 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
596 "P2P: Unsupported NQP vendor type %u", *pos);
603 update_indic = WPA_GET_LE16(pos);
604 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
605 "P2P: Service Update Indicator: %u", update_indic);
608 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
609 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
613 if (!p2p->sd_query->for_all_peers) {
614 struct p2p_sd_query *q;
615 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
616 "P2P: Remove completed SD query %p",
619 p2p_unlink_sd_query(p2p, p2p->sd_query);
620 p2p_free_sd_query(q);
622 p2p->sd_query = NULL;
625 if (p2p->cfg->sd_response)
626 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
628 p2p_continue_find(p2p);
632 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
633 const u8 *data, size_t len, int rx_freq)
640 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
643 dialog_token = *data;
644 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
646 if (dialog_token != p2p->sd_resp_dialog_token) {
647 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
648 "response fragment for dialog token %u", dialog_token);
652 if (p2p->sd_resp == NULL) {
653 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
654 "response fragment available");
657 if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
658 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
659 "response fragment for " MACSTR, MAC2STR(sa));
663 frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
664 if (frag_len > 1400) {
668 resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
669 p2p->srv_update_indic,
670 wpabuf_head_u8(p2p->sd_resp) +
671 p2p->sd_resp_pos, frag_len,
672 p2p->sd_frag_id, more,
673 wpabuf_len(p2p->sd_resp));
676 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
677 "Response (frag_id %d more=%d frag_len=%d)",
678 p2p->sd_frag_id, more, (int) frag_len);
680 p2p->sd_resp_pos += frag_len;
683 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
685 (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
687 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
689 wpabuf_free(p2p->sd_resp);
693 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
694 if (p2p->cfg->send_action(p2p->cfg->cb_ctx, rx_freq,
695 sa, p2p->cfg->dev_addr, p2p->cfg->dev_addr,
696 wpabuf_head(resp), wpabuf_len(resp), 200) <
698 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
699 "P2P: Failed to send Action frame");
705 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
706 const u8 *data, size_t len, int rx_freq)
708 const u8 *pos = data;
709 const u8 *end = data + len;
718 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
720 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
721 os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) {
722 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
723 "P2P: Ignore unexpected GAS Comeback Response from "
724 MACSTR, MAC2STR(sa));
727 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
728 p2p_clear_timeout(p2p);
730 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
731 "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)",
732 MAC2STR(sa), (int) len);
735 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
736 "P2P: Too short GAS Comeback Response frame");
740 dialog_token = *pos++;
741 /* TODO: check dialog_token match */
742 status_code = WPA_GET_LE16(pos);
744 frag_id = *pos & 0x7f;
745 more_frags = (*pos & 0x80) >> 7;
747 comeback_delay = WPA_GET_LE16(pos);
749 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
750 "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
752 dialog_token, status_code, frag_id, more_frags,
754 /* TODO: check frag_id match */
756 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
757 "P2P: Service Discovery failed: status code %u",
762 if (*pos != WLAN_EID_ADV_PROTO) {
763 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
764 "P2P: Unexpected IE in GAS Comeback Response: %u",
772 if (next > end || slen < 2) {
773 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
774 "P2P: Invalid IE in GAS Comeback Response");
777 pos++; /* skip QueryRespLenLimit and PAME-BI */
779 if (*pos != NATIVE_QUERY_PROTOCOL) {
780 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
781 "P2P: Unsupported GAS advertisement protocol id %u",
789 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
793 slen = WPA_GET_LE16(pos);
795 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
797 if (pos + slen > end) {
798 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
803 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
809 if (p2p->sd_rx_resp) {
811 * NQP header is only included in the first fragment; rest of
812 * the fragments start with continue TLVs.
814 goto skip_nqp_header;
817 /* NQP Query Response */
820 if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
821 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
822 "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
827 slen = WPA_GET_LE16(pos);
829 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: NQP Query Response "
832 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
833 "P2P: Invalid NQP Query Response length");
839 if (WPA_GET_BE24(pos) != OUI_WFA) {
840 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
841 "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
846 if (*pos != P2P_OUI_TYPE) {
847 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
848 "P2P: Unsupported NQP vendor type %u", *pos);
855 p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
856 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
857 "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic);
861 if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
863 wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
864 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
866 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
869 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments "
871 /* TODO: what would be a good size limit? */
872 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
873 wpabuf_free(p2p->sd_rx_resp);
874 p2p->sd_rx_resp = NULL;
875 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
876 "SD response - drop it");
879 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
883 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
884 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
888 if (!p2p->sd_query->for_all_peers) {
889 struct p2p_sd_query *q;
890 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
891 "P2P: Remove completed SD query %p",
894 p2p_unlink_sd_query(p2p, p2p->sd_query);
895 p2p_free_sd_query(q);
897 p2p->sd_query = NULL;
900 if (p2p->cfg->sd_response)
901 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
902 p2p->sd_rx_update_indic,
903 wpabuf_head(p2p->sd_rx_resp),
904 wpabuf_len(p2p->sd_rx_resp));
905 wpabuf_free(p2p->sd_rx_resp);
906 p2p->sd_rx_resp = NULL;
908 p2p_continue_find(p2p);
912 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
913 const struct wpabuf *tlvs)
915 struct p2p_sd_query *q;
917 q = os_zalloc(sizeof(*q));
922 os_memcpy(q->peer, dst, ETH_ALEN);
924 q->for_all_peers = 1;
926 q->tlvs = wpabuf_dup(tlvs);
927 if (q->tlvs == NULL) {
928 p2p_free_sd_query(q);
932 q->next = p2p->sd_queries;
934 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
940 void p2p_sd_service_update(struct p2p_data *p2p)
942 p2p->srv_update_indic++;
946 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
948 if (p2p_unlink_sd_query(p2p, req)) {
949 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
950 "P2P: Cancel pending SD query %p", req);
951 p2p_free_sd_query(req);