From 4660e732139cde02f26459021dcc16a01360120b Mon Sep 17 00:00:00 2001 From: Brian Gix Date: Fri, 5 Sep 2014 17:06:25 +0300 Subject: [PATCH] P2PS: Add Advertised Service Info into Probe Response frames Signed-off-by: Jouni Malinen --- src/p2p/p2p.c | 10 +++ src/p2p/p2p_build.c | 150 ++++++++++++++++++++++++++++++++++++++++++++ src/p2p/p2p_i.h | 10 +++ 3 files changed, 170 insertions(+) diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index ae4f5fc65..f339e2259 100644 --- a/src/p2p/p2p.c +++ b/src/p2p/p2p.c @@ -2116,6 +2116,9 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]) extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]); + if (p2p->query_count) + extra += MAX_SVC_ADV_IE_LEN; + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -2150,6 +2153,12 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) p2p_buf_add_device_info(buf, p2p, NULL); p2p_buf_update_ie_hdr(buf, len); + if (p2p->query_count) { + p2p_buf_add_service_instance(buf, p2p, p2p->query_count, + p2p->query_hash, + p2p->p2ps_adv_list); + } + return buf; } @@ -2391,6 +2400,7 @@ p2p_probe_req_rx(struct p2p_data *p2p, const u8 *addr, const u8 *dst, p2p_add_dev_from_probe_req(p2p, addr, ie, ie_len); res = p2p_reply_probe(p2p, addr, dst, bssid, ie, ie_len); + p2p->query_count = 0; if ((p2p->state == P2P_CONNECT || p2p->state == P2P_CONNECT_LISTEN) && p2p->go_neg_peer && diff --git a/src/p2p/p2p_build.c b/src/p2p/p2p_build.c index 0789f9943..92c920662 100644 --- a/src/p2p/p2p_build.c +++ b/src/p2p/p2p_build.c @@ -404,6 +404,156 @@ void p2p_buf_add_advertisement_id(struct wpabuf *buf, u32 id, const u8 *mac) } +void p2p_buf_add_service_instance(struct wpabuf *buf, struct p2p_data *p2p, + u8 hash_count, const u8 *hash, + struct p2ps_advertisement *adv_list) +{ + struct p2ps_advertisement *adv; + struct wpabuf *tmp_buf; + u8 *tag_len = NULL, *ie_len = NULL; + size_t svc_len = 0, remaining = 0, total_len = 0; + + if (!adv_list || !hash) + return; + + /* Allocate temp buffer, allowing for overflow of 1 instance */ + tmp_buf = wpabuf_alloc(MAX_SVC_ADV_IE_LEN + 256 + P2PS_HASH_LEN); + if (!tmp_buf) + return; + + for (adv = adv_list; adv && total_len <= MAX_SVC_ADV_LEN; + adv = adv->next) { + u8 count = hash_count; + const u8 *test = hash; + + while (count--) { + /* Check for wildcard */ + if (os_memcmp(test, p2p->wild_card_hash, + P2PS_HASH_LEN) == 0) { + total_len = MAX_SVC_ADV_LEN + 1; + goto wild_hash; + } + + if (os_memcmp(test, adv->hash, P2PS_HASH_LEN) == 0) + goto hash_match; + + test += P2PS_HASH_LEN; + } + + /* No matches found - Skip this Adv Instance */ + continue; + +hash_match: + if (!tag_len) { + tag_len = p2p_buf_add_ie_hdr(tmp_buf); + remaining = 255 - 4; + if (!ie_len) { + wpabuf_put_u8(tmp_buf, + P2P_ATTR_ADVERTISED_SERVICE); + ie_len = wpabuf_put(tmp_buf, sizeof(u16)); + remaining -= (sizeof(u8) + sizeof(u16)); + } + } + + svc_len = os_strlen(adv->svc_name); + + if (7 + svc_len + total_len > MAX_SVC_ADV_LEN) { + /* Can't fit... return wildcard */ + total_len = MAX_SVC_ADV_LEN + 1; + break; + } + + if (remaining <= (sizeof(adv->id) + + sizeof(adv->config_methods))) { + size_t front = remaining; + size_t back = (sizeof(adv->id) + + sizeof(adv->config_methods)) - front; + u8 holder[sizeof(adv->id) + + sizeof(adv->config_methods)]; + + /* This works even if front or back == 0 */ + WPA_PUT_LE32(holder, adv->id); + WPA_PUT_BE16(&holder[sizeof(adv->id)], + adv->config_methods); + wpabuf_put_data(tmp_buf, holder, front); + p2p_buf_update_ie_hdr(tmp_buf, tag_len); + tag_len = p2p_buf_add_ie_hdr(tmp_buf); + wpabuf_put_data(tmp_buf, &holder[front], back); + remaining = 255 - (sizeof(adv->id) + + sizeof(adv->config_methods)) - back; + } else { + wpabuf_put_le32(tmp_buf, adv->id); + wpabuf_put_be16(tmp_buf, adv->config_methods); + remaining -= (sizeof(adv->id) + + sizeof(adv->config_methods)); + } + + /* We are guaranteed at least one byte for svc_len */ + wpabuf_put_u8(tmp_buf, svc_len); + remaining -= sizeof(u8); + + if (remaining < svc_len) { + size_t front = remaining; + size_t back = svc_len - front; + + wpabuf_put_data(tmp_buf, adv->svc_name, front); + p2p_buf_update_ie_hdr(tmp_buf, tag_len); + tag_len = p2p_buf_add_ie_hdr(tmp_buf); + + /* In rare cases, we must split across 3 attributes */ + if (back > 255 - 4) { + wpabuf_put_data(tmp_buf, + &adv->svc_name[front], 255 - 4); + back -= 255 - 4; + front += 255 - 4; + p2p_buf_update_ie_hdr(tmp_buf, tag_len); + tag_len = p2p_buf_add_ie_hdr(tmp_buf); + } + + wpabuf_put_data(tmp_buf, &adv->svc_name[front], back); + remaining = 255 - 4 - back; + } else { + wpabuf_put_data(tmp_buf, adv->svc_name, svc_len); + remaining -= svc_len; + } + + /* adv_id config_methods svc_string */ + total_len += sizeof(u32) + sizeof(u16) + sizeof(u8) + svc_len; + } + + if (tag_len) + p2p_buf_update_ie_hdr(tmp_buf, tag_len); + + if (ie_len) + WPA_PUT_LE16(ie_len, (u16) total_len); + +wild_hash: + /* If all fit, return matching instances, otherwise the wildcard */ + if (total_len <= MAX_SVC_ADV_LEN) { + wpabuf_put_buf(buf, tmp_buf); + } else { + char *wild_card = P2PS_WILD_HASH_STR; + u8 wild_len; + + /* Insert wildcard instance */ + tag_len = p2p_buf_add_ie_hdr(buf); + wpabuf_put_u8(buf, P2P_ATTR_ADVERTISED_SERVICE); + ie_len = wpabuf_put(buf, sizeof(u16)); + + wild_len = (u8) os_strlen(wild_card); + wpabuf_put_le32(buf, 0); + wpabuf_put_be16(buf, 0); + wpabuf_put_u8(buf, wild_len); + wpabuf_put_data(buf, wild_card, wild_len); + + WPA_PUT_LE16(ie_len, 4 + 2 + 1 + wild_len); + p2p_buf_update_ie_hdr(buf, tag_len); + } + + wpabuf_free(tmp_buf); +} + + void p2p_buf_add_session_id(struct wpabuf *buf, u32 id, const u8 *mac) { if (!buf || !mac) diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index bb9ff4dc8..91c6c5820 100644 --- a/src/p2p/p2p_i.h +++ b/src/p2p/p2p_i.h @@ -16,6 +16,13 @@ enum p2p_role_indication; +/* + * To force Service Instances to fit within a single P2P Tag, MAX_SVC_ADV_LEN + * must equal 248 or less. Must have a minimum size of 19. + */ +#define MAX_SVC_ADV_LEN 600 +#define MAX_SVC_ADV_IE_LEN (9 + MAX_SVC_ADV_LEN + (5 * (MAX_SVC_ADV_LEN / 240))) + enum p2p_go_state { UNKNOWN_GO, LOCAL_GO, @@ -736,6 +743,9 @@ void p2p_buf_add_service_hash(struct wpabuf *buf, struct p2p_data *p2p); void p2p_buf_add_session_info(struct wpabuf *buf, const char *info); void p2p_buf_add_connection_capability(struct wpabuf *buf, u8 connection_cap); void p2p_buf_add_advertisement_id(struct wpabuf *buf, u32 id, const u8 *mac); +void p2p_buf_add_service_instance(struct wpabuf *buf, struct p2p_data *p2p, + u8 count, const u8 *hash, + struct p2ps_advertisement *adv_list); void p2p_buf_add_session_id(struct wpabuf *buf, u32 id, const u8 *mac); void p2p_buf_add_feature_capability(struct wpabuf *buf, u16 len, const u8 *mask);