int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
{
if (p2p_unlink_sd_query(p2p, req)) {
p2p_dbg(p2p, "Cancel pending SD query %p", req);
p2p_free_sd_query(req);
return 0;
}
return -1;
}
int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
{
if (p2p_unlink_sd_query(p2p, req)) {
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
"P2P: Cancel pending SD query %p", req);
p2p_free_sd_query(req);
return 0;
}
return -1;
}
void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
const u8 *data, size_t len, int rx_freq)
{
const u8 *pos = data;
const u8 *end = data + len;
const u8 *next;
u8 dialog_token;
u16 status_code;
u8 frag_id;
u8 more_frags;
u16 comeback_delay;
u16 slen;
wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
MACSTR, MAC2STR(sa));
return;
}
p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
p2p_clear_timeout(p2p);
p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
MAC2STR(sa), (int) len);
if (len < 6 + 2) {
p2p_dbg(p2p, "Too short GAS Comeback Response frame");
return;
}
dialog_token = *pos++;
/* TODO: check dialog_token match */
status_code = WPA_GET_LE16(pos);
pos += 2;
frag_id = *pos & 0x7f;
more_frags = (*pos & 0x80) >> 7;
pos++;
comeback_delay = WPA_GET_LE16(pos);
pos += 2;
p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
"comeback_delay=%u",
dialog_token, status_code, frag_id, more_frags,
comeback_delay);
/* TODO: check frag_id match */
if (status_code) {
p2p_dbg(p2p, "Service Discovery failed: status code %u",
status_code);
return;
}
if (*pos != WLAN_EID_ADV_PROTO) {
p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
*pos);
return;
}
pos++;
slen = *pos++;
next = pos + slen;
if (next > end || slen < 2) {
p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
return;
}
pos++; /* skip QueryRespLenLimit and PAME-BI */
if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
*pos);
return;
}
pos = next;
/* Query Response */
if (pos + 2 > end) {
p2p_dbg(p2p, "Too short Query Response");
return;
}
slen = WPA_GET_LE16(pos);
pos += 2;
p2p_dbg(p2p, "Query Response Length: %d", slen);
if (pos + slen > end) {
p2p_dbg(p2p, "Not enough Query Response data");
return;
}
if (slen == 0) {
p2p_dbg(p2p, "No Query Response data");
return;
}
end = pos + slen;
if (p2p->sd_rx_resp) {
/*
* ANQP header is only included in the first fragment; rest of
* the fragments start with continue TLVs.
*/
goto skip_nqp_header;
}
/* ANQP Query Response */
if (pos + 4 > end)
return;
if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
return;
}
pos += 2;
slen = WPA_GET_LE16(pos);
pos += 2;
p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
if (slen < 3 + 1) {
p2p_dbg(p2p, "Invalid ANQP Query Response length");
return;
}
if (pos + 4 > end)
return;
if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
WPA_GET_BE32(pos));
return;
}
pos += 4;
if (pos + 2 > end)
return;
p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
pos += 2;
skip_nqp_header:
if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
return;
wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
(unsigned int) wpabuf_len(p2p->sd_rx_resp));
if (more_frags) {
p2p_dbg(p2p, "More fragments remains");
/* TODO: what would be a good size limit? */
if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
wpabuf_free(p2p->sd_rx_resp);
p2p->sd_rx_resp = NULL;
p2p_dbg(p2p, "Too long SD response - drop it");
return;
}
p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
return;
}
p2p->sd_peer = NULL;
if (p2p->sd_query) {
if (!p2p->sd_query->for_all_peers) {
struct p2p_sd_query *q;
p2p_dbg(p2p, "Remove completed SD query %p",
p2p->sd_query);
q = p2p->sd_query;
p2p_unlink_sd_query(p2p, p2p->sd_query);
p2p_free_sd_query(q);
}
p2p->sd_query = NULL;
}
if (p2p->cfg->sd_response)
p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
p2p->sd_rx_update_indic,
wpabuf_head(p2p->sd_rx_resp),
wpabuf_len(p2p->sd_rx_resp));
wpabuf_free(p2p->sd_rx_resp);
p2p->sd_rx_resp = NULL;
p2p_continue_find(p2p);
}
void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
const u8 *data, size_t len, int rx_freq)
{
const u8 *pos = data;
const u8 *end = data + len;
const u8 *next;
u8 dialog_token;
u16 status_code;
u16 comeback_delay;
u16 slen;
u16 update_indic;
if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
MACSTR, MAC2STR(sa));
return;
}
p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
p2p_clear_timeout(p2p);
p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
MAC2STR(sa), (int) len);
if (len < 5 + 2) {
p2p_dbg(p2p, "Too short GAS Initial Response frame");
return;
}
dialog_token = *pos++;
/* TODO: check dialog_token match */
status_code = WPA_GET_LE16(pos);
pos += 2;
comeback_delay = WPA_GET_LE16(pos);
pos += 2;
p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
dialog_token, status_code, comeback_delay);
if (status_code) {
p2p_dbg(p2p, "Service Discovery failed: status code %u",
status_code);
return;
}
if (*pos != WLAN_EID_ADV_PROTO) {
p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
return;
}
pos++;
slen = *pos++;
next = pos + slen;
if (next > end || slen < 2) {
p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
return;
}
pos++; /* skip QueryRespLenLimit and PAME-BI */
if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
*pos);
return;
}
pos = next;
/* Query Response */
if (pos + 2 > end) {
p2p_dbg(p2p, "Too short Query Response");
return;
}
slen = WPA_GET_LE16(pos);
pos += 2;
p2p_dbg(p2p, "Query Response Length: %d", slen);
if (pos + slen > end) {
p2p_dbg(p2p, "Not enough Query Response data");
return;
}
end = pos + slen;
if (comeback_delay) {
p2p_dbg(p2p, "Fragmented response - request fragments");
if (p2p->sd_rx_resp) {
p2p_dbg(p2p, "Drop old SD reassembly buffer");
wpabuf_free(p2p->sd_rx_resp);
p2p->sd_rx_resp = NULL;
}
p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
return;
}
/* ANQP Query Response */
if (pos + 4 > end)
return;
if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
return;
}
pos += 2;
slen = WPA_GET_LE16(pos);
pos += 2;
if (pos + slen > end || slen < 3 + 1) {
p2p_dbg(p2p, "Invalid ANQP Query Response length");
return;
}
if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
WPA_GET_BE32(pos));
return;
}
pos += 4;
if (pos + 2 > end)
return;
update_indic = WPA_GET_LE16(pos);
p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
pos += 2;
p2p->sd_peer = NULL;
if (p2p->sd_query) {
if (!p2p->sd_query->for_all_peers) {
struct p2p_sd_query *q;
p2p_dbg(p2p, "Remove completed SD query %p",
p2p->sd_query);
q = p2p->sd_query;
p2p_unlink_sd_query(p2p, p2p->sd_query);
p2p_free_sd_query(q);
}
p2p->sd_query = NULL;
}
if (p2p->cfg->sd_response)
p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
pos, end - pos);
p2p_continue_find(p2p);
}
