diff --git a/src/p2p/p2p_build.c b/src/p2p/p2p_build.c index 8cffa26ed..fa7f589bb 100644 --- a/src/p2p/p2p_build.c +++ b/src/p2p/p2p_build.c @@ -404,150 +404,229 @@ void p2p_buf_add_advertisement_id(struct wpabuf *buf, u32 id, const u8 *mac) } +static int p2ps_wildcard_hash(struct p2p_data *p2p, + const u8 *hash, u8 hash_count) +{ + u8 i; + const u8 *test = hash; + + for (i = 0; i < hash_count; i++) { + if (os_memcmp(test, p2p->wild_card_hash, P2PS_HASH_LEN) == 0) + return 1; + test += P2PS_HASH_LEN; + } + + return 0; +} + + +static int p2p_buf_add_service_info(struct wpabuf *buf, struct p2p_data *p2p, + u32 adv_id, u16 config_methods, + const char *svc_name, u8 **ie_len, u8 **pos, + size_t *total_len, u8 *attr_len) +{ + size_t svc_len; + size_t remaining; + size_t info_len; + + svc_len = os_strlen(svc_name); + info_len = sizeof(adv_id) + sizeof(config_methods) + sizeof(u8) + + svc_len; + + if (info_len + *total_len > MAX_SVC_ADV_LEN) { + p2p_dbg(p2p, + "Unsufficient buffer, failed to add advertised service info"); + return -1; + } + + if (svc_len > 255) { + p2p_dbg(p2p, + "Invalid service name length (%u bytes), failed to add advertised service info", + (unsigned int) svc_len); + return -1; + } + + if (*ie_len) { + int ie_data_len = (*pos - *ie_len) - 1; + + if (ie_data_len < 0 || ie_data_len > 255) { + p2p_dbg(p2p, + "Invalid IE length, failed to add advertised service info"); + return -1; + } + remaining = 255 - ie_data_len; + } else { + /* + * Adding new P2P IE header takes 6 extra bytes: + * - 2 byte IE header (1 byte IE id and 1 byte length) + * - 4 bytes of IE_VENDOR_TYPE are reduced from 255 below + */ + *ie_len = p2p_buf_add_ie_hdr(buf); + remaining = 255 - 4; + } + + if (remaining < sizeof(u32) + sizeof(u16) + sizeof(u8)) { + /* + * Split adv_id, config_methods, and svc_name_len between two + * IEs. + */ + size_t front = remaining; + size_t back = sizeof(u32) + sizeof(u16) + sizeof(u8) - front; + u8 holder[sizeof(u32) + sizeof(u16) + sizeof(u8)]; + + WPA_PUT_LE32(holder, adv_id); + WPA_PUT_BE16(&holder[sizeof(u32)], config_methods); + holder[sizeof(u32) + sizeof(u16)] = svc_len; + + if (front) + wpabuf_put_data(buf, holder, front); + + p2p_buf_update_ie_hdr(buf, *ie_len); + *ie_len = p2p_buf_add_ie_hdr(buf); + + wpabuf_put_data(buf, &holder[front], back); + remaining = 255 - 4 - (sizeof(u32) + sizeof(u16) + sizeof(u8)) - + back; + } else { + wpabuf_put_le32(buf, adv_id); + wpabuf_put_be16(buf, config_methods); + wpabuf_put_u8(buf, svc_len); + remaining -= sizeof(adv_id) + sizeof(config_methods) + + sizeof(u8); + } + + if (remaining < svc_len) { + /* split svc_name between two or three IEs */ + size_t front = remaining; + size_t back = svc_len - front; + + if (front) + wpabuf_put_data(buf, svc_name, front); + + p2p_buf_update_ie_hdr(buf, *ie_len); + *ie_len = p2p_buf_add_ie_hdr(buf); + + /* In rare cases, we must split across 3 attributes */ + if (back > 255 - 4) { + wpabuf_put_data(buf, &svc_name[front], 255 - 4); + back -= 255 - 4; + front += 255 - 4; + p2p_buf_update_ie_hdr(buf, *ie_len); + *ie_len = p2p_buf_add_ie_hdr(buf); + } + + wpabuf_put_data(buf, &svc_name[front], back); + remaining = 255 - 4 - back; + } else { + wpabuf_put_data(buf, svc_name, svc_len); + remaining -= svc_len; + } + + p2p_buf_update_ie_hdr(buf, *ie_len); + + /* set *ie_len to NULL if a new IE has to be added on the next call */ + if (!remaining) + *ie_len = NULL; + + /* set *pos to point to the next byte to update */ + *pos = wpabuf_put(buf, 0); + + *total_len += info_len; + WPA_PUT_LE16(attr_len, (u16) *total_len); + return 0; +} + + 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; + int p2ps_wildcard; + size_t total_len; + struct wpabuf *tmp_buf = NULL; + u8 *pos, *attr_len, *ie_len = NULL; - if (!adv_list || !hash) + if (!adv_list || !hash || !hash_count) return; + p2ps_wildcard = p2ps_wildcard_hash(p2p, hash, hash_count); + if (p2ps_wildcard) + goto end; + /* 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; + /* + * Attribute data can be split into a number of IEs. Start with the + * first IE and the attribute headers here. + */ + ie_len = p2p_buf_add_ie_hdr(tmp_buf); + + total_len = 0; + + wpabuf_put_u8(tmp_buf, P2P_ATTR_ADVERTISED_SERVICE); + attr_len = wpabuf_put(tmp_buf, sizeof(u16)); + WPA_PUT_LE16(attr_len, (u16) total_len); + p2p_buf_update_ie_hdr(tmp_buf, ie_len); + pos = wpabuf_put(tmp_buf, 0); + + /* add advertised service info of matching services */ for (adv = adv_list; adv && total_len <= MAX_SVC_ADV_LEN; adv = adv->next) { - u8 count = hash_count; const u8 *test = hash; + u8 i; - 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; + for (i = 0; i < hash_count; i++) { + /* exact name hash match */ + if (os_memcmp(test, adv->hash, P2PS_HASH_LEN) == 0 && + p2p_buf_add_service_info(tmp_buf, p2p, + adv->id, + adv->config_methods, + adv->svc_name, + &ie_len, &pos, + &total_len, + attr_len)) { + /* + * We cannot return all services matching + * the Probe Request frame hash attribute. In + * this case, drop currently written entries and + * return only a single wildcard advertised + * service info in the Probe Response frame. + */ + p2ps_wildcard = 1; + goto end; } - - 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); +end: + if (p2ps_wildcard) { + /* + * Add the attribute with P2PS wildcard if either a wildcard + * hash was present in a Probe Request frame hash attribute or + * we failed to add at least one matching advertisement. + */ + ie_len = p2p_buf_add_ie_hdr(buf); wpabuf_put_u8(buf, P2P_ATTR_ADVERTISED_SERVICE); - ie_len = wpabuf_put(buf, sizeof(u16)); + attr_len = wpabuf_put(buf, sizeof(u16)); + pos = wpabuf_put(buf, 0); + total_len = 0; - 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); + p2p_buf_add_service_info(buf, p2p, + 0, 0, P2PS_WILD_HASH_STR, + &ie_len, &pos, &total_len, attr_len); + } else if (tmp_buf) { + /* + * TODO: An empty attribute is returned if a device is not able + * to match advertised services. The P2PS specification defines + * that if the device is not a GO it shall not send a P2PS + * related Probe Response frame in this case. + */ + wpabuf_put_buf(buf, tmp_buf); } wpabuf_free(tmp_buf);