P2P: Initial support for SD fragmentation (GAS Comeback Request/Response)
This commit is contained in:
parent
2a43101e48
commit
18708aadfc
5 changed files with 469 additions and 22 deletions
|
@ -1590,6 +1590,12 @@ static int nl80211_register_action_frames(struct wpa_driver_nl80211_data *drv)
|
|||
/* GAS Initial Response */
|
||||
if (nl80211_register_action_frame(drv, (u8 *) "\x04\x0b", 2) < 0)
|
||||
return -1;
|
||||
/* GAS Comeback Request */
|
||||
if (nl80211_register_action_frame(drv, (u8 *) "\x04\x0c", 2) < 0)
|
||||
return -1;
|
||||
/* GAS Comeback Response */
|
||||
if (nl80211_register_action_frame(drv, (u8 *) "\x04\x0d", 2) < 0)
|
||||
return -1;
|
||||
/* P2P Public Action */
|
||||
if (nl80211_register_action_frame(drv,
|
||||
(u8 *) "\x04\x09\x50\x6f\x9a\x09",
|
||||
|
|
|
@ -1180,7 +1180,13 @@ void p2p_rx_action_public(struct p2p_data *p2p, const u8 *da, const u8 *sa,
|
|||
p2p_rx_gas_initial_req(p2p, sa, data + 1, len - 1, freq);
|
||||
break;
|
||||
case WLAN_PA_GAS_INITIAL_RESP:
|
||||
p2p_rx_gas_initial_resp(p2p, sa, data + 1, len - 1);
|
||||
p2p_rx_gas_initial_resp(p2p, sa, data + 1, len - 1, freq);
|
||||
break;
|
||||
case WLAN_PA_GAS_COMEBACK_REQ:
|
||||
p2p_rx_gas_comeback_req(p2p, sa, data + 1, len - 1, freq);
|
||||
break;
|
||||
case WLAN_PA_GAS_COMEBACK_RESP:
|
||||
p2p_rx_gas_comeback_resp(p2p, sa, data + 1, len - 1, freq);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -1772,6 +1778,7 @@ void p2p_deinit(struct p2p_data *p2p)
|
|||
p2p_flush(p2p);
|
||||
os_free(p2p->cfg->dev_name);
|
||||
os_free(p2p->groups);
|
||||
wpabuf_free(p2p->sd_resp);
|
||||
os_free(p2p);
|
||||
}
|
||||
|
||||
|
|
|
@ -314,6 +314,14 @@ struct p2p_data {
|
|||
*/
|
||||
u16 srv_update_indic;
|
||||
|
||||
struct wpabuf *sd_resp; /* Fragmented SD response */
|
||||
u8 sd_resp_addr[ETH_ALEN];
|
||||
u8 sd_resp_dialog_token;
|
||||
size_t sd_resp_pos; /* Offset in sd_resp */
|
||||
u8 sd_frag_id;
|
||||
|
||||
struct wpabuf *sd_rx_resp; /* Reassembled SD response */
|
||||
|
||||
/* P2P Invitation data */
|
||||
enum p2p_invite_role inv_role;
|
||||
u8 inv_bssid[ETH_ALEN];
|
||||
|
@ -509,7 +517,11 @@ void p2p_free_sd_queries(struct p2p_data *p2p);
|
|||
void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len, int rx_freq);
|
||||
void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len);
|
||||
const u8 *data, size_t len, int rx_freq);
|
||||
void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len, int rx_freq);
|
||||
void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len, int rx_freq);
|
||||
int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev);
|
||||
|
||||
/* p2p_go_neg.c */
|
||||
|
|
437
src/p2p/p2p_sd.c
437
src/p2p/p2p_sd.c
|
@ -123,14 +123,51 @@ static struct wpabuf * p2p_build_sd_query(u16 update_indic,
|
|||
}
|
||||
|
||||
|
||||
static struct wpabuf * p2p_build_gas_comeback_req(u8 dialog_token)
|
||||
{
|
||||
struct wpabuf *buf;
|
||||
|
||||
buf = wpabuf_alloc(3);
|
||||
if (buf == NULL)
|
||||
return NULL;
|
||||
|
||||
wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
|
||||
wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_REQ);
|
||||
wpabuf_put_u8(buf, dialog_token);
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
|
||||
static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
|
||||
u8 dialog_token, int freq)
|
||||
{
|
||||
struct wpabuf *req;
|
||||
|
||||
req = p2p_build_gas_comeback_req(dialog_token);
|
||||
if (req == NULL)
|
||||
return;
|
||||
|
||||
p2p->pending_action_state = P2P_NO_PENDING_ACTION;
|
||||
if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
|
||||
dst, p2p->cfg->dev_addr, dst,
|
||||
wpabuf_head(req), wpabuf_len(req), 200) < 0)
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Failed to send Action frame");
|
||||
|
||||
wpabuf_free(req);
|
||||
}
|
||||
|
||||
|
||||
static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
|
||||
u16 comeback_delay,
|
||||
u16 update_indic,
|
||||
const struct wpabuf *tlvs)
|
||||
{
|
||||
struct wpabuf *buf;
|
||||
u8 *len_pos, *len_pos2;
|
||||
|
||||
buf = wpabuf_alloc(1000 + wpabuf_len(tlvs));
|
||||
buf = wpabuf_alloc(1000 + (tlvs ? wpabuf_len(tlvs) : 0));
|
||||
if (buf == NULL)
|
||||
return NULL;
|
||||
|
||||
|
@ -138,7 +175,56 @@ static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
|
|||
wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP);
|
||||
wpabuf_put_u8(buf, dialog_token);
|
||||
wpabuf_put_le16(buf, status_code);
|
||||
wpabuf_put_le16(buf, 0); /* GAS Comeback Delay */
|
||||
wpabuf_put_le16(buf, comeback_delay);
|
||||
|
||||
/* Advertisement Protocol IE */
|
||||
wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
|
||||
wpabuf_put_u8(buf, 2); /* Length */
|
||||
wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */
|
||||
wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */
|
||||
|
||||
/* Query Response */
|
||||
len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */
|
||||
|
||||
if (tlvs) {
|
||||
/* NQP Query Response Frame */
|
||||
wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
|
||||
len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
|
||||
wpabuf_put_be24(buf, OUI_WFA);
|
||||
wpabuf_put_u8(buf, P2P_OUI_TYPE);
|
||||
/* Service Update Indicator */
|
||||
wpabuf_put_le16(buf, update_indic);
|
||||
wpabuf_put_buf(buf, tlvs);
|
||||
|
||||
WPA_PUT_LE16(len_pos2,
|
||||
(u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
|
||||
}
|
||||
|
||||
WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
|
||||
static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
|
||||
u16 status_code,
|
||||
u16 update_indic,
|
||||
const u8 *data, size_t len,
|
||||
u8 frag_id, u8 more)
|
||||
{
|
||||
struct wpabuf *buf;
|
||||
u8 *len_pos, *len_pos2;
|
||||
|
||||
buf = wpabuf_alloc(1000 + len);
|
||||
if (buf == NULL)
|
||||
return NULL;
|
||||
|
||||
wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
|
||||
wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_RESP);
|
||||
wpabuf_put_u8(buf, dialog_token);
|
||||
wpabuf_put_le16(buf, status_code);
|
||||
wpabuf_put_u8(buf, frag_id | (more ? 0x80 : 0));
|
||||
wpabuf_put_le16(buf, 0); /* Comeback Delay */
|
||||
|
||||
/* Advertisement Protocol IE */
|
||||
wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
|
||||
|
@ -154,8 +240,10 @@ static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
|
|||
len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
|
||||
wpabuf_put_be24(buf, OUI_WFA);
|
||||
wpabuf_put_u8(buf, P2P_OUI_TYPE);
|
||||
wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
|
||||
wpabuf_put_buf(buf, tlvs);
|
||||
/* Service Update Indicator */
|
||||
wpabuf_put_le16(buf, update_indic);
|
||||
|
||||
wpabuf_put_data(buf, data, len);
|
||||
|
||||
WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
|
||||
WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
|
||||
|
@ -327,8 +415,37 @@ void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
|
|||
u8 dialog_token, const struct wpabuf *resp_tlvs)
|
||||
{
|
||||
struct wpabuf *resp;
|
||||
resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
|
||||
p2p->srv_update_indic, resp_tlvs);
|
||||
|
||||
/* TODO: fix the length limit to match with the maximum frame length */
|
||||
if (wpabuf_len(resp_tlvs) > 1400) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long "
|
||||
"enough to require fragmentation");
|
||||
if (p2p->sd_resp) {
|
||||
/*
|
||||
* TODO: Could consider storing the fragmented response
|
||||
* separately for each peer to avoid having to drop old
|
||||
* one if there is more than one pending SD query.
|
||||
* Though, that would eat more memory, so there are
|
||||
* also benefits to just using a single buffer.
|
||||
*/
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
|
||||
"previous SD response");
|
||||
wpabuf_free(p2p->sd_resp);
|
||||
}
|
||||
os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
|
||||
p2p->sd_resp_dialog_token = dialog_token;
|
||||
p2p->sd_resp = wpabuf_dup(resp_tlvs);
|
||||
p2p->sd_resp_pos = 0;
|
||||
p2p->sd_frag_id = 0;
|
||||
resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
|
||||
1, p2p->srv_update_indic, NULL);
|
||||
} else {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits "
|
||||
"in initial response");
|
||||
resp = p2p_build_sd_response(dialog_token,
|
||||
WLAN_STATUS_SUCCESS, 0,
|
||||
p2p->srv_update_indic, resp_tlvs);
|
||||
}
|
||||
if (resp == NULL)
|
||||
return;
|
||||
|
||||
|
@ -345,7 +462,7 @@ void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
|
|||
|
||||
|
||||
void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len)
|
||||
const u8 *data, size_t len, int rx_freq)
|
||||
{
|
||||
const u8 *pos = data;
|
||||
const u8 *end = data + len;
|
||||
|
@ -367,11 +484,14 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
|||
p2p_clear_timeout(p2p);
|
||||
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Received GAS Initial Response from " MACSTR,
|
||||
MAC2STR(sa));
|
||||
"P2P: Received GAS Initial Response from " MACSTR " (len=%d)",
|
||||
MAC2STR(sa), (int) len);
|
||||
|
||||
if (len < 5 + 2)
|
||||
if (len < 5 + 2) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Too short GAS Initial Response frame");
|
||||
return;
|
||||
}
|
||||
|
||||
dialog_token = *pos++;
|
||||
/* TODO: check dialog_token match */
|
||||
|
@ -382,6 +502,12 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
|||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: dialog_token=%u status_code=%u comeback_delay=%u",
|
||||
dialog_token, status_code, comeback_delay);
|
||||
if (status_code) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Service Discovery failed: status code %u",
|
||||
status_code);
|
||||
return;
|
||||
}
|
||||
|
||||
if (*pos != WLAN_EID_ADV_PROTO) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
|
@ -409,14 +535,35 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
|||
|
||||
pos = next;
|
||||
/* Query Response */
|
||||
if (pos + 2 > end)
|
||||
if (pos + 2 > end) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
|
||||
"Response");
|
||||
return;
|
||||
}
|
||||
slen = WPA_GET_LE16(pos);
|
||||
pos += 2;
|
||||
if (pos + slen > end)
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
|
||||
slen);
|
||||
if (pos + slen > end) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
|
||||
"Response data");
|
||||
return;
|
||||
}
|
||||
end = pos + slen;
|
||||
|
||||
if (comeback_delay) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
|
||||
"response - request fragments");
|
||||
if (p2p->sd_rx_resp) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "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;
|
||||
}
|
||||
|
||||
/* NQP Query Response */
|
||||
if (pos + 4 > end)
|
||||
return;
|
||||
|
@ -480,6 +627,272 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
|
|||
}
|
||||
|
||||
|
||||
void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
|
||||
const u8 *data, size_t len, int rx_freq)
|
||||
{
|
||||
struct wpabuf *resp;
|
||||
u8 dialog_token;
|
||||
size_t frag_len;
|
||||
int more = 0;
|
||||
|
||||
wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
|
||||
if (len < 1)
|
||||
return;
|
||||
dialog_token = *data;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
|
||||
dialog_token);
|
||||
if (dialog_token != p2p->sd_resp_dialog_token) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
|
||||
"response fragment for dialog token %u", dialog_token);
|
||||
return;
|
||||
}
|
||||
|
||||
if (p2p->sd_resp == NULL) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
|
||||
"response fragment available");
|
||||
return;
|
||||
}
|
||||
if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
|
||||
"response fragment for " MACSTR, MAC2STR(sa));
|
||||
return;
|
||||
}
|
||||
|
||||
frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
|
||||
if (frag_len > 1400) {
|
||||
frag_len = 1400;
|
||||
more = 1;
|
||||
}
|
||||
resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
|
||||
p2p->srv_update_indic,
|
||||
wpabuf_head_u8(p2p->sd_resp) +
|
||||
p2p->sd_resp_pos, frag_len,
|
||||
p2p->sd_frag_id, more);
|
||||
if (resp == NULL)
|
||||
return;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
|
||||
"Response (frag_id %d more=%d frag_len=%d)",
|
||||
p2p->sd_frag_id, more, (int) frag_len);
|
||||
p2p->sd_frag_id++;
|
||||
p2p->sd_resp_pos += frag_len;
|
||||
|
||||
if (more) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
|
||||
"remain to be sent",
|
||||
(int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
|
||||
} else {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
|
||||
"SD response sent");
|
||||
wpabuf_free(p2p->sd_resp);
|
||||
p2p->sd_resp = NULL;
|
||||
}
|
||||
|
||||
p2p->pending_action_state = P2P_NO_PENDING_ACTION;
|
||||
if (p2p->cfg->send_action(p2p->cfg->cb_ctx, rx_freq,
|
||||
sa, p2p->cfg->dev_addr, p2p->cfg->dev_addr,
|
||||
wpabuf_head(resp), wpabuf_len(resp), 200) <
|
||||
0)
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Failed to send Action frame");
|
||||
|
||||
wpabuf_free(resp);
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
u16 update_indic;
|
||||
|
||||
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->p2p_device_addr, ETH_ALEN) != 0) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Ignore unexpected GAS Comeback Response from "
|
||||
MACSTR, MAC2STR(sa));
|
||||
return;
|
||||
}
|
||||
p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
|
||||
p2p_clear_timeout(p2p);
|
||||
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Received GAS Comeback Response from " MACSTR " (len=%d)",
|
||||
MAC2STR(sa), (int) len);
|
||||
|
||||
if (len < 6 + 2) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"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;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"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) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Service Discovery failed: status code %u",
|
||||
status_code);
|
||||
return;
|
||||
}
|
||||
|
||||
if (*pos != WLAN_EID_ADV_PROTO) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Unexpected IE in GAS Comeback Response: %u",
|
||||
*pos);
|
||||
return;
|
||||
}
|
||||
pos++;
|
||||
|
||||
slen = *pos++;
|
||||
next = pos + slen;
|
||||
if (next > end || slen < 2) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Invalid IE in GAS Comeback Response");
|
||||
return;
|
||||
}
|
||||
pos++; /* skip QueryRespLenLimit and PAME-BI */
|
||||
|
||||
if (*pos != NATIVE_QUERY_PROTOCOL) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Unsupported GAS advertisement protocol id %u",
|
||||
*pos);
|
||||
return;
|
||||
}
|
||||
|
||||
pos = next;
|
||||
/* Query Response */
|
||||
if (pos + 2 > end) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
|
||||
"Response");
|
||||
return;
|
||||
}
|
||||
slen = WPA_GET_LE16(pos);
|
||||
pos += 2;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
|
||||
slen);
|
||||
if (pos + slen > end) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
|
||||
"Response data");
|
||||
return;
|
||||
}
|
||||
if (slen == 0) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
|
||||
"data");
|
||||
return;
|
||||
}
|
||||
end = pos + slen;
|
||||
|
||||
/* NQP Query Response */
|
||||
if (pos + 4 > end)
|
||||
return;
|
||||
if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
|
||||
return;
|
||||
}
|
||||
pos += 2;
|
||||
|
||||
slen = WPA_GET_LE16(pos);
|
||||
pos += 2;
|
||||
if (pos + slen > end || slen < 3 + 1) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Invalid NQP Query Response length");
|
||||
return;
|
||||
}
|
||||
|
||||
if (WPA_GET_BE24(pos) != OUI_WFA) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
|
||||
return;
|
||||
}
|
||||
pos += 3;
|
||||
|
||||
if (*pos != P2P_OUI_TYPE) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Unsupported NQP vendor type %u", *pos);
|
||||
return;
|
||||
}
|
||||
pos++;
|
||||
|
||||
if (pos + 2 > end)
|
||||
return;
|
||||
update_indic = WPA_GET_LE16(pos);
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"P2P: Service Update Indicator: %u", update_indic);
|
||||
pos += 2;
|
||||
|
||||
if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
|
||||
return;
|
||||
wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
|
||||
"buffer length: %u",
|
||||
(unsigned int) wpabuf_len(p2p->sd_rx_resp));
|
||||
|
||||
if (more_frags) {
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "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;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
|
||||
"SD response - drop it");
|
||||
return;
|
||||
}
|
||||
p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
|
||||
return;
|
||||
}
|
||||
|
||||
p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
|
||||
p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
|
||||
p2p->sd_peer = NULL;
|
||||
|
||||
if (p2p->sd_query) {
|
||||
if (!p2p->sd_query->for_all_peers) {
|
||||
struct p2p_sd_query *q;
|
||||
wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
|
||||
"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,
|
||||
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_sd_request(struct p2p_data *p2p, const u8 *dst,
|
||||
const struct wpabuf *tlvs)
|
||||
{
|
||||
|
|
|
@ -1413,14 +1413,23 @@ void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic,
|
|||
|
||||
wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs",
|
||||
tlvs, tlvs_len);
|
||||
buf_len = 2 * tlvs_len + 1;
|
||||
buf = os_malloc(buf_len);
|
||||
if (buf) {
|
||||
wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len);
|
||||
wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_RESP MACSTR
|
||||
" %u %s",
|
||||
MAC2STR(sa), update_indic, buf);
|
||||
os_free(buf);
|
||||
if (tlvs_len > 1500) {
|
||||
/* TODO: better way for handling this */
|
||||
wpa_msg_ctrl(wpa_s, MSG_INFO,
|
||||
P2P_EVENT_SERV_DISC_RESP MACSTR
|
||||
" %u <long response: %u bytes>",
|
||||
MAC2STR(sa), update_indic,
|
||||
(unsigned int) tlvs_len);
|
||||
} else {
|
||||
buf_len = 2 * tlvs_len + 1;
|
||||
buf = os_malloc(buf_len);
|
||||
if (buf) {
|
||||
wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len);
|
||||
wpa_msg_ctrl(wpa_s, MSG_INFO,
|
||||
P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s",
|
||||
MAC2STR(sa), update_indic, buf);
|
||||
os_free(buf);
|
||||
}
|
||||
}
|
||||
|
||||
while (pos < end) {
|
||||
|
|
Loading…
Reference in a new issue