}
-void p2p_buf_add_service_instance(struct wpabuf *buf, struct p2p_data *p2p,
- u8 hash_count, const u8 *hash,
- struct p2ps_advertisement *adv_list)
+static int p2ps_wildcard_hash(struct p2p_data *p2p,
+ const u8 *hash, u8 hash_count)
{
- 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;
+ u8 i;
+ const u8 *test = hash;
- if (!adv_list || !hash)
- return;
+ 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;
+ }
- /* 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;
+ return 0;
+}
- 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;
- }
+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;
- if (os_memcmp(test, adv->hash, P2PS_HASH_LEN) == 0)
- goto hash_match;
+ svc_len = os_strlen(svc_name);
+ info_len = sizeof(adv_id) + sizeof(config_methods) + sizeof(u8) +
+ svc_len;
- test += P2PS_HASH_LEN;
- }
+ if (info_len + *total_len > MAX_SVC_ADV_LEN) {
+ p2p_dbg(p2p,
+ "Unsufficient buffer, failed to add advertised service info");
+ return -1;
+ }
- /* 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));
- }
- }
+ 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;
+ }
- svc_len = os_strlen(adv->svc_name);
+ if (*ie_len) {
+ int ie_data_len = (*pos - *ie_len) - 1;
- if (7 + svc_len + total_len > MAX_SVC_ADV_LEN) {
- /* Can't fit... return wildcard */
- total_len = MAX_SVC_ADV_LEN + 1;
- break;
+ 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(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));
- }
+ 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)];
- /* 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);
- }
+ WPA_PUT_LE32(holder, adv_id);
+ WPA_PUT_BE16(&holder[sizeof(u32)], config_methods);
+ holder[sizeof(u32) + sizeof(u16)] = svc_len;
- 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;
- }
+ if (front)
+ wpabuf_put_data(buf, holder, front);
- /* adv_id config_methods svc_string */
- total_len += sizeof(u32) + sizeof(u16) + sizeof(u8) + svc_len;
+ 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 (tag_len)
- p2p_buf_update_ie_hdr(tmp_buf, tag_len);
+ if (remaining < svc_len) {
+ /* split svc_name between two or three IEs */
+ size_t front = remaining;
+ size_t back = svc_len - front;
- if (ie_len)
- WPA_PUT_LE16(ie_len, (u16) total_len);
+ if (front)
+ wpabuf_put_data(buf, svc_name, front);
-wild_hash:
- /* If all fit, return matching instances, otherwise the wildcard */
- if (total_len <= MAX_SVC_ADV_LEN) {
- wpabuf_put_buf(buf, tmp_buf);
+ 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 {
- char *wild_card = P2PS_WILD_HASH_STR;
- u8 wild_len;
+ wpabuf_put_data(buf, svc_name, svc_len);
+ remaining -= svc_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));
+ p2p_buf_update_ie_hdr(buf, *ie_len);
- 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);
+ /* set *ie_len to NULL if a new IE has to be added on the next call */
+ if (!remaining)
+ *ie_len = NULL;
- WPA_PUT_LE16(ie_len, 4 + 2 + 1 + wild_len);
- p2p_buf_update_ie_hdr(buf, tag_len);
+ /* 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;
+ int p2ps_wildcard;
+ size_t total_len;
+ struct wpabuf *tmp_buf = NULL;
+ u8 *pos, *attr_len, *ie_len = NULL;
+
+ 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) {
+ const u8 *test = hash;
+ u8 i;
+
+ 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;
+ }
+ test += P2PS_HASH_LEN;
+ }
+ }
+
+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);
+ attr_len = wpabuf_put(buf, sizeof(u16));
+ pos = wpabuf_put(buf, 0);
+ total_len = 0;
+
+ 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);
projects / mech_eap.git / commitdiff