P2PS: Re-factor p2p_buf_add_service_instance function

Add auxiliary functions to write a single advertised service info record
into a wpabuf and to find P2PS wildcard hash in a received hash
attribute. Re-factor p2p_buf_add_service_instance() function to allow
adding new wildcard types in future commits.

Signed-off-by: Max Stepanov <Max.Stepanov@intel.com>
Reviewed-by: Ilan Peer <ilan.peer@intel.com>

src/p2p/p2p_build.c

@@ -404,150 +404,229 @@ 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)
+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);