[PATCH 1/6] ap: Make ap_update_beacon public

[PATCH 1/6] ap: Make ap_update_beacon public

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 1293 bytes --]

Let users call ap_update_beacon when a value has changed which should be
reflected in the beacon IEs.
---
 src/ap.c | 5 ++++-
 src/ap.h | 1 +
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/ap.c b/src/ap.c
index 5a86527f..6ab0c0fd 100644
--- a/src/ap.c
+++ b/src/ap.c
@@ -641,7 +641,7 @@ static void ap_set_beacon_cb(struct l_genl_msg *msg, void *user_data)
 		l_error("SET_BEACON failed: %s (%i)", strerror(-error), -error);
 }
 
-static void ap_update_beacon(struct ap_state *ap)
+void ap_update_beacon(struct ap_state *ap)
 {
 	struct l_genl_msg *cmd;
 	uint8_t head[256], tail[256];
@@ -651,6 +651,9 @@ static void ap_update_beacon(struct ap_state *ap)
 		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
 	};
 
+	if (L_WARN_ON(!ap->started))
+		return;
+
 	head_len = ap_build_beacon_pr_head(ap, MPDU_MANAGEMENT_SUBTYPE_BEACON,
 						bcast_addr, head, sizeof(head));
 	tail_len = ap_build_beacon_pr_tail(ap, false, tail);
diff --git a/src/ap.h b/src/ap.h
index dc57a0bb..6199bcbe 100644
--- a/src/ap.h
+++ b/src/ap.h
@@ -87,3 +87,4 @@ bool ap_station_disconnect(struct ap_state *ap, const uint8_t *mac,
 				enum mmpdu_reason_code reason);
 
 bool ap_push_button(struct ap_state *ap);
+void ap_update_beacon(struct ap_state *ap);
-- 
2.27.0

[PATCH 2/6] ap: Pass frame IEs from clients to the ap_state user

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 2927 bytes --]

