diff options
Diffstat (limited to 'src/p2p/p2p_build.c')
-rw-r--r-- | src/p2p/p2p_build.c | 257 |
1 files changed, 255 insertions, 2 deletions
diff --git a/src/p2p/p2p_build.c b/src/p2p/p2p_build.c index e9b683d9..92c92066 100644 --- a/src/p2p/p2p_build.c +++ b/src/p2p/p2p_build.c @@ -164,15 +164,18 @@ void p2p_buf_add_device_info(struct wpabuf *buf, struct p2p_data *p2p, if (peer->wps_method == WPS_PBC) methods |= WPS_CONFIG_PUSHBUTTON; else if (peer->wps_method == WPS_PIN_DISPLAY || - peer->wps_method == WPS_PIN_KEYPAD) + peer->wps_method == WPS_PIN_KEYPAD) { methods |= WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD; + methods |= WPS_CONFIG_P2PS; + } } else if (p2p->cfg->config_methods) { methods |= p2p->cfg->config_methods & (WPS_CONFIG_PUSHBUTTON | WPS_CONFIG_DISPLAY | - WPS_CONFIG_KEYPAD); + WPS_CONFIG_KEYPAD | WPS_CONFIG_P2PS); } else { methods |= WPS_CONFIG_PUSHBUTTON; methods |= WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD; + methods |= WPS_CONFIG_P2PS; } wpabuf_put_be16(buf, methods); @@ -342,6 +345,256 @@ void p2p_buf_add_oob_go_neg_channel(struct wpabuf *buf, const char *country, } +void p2p_buf_add_service_hash(struct wpabuf *buf, struct p2p_data *p2p) +{ + if (!p2p) + return; + + /* Service Hash */ + wpabuf_put_u8(buf, P2P_ATTR_SERVICE_HASH); + wpabuf_put_le16(buf, p2p->p2ps_seek_count * P2PS_HASH_LEN); + wpabuf_put_data(buf, p2p->query_hash, + p2p->p2ps_seek_count * P2PS_HASH_LEN); + wpa_hexdump(MSG_DEBUG, "P2P: * Service Hash", + p2p->query_hash, p2p->p2ps_seek_count * P2PS_HASH_LEN); +} + + +void p2p_buf_add_session_info(struct wpabuf *buf, const char *info) +{ + size_t info_len = 0; + + if (info && info[0]) + info_len = os_strlen(info); + + /* Session Information Data Info */ + wpabuf_put_u8(buf, P2P_ATTR_SESSION_INFORMATION_DATA); + wpabuf_put_le16(buf, (u16) info_len); + + if (info) { + wpabuf_put_data(buf, info, info_len); + wpa_printf(MSG_DEBUG, "P2P: * Session Info Data (%s)", info); + } +} + + +void p2p_buf_add_connection_capability(struct wpabuf *buf, u8 connection_cap) +{ + /* Connection Capability Info */ + wpabuf_put_u8(buf, P2P_ATTR_CONNECTION_CAPABILITY); + wpabuf_put_le16(buf, 1); + wpabuf_put_u8(buf, connection_cap); + wpa_printf(MSG_DEBUG, "P2P: * Connection Capability: 0x%x", + connection_cap); +} + + +void p2p_buf_add_advertisement_id(struct wpabuf *buf, u32 id, const u8 *mac) +{ + if (!buf || !mac) + return; + + /* Advertisement ID Info */ + wpabuf_put_u8(buf, P2P_ATTR_ADVERTISEMENT_ID); + wpabuf_put_le16(buf, (u16) (sizeof(u32) + ETH_ALEN)); + wpabuf_put_le32(buf, id); + wpabuf_put_data(buf, mac, ETH_ALEN); + wpa_printf(MSG_DEBUG, "P2P: * Advertisement ID (%x) " MACSTR, + id, MAC2STR(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) + return; + + /* Session ID Info */ + wpabuf_put_u8(buf, P2P_ATTR_SESSION_ID); + wpabuf_put_le16(buf, (u16) (sizeof(u32) + ETH_ALEN)); + wpabuf_put_le32(buf, id); + wpabuf_put_data(buf, mac, ETH_ALEN); + wpa_printf(MSG_DEBUG, "P2P: * Session ID Info (%x) " MACSTR, + id, MAC2STR(mac)); +} + + +void p2p_buf_add_feature_capability(struct wpabuf *buf, u16 len, const u8 *mask) +{ + if (!buf || !len || !mask) + return; + + /* Feature Capability */ + wpabuf_put_u8(buf, P2P_ATTR_FEATURE_CAPABILITY); + wpabuf_put_le16(buf, len); + wpabuf_put_data(buf, mask, len); + wpa_printf(MSG_DEBUG, "P2P: * Feature Capability (%d)", len); +} + + +void p2p_buf_add_persistent_group_info(struct wpabuf *buf, const u8 *dev_addr, + const u8 *ssid, size_t ssid_len) +{ + /* P2P Group ID */ + wpabuf_put_u8(buf, P2P_ATTR_PERSISTENT_GROUP); + wpabuf_put_le16(buf, ETH_ALEN + ssid_len); + wpabuf_put_data(buf, dev_addr, ETH_ALEN); + wpabuf_put_data(buf, ssid, ssid_len); + wpa_printf(MSG_DEBUG, "P2P: * P2P Group ID " MACSTR, + MAC2STR(dev_addr)); +} + + static int p2p_add_wps_string(struct wpabuf *buf, enum wps_attribute attr, const char *val) { |