p2putil: Extract WFD IE payloads from P2P Action frames

src/p2putil.c

@@ -984,6 +984,8 @@ int p2p_parse_go_negotiation_req(const uint8_t *pdu, size_t len,
 	d.go_intent = go_intent.intent;
 	d.go_tie_breaker = go_intent.tie_breaker;
 
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
+
 	memcpy(out, &d, sizeof(d));
 	return 0;
 
@@ -1043,6 +1045,8 @@ int p2p_parse_go_negotiation_resp(const uint8_t *pdu, size_t len,
 	d.go_intent = go_intent.intent;
 	d.go_tie_breaker = go_intent.tie_breaker;
 
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
+
 	memcpy(out, &d, sizeof(d));
 	return 0;
 
@@ -1072,12 +1076,16 @@ int p2p_parse_go_negotiation_confirmation(const uint8_t *pdu, size_t len,
 			REQUIRED(CHANNEL_LIST, &d.channel_list),
 			OPTIONAL(P2P_GROUP_ID, &d.group_id),
 			-1);
+	if (r < 0)
+		goto error;
+
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
 
-	if (r >= 0)
 	memcpy(out, &d, sizeof(d));
-	else
+	return 0;
-		p2p_clear_go_negotiation_confirmation(&d);
 
+error:
+	p2p_clear_go_negotiation_confirmation(&d);
 	return r;
 }
 
@@ -1109,27 +1117,30 @@ int p2p_parse_invitation_req(const uint8_t *pdu, size_t len,
 			REQUIRED(P2P_DEVICE_INFO, &d.device_info),
 			-1);
 	if (r < 0)
-		goto done;
+		goto error;
 
 	/* A WSC IE is optional */
 	wsc_data = ie_tlv_extract_wsc_payload(pdu + 1, len - 1, &wsc_len);
-	if (!wsc_data)
+	if (wsc_data) {
-		goto done;
+		r = wsc_parse_attrs(
-
+			wsc_data, wsc_len, &wsc_version2, NULL, 0, NULL,
-	r = wsc_parse_attrs(wsc_data, wsc_len, &wsc_version2, NULL, 0, NULL,
 			WSC_REQUIRED(DEVICE_PASSWORD_ID, &d.device_password_id),
 			WSC_ATTR_INVALID);
 		l_free(wsc_data);
 
-	if (r >= 0 && !wsc_version2)
+		if (r >= 0 && !wsc_version2) {
 			r = -EINVAL;
+			goto error;
+		}
+	}
+
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
 
-done:
-	if (r >= 0)
 	memcpy(out, &d, sizeof(d));
-	else
+	return 0;
-		p2p_clear_invitation_req(&d);
 
+error:
+	p2p_clear_invitation_req(&d);
 	return r;
 }
 
@@ -1154,12 +1165,16 @@ int p2p_parse_invitation_resp(const uint8_t *pdu, size_t len,
 			OPTIONAL(P2P_GROUP_BSSID, &d.group_bssid),
 			OPTIONAL(CHANNEL_LIST, &d.channel_list),
 			-1);
+	if (r < 0)
+		goto error;
+
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
 
-	if (r >= 0)
 	memcpy(out, &d, sizeof(d));
-	else
+	return 0;
-		p2p_clear_invitation_resp(&d);
 
+error:
+	p2p_clear_invitation_resp(&d);
 	return r;
 }
 
@@ -1277,6 +1292,8 @@ int p2p_parse_provision_disc_req(const uint8_t *pdu, size_t len,
 		goto error;
 	}
 
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
+
 	memcpy(out, &d, sizeof(d));
 	return 0;
 
@@ -1355,6 +1372,8 @@ int p2p_parse_provision_disc_resp(const uint8_t *pdu, size_t len,
 		goto error;
 	}
 
+	d.wfd = ie_tlv_extract_wfd_payload(pdu + 1, len - 1, &d.wfd_size);
+
 	memcpy(out, &d, sizeof(d));
 	return 0;
 
@@ -1523,41 +1542,55 @@ void p2p_clear_go_negotiation_req(struct p2p_go_negotiation_req *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
 	p2p_clear_device_info_attr(&data->device_info);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_go_negotiation_resp(struct p2p_go_negotiation_resp *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
 	p2p_clear_device_info_attr(&data->device_info);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_go_negotiation_confirmation(
 				struct p2p_go_negotiation_confirmation *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_invitation_req(struct p2p_invitation_req *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
 	p2p_clear_device_info_attr(&data->device_info);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_invitation_resp(struct p2p_invitation_resp *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_provision_disc_req(struct p2p_provision_discovery_req *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
 	p2p_clear_device_info_attr(&data->device_info);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_provision_disc_resp(struct p2p_provision_discovery_resp *data)
 {
 	p2p_clear_channel_list_attr(&data->channel_list);
 	p2p_clear_device_info_attr(&data->device_info);
+	l_free(data->wfd);
+	data->wfd = NULL;
 }
 
 void p2p_clear_notice_of_absence(struct p2p_notice_of_absence *data)

src/p2putil.h

@@ -377,6 +377,8 @@ struct p2p_go_negotiation_req {
 	struct p2p_device_info_attr device_info;
 	struct p2p_channel_attr operating_channel;
 	enum wsc_device_password_id device_password_id;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_go_negotiation_resp {
@@ -392,6 +394,8 @@ struct p2p_go_negotiation_resp {
 	struct p2p_device_info_attr device_info;
 	struct p2p_group_id_attr group_id;
 	enum wsc_device_password_id device_password_id;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_go_negotiation_confirmation {
@@ -401,6 +405,8 @@ struct p2p_go_negotiation_confirmation {
 	struct p2p_channel_attr operating_channel;
 	struct p2p_channel_list_attr channel_list;
 	struct p2p_group_id_attr group_id;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_invitation_req {
@@ -413,6 +419,8 @@ struct p2p_invitation_req {
 	struct p2p_group_id_attr group_id;
 	struct p2p_device_info_attr device_info;
 	enum wsc_device_password_id device_password_id;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_invitation_resp {
@@ -422,6 +430,8 @@ struct p2p_invitation_resp {
 	struct p2p_channel_attr operating_channel;
 	uint8_t group_bssid[6];
 	struct p2p_channel_list_attr channel_list;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_device_discoverability_req {
@@ -453,6 +463,8 @@ struct p2p_provision_discovery_req {
 	enum p2p_asp_coordination_transport_protocol transport_protocol;
 	struct p2p_group_id_attr persistent_group_info;
 	uint16_t wsc_config_method;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_provision_discovery_resp {
@@ -472,6 +484,8 @@ struct p2p_provision_discovery_resp {
 	struct p2p_group_id_attr persistent_group_info;
 	struct p2p_session_info_data_attr session_info;
 	uint16_t wsc_config_method;
+	uint8_t *wfd;
+	ssize_t wfd_size;
 };
 
 struct p2p_notice_of_absence {