Pass the string IEs from the incoming STA association frames to
the user in the AP event data.  I drop
ap_event_station_added_data.rsn_ie because that probably wasn't
going to ever be useful and the RSN IE is included in the .assoc_ies
array in any case.
---
 src/ap.c | 22 +++++++++++++++-------
 src/ap.h |  5 ++++-
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/ap.c b/src/ap.c
index 6ab0c0fd..eeab3408 100644
--- a/src/ap.c
+++ b/src/ap.c
@@ -99,6 +99,8 @@ struct sta_state {
 	struct l_uintset *rates;
 	uint32_t assoc_resp_cmd_id;
 	struct ap_state *ap;
+	uint8_t *assoc_ies;
+	size_t assoc_ies_len;
 	uint8_t *assoc_rsne;
 	struct eapol_sm *sm;
 	struct handshake_state *hs;
@@ -279,7 +281,7 @@ static void ap_sta_free(void *data)
 	struct ap_state *ap = sta->ap;
 
 	l_uintset_free(sta->rates);
-	l_free(sta->assoc_rsne);
+	l_free(sta->assoc_ies);
 
 	if (sta->assoc_resp_cmd_id)
 		l_genl_family_cancel(ap->nl80211, sta->assoc_resp_cmd_id);
@@ -411,7 +413,8 @@ static void ap_new_rsna(struct sta_state *sta)
 	if (ap->ops->handle_event) {
 		struct ap_event_station_added_data event_data = {};
 		event_data.mac = sta->addr;
-		event_data.rsn_ie = sta->assoc_rsne;
+		event_data.assoc_ies = sta->assoc_ies;
+		event_data.assoc_ies_len = sta->assoc_ies_len;
 		ap->ops->handle_event(AP_EVENT_STATION_ADDED, &event_data,
 					ap->user_data);
 	}
@@ -1474,6 +1477,8 @@ static void ap_assoc_reassoc(struct sta_state *sta, bool reassoc,
 		sta->wsc_v2 = wsc_req.version2;
 
 		event_data.mac = sta->addr;
+		event_data.assoc_ies = ies;
+		event_data.assoc_ies_len = ies_len;
 		ap->ops->handle_event(AP_EVENT_REGISTRATION_START, &event_data,
 					ap->user_data);
 
@@ -1524,13 +1529,16 @@ static void ap_assoc_reassoc(struct sta_state *sta, bool reassoc,
 
 	sta->rates = rates;
 
-	if (sta->assoc_rsne)
-		l_free(sta->assoc_rsne);
+	l_free(sta->assoc_ies);
 
-	if (rsn)
-		sta->assoc_rsne = l_memdup(rsn, rsn[1] + 2);
-	else
+	if (rsn) {
+		sta->assoc_ies = l_memdup(ies, ies_len);
+		sta->assoc_ies_len = ies_len;
+		sta->assoc_rsne = sta->assoc_ies + (rsn - ies);
+	} else {
+		sta->assoc_ies = NULL;
 		sta->assoc_rsne = NULL;
+	}
 
 	sta->assoc_resp_cmd_id = ap_assoc_resp(ap, sta, sta->addr, 0, reassoc,
 						ap_success_assoc_resp_cb);
diff --git a/src/ap.h b/src/ap.h
index 6199bcbe..0bdbf0bb 100644
--- a/src/ap.h
+++ b/src/ap.h
@@ -36,7 +36,8 @@ enum ap_event_type {
 
 struct ap_event_station_added_data {
 	const uint8_t *mac;
-	const uint8_t *rsn_ie;
+	const uint8_t *assoc_ies;
+	size_t assoc_ies_len;
 };
 
 struct ap_event_station_removed_data {
@@ -46,6 +47,8 @@ struct ap_event_station_removed_data {
 
 struct ap_event_registration_start_data {
 	const uint8_t *mac;
+	const uint8_t *assoc_ies;
+	size_t assoc_ies_len;
 };
 
 struct ap_event_registration_success_data {
-- 
2.27.0

[PATCH 3/6] ap: Write extra frame IEs from the user

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 11556 bytes --]

Add an API for the ap.h users to add extra IEs to outgoing management
frames: beacons, etc.
---
Changes in this version:
 - a "client_frame" parameter is added to the callbacks so that the
   ap_state API user can also react to IEs such as the P2P IE from
   the client, and write its own IEs based on the contents.  The P2P
   IE for example should only be in a Probe Response if the Probe
   Request had one also.
   I gave up on the idea of having multiple callbacks for multiple
   frame types as in the end it is more limited.
 - a single callback can be registered in the ap_ops structure,
   dropped the method for dynamically registering those callbacks.
 - buffers for the frames are l_malloc'd instead of being on the stack.

 src/ap.c | 107 +++++++++++++++++++++++++++++++++++++++++++++----------
 src/ap.h |  23 ++++++++++++
 2 files changed, 111 insertions(+), 19 deletions(-)

diff --git a/src/ap.c b/src/ap.c
index eeab3408..a79a71ca 100644
--- a/src/ap.c
+++ b/src/ap.c
@@ -466,11 +466,48 @@ static void ap_set_rsn_info(struct ap_state *ap, struct ie_rsn_info *rsn)
 	rsn->group_cipher = ap->group_cipher;
 }
 
+static size_t ap_get_extra_ies_len(struct ap_state *ap,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len)
+{
+	size_t len = 0;
+
+	if (ap->ops->get_extra_ies_len)
+		len += ap->ops->get_extra_ies_len(type, client_frame,
+							client_frame_len,
+							ap->user_data);
+
+	return len;
+}
+
+static size_t ap_write_extra_ies(struct ap_state *ap,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					uint8_t *out_buf)
+{
+	size_t len = 0;
+
+	if (ap->ops->write_extra_ies)
+		len += ap->ops->write_extra_ies(type,
+						client_frame, client_frame_len,
+						out_buf + len, ap->user_data);
+
+	return len;
+}
+
 /*
  * Build a Beacon frame or a Probe Response frame's header and body until
  * the TIM IE.  Except for the optional TIM IE which is inserted by the
  * kernel when needed, our contents for both frames are the same.
  * See Beacon format in 8.3.3.2 and Probe Response format in 8.3.3.10.
+ *
+ * 802.11-2016, Section 9.4.2.1:
+ * "The frame body components specified for many management subtypes result
+ * in elements ordered by ascending values of the Element ID field and then
+ * the Element ID Extension field (when present), with the exception of the
+ * MIC Management element (9.4.2.55)."
  */
 static size_t ap_build_beacon_pr_head(struct ap_state *ap,
 					enum mpdu_management_subtype stype,
@@ -536,8 +573,10 @@ static size_t ap_build_beacon_pr_head(struct ap_state *ap,
 }
 
 /* Beacon / Probe Response frame portion after the TIM IE */
-static size_t ap_build_beacon_pr_tail(struct ap_state *ap, bool pr,
-					uint8_t *out_buf)
+static size_t ap_build_beacon_pr_tail(struct ap_state *ap,
+					enum mpdu_management_subtype stype,
+					const struct mmpdu_header *req,
+					size_t req_len, uint8_t *out_buf)
 {
 	size_t len;
 	struct ie_rsn_info rsn;
@@ -633,6 +672,7 @@ static size_t ap_build_beacon_pr_tail(struct ap_state *ap, bool pr,
 	len += wsc_ie_size;
 	l_free(wsc_ie);
 
+	len += ap_write_extra_ies(ap, stype, req, req_len, out_buf + len);
 	return len;
 }
 
@@ -647,7 +687,11 @@ static void ap_set_beacon_cb(struct l_genl_msg *msg, void *user_data)
 void ap_update_beacon(struct ap_state *ap)
 {
 	struct l_genl_msg *cmd;
-	uint8_t head[256], tail[256];
+	uint8_t head[256];
+	L_AUTO_FREE_VAR(uint8_t *, tail) =
+		l_malloc(256 + ap_get_extra_ies_len(ap,
+						MPDU_MANAGEMENT_SUBTYPE_BEACON,
+						NULL, 0));
 	size_t head_len, tail_len;
 	uint64_t wdev_id = netdev_get_wdev_id(ap->netdev);
 	static const uint8_t bcast_addr[6] = {
@@ -659,7 +703,8 @@ void ap_update_beacon(struct ap_state *ap)
 
 	head_len = ap_build_beacon_pr_head(ap, MPDU_MANAGEMENT_SUBTYPE_BEACON,
 						bcast_addr, head, sizeof(head));
-	tail_len = ap_build_beacon_pr_tail(ap, false, tail);
+	tail_len = ap_build_beacon_pr_tail(ap, MPDU_MANAGEMENT_SUBTYPE_BEACON,
+						NULL, 0, tail);
 	if (L_WARN_ON(!head_len || !tail_len))
 		return;
 
@@ -1197,10 +1242,15 @@ static void ap_fail_assoc_resp_cb(int err, void *user_data)
 static uint32_t ap_assoc_resp(struct ap_state *ap, struct sta_state *sta,
 				const uint8_t *dest,
 				enum mmpdu_reason_code status_code,
-				bool reassoc, frame_xchg_cb_t callback)
+				bool reassoc, const struct mmpdu_header *req,
+				size_t req_len, frame_xchg_cb_t callback)
 {
 	const uint8_t *addr = netdev_get_address(ap->netdev);
-	uint8_t mpdu_buf[128];
+	enum mpdu_management_subtype stype = reassoc ?
+		MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE :
+		MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE;
+	L_AUTO_FREE_VAR(uint8_t *, mpdu_buf) =
+		l_malloc(128 + ap_get_extra_ies_len(ap, stype, req, req_len));
 	struct mmpdu_header *mpdu = (void *) mpdu_buf;
 	struct mmpdu_association_response *resp;
 	size_t ies_len = 0;
@@ -1212,9 +1262,7 @@ static uint32_t ap_assoc_resp(struct ap_state *ap, struct sta_state *sta,
 	/* Header */
 	mpdu->fc.protocol_version = 0;
 	mpdu->fc.type = MPDU_TYPE_MANAGEMENT;
-	mpdu->fc.subtype = reassoc ?
-		MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE :
-		MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE;
+	mpdu->fc.subtype = stype;
 	memcpy(mpdu->address_1, dest, 6);	/* DA */
 	memcpy(mpdu->address_2, addr, 6);	/* SA */
 	memcpy(mpdu->address_3, addr, 6);	/* BSSID */
@@ -1276,6 +1324,9 @@ static uint32_t ap_assoc_resp(struct ap_state *ap, struct sta_state *sta,
 		l_free(wsc_ie);
 	}
 
+	ies_len += ap_write_extra_ies(ap, stype, req, req_len,
+					resp->ies + ies_len);
+
 send_frame:
 	return ap_send_mgmt_frame(ap, mpdu, resp->ies + ies_len - mpdu_buf,
 					callback, sta);
@@ -1351,8 +1402,9 @@ static int ap_parse_supported_rates(struct ie_tlv_iter *iter,
  */
 static void ap_assoc_reassoc(struct sta_state *sta, bool reassoc,
 				const struct mmpdu_field_capability *capability,
-				uint16_t listen_interval, const uint8_t *ies,
-				size_t ies_len)
+				uint16_t listen_interval,
+				const uint8_t *ies, size_t ies_len,
+				const struct mmpdu_header *req)
 {
 	struct ap_state *ap = sta->ap;
 	const char *ssid = NULL;
@@ -1541,6 +1593,8 @@ static void ap_assoc_reassoc(struct sta_state *sta, bool reassoc,
 	}
 
 	sta->assoc_resp_cmd_id = ap_assoc_resp(ap, sta, sta->addr, 0, reassoc,
+						req, (void *) ies + ies_len -
+						(void *) req,
 						ap_success_assoc_resp_cb);
 	if (!sta->assoc_resp_cmd_id)
 		l_error("Sending success (Re)Association Response failed");
@@ -1572,6 +1626,7 @@ bad_frame:
 	l_free(wsc_data);
 
 	if (!ap_assoc_resp(ap, sta, sta->addr, err, reassoc,
+				req, (void *) ies + ies_len - (void *) req,
 				ap_fail_assoc_resp_cb))
 		l_error("Sending error (Re)Association Response failed");
 }
@@ -1596,7 +1651,8 @@ static void ap_assoc_req_cb(const struct mmpdu_header *hdr, const void *body,
 	if (!sta) {
 		if (!ap_assoc_resp(ap, NULL, from,
 				MMPDU_REASON_CODE_STA_REQ_ASSOC_WITHOUT_AUTH,
-				false, ap_fail_assoc_resp_cb))
+				false, hdr, body + body_len - (void *) hdr,
+				ap_fail_assoc_resp_cb))
 			l_error("Sending error Association Response failed");
 
 		return;
@@ -1604,7 +1660,7 @@ static void ap_assoc_req_cb(const struct mmpdu_header *hdr, const void *body,
 
 	ap_assoc_reassoc(sta, false, &req->capability,
 				L_LE16_TO_CPU(req->listen_interval),
-				req->ies, body_len - sizeof(*req));
+				req->ies, body_len - sizeof(*req), hdr);
 }
 
 /* 802.11-2016 9.3.3.8 */
@@ -1638,11 +1694,13 @@ static void ap_reassoc_req_cb(const struct mmpdu_header *hdr, const void *body,
 
 	ap_assoc_reassoc(sta, true, &req->capability,
 				L_LE16_TO_CPU(req->listen_interval),
-				req->ies, body_len - sizeof(*req));
+				req->ies, body_len - sizeof(*req), hdr);
 	return;
 
 bad_frame:
-	if (!ap_assoc_resp(ap, NULL, from, err, true, ap_fail_assoc_resp_cb))
+	if (!ap_assoc_resp(ap, NULL, from, err, true,
+				hdr, body + body_len - (void *) hdr,
+				ap_fail_assoc_resp_cb))
 		l_error("Sending error Reassociation Response failed");
 }
 
@@ -1785,7 +1843,10 @@ static void ap_probe_req_cb(const struct mmpdu_header *hdr, const void *body,
 	struct ie_tlv_iter iter;
 	const uint8_t *bssid = netdev_get_address(ap->netdev);
 	bool match = false;
-	uint8_t resp[512];
+	L_AUTO_FREE_VAR(uint8_t *, resp) =
+		l_malloc(512 + ap_get_extra_ies_len(ap,
+				MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE, hdr,
+				body + body_len - (void *) hdr));
 	uint8_t *wsc_data;
 	ssize_t wsc_data_len;
 
@@ -1875,7 +1936,10 @@ static void ap_probe_req_cb(const struct mmpdu_header *hdr, const void *body,
 	len = ap_build_beacon_pr_head(ap,
 					MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE,
 					hdr->address_2, resp, sizeof(resp));
-	len += ap_build_beacon_pr_tail(ap, true, resp + len);
+	len += ap_build_beacon_pr_tail(ap,
+					MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE,
+					hdr, body + body_len - (void *) hdr,
+					resp + len);
 
 	ap_send_mgmt_frame(ap, (struct mmpdu_header *) resp, len,
 				ap_probe_resp_cb, NULL);
@@ -2108,7 +2172,11 @@ static struct l_genl_msg *ap_build_cmd_start_ap(struct ap_state *ap)
 {
 	struct l_genl_msg *cmd;
 
-	uint8_t head[256], tail[256];
+	uint8_t head[256];
+	L_AUTO_FREE_VAR(uint8_t *, tail) =
+		l_malloc(256 + ap_get_extra_ies_len(ap,
+						MPDU_MANAGEMENT_SUBTYPE_BEACON,
+						NULL, 0));
 	size_t head_len, tail_len;
 
 	uint32_t dtim_period = 3;
@@ -2138,7 +2206,8 @@ static struct l_genl_msg *ap_build_cmd_start_ap(struct ap_state *ap)
 
 	head_len = ap_build_beacon_pr_head(ap, MPDU_MANAGEMENT_SUBTYPE_BEACON,
 						bcast_addr, head, sizeof(head));
-	tail_len = ap_build_beacon_pr_tail(ap, false, tail);
+	tail_len = ap_build_beacon_pr_tail(ap, MPDU_MANAGEMENT_SUBTYPE_BEACON,
+						NULL, 0, tail);
 
 	if (!head_len || !tail_len)
 		return NULL;
diff --git a/src/ap.h b/src/ap.h
index 0bdbf0bb..729a6648 100644
--- a/src/ap.h
+++ b/src/ap.h
@@ -22,6 +22,7 @@
 
 struct ap_state;
 struct iovec;
+enum mpdu_management_subtype;
 
 enum ap_event_type {
 	AP_EVENT_START_FAILED,
@@ -75,6 +76,28 @@ struct ap_config {
 struct ap_ops {
 	void (*handle_event)(enum ap_event_type type, const void *event_data,
 				void *user_data);
+	/*
+	 * If .write_extra_ies is provided, this callback must return an upper
+	 * bound on the buffer space needed for the extra IEs to be sent in
+	 * the frame of given type and, if it's not a beacon frame, in
+	 * response to a given client frame.
+	 */
+	size_t (*get_extra_ies_len)(enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					void *user_data);
+	/*
+	 * If not null, writes extra IEs to be added to the outgoing frame of
+	 * given type and, if it's not a beacon frame, in reponse to a given
+	 * client frame.  May also react to the extra IEs in that frame.
+	 * Returns the number of bytes written which must be less than or
+	 * equal to the number returned by .get_extra_ies_len when called
+	 * with the same parameters.
+	 */
+	size_t (*write_extra_ies)(enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					uint8_t *out_buf, void *user_data);
 };
 
 void ap_config_free(struct ap_config *config);
-- 
2.27.0

Re: [PATCH 3/6] ap: Write extra frame IEs from the user

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 1578 bytes --]

On 3/10/21 4:06 PM, Andrew Zaborowski wrote:
> Add an API for the ap.h users to add extra IEs to outgoing management
> frames: beacons, etc.
> ---
> Changes in this version:
>   - a "client_frame" parameter is added to the callbacks so that the
>     ap_state API user can also react to IEs such as the P2P IE from
>     the client, and write its own IEs based on the contents.  The P2P
>     IE for example should only be in a Probe Response if the Probe
>     Request had one also.
>     I gave up on the idea of having multiple callbacks for multiple
>     frame types as in the end it is more limited.
>   - a single callback can be registered in the ap_ops structure,
>     dropped the method for dynamically registering those callbacks.
>   - buffers for the frames are l_malloc'd instead of being on the stack.
> 
>   src/ap.c | 107 +++++++++++++++++++++++++++++++++++++++++++++----------
>   src/ap.h |  23 ++++++++++++
>   2 files changed, 111 insertions(+), 19 deletions(-)
> 

This looks good to me.  I'm getting a compiler error after applying this patch 
though:

denkenz(a)localhost ~/iwd-master $ make
make --no-print-directory all-am
   CC       src/ap.o
src/ap.c: In function ‘ap_build_beacon_pr_tail’:
src/ap.c:597:6: error: ‘pr’ undeclared (first use in this function)
   597 |  if (pr) {
       |      ^~
src/ap.c:597:6: note: each undeclared identifier is reported only once for each 
function it appears in
make[1]: *** [Makefile:2239: src/ap.o] Error 1
make: *** [Makefile:1477: all] Error 2

Regards,
-Denis

Re: [PATCH 3/6] ap: Write extra frame IEs from the user

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 262 bytes --]

Hi Denis,

On Wed, 10 Mar 2021 at 23:48, Denis Kenzior <denkenz@gmail.com> wrote:
> This looks good to me.  I'm getting a compiler error after applying this patch
> though:

Oops, overlooked this when splitting changes into smaller patches.

Best regards

[PATCH 4/6] ap: Handle most WSC IEs using ap_write_extra_ies

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 19440 bytes --]

Make the WSC IE processing and writing more self-contained (i.e. so that
it can be more easily moved to a separate file if desired) by using the
new ap_write_extra_ies() mechanism.
---
 src/ap.c | 559 ++++++++++++++++++++++++++++---------------------------
 1 file changed, 288 insertions(+), 271 deletions(-)

diff --git a/src/ap.c b/src/ap.c
index a79a71ca..ece2ef3c 100644
--- a/src/ap.c
+++ b/src/ap.c
@@ -466,6 +466,289 @@ static void ap_set_rsn_info(struct ap_state *ap, struct ie_rsn_info *rsn)
 	rsn->group_cipher = ap->group_cipher;
 }
 
+static void ap_wsc_exit_pbc(struct ap_state *ap)
+{
+	if (!ap->wsc_pbc_timeout)
+		return;
+
+	l_timeout_remove(ap->wsc_pbc_timeout);
+	ap->wsc_dpid = 0;
+	ap_update_beacon(ap);
+
+	ap->ops->handle_event(AP_EVENT_PBC_MODE_EXIT, NULL, ap->user_data);
+}
+
+struct ap_pbc_record_expiry_data {
+	uint64_t min_time;
+	const uint8_t *mac;
+};
+
+static bool ap_wsc_pbc_record_expire(void *data, void *user_data)
+{
+	struct ap_wsc_pbc_probe_record *record = data;
+	const struct ap_pbc_record_expiry_data *expiry_data = user_data;
+
+	if (record->timestamp > expiry_data->min_time &&
+			memcmp(record->mac, expiry_data->mac, 6))
+		return false;
+
+	l_free(record);
+	return true;
+}
+
+#define AP_WSC_PBC_MONITOR_TIME	120
+#define AP_WSC_PBC_WALK_TIME	120
+
+static void ap_process_wsc_probe_req(struct ap_state *ap, const uint8_t *from,
+					const uint8_t *wsc_data,
+					size_t wsc_data_len)
+{
+	struct wsc_probe_request req;
+	struct ap_pbc_record_expiry_data expiry_data;
+	struct ap_wsc_pbc_probe_record *record;
+	uint64_t now;
+	bool empty;
+	uint8_t first_sta_addr[6] = {};
+	const struct l_queue_entry *entry;
+
+	if (wsc_parse_probe_request(wsc_data, wsc_data_len, &req) < 0)
+		return;
+
+	if (!(req.config_methods & WSC_CONFIGURATION_METHOD_PUSH_BUTTON))
+		return;
+
+	if (req.device_password_id != WSC_DEVICE_PASSWORD_ID_PUSH_BUTTON)
+		return;
+
+	/* Save the address of the first enrollee record */
+	record = l_queue_peek_head(ap->wsc_pbc_probes);
+	if (record)
+		memcpy(first_sta_addr, record->mac, 6);
+
+	now = l_time_now();
+
+	/*
+	 * Expire entries older than PBC Monitor Time.  While there also drop
+	 * older entries from the same Enrollee that sent us this new Probe
+	 * Request.  It's unclear whether we should also match by the UUID-E.
+	 */
+	expiry_data.min_time = now - AP_WSC_PBC_MONITOR_TIME * 1000000;
+	expiry_data.mac = from;
+	l_queue_foreach_remove(ap->wsc_pbc_probes, ap_wsc_pbc_record_expire,
+				&expiry_data);
+
+	empty = l_queue_isempty(ap->wsc_pbc_probes);
+
+	if (!ap->wsc_pbc_probes)
+		ap->wsc_pbc_probes = l_queue_new();
+
+	/* Add new record */
+	record = l_new(struct ap_wsc_pbc_probe_record, 1);
+	memcpy(record->mac, from, 6);
+	memcpy(record->uuid_e, req.uuid_e, sizeof(record->uuid_e));
+	record->timestamp = now;
+	l_queue_push_tail(ap->wsc_pbc_probes, record);
+
+	/*
+	 * If queue was non-empty and we've added one more record then we
+	 * now have seen more than one PBC enrollee during the PBC Monitor
+	 * Time and must exit "active PBC mode" due to "session overlap".
+	 * WSC v2.0.5 Section 11.3:
+	 * "Within the PBC Monitor Time, if the Registrar receives PBC
+	 * probe requests from more than one Enrollee [...] then the
+	 * Registrar SHALL signal a "session overlap" error.  As a result,
+	 * the Registrar shall refuse to enter active PBC mode and shall
+	 * also refuse to perform a PBC-based Registration Protocol
+	 * exchange [...]"
+	 */
+	if (empty)
+		return;
+
+	if (ap->wsc_pbc_timeout) {
+		l_debug("Exiting PBC mode due to Session Overlap");
+		ap_wsc_exit_pbc(ap);
+	}
+
+	/*
+	 * "If the Registrar is engaged in PBC Registration Protocol
+	 * exchange with an Enrollee and receives a Probe Request or M1
+	 * Message from another Enrollee, then the Registrar should
+	 * signal a "session overlap" error".
+	 *
+	 * For simplicity just interrupt the handshake with that enrollee.
+	 */
+	for (entry = l_queue_get_entries(ap->sta_states); entry;
+			entry = entry->next) {
+		struct sta_state *sta = entry->data;
+
+		if (!sta->associated || sta->assoc_rsne)
+			continue;
+
+		/*
+		 * Check whether this enrollee is in PBC Registration
+		 * Protocol by comparing its mac with the first (and only)
+		 * record we had in ap->wsc_pbc_probes.  If we had more
+		 * than one record we wouldn't have been in
+		 * "active PBC mode".
+		 */
+		if (memcmp(sta->addr, first_sta_addr, 6) ||
+				!memcmp(sta->addr, from, 6))
+			continue;
+
+		l_debug("Interrupting handshake with %s due to Session Overlap",
+			util_address_to_string(sta->addr));
+
+		if (sta->hs) {
+			netdev_handshake_failed(sta->hs,
+					MMPDU_REASON_CODE_DISASSOC_AP_BUSY);
+			sta->sm = NULL;
+		}
+
+		ap_remove_sta(sta);
+	}
+}
+
+static size_t ap_get_wsc_ie_len(struct ap_state *ap,
+				enum mpdu_management_subtype type,
+				const struct mmpdu_header *client_frame,
+				size_t client_frame_len)
+{
+	return 256;
+}
+
+static size_t ap_write_wsc_ie(struct ap_state *ap,
+				enum mpdu_management_subtype type,
+				const struct mmpdu_header *client_frame,
+				size_t client_frame_len,
+				uint8_t *out_buf)
+{
+	const uint8_t *from = client_frame->address_2;
+	uint8_t *wsc_data;
+	size_t wsc_data_size;
+	uint8_t *wsc_ie;
+	size_t wsc_ie_size;
+	size_t len = 0;
+
+	/* WSC IE */
+	if (type == MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE) {
+		struct wsc_probe_response wsc_pr = {};
+		const struct mmpdu_probe_request *req =
+			mmpdu_body(client_frame);
+		size_t req_ies_len = (void *) client_frame + client_frame_len -
+			(void *) req->ies;
+		ssize_t req_wsc_data_size;
+
+		/*
+		 * Process the client Probe Request WSC IE first as it may
+		 * cause us to exit "active PBC mode" and that will be
+		 * immediately reflected in our Probe Response WSC IE.
+		 */
+		wsc_data = ie_tlv_extract_wsc_payload(req->ies, req_ies_len,
+							&req_wsc_data_size);
+		if (wsc_data) {
+			ap_process_wsc_probe_req(ap, from, wsc_data,
+							req_wsc_data_size);
+			l_free(wsc_data);
+		}
+
+		wsc_pr.version2 = true;
+		wsc_pr.state = WSC_STATE_CONFIGURED;
+
+		if (ap->wsc_pbc_timeout) {
+			wsc_pr.selected_registrar = true;
+			wsc_pr.device_password_id = ap->wsc_dpid;
+			wsc_pr.selected_reg_config_methods =
+				WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
+		}
+
+		wsc_pr.response_type = WSC_RESPONSE_TYPE_AP;
+		memcpy(wsc_pr.uuid_e, ap->wsc_uuid_r, sizeof(wsc_pr.uuid_e));
+		wsc_pr.primary_device_type =
+			ap->config->wsc_primary_device_type;
+
+		if (ap->config->wsc_name)
+			l_strlcpy(wsc_pr.device_name, ap->config->wsc_name,
+					sizeof(wsc_pr.device_name));
+
+		wsc_pr.config_methods =
+			WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
+
+		if (ap->config->authorized_macs_num) {
+			size_t len;
+
+			len = ap->config->authorized_macs_num * 6;
+			if (len > sizeof(wsc_pr.authorized_macs))
+				len = sizeof(wsc_pr.authorized_macs);
+
+			memcpy(wsc_pr.authorized_macs,
+				ap->config->authorized_macs, len);
+		}
+
+		wsc_data = wsc_build_probe_response(&wsc_pr, &wsc_data_size);
+	} else if (type == MPDU_MANAGEMENT_SUBTYPE_BEACON) {
+		struct wsc_beacon wsc_beacon = {};
+
+		wsc_beacon.version2 = true;
+		wsc_beacon.state = WSC_STATE_CONFIGURED;
+
+		if (ap->wsc_pbc_timeout) {
+			wsc_beacon.selected_registrar = true;
+			wsc_beacon.device_password_id = ap->wsc_dpid;
+			wsc_beacon.selected_reg_config_methods =
+				WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
+		}
+
+		if (ap->config->authorized_macs_num) {
+			size_t len;
+
+			len = ap->config->authorized_macs_num * 6;
+			if (len > sizeof(wsc_beacon.authorized_macs))
+				len = sizeof(wsc_beacon.authorized_macs);
+
+			memcpy(wsc_beacon.authorized_macs,
+				ap->config->authorized_macs, len);
+		}
+
+		wsc_data = wsc_build_beacon(&wsc_beacon, &wsc_data_size);
+	} else if (L_IN_SET(type, MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE,
+			MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE)) {
+		struct wsc_association_response wsc_resp = {};
+		struct sta_state *sta =
+			l_queue_find(ap->sta_states, ap_sta_match_addr, from);
+
+		if (!sta || sta->assoc_rsne)
+			return 0;
+
+		wsc_resp.response_type = WSC_RESPONSE_TYPE_AP;
+		wsc_resp.version2 = sta->wsc_v2;
+
+		wsc_data = wsc_build_association_response(&wsc_resp,
+								&wsc_data_size);
+	} else
+		return 0;
+
+	if (!wsc_data) {
+		l_error("wsc_build_<mgmt-subtype> error (stype 0x%x)", type);
+		return 0;
+	}
+
+	wsc_ie = ie_tlv_encapsulate_wsc_payload(wsc_data, wsc_data_size,
+						&wsc_ie_size);
+	l_free(wsc_data);
+
+	if (!wsc_ie) {
+		l_error("ie_tlv_encapsulate_wsc_payload error (stype 0x%x)",
+			type);
+		return 0;
+	}
+
+	memcpy(out_buf + len, wsc_ie, wsc_ie_size);
+	len += wsc_ie_size;
+	l_free(wsc_ie);
+
+	return len;
+}
+
 static size_t ap_get_extra_ies_len(struct ap_state *ap,
 					enum mpdu_management_subtype type,
 					const struct mmpdu_header *client_frame,
@@ -473,6 +756,8 @@ static size_t ap_get_extra_ies_len(struct ap_state *ap,
 {
 	size_t len = 0;
 
+	len += ap_get_wsc_ie_len(ap, type, client_frame, client_frame_len);
+
 	if (ap->ops->get_extra_ies_len)
 		len += ap->ops->get_extra_ies_len(type, client_frame,
 							client_frame_len,
@@ -489,6 +774,9 @@ static size_t ap_write_extra_ies(struct ap_state *ap,
 {
 	size_t len = 0;
 
+	len += ap_write_wsc_ie(ap, type, client_frame, client_frame_len,
+				out_buf + len);
+
 	if (ap->ops->write_extra_ies)
 		len += ap->ops->write_extra_ies(type,
 						client_frame, client_frame_len,
@@ -580,10 +868,6 @@ static size_t ap_build_beacon_pr_tail(struct ap_state *ap,
 {
 	size_t len;
 	struct ie_rsn_info rsn;
-	uint8_t *wsc_data;
-	size_t wsc_data_size;
-	uint8_t *wsc_ie;
-	size_t wsc_ie_size;
 
 	/* TODO: Country IE between TIM IE and RSNE */
 
@@ -593,85 +877,6 @@ static size_t ap_build_beacon_pr_tail(struct ap_state *ap,
 		return 0;
 	len = 2 + out_buf[1];
 
-	/* WSC IE */
-	if (pr) {
-		struct wsc_probe_response wsc_pr = {};
-
-		wsc_pr.version2 = true;
-		wsc_pr.state = WSC_STATE_CONFIGURED;
-
-		if (ap->wsc_pbc_timeout) {
-			wsc_pr.selected_registrar = true;
-			wsc_pr.device_password_id = ap->wsc_dpid;
-			wsc_pr.selected_reg_config_methods =
-				WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
-		}
-
-		wsc_pr.response_type = WSC_RESPONSE_TYPE_AP;
-		memcpy(wsc_pr.uuid_e, ap->wsc_uuid_r, sizeof(wsc_pr.uuid_e));
-		wsc_pr.primary_device_type =
-			ap->config->wsc_primary_device_type;
-
-		if (ap->config->wsc_name)
-			l_strlcpy(wsc_pr.device_name, ap->config->wsc_name,
-					sizeof(wsc_pr.device_name));
-
-		wsc_pr.config_methods =
-			WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
-
-		if (ap->config->authorized_macs_num) {
-			size_t len;
-
-			len = ap->config->authorized_macs_num * 6;
-			if (len > sizeof(wsc_pr.authorized_macs))
-				len = sizeof(wsc_pr.authorized_macs);
-
-			memcpy(wsc_pr.authorized_macs,
-				ap->config->authorized_macs, len);
-		}
-
-		wsc_data = wsc_build_probe_response(&wsc_pr, &wsc_data_size);
-	} else {
-		struct wsc_beacon wsc_beacon = {};
-
-		wsc_beacon.version2 = true;
-		wsc_beacon.state = WSC_STATE_CONFIGURED;
-
-		if (ap->wsc_pbc_timeout) {
-			wsc_beacon.selected_registrar = true;
-			wsc_beacon.device_password_id = ap->wsc_dpid;
-			wsc_beacon.selected_reg_config_methods =
-				WSC_CONFIGURATION_METHOD_PUSH_BUTTON;
-		}
-
-		if (ap->config->authorized_macs_num) {
-			size_t len;
-
-			len = ap->config->authorized_macs_num * 6;
-			if (len > sizeof(wsc_beacon.authorized_macs))
-				len = sizeof(wsc_beacon.authorized_macs);
-
-			memcpy(wsc_beacon.authorized_macs,
-				ap->config->authorized_macs, len);
-		}
-
-		wsc_data = wsc_build_beacon(&wsc_beacon, &wsc_data_size);
-	}
-
-	if (!wsc_data)
-		return 0;
-
-	wsc_ie = ie_tlv_encapsulate_wsc_payload(wsc_data, wsc_data_size,
-						&wsc_ie_size);
-	l_free(wsc_data);
-
-	if (!wsc_ie)
-		return 0;
-
-	memcpy(out_buf + len, wsc_ie, wsc_ie_size);
-	len += wsc_ie_size;
-	l_free(wsc_ie);
-
 	len += ap_write_extra_ies(ap, stype, req, req_len, out_buf + len);
 	return len;
 }
@@ -724,18 +929,6 @@ void ap_update_beacon(struct ap_state *ap)
 	l_error("Issuing SET_BEACON failed");
 }
 
-static void ap_wsc_exit_pbc(struct ap_state *ap)
-{
-	if (!ap->wsc_pbc_timeout)
-		return;
-
-	l_timeout_remove(ap->wsc_pbc_timeout);
-	ap->wsc_dpid = 0;
-	ap_update_beacon(ap);
-
-	ap->ops->handle_event(AP_EVENT_PBC_MODE_EXIT, NULL, ap->user_data);
-}
-
 static uint32_t ap_send_mgmt_frame(struct ap_state *ap,
 					const struct mmpdu_header *frame,
 					size_t frame_len,
@@ -858,24 +1051,6 @@ error:
 	ap_del_station(sta, MMPDU_REASON_CODE_UNSPECIFIED, true);
 }
 
-struct ap_pbc_record_expiry_data {
-	uint64_t min_time;
-	const uint8_t *mac;
-};
-
-static bool ap_wsc_pbc_record_expire(void *data, void *user_data)
-{
-	struct ap_wsc_pbc_probe_record *record = data;
-	const struct ap_pbc_record_expiry_data *expiry_data = user_data;
-
-	if (record->timestamp > expiry_data->min_time &&
-			memcmp(record->mac, expiry_data->mac, 6))
-		return false;
-
-	l_free(record);
-	return true;
-}
-
 static void ap_stop_handshake_schedule(struct sta_state *sta)
 {
 	if (sta->stop_handshake_work)
@@ -1293,41 +1468,9 @@ static uint32_t ap_assoc_resp(struct ap_state *ap, struct sta_state *sta,
 	resp->ies[ies_len++] = count;
 	ies_len += count;
 
-	if (sta && !sta->assoc_rsne) {
-		struct wsc_association_response wsc_resp = {};
-		uint8_t *wsc_data;
-		size_t wsc_data_len;
-		uint8_t *wsc_ie;
-		size_t wsc_ie_len;
-
-		wsc_resp.response_type = WSC_RESPONSE_TYPE_AP;
-		wsc_resp.version2 = sta->wsc_v2;
-
-		wsc_data = wsc_build_association_response(&wsc_resp,
-								&wsc_data_len);
-		if (!wsc_data) {
-			l_error("wsc_build_beacon error");
-			goto send_frame;
-		}
-
-		wsc_ie = ie_tlv_encapsulate_wsc_payload(wsc_data, wsc_data_len,
-							&wsc_ie_len);
-		l_free(wsc_data);
-
-		if (!wsc_ie) {
-			l_error("ie_tlv_encapsulate_wsc_payload error");
-			goto send_frame;
-		}
-
-		memcpy(resp->ies + ies_len, wsc_ie, wsc_ie_len);
-		ies_len += wsc_ie_len;
-		l_free(wsc_ie);
-	}
-
 	ies_len += ap_write_extra_ies(ap, stype, req, req_len,
 					resp->ies + ies_len);
 
-send_frame:
 	return ap_send_mgmt_frame(ap, mpdu, resp->ies + ies_len - mpdu_buf,
 					callback, sta);
 }
@@ -1704,118 +1847,6 @@ bad_frame:
 		l_error("Sending error Reassociation Response failed");
 }
 
-#define AP_WSC_PBC_MONITOR_TIME	120
-#define AP_WSC_PBC_WALK_TIME	120
-
-static void ap_process_wsc_probe_req(struct ap_state *ap, const uint8_t *from,
-					const uint8_t *wsc_data,
-					size_t wsc_data_len)
-{
-	struct wsc_probe_request req;
-	struct ap_pbc_record_expiry_data expiry_data;
-	struct ap_wsc_pbc_probe_record *record;
-	uint64_t now;
-	bool empty;
-	uint8_t first_sta_addr[6] = {};
-	const struct l_queue_entry *entry;
-
-	if (wsc_parse_probe_request(wsc_data, wsc_data_len, &req) < 0)
-		return;
-
-	if (!(req.config_methods & WSC_CONFIGURATION_METHOD_PUSH_BUTTON))
-		return;
-
-	if (req.device_password_id != WSC_DEVICE_PASSWORD_ID_PUSH_BUTTON)
-		return;
-
-	/* Save the address of the first enrollee record */
-	record = l_queue_peek_head(ap->wsc_pbc_probes);
-	if (record)
-		memcpy(first_sta_addr, record->mac, 6);
-
-	now = l_time_now();
-
-	/*
-	 * Expire entries older than PBC Monitor Time.  While there also drop
-	 * older entries from the same Enrollee that sent us this new Probe
-	 * Request.  It's unclear whether we should also match by the UUID-E.
-	 */
-	expiry_data.min_time = now - AP_WSC_PBC_MONITOR_TIME * 1000000;
-	expiry_data.mac = from;
-	l_queue_foreach_remove(ap->wsc_pbc_probes, ap_wsc_pbc_record_expire,
-				&expiry_data);
-
-	empty = l_queue_isempty(ap->wsc_pbc_probes);
-
-	if (!ap->wsc_pbc_probes)
-		ap->wsc_pbc_probes = l_queue_new();
-
-	/* Add new record */
-	record = l_new(struct ap_wsc_pbc_probe_record, 1);
-	memcpy(record->mac, from, 6);
-	memcpy(record->uuid_e, req.uuid_e, sizeof(record->uuid_e));
-	record->timestamp = now;
-	l_queue_push_tail(ap->wsc_pbc_probes, record);
-
-	/*
-	 * If queue was non-empty and we've added one more record then we
-	 * now have seen more than one PBC enrollee during the PBC Monitor
-	 * Time and must exit "active PBC mode" due to "session overlap".
-	 * WSC v2.0.5 Section 11.3:
-	 * "Within the PBC Monitor Time, if the Registrar receives PBC
-	 * probe requests from more than one Enrollee [...] then the
-	 * Registrar SHALL signal a "session overlap" error.  As a result,
-	 * the Registrar shall refuse to enter active PBC mode and shall
-	 * also refuse to perform a PBC-based Registration Protocol
-	 * exchange [...]"
-	 */
-	if (empty)
-		return;
-
-	if (ap->wsc_pbc_timeout) {
-		l_debug("Exiting PBC mode due to Session Overlap");
-		ap_wsc_exit_pbc(ap);
-	}
-
-	/*
-	 * "If the Registrar is engaged in PBC Registration Protocol
-	 * exchange with an Enrollee and receives a Probe Request or M1
-	 * Message from another Enrollee, then the Registrar should
-	 * signal a "session overlap" error".
-	 *
-	 * For simplicity just interrupt the handshake with that enrollee.
-	 */
-	for (entry = l_queue_get_entries(ap->sta_states); entry;
-			entry = entry->next) {
-		struct sta_state *sta = entry->data;
-
-		if (!sta->associated || sta->assoc_rsne)
-			continue;
-
-		/*
-		 * Check whether this enrollee is in PBC Registration
-		 * Protocol by comparing its mac with the first (and only)
-		 * record we had in ap->wsc_pbc_probes.  If we had more
-		 * than one record we wouldn't have been in
-		 * "active PBC mode".
-		 */
-		if (memcmp(sta->addr, first_sta_addr, 6) ||
-				!memcmp(sta->addr, from, 6))
-			continue;
-
-		l_debug("Interrupting handshake with %s due to Session Overlap",
-			util_address_to_string(sta->addr));
-
-		if (sta->hs) {
-			netdev_handshake_failed(sta->hs,
-					MMPDU_REASON_CODE_DISASSOC_AP_BUSY);
-			sta->sm = NULL;
-		}
-
-		ap_remove_sta(sta);
-	}
-}
-
 static void ap_probe_resp_cb(int err, void *user_data)
 {
 	if (err == -ECOMM)
@@ -1847,8 +1878,6 @@ static void ap_probe_req_cb(const struct mmpdu_header *hdr, const void *body,
 		l_malloc(512 + ap_get_extra_ies_len(ap,
 				MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE, hdr,
 				body + body_len - (void *) hdr));
-	uint8_t *wsc_data;
-	ssize_t wsc_data_len;
 
 	l_info("AP Probe Request from %s",
 		util_address_to_string(hdr->address_2));
@@ -1921,18 +1950,6 @@ static void ap_probe_req_cb(const struct mmpdu_header *hdr, const void *body,
 	if (!match)
 		return;
 
-	/*
-	 * Process the WSC IE first as it may cause us to exit "active PBC
-	 * mode" and that can be immediately reflected in our Probe Response.
-	 */
-	wsc_data = ie_tlv_extract_wsc_payload(req->ies, body_len - sizeof(*req),
-						&wsc_data_len);
-	if (wsc_data) {
-		ap_process_wsc_probe_req(ap, hdr->address_2,
-						wsc_data, wsc_data_len);
-		l_free(wsc_data);
-	}
-
 	len = ap_build_beacon_pr_head(ap,
 					MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE,
 					hdr->address_2, resp, sizeof(resp));
-- 
2.27.0

[PATCH 5/6] p2p: Parse P2P IEs and WFD IEs in Association Requests

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 13454 bytes --]

Roughly validate the IEs and save some information for use in our own
IEs. p2p_extract_wfd_properties and p2p_device_validate_conn_wfd are
being moved unchanged to be usable in p2p_group_event without forward
declarations and to be next to p2p_build_wfd_ie.
---
 src/p2p.c | 385 ++++++++++++++++++++++++++++++++----------------------
 1 file changed, 228 insertions(+), 157 deletions(-)

diff --git a/src/p2p.c b/src/p2p.c
index a3d53abf..19bf7692 100644
--- a/src/p2p.c
+++ b/src/p2p.c
@@ -114,6 +114,7 @@ struct p2p_device {
 	uint32_t conn_go_oper_freq;
 	uint8_t conn_peer_interface_addr[6];
 	struct p2p_capability_attr conn_peer_capability;
+	struct p2p_device_info_attr conn_peer_dev_info;
 
 	struct p2p_group_id_attr go_group_id;
 	struct ap_state *group;
@@ -335,6 +336,163 @@ static size_t p2p_build_wfd_ie(const struct p2p_wfd_properties *wfd,
 	return size;
 }
 
+static bool p2p_extract_wfd_properties(const uint8_t *ie, size_t ie_size,
+					struct p2p_wfd_properties *out)
+{
+	struct wfd_subelem_iter iter;
+	const uint8_t *devinfo = NULL;
+	const uint8_t *ext_caps = NULL;
+	const uint8_t *r2 = NULL;
+
+	if (!ie)
+		return false;
+
+	wfd_subelem_iter_init(&iter, ie, ie_size);
+
+	while (wfd_subelem_iter_next(&iter)) {
+		enum wfd_subelem_type type = wfd_subelem_iter_get_type(&iter);
+		size_t len = wfd_subelem_iter_get_length(&iter);
+		const uint8_t *data = wfd_subelem_iter_get_data(&iter);
+
+		switch (type) {
+#define SUBELEM_CHECK(var, expected_len, name)		\
+			if (len != expected_len) {	\
+				l_debug(name " length wrong in WFD IE");\
+				return false;		\
+			}				\
+							\
+			if (var) {			\
+				l_debug("Duplicate" name " in WFD IE");\
+				return false;		\
+			}				\
+							\
+			var = data;
+		case WFD_SUBELEM_WFD_DEVICE_INFORMATION:
+			SUBELEM_CHECK(devinfo, 6, "Device Information");
+			break;
+		case WFD_SUBELEM_EXTENDED_CAPABILITY:
+			SUBELEM_CHECK(ext_caps, 2, "Extended Capability");
+			break;
+		case WFD_SUBELEM_R2_DEVICE_INFORMATION:
+			SUBELEM_CHECK(r2, 2, "R2 Device Information");
+			break;
+#undef SUBELEM_CHECK
+		default:
+			/* Skip unknown IEs */
+			break;
+		}
+	}
+
+	if (devinfo) {
+		uint16_t capability = l_get_be16(devinfo + 0);
+		bool source;
+		bool sink;
+		uint16_t port;
+
+		source = (capability & WFD_DEV_INFO_DEVICE_TYPE) ==
+			WFD_DEV_INFO_TYPE_SOURCE ||
+			(capability & WFD_DEV_INFO_DEVICE_TYPE) ==
+			WFD_DEV_INFO_TYPE_DUAL_ROLE;
+		sink = (capability & WFD_DEV_INFO_DEVICE_TYPE) ==
+			WFD_DEV_INFO_TYPE_PRIMARY_SINK ||
+			(capability & WFD_DEV_INFO_DEVICE_TYPE) ==
+			WFD_DEV_INFO_TYPE_DUAL_ROLE;
+
+		if (!source && !sink)
+			return false;
+
+		port = l_get_be16(devinfo + 2);
+
+		if (source && port == 0) {
+			l_debug("0 port number in WFD IE Device Information");
+			return false;
+		}
+
+		memset(out, 0, sizeof(*out));
+		out->available =
+			(capability & WFD_DEV_INFO_SESSION_AVAILABILITY) ==
+			WFD_DEV_INFO_SESSION_AVAILABLE;
+		out->source = source;
+		out->sink = sink;
+		out->port = port;
+		out->cp = capability & WFD_DEV_INFO_CONTENT_PROTECTION_SUPPORT;
+		out->audio = !sink ||
+			!(capability & WFD_DEV_INFO_NO_AUDIO_AT_PRIMARY_SINK);
+	} else {
+		l_error("Device Information missing in WFD IE");
+		return false;
+	}
+
+	if (ext_caps && (l_get_be16(ext_caps) & 1))
+		out->uibc = 1;
+
+	if (r2) {
+		uint8_t role = l_get_be16(r2) & 3;
+
+		if ((out->source && role != 0 && role != 3) ||
+				(out->sink && role != 1 && role != 3))
+			l_debug("Invalid role in WFD R2 Device Information");
+		else
+			out->r2 = true;
+	}
+
+	return true;
+}
+
+static bool p2p_device_validate_conn_wfd(struct p2p_device *dev,
+						const uint8_t *ie,
+						ssize_t ie_size)
+{
+	struct p2p_wfd_properties wfd;
+
+	if (!dev->conn_own_wfd)
+		return true;
+
+	/*
+	 * WFD IEs are optional in Association Request/Response and P2P Public
+	 * Action frames for R2 devices and required for R1 devices.
+	 * Wi-Fi Display Technical Specification v2.1.0 section 5.2:
+	 * "A WFD R2 Device shall include the WFD IE in Beacon, Probe
+	 * Request/Response, Association Request/Response and P2P Public Action
+	 * frames in order to be interoperable with R1 devices. If a WFD R2
+	 * Device discovers that the peer device is also a WFD R2 Device, then
+	 * it may include the WFD IE in Association Request/Response and P2P
+	 * Public Action frames."
+	 */
+	if (!ie)
+		return dev->conn_own_wfd->r2;
+
+	if (!p2p_extract_wfd_properties(ie, ie_size, &wfd)) {
+		l_error("Could not parse the WFD IE contents");
+		return false;
+	}
+
+	if ((dev->conn_own_wfd->source && !wfd.sink) ||
+			(dev->conn_own_wfd->sink && !wfd.source)) {
+		l_error("Wrong role in peer's WFD IE");
+		return false;
+	}
+
+	if (wfd.r2 != dev->conn_own_wfd->r2) {
+		l_error("Wrong version in peer's WFD IE");
+		return false;
+	}
+
+	/*
+	 * Ignore the session available state because it's not 100% clear
+	 * at what point the peer switches to SESSION_NOT_AVAILABLE in its
+	 * Device Information.
+	 * But we might want to check that other bits have not changed from
+	 * what the peer reported during discovery.
+	 * Wi-Fi Display Technical Specification v2.1.0 section 4.5.2.1:
+	 * "The content of the WFD Device Information subelement should be
+	 * immutable during the period of P2P connection establishment, with
+	 * [...] exceptions [...]"
+	 */
+
+	return true;
+}
+
 /* TODO: convert to iovecs */
 static uint8_t *p2p_build_scan_ies(struct p2p_device *dev, uint8_t *buf,
 					size_t buf_len, size_t *out_len)
@@ -691,9 +849,72 @@ static void p2p_group_event(enum ap_event_type type, const void *event_data,
 		break;
 
 	case AP_EVENT_STATION_ADDED:
+	{
+		const struct ap_event_station_added_data *data = event_data;
+		L_AUTO_FREE_VAR(uint8_t *, p2p_data) = NULL;
+		ssize_t p2p_data_len;
+		L_AUTO_FREE_VAR(uint8_t *, wfd_data) = NULL;
+		ssize_t wfd_data_len;
+		struct p2p_association_req req_info;
+		int r;
+
+		p2p_data = ie_tlv_extract_p2p_payload(data->assoc_ies,
+							data->assoc_ies_len,
+							&p2p_data_len);
+		if (!p2p_data) {
+			l_error("Missing or invalid P2P IEs: %s (%i)",
+				strerror(-p2p_data_len), (int) -p2p_data_len);
+			goto invalid_ie;
+		}
+
+		/*
+		 * We don't need to validate most of the Association Request
+		 * P2P IE contents as we already have all the information there
+		 * may be but we need to save some of the attributes because
+		 * section 3.2.3 requires that our Group Info includes
+		 * specifically the data from the association, not any of the
+		 * earlier exchanges:
+		 * "When a P2P Client associates with a P2P Group Owner, it
+		 * provides [...] the P2P Device Info attribute (see Section
+		 * 4.1.15) and the P2P Capability attribute (see Section 4.1.4)
+		 * in the P2P IE in the Association Request frame.  This
+		 * information shall be used by the P2P Group Owner for Group
+		 * Information Advertisement.
+		 */
+		r = p2p_parse_association_req(p2p_data, p2p_data_len,
+						&req_info);
+		if (r < 0) {
+			l_error("Can't parse P2P Association Request: %s (%i)",
+				strerror(-r), -r);
+			goto invalid_ie;
+		}
+
+		/*
+		 * Most of this duplicates the information we already have in
+		 * dev->conn_peer.
+		 */
+		dev->conn_peer_capability = req_info.capability;
+		dev->conn_peer_dev_info = req_info.device_info;
+		p2p_clear_association_req(&req_info);
+
+		if (dev->conn_own_wfd)
+			wfd_data = ie_tlv_extract_p2p_payload(data->assoc_ies,
+							data->assoc_ies_len,
+							&wfd_data_len);
+
+		if (!p2p_device_validate_conn_wfd(dev, wfd_data,
+							wfd_data_len))
+			goto invalid_ie;
+
+		/* Take the chance to update WFD attributes for Session Info */
+		if (wfd_data)
+			p2p_extract_wfd_properties(wfd_data, wfd_data_len,
+							dev->conn_peer->wfd);
+
 		dev->conn_peer_added = true;
 		p2p_peer_connect_done(dev);
 		break;
+	}
 
 	case AP_EVENT_STATION_REMOVED:
 		dev->conn_peer_added = false;
@@ -709,6 +930,13 @@ static void p2p_group_event(enum ap_event_type type, const void *event_data,
 	case AP_EVENT_PBC_MODE_EXIT:
 		break;
 	};
+
+	return;
+
+invalid_ie:
+	ap_station_disconnect(dev->group, dev->conn_peer_interface_addr,
+				MMPDU_REASON_CODE_INVALID_IE);
+	p2p_connect_failed(dev);
 }
 
 static const struct ap_ops p2p_go_ops = {
@@ -1627,163 +1855,6 @@ static void p2p_device_fill_channel_list(struct p2p_device *dev,
 	l_queue_push_tail(attr->channel_entries, channel_entry);
 }
 
-static bool p2p_extract_wfd_properties(const uint8_t *ie, size_t ie_size,
-					struct p2p_wfd_properties *out)
-{
-	struct wfd_subelem_iter iter;
-	const uint8_t *devinfo = NULL;
-	const uint8_t *ext_caps = NULL;
-	const uint8_t *r2 = NULL;
-
-	if (!ie)
-		return false;
-
-	wfd_subelem_iter_init(&iter, ie, ie_size);
-
-	while (wfd_subelem_iter_next(&iter)) {
-		enum wfd_subelem_type type = wfd_subelem_iter_get_type(&iter);
-		size_t len = wfd_subelem_iter_get_length(&iter);
-		const uint8_t *data = wfd_subelem_iter_get_data(&iter);
-
-		switch (type) {
-#define SUBELEM_CHECK(var, expected_len, name)		\
-			if (len != expected_len) {	\
-				l_debug(name " length wrong in WFD IE");\
-				return false;		\
-			}				\
-							\
-			if (var) {			\
-				l_debug("Duplicate" name " in WFD IE");\
-				return false;		\
-			}				\
-							\
-			var = data;
-		case WFD_SUBELEM_WFD_DEVICE_INFORMATION:
-			SUBELEM_CHECK(devinfo, 6, "Device Information");
-			break;
-		case WFD_SUBELEM_EXTENDED_CAPABILITY:
-			SUBELEM_CHECK(ext_caps, 2, "Extended Capability");
-			break;
-		case WFD_SUBELEM_R2_DEVICE_INFORMATION:
-			SUBELEM_CHECK(r2, 2, "R2 Device Information");
-			break;
-#undef SUBELEM_CHECK
-		default:
-			/* Skip unknown IEs */
-			break;
-		}
-	}
-
-	if (devinfo) {
-		uint16_t capability = l_get_be16(devinfo + 0);
-		bool source;
-		bool sink;
-		uint16_t port;
-
-		source = (capability & WFD_DEV_INFO_DEVICE_TYPE) ==
-			WFD_DEV_INFO_TYPE_SOURCE ||
-			(capability & WFD_DEV_INFO_DEVICE_TYPE) ==
-			WFD_DEV_INFO_TYPE_DUAL_ROLE;
-		sink = (capability & WFD_DEV_INFO_DEVICE_TYPE) ==
-			WFD_DEV_INFO_TYPE_PRIMARY_SINK ||
-			(capability & WFD_DEV_INFO_DEVICE_TYPE) ==
-			WFD_DEV_INFO_TYPE_DUAL_ROLE;
-
-		if (!source && !sink)
-			return false;
-
-		port = l_get_be16(devinfo + 2);
-
-		if (source && port == 0) {
-			l_debug("0 port number in WFD IE Device Information");
-			return false;
-		}
-
-		memset(out, 0, sizeof(*out));
-		out->available =
-			(capability & WFD_DEV_INFO_SESSION_AVAILABILITY) ==
-			WFD_DEV_INFO_SESSION_AVAILABLE;
-		out->source = source;
-		out->sink = sink;
-		out->port = port;
-		out->cp = capability & WFD_DEV_INFO_CONTENT_PROTECTION_SUPPORT;
-		out->audio = !sink ||
-			!(capability & WFD_DEV_INFO_NO_AUDIO_AT_PRIMARY_SINK);
-	} else {
-		l_error("Device Information missing in WFD IE");
-		return false;
-	}
-
-	if (ext_caps && (l_get_be16(ext_caps) & 1))
-		out->uibc = 1;
-
-	if (r2) {
-		uint8_t role = l_get_be16(r2) & 3;
-
-		if ((out->source && role != 0 && role != 3) ||
-				(out->sink && role != 1 && role != 3))
-			l_debug("Invalid role in WFD R2 Device Information");
-		else
-			out->r2 = true;
-	}
-
-	return true;
-}
-
-static bool p2p_device_validate_conn_wfd(struct p2p_device *dev,
-						const uint8_t *ie,
-						ssize_t ie_size)
-{
-	struct p2p_wfd_properties wfd;
-
-	if (!dev->conn_own_wfd)
-		return true;
-
-	/*
-	 * WFD IEs are optional in Association Request/Response and P2P Public
-	 * Action frames for R2 devices and required for R1 devices.
-	 * Wi-Fi Display Technical Specification v2.1.0 section 5.2:
-	 * "A WFD R2 Device shall include the WFD IE in Beacon, Probe
-	 * Request/Response, Association Request/Response and P2P Public Action
-	 * frames in order to be interoperable with R1 devices. If a WFD R2
-	 * Device discovers that the peer device is also a WFD R2 Device, then
-	 * it may include the WFD IE in Association Request/Response and P2P
-	 * Public Action frames."
-	 */
-	if (!ie)
-		return dev->conn_own_wfd->r2;
-
-	if (!p2p_extract_wfd_properties(ie, ie_size, &wfd)) {
-		l_error("Could not parse the WFD IE contents");
-		return false;
-	}
-
-	if ((dev->conn_own_wfd->source && !wfd.sink) ||
-			(dev->conn_own_wfd->sink && !wfd.source)) {
-		l_error("Wrong role in peer's WFD IE");
-		return false;
-	}
-
-	if (wfd.r2 != dev->conn_own_wfd->r2) {
-		l_error("Wrong version in peer's WFD IE");
-		return false;
-	}
-
-	/*
-	 * Ignore the session available state because it's not 100% clear
-	 * at what point the peer switches to SESSION_NOT_AVAILABLE in its
-	 * Device Information.
-	 * But we might want to check that other bits have not changed from
-	 * what the peer reported during discovery.
-	 * Wi-Fi Display Technical Specification v2.1.0 section 4.5.2.1:
-	 * "The content of the WFD Device Information subelement should be
-	 * immutable during the period of P2P connection establishment, with
-	 * [...] exceptions [...]"
-	 */
-
-	return true;
-}
-
 static bool p2p_go_negotiation_confirm_cb(const struct mmpdu_header *mpdu,
 					const void *body, size_t body_len,
 					int rssi, struct p2p_device *dev)
-- 
2.27.0

[PATCH 6/6] p2p: Build P2P and WFD IEs for group's management frames

[Andrew Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 12913 bytes --]

Register P2P group's vendor IE writers using the new API to build and
attach the necessary P2P IE and WFD IEs to the (Re)Association Response,
Probe Response and Beacon frames sent by the GO.
---
 src/p2p.c | 266 +++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 255 insertions(+), 11 deletions(-)

diff --git a/src/p2p.c b/src/p2p.c
index 19bf7692..542ed1d1 100644
--- a/src/p2p.c
+++ b/src/p2p.c
@@ -162,6 +162,10 @@ struct p2p_wfd_properties {
 	bool uibc;
 	bool cp;
 	bool r2;
+	uint16_t raw_dev_info;
+	uint8_t associated_bssid[6];
+	uint8_t raw_coupled_sink_status;
+	uint8_t coupled_sink_mac[6];
 };
 
 static struct l_queue *p2p_device_list;
@@ -277,9 +281,9 @@ static void p2p_peer_put(void *user_data)
 static void p2p_device_discovery_start(struct p2p_device *dev);
 static void p2p_device_discovery_stop(struct p2p_device *dev);
 
-/* Callers should reserve 32 bytes */
+/* Callers should reserve 32 bytes, 64 with non-NULL @wfd_clients */
 static size_t p2p_build_wfd_ie(const struct p2p_wfd_properties *wfd,
-				uint8_t *buf)
+				const struct p2p_peer *wfd_client, uint8_t *buf)
 {
 	/*
 	 * Wi-Fi Display Technical Specification v2.1.0
@@ -324,6 +328,30 @@ static size_t p2p_build_wfd_ie(const struct p2p_wfd_properties *wfd,
 		buf[size++] = 0x01;	/* UIBC Support */
 	}
 
+	/*
+	 * Wi-Fi Display Technical Specification v2.1.0 section 5.2.3:
+	 * "If a WFD Capable GO has at least one associated client that is
+	 * WFD capable, the WFD capable GO shall include the WFD Session
+	 * Information subelement in the WFD IE in the Probe Response
+	 * frames it transmits."
+	 */
+	if (wfd_client && !L_WARN_ON(!wfd_client->wfd)) {
+		buf[size++] = WFD_SUBELEM_SESION_INFORMATION;
+		buf[size++] = 0;		/* WFD Subelement length */
+		buf[size++] = 23;
+		memcpy(buf + size, wfd_client->device_addr, 6);
+		size += 6;
+		memcpy(buf + size, wfd_client->wfd->associated_bssid, 6);
+		size += 6;
+		buf[size++] = wfd_client->wfd->raw_dev_info >> 8;
+		buf[size++] = wfd_client->wfd->raw_dev_info & 255;
+		buf[size++] = wfd_client->wfd->throughput >> 8;
+		buf[size++] = wfd_client->wfd->throughput & 255;
+		buf[size++] = wfd_client->wfd->raw_coupled_sink_status;
+		memcpy(buf + size, wfd_client->wfd->coupled_sink_mac, 6);
+		size += 6;
+	}
+
 	if (wfd->r2) {
 		buf[size++] = WFD_SUBELEM_R2_DEVICE_INFORMATION;
 		buf[size++] = 0;	/* WFD Subelement length */
@@ -341,6 +369,8 @@ static bool p2p_extract_wfd_properties(const uint8_t *ie, size_t ie_size,
 {
 	struct wfd_subelem_iter iter;
 	const uint8_t *devinfo = NULL;
+	const uint8_t *associated_bssid = NULL;
+	const uint8_t *coupled_sink_info = NULL;
 	const uint8_t *ext_caps = NULL;
 	const uint8_t *r2 = NULL;
 
@@ -370,6 +400,13 @@ static bool p2p_extract_wfd_properties(const uint8_t *ie, size_t ie_size,
 		case WFD_SUBELEM_WFD_DEVICE_INFORMATION:
 			SUBELEM_CHECK(devinfo, 6, "Device Information");
 			break;
+		case WFD_SUBELEM_ASSOCIATED_BSSID:
+			SUBELEM_CHECK(associated_bssid, 6, "Associated BSSID");
+			break;
+		case WFD_SUBELEM_COUPLED_SINK_INFORMATION:
+			SUBELEM_CHECK(coupled_sink_info, 7,
+					"Coupled Sink Information");
+			break;
 		case WFD_SUBELEM_EXTENDED_CAPABILITY:
 			SUBELEM_CHECK(ext_caps, 2, "Extended Capability");
 			break;
@@ -418,11 +455,20 @@ static bool p2p_extract_wfd_properties(const uint8_t *ie, size_t ie_size,
 		out->cp = capability & WFD_DEV_INFO_CONTENT_PROTECTION_SUPPORT;
 		out->audio = !sink ||
 			!(capability & WFD_DEV_INFO_NO_AUDIO_AT_PRIMARY_SINK);
+		out->raw_dev_info = l_get_be16(devinfo);
 	} else {
 		l_error("Device Information missing in WFD IE");
 		return false;
 	}
 
+	if (associated_bssid)
+		memcpy(out->associated_bssid, associated_bssid, 6);
+
+	if (coupled_sink_info) {
+		out->raw_coupled_sink_status = coupled_sink_info[0];
+		memcpy(out->coupled_sink_mac, coupled_sink_info + 1, 6);
+	}
+
 	if (ext_caps && (l_get_be16(ext_caps) & 1))
 		out->uibc = 1;
 
@@ -559,7 +605,7 @@ static uint8_t *p2p_build_scan_ies(struct p2p_device *dev, uint8_t *buf,
 		return NULL;
 
 	if (p2p_own_wfd)
-		wfd_ie_size = p2p_build_wfd_ie(p2p_own_wfd, wfd_ie);
+		wfd_ie_size = p2p_build_wfd_ie(p2p_own_wfd, NULL, wfd_ie);
 	else
 		wfd_ie_size = 0;
 
@@ -925,7 +971,9 @@ static void p2p_group_event(enum ap_event_type type, const void *event_data,
 		/* Don't validate the P2P IE or WFD IE at this stage */
 		break;
 	case AP_EVENT_REGISTRATION_SUCCESS:
+		/* Update the Group Formation bit in our beacons */
 		dev->capability.group_caps &= ~P2P_GROUP_CAP_GROUP_FORMATION;
+		ap_update_beacon(dev->group);
 		break;
 	case AP_EVENT_PBC_MODE_EXIT:
 		break;
@@ -939,8 +987,201 @@ invalid_ie:
 	p2p_connect_failed(dev);
 }
 
+static size_t p2p_group_get_p2p_ie_len(struct p2p_device *dev,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len)
+{
+	switch (type) {
+	case MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_BEACON:
+		return 256;
+	default:
+		return 0;
+	}
+}
+
+static size_t p2p_group_write_p2p_ie(struct p2p_device *dev,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					uint8_t *out_buf)
+{
+	L_AUTO_FREE_VAR(uint8_t *, p2p_ie) = NULL;
+	size_t p2p_ie_len;
+
+	switch (type) {
+	case MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE:
+	{
+		/*
+		 * Wi-Fi P2P Technical Specification v1.7 Section 4.2.5:
+		 * "If neither P2P attribute is required according to the
+		 * conditions in Table 55, then a P2P IE containing no P2P
+		 * attributes is included."
+		 * This is going to be our case.
+		 */
+		struct p2p_association_resp info = {};
+
+		p2p_ie = p2p_build_association_resp(&info, &p2p_ie_len);
+		break;
+	}
+
+	case MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE:
+	{
+		struct p2p_probe_resp info = {};
+		const struct mmpdu_probe_request *req =
+			mmpdu_body(client_frame);
+		size_t req_ies_len = (void *) client_frame + client_frame_len -
+			(void *) req->ies;
+		ssize_t req_p2p_data_size;
+
+		/*
+		 * Wi-Fi P2P Technical Specification v1.7 Section 3.2.2:
+		 * "A P2P Group Owner shall not include a P2P IE in the Probe
+		 * Response frame if the received Probe Request frame does
+		 * not contain a P2P IE."
+		 */
+		if (!ie_tlv_extract_p2p_payload(req->ies, req_ies_len,
+							&req_p2p_data_size))
+			return 0;
+
+		info.capability = dev->capability;
+		info.device_info = dev->device_info;
+
+		if (dev->conn_peer_added) {
+			struct p2p_client_info_descriptor client = {};
+
+			memcpy(client.device_addr,
+				dev->conn_peer_dev_info.device_addr, 6);
+			memcpy(client.interface_addr,
+				dev->conn_peer_interface_addr, 6);
+			client.device_caps =
+				dev->conn_peer_capability.device_caps;
+			client.wsc_config_methods =
+				dev->conn_peer_dev_info.wsc_config_methods;
+			client.primary_device_type =
+				dev->conn_peer_dev_info.primary_device_type;
+			l_strlcpy(client.device_name,
+					dev->conn_peer_dev_info.device_name,
+					sizeof(client.device_name));
+
+			info.group_clients = l_queue_new();
+			l_queue_push_tail(info.group_clients,
+					l_memdup(&client, sizeof(client)));
+		}
+
+		p2p_ie = p2p_build_probe_resp(&info, &p2p_ie_len);
+		p2p_clear_probe_resp(&info);
+		break;
+	}
+
+	case MPDU_MANAGEMENT_SUBTYPE_BEACON:
+	{
+		struct p2p_beacon info = {};
+
+		info.capability = dev->capability;
+		memcpy(info.device_addr, dev->addr, 6);
+		p2p_ie = p2p_build_beacon(&info, &p2p_ie_len);
+		break;
+	}
+
+	default:
+		return 0;
+	}
+
+	memcpy(out_buf, p2p_ie, p2p_ie_len);
+	return p2p_ie_len;
+}
+
+static size_t p2p_group_get_wfd_ie_len(struct p2p_device *dev,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len)
+{
+	switch (type) {
+	case MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE:
+		return (dev->conn_own_wfd && !dev->conn_own_wfd->r2) ? 32 : 0;
+	case MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_BEACON:
+		return p2p_own_wfd ? 64 : 0;
+	default:
+		return 0;
+	}
+}
+
+static size_t p2p_group_write_wfd_ie(struct p2p_device *dev,
+					enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					uint8_t *out_buf)
+{
+	switch (type) {
+	case MPDU_MANAGEMENT_SUBTYPE_ASSOCIATION_RESPONSE:
+	case MPDU_MANAGEMENT_SUBTYPE_REASSOCIATION_RESPONSE:
+		if (dev->conn_own_wfd && !dev->conn_own_wfd->r2)
+			return p2p_build_wfd_ie(dev->conn_own_wfd, NULL,
+						out_buf);
+
+		break;
+	case MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE:
+		if (p2p_own_wfd)
+			return p2p_build_wfd_ie(p2p_own_wfd,
+						dev->conn_own_wfd ?
+						dev->conn_peer : NULL, out_buf);
+
+		break;
+	case MPDU_MANAGEMENT_SUBTYPE_BEACON:
+		if (p2p_own_wfd)
+			return p2p_build_wfd_ie(p2p_own_wfd, NULL, out_buf);
+
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+
+static size_t p2p_group_get_ies_len(enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					void *user_data)
+{
+	struct p2p_device *dev = user_data;
+
+	return p2p_group_get_p2p_ie_len(dev, type,
+					client_frame, client_frame_len) +
+		p2p_group_get_wfd_ie_len(dev, type,
+						client_frame, client_frame_len);
+}
+
+static size_t p2p_group_write_ies(enum mpdu_management_subtype type,
+					const struct mmpdu_header *client_frame,
+					size_t client_frame_len,
+					uint8_t *out_buf, void *user_data)
+{
+	struct p2p_device *dev = user_data;
+	size_t len;
+
+	len = p2p_group_write_p2p_ie(dev, type,
+					client_frame, client_frame_len,
+					out_buf);
+	len += p2p_group_write_wfd_ie(dev, type,
+					client_frame, client_frame_len,
+					out_buf + len);
+
+	return len;
+}
+
 static const struct ap_ops p2p_go_ops = {
 	.handle_event = p2p_group_event,
+	.get_extra_ies_len = p2p_group_get_ies_len,
+	.write_extra_ies = p2p_group_write_ies,
 };
 
 static void p2p_group_start(struct p2p_device *dev)
@@ -1227,7 +1468,7 @@ static void p2p_try_connect_group(struct p2p_device *dev)
 	if (dev->conn_own_wfd) {
 		ie_iov[ie_num].iov_base = wfd_ie;
 		ie_iov[ie_num].iov_len = p2p_build_wfd_ie(dev->conn_own_wfd,
-								wfd_ie);
+								NULL, wfd_ie);
 		ie_num++;
 	}
 
@@ -1376,7 +1617,7 @@ static void p2p_provision_connect(struct p2p_device *dev)
 	if (dev->conn_own_wfd) {
 		iov[iov_num].iov_base = wfd_ie;
 		iov[iov_num].iov_len = p2p_build_wfd_ie(dev->conn_own_wfd,
-							wfd_ie);
+							NULL, wfd_ie);
 		iov_num++;
 	}
 
@@ -2211,7 +2452,7 @@ respond:
 	if (dev->conn_own_wfd) {
 		resp_info.wfd = wfd_ie;
 		resp_info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd,
-							wfd_ie);
+							NULL, wfd_ie);
 	}
 
 	resp_body = p2p_build_go_negotiation_resp(&resp_info, &resp_len);
@@ -2488,7 +2729,7 @@ static bool p2p_go_negotiation_resp_cb(const struct mmpdu_header *mpdu,
 	if (dev->conn_own_wfd) {
 		confirm_info.wfd = wfd_ie;
 		confirm_info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd,
-								wfd_ie);
+								NULL, wfd_ie);
 	}
 
 	confirm_body = p2p_build_go_negotiation_confirmation(&confirm_info,
@@ -2576,7 +2817,8 @@ static void p2p_start_go_negotiation(struct p2p_device *dev)
 
 	if (dev->conn_own_wfd) {
 		info.wfd = wfd_ie;
-		info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd, wfd_ie);
+		info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd,
+							NULL, wfd_ie);
 	}
 
 	req_body = p2p_build_go_negotiation_req(&info, &req_len);
@@ -2714,7 +2956,8 @@ static void p2p_start_provision_discovery(struct p2p_device *dev)
 
 	if (dev->conn_own_wfd) {
 		info.wfd = wfd_ie;
-		info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd, wfd_ie);
+		info.wfd_size = p2p_build_wfd_ie(dev->conn_own_wfd,
+							NULL, wfd_ie);
 	}
 
 	req_body = p2p_build_provision_disc_req(&info, &req_len);
@@ -3696,11 +3939,12 @@ static void p2p_device_send_probe_resp(struct p2p_device *dev,
 	if (to_conn_peer && dev->conn_own_wfd) {
 		iov[iov_len].iov_base = wfd_ie;
 		iov[iov_len].iov_len = p2p_build_wfd_ie(dev->conn_own_wfd,
-							wfd_ie);
+							NULL, wfd_ie);
 		iov_len++;
 	} else if (p2p_own_wfd) {
 		iov[iov_len].iov_base = wfd_ie;
-		iov[iov_len].iov_len = p2p_build_wfd_ie(p2p_own_wfd, wfd_ie);
+		iov[iov_len].iov_len = p2p_build_wfd_ie(p2p_own_wfd,
+							NULL, wfd_ie);
 		iov_len++;
 	}
 
-- 
2.27.0

Re: [PATCH 1/6] ap: Make ap_update_beacon public

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 330 bytes --]

Hi Andrew,

On 3/10/21 4:06 PM, Andrew Zaborowski wrote:
> Let users call ap_update_beacon when a value has changed which should be
> reflected in the beacon IEs.
> ---
>   src/ap.c | 5 ++++-
>   src/ap.h | 1 +
>   2 files changed, 5 insertions(+), 1 deletion(-)
> 

Patches 1 & 2 applied, thanks.

Regards,
-Denis