[11/13] p2p: Add p2p2 support for group formation on sucessful negotiation

diff --git a/src/ap/ieee802_11.c b/src/ap/ieee802_11.c
index 090bb2655..928e657da 100644
--- a/src/ap/ieee802_11.c
+++ b/src/ap/ieee802_11.c
@@ -874,8 +874,8 @@  static void sae_set_retransmit_timer(struct hostapd_data *hapd,
 }
 
 
-static void sae_sme_send_external_auth_status(struct hostapd_data *hapd,
-					      struct sta_info *sta, u16 status)
+void sae_sme_send_external_auth_status(struct hostapd_data *hapd,
+				       struct sta_info *sta, u16 status)
 {
 	struct external_auth params;
 
diff --git a/src/ap/ieee802_11.h b/src/ap/ieee802_11.h
index dd4995f3f..167667bd9 100644
--- a/src/ap/ieee802_11.h
+++ b/src/ap/ieee802_11.h
@@ -155,6 +155,8 @@  int auth_sae_init_committed(struct hostapd_data *hapd, struct sta_info *sta);
 void sae_clear_retransmit_timer(struct hostapd_data *hapd,
 				struct sta_info *sta);
 void sae_accept_sta(struct hostapd_data *hapd, struct sta_info *sta);
+void sae_sme_send_external_auth_status(struct hostapd_data *hapd,
+				       struct sta_info *sta, u16 status);
 #else /* CONFIG_SAE */
 static inline void sae_clear_retransmit_timer(struct hostapd_data *hapd,
 					      struct sta_info *sta)
diff --git a/src/ap/wpa_auth_ie.c b/src/ap/wpa_auth_ie.c
index 2efadf896..f1ad25d27 100644
--- a/src/ap/wpa_auth_ie.c
+++ b/src/ap/wpa_auth_ie.c
@@ -423,6 +423,8 @@  int wpa_write_rsnxe(struct wpa_auth_config *conf, u8 *buf, size_t len)
 #endif /* CONFIG_SAE_PK */
 	}
 
+	capab |= BIT(WLAN_RSNX_CAPAB_SAE_H2E);
+
 	if (conf->secure_ltf)
 		capab |= BIT(WLAN_RSNX_CAPAB_SECURE_LTF);
 	if (conf->secure_rtt)
@@ -998,6 +1000,18 @@  wpa_validate_wpa_ie(struct wpa_authenticator *wpa_auth,
 			pmkid = sm->pmksa->pmkid;
 			break;
 		}
+
+		if (!is_zero_ether_addr(sm->p2p_dev_addr)) {
+			wpa_hexdump(MSG_DEBUG, "RSN IE: P2P DEV PMKID",
+					&data.pmkid[i * PMKID_LEN], PMKID_LEN);
+			sm->pmksa = pmksa_cache_auth_get(wpa_auth->pmksa, sm->p2p_dev_addr,
+					&data.pmkid[i * PMKID_LEN]);
+			if (sm->pmksa) {
+				pmkid = sm->pmksa->pmkid;
+				break;
+			}
+		}
+
 	}
 	for (i = 0; sm->pmksa == NULL && wpa_auth->conf.okc &&
 		     i < data.num_pmkid; i++) {
diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index 4759f649d..45fda8701 100644
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -251,6 +251,7 @@  void p2p_go_neg_failed(struct p2p_data *p2p, int status)
 
 	os_memset(&res, 0, sizeof(res));
 	res.status = status;
+	res.p2p2 = peer->p2p2;
 	os_memcpy(res.peer_device_addr, peer->info.p2p_device_addr, ETH_ALEN);
 	os_memcpy(res.peer_interface_addr, peer->intended_addr, ETH_ALEN);
 	p2p->cfg->go_neg_completed(p2p->cfg->cb_ctx, &res);
@@ -1068,7 +1069,8 @@  static void p2p_search(struct p2p_data *p2p)
 
 	res = p2p->cfg->p2p_scan(p2p->cfg->cb_ctx, type, freq,
 				 p2p->num_req_dev_types, p2p->req_dev_types,
-				 p2p->find_dev_id, pw_id, p2p->include_6ghz);
+				 p2p->find_dev_id, pw_id, p2p->include_6ghz,
+				 false);
 	if (res < 0) {
 		p2p_dbg(p2p, "Scan request schedule failed");
 		p2p_continue_find(p2p);
@@ -1295,7 +1297,7 @@  int p2p_find(struct p2p_data *p2p, unsigned int timeout,
 						 p2p->num_req_dev_types,
 						 p2p->req_dev_types, dev_id,
 						 DEV_PW_DEFAULT,
-						 p2p->include_6ghz);
+						 p2p->include_6ghz, false);
 			break;
 		}
 		/* fall through */
@@ -1303,13 +1305,15 @@  int p2p_find(struct p2p_data *p2p, unsigned int timeout,
 		res = p2p->cfg->p2p_scan(p2p->cfg->cb_ctx, P2P_SCAN_FULL, 0,
 					 p2p->num_req_dev_types,
 					 p2p->req_dev_types, dev_id,
-					 DEV_PW_DEFAULT, p2p->include_6ghz);
+					 DEV_PW_DEFAULT, p2p->include_6ghz,
+					 false);
 		break;
 	case P2P_FIND_ONLY_SOCIAL:
 		res = p2p->cfg->p2p_scan(p2p->cfg->cb_ctx, P2P_SCAN_SOCIAL, 0,
 					 p2p->num_req_dev_types,
 					 p2p->req_dev_types, dev_id,
-					 DEV_PW_DEFAULT, p2p->include_6ghz);
+					 DEV_PW_DEFAULT, p2p->include_6ghz,
+					 false);
 		break;
 	default:
 		return -1;
@@ -1924,8 +1928,36 @@  void p2p_go_complete(struct p2p_data *p2p, struct p2p_device *peer)
 	wpabuf_free(peer->go_neg_conf);
 	peer->go_neg_conf = NULL;
 
-	if (peer->p2p2 && peer->pasn)
+	if (peer->p2p2 && peer->pasn) {
+		res.p2p2 = peer->p2p2;
+		res.akmp = peer->pasn->akmp;
+		res.cipher = peer->pasn->cipher;
+
+		if (res.akmp == WPA_KEY_MGMT_PASN) {
+			if (peer->info.password_len) {
+				p2p_dbg(p2p, "Password Invalid for P2P2 group formation");
+				return;
+			}
+			res.password_len = peer->dev_password_len;
+			memcpy(res.password, peer->dev_password, res.password_len);
+		} else if (res.akmp == WPA_KEY_MGMT_SAE) {
+			res.password_len = peer->password_len;
+			memcpy(res.password, peer->password, res.password_len);
+			if (peer->role == P2P_ROLE_PAIRING_INITIATOR) {
+				pasn_initiator_pmksa_cache_get(peer->pasn->pmksa,
+						peer->pasn->peer_addr,
+						res.pmkid, res.pmk,
+						&res.pmk_len);
+			} else {
+				pasn_responder_pmksa_cache_get(peer->pasn->pmksa,
+						peer->pasn->peer_addr,
+						res.pmkid, res.pmk,
+						&res.pmk_len);
+			}
+		}
+
 		wpa_pasn_reset(peer->pasn);
+	}
 
 	p2p_set_state(p2p, P2P_PROVISIONING);
 	p2p->cfg->go_neg_completed(p2p->cfg->cb_ctx, &res);
diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h
index 13e61294c..5ac280aee 100644
--- a/src/p2p/p2p.h
+++ b/src/p2p/p2p.h
@@ -11,6 +11,7 @@ 
 
 #include "common/ieee802_11_defs.h"
 #include "wps/wps.h"
+#include "common/wpa_common.h"
 
 struct weighted_pcl;
 
@@ -171,6 +172,22 @@  struct p2p_go_neg_results {
 	 * peer_config_timeout - Peer configuration timeout (in 10 msec units)
 	 */
 	unsigned int peer_config_timeout;
+
+	bool p2p2;
+
+	int akmp;
+
+	int cipher;
+
+	u8 pmkid[PMKID_LEN];
+
+	u8 pmk[PMK_LEN_MAX];
+
+	size_t pmk_len;
+
+	char password[100];
+
+	size_t password_len;
 };
 
 struct p2ps_provision {
@@ -745,7 +762,7 @@  struct p2p_config {
 	int (*p2p_scan)(void *ctx, enum p2p_scan_type type, int freq,
 			unsigned int num_req_dev_types,
 			const u8 *req_dev_types, const u8 *dev_id, u16 pw_id,
-			bool include_6ghz);
+			bool include_6ghz, bool p2p2);
 
 	/**
 	 * send_probe_resp - Transmit a Probe Response frame
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index 5e04add1c..61da9fb1e 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -37,6 +37,8 @@ 
 #include "wps_supplicant.h"
 #include "p2p_supplicant.h"
 #include "wifi_display.h"
+#include "ap/ieee802_11.h"
+#include "ap/wpa_auth.h"
 
 /*
  * How many times to try to scan to find the GO before giving up on join
@@ -420,11 +422,11 @@  static int wpas_p2p_search_social_channel(struct wpa_supplicant *wpa_s,
 static int wpas_p2p_scan(void *ctx, enum p2p_scan_type type, int freq,
 			 unsigned int num_req_dev_types,
 			 const u8 *req_dev_types, const u8 *dev_id, u16 pw_id,
-			 bool include_6ghz)
+			 bool include_6ghz, bool p2p2)
 {
 	struct wpa_supplicant *wpa_s = ctx;
 	struct wpa_driver_scan_params *params = NULL;
-	struct wpabuf *wps_ie, *ies;
+	struct wpabuf *wps_ie = NULL, *ies;
 	unsigned int num_channels = 0;
 	int social_channels_freq[] = { 2412, 2437, 2462, 60480 };
 	size_t ielen;
@@ -453,11 +455,16 @@  static int wpas_p2p_scan(void *ctx, enum p2p_scan_type type, int freq,
 	params->ssids[0].ssid_len = P2P_WILDCARD_SSID_LEN;
 
 	wpa_s->wps->dev.p2p = 1;
-	wps_ie = wps_build_probe_req_ie(pw_id, &wpa_s->wps->dev,
-					wpa_s->wps->uuid, WPS_REQ_ENROLLEE,
-					num_req_dev_types, req_dev_types);
-	if (wps_ie == NULL)
-		goto fail;
+
+	if (!p2p2) {
+		wps_ie = wps_build_probe_req_ie(pw_id, &wpa_s->wps->dev,
+						wpa_s->wps->uuid,
+						WPS_REQ_ENROLLEE,
+						num_req_dev_types,
+						req_dev_types);
+		if (wps_ie == NULL)
+			goto fail;
+	}
 
 	/*
 	 * In case 6 GHz channels are requested as part of the P2P scan, only
@@ -508,13 +515,20 @@  static int wpas_p2p_scan(void *ctx, enum p2p_scan_type type, int freq,
 	}
 
 	ielen = p2p_scan_ie_buf_len(wpa_s->global->p2p);
-	ies = wpabuf_alloc(wpabuf_len(wps_ie) + ielen);
-	if (ies == NULL) {
+
+	if (wps_ie)
+		ielen += wpabuf_len(wps_ie);
+
+	ies = wpabuf_alloc(ielen);
+	if (!ies) {
 		wpabuf_free(wps_ie);
 		goto fail;
 	}
-	wpabuf_put_buf(ies, wps_ie);
-	wpabuf_free(wps_ie);
+
+	if (wps_ie) {
+		wpabuf_put_buf(ies, wps_ie);
+		wpabuf_free(wps_ie);
+	}
 
 	bands = wpas_get_bands(wpa_s, params->freqs);
 	p2p_scan_ie(wpa_s->global->p2p, ies, dev_id, bands);
@@ -1750,6 +1764,77 @@  static int wpas_copy_go_neg_results(struct wpa_supplicant *wpa_s,
 	return 0;
 }
 
+static void wpas_start_gc(struct wpa_supplicant *wpa_s,
+			  struct p2p_go_neg_results *res)
+{
+	struct wpa_ssid *ssid;
+
+	if (!res->ssid_len) {
+		wpa_dbg(wpa_s, MSG_DEBUG, "P2P: SSID info not present");
+		return;
+	}
+
+	wpa_s->group_formation_reported = 0;
+	wpa_printf(MSG_DEBUG, "P2P: Start connect for peer " MACSTR
+		   " dev_addr " MACSTR,
+		   MAC2STR(res->peer_interface_addr),
+		   MAC2STR(res->peer_device_addr));
+	wpa_hexdump_ascii(MSG_DEBUG, "P2P: Start connect for SSID",
+			  res->ssid, res->ssid_len);
+	wpa_supplicant_ap_deinit(wpa_s);
+	wpas_copy_go_neg_results(wpa_s, res);
+
+	ssid = wpa_config_add_network(wpa_s->conf);
+	if (ssid == NULL) {
+		wpa_dbg(wpa_s, MSG_DEBUG, "P2P: Could not add network for Client");
+		return;
+        }
+	os_memset(wpa_s->go_dev_addr, 0, ETH_ALEN);
+	wpa_config_set_network_defaults(ssid);
+	ssid->temporary = 1;
+	ssid->p2p_group = 1;
+
+	ssid->ssid = os_zalloc(res->ssid_len);
+	if (!ssid->ssid)
+		return;
+
+	ssid->ssid_len = res->ssid_len;
+	os_memcpy(ssid->ssid, res->ssid, ssid->ssid_len);
+
+	memcpy(ssid->bssid, res->peer_interface_addr, ETH_ALEN);
+
+	if (res->akmp == WPA_KEY_MGMT_PASN) {
+		ssid->auth_alg = WPA_AUTH_ALG_SAE;
+		ssid->sae_password = os_strdup(res->password);
+	} else if (res->akmp == WPA_KEY_MGMT_SAE) {
+		ssid->auth_alg = WPA_AUTH_ALG_OPEN;
+		ssid->sae_password = os_strdup(res->password);
+		wpa_sm_set_pmk(wpa_s->wpa, res->pmk, res->pmk_len,
+			       res->pmkid, res->peer_interface_addr);
+	}
+
+	if (res->psk_set) {
+		os_memcpy(ssid->psk, res->psk, 32);
+		ssid->psk_set = 1;
+	}
+	ssid->proto = WPA_PROTO_RSN;
+	ssid->key_mgmt = WPA_KEY_MGMT_SAE;
+	ssid->pairwise_cipher = res->cipher;
+	ssid->group_cipher = res->cipher;
+	wpa_s->conf->sae_pwe = SAE_PWE_HASH_TO_ELEMENT;
+	ssid->ieee80211w = MGMT_FRAME_PROTECTION_REQUIRED;
+	ssid->disabled = 0;
+	wpa_s->show_group_started = 1;
+	wpa_s->p2p_in_invitation = 1;
+	wpa_s->p2p_go_group_formation_completed = 0;
+	wpa_s->global->p2p_group_formation = wpa_s;
+
+	wpa_s->current_ssid = ssid;
+	wpa_supplicant_update_scan_results(wpa_s, res->peer_interface_addr);
+	wpa_supplicant_select_network(wpa_s, ssid);
+	return;
+}
+
 
 static void wpas_start_wps_enrollee(struct wpa_supplicant *wpa_s,
 				    struct p2p_go_neg_results *res)
@@ -1885,6 +1970,9 @@  static void p2p_go_configured(void *ctx, void *data)
 	struct wpa_supplicant *wpa_s = ctx;
 	struct p2p_go_neg_results *params = data;
 	struct wpa_ssid *ssid;
+	struct sta_info sta;
+	struct sae_data *sae;
+	struct hostapd_data *hapd;
 
 	wpa_s->ap_configured_cb = NULL;
 	wpa_s->ap_configured_cb_ctx = NULL;
@@ -1894,6 +1982,24 @@  static void p2p_go_configured(void *ctx, void *data)
 			   "P2P: p2p_go_configured() called with wpa_s->go_params == NULL");
 		return;
 	}
+	if (wpa_s->ap_iface && params->p2p2 &&
+	    params->akmp == WPA_KEY_MGMT_SAE) {
+		hapd = wpa_s->ap_iface->bss[0];
+		memset(&sta, 0, sizeof(struct sta_info));
+		memcpy(sta.addr, params->peer_device_addr, ETH_ALEN);
+		sae = os_zalloc(sizeof(struct sae_data));
+		if (sae) {
+			sta.sae = sae;
+			memcpy(sta.sae->pmkid, params->pmkid, PMKID_LEN);
+			wpa_auth_pmksa_add_sae(hapd->wpa_auth,
+					       params->peer_device_addr,
+					       params->pmk, params->pmk_len,
+					       params->pmkid, WPA_KEY_MGMT_SAE);
+			sae_sme_send_external_auth_status(hapd, &sta, 0);
+			memset(&sta, 0, sizeof(struct sta_info));
+			os_free(sae);
+		}
+	}
 
 	p2p_go_save_group_common_freqs(wpa_s, params);
 	p2p_go_dump_common_freqs(wpa_s);
@@ -1960,13 +2066,20 @@  static void p2p_go_configured(void *ctx, void *data)
 		return;
 	}
 
-	wpa_printf(MSG_DEBUG, "P2P: Setting up WPS for GO provisioning");
 	if (wpa_supplicant_ap_mac_addr_filter(wpa_s,
 					      params->peer_interface_addr)) {
 		wpa_printf(MSG_DEBUG, "P2P: Failed to setup MAC address "
 			   "filtering");
 		return;
 	}
+
+	if (params->p2p2) {
+		wpas_group_formation_completed(wpa_s, 1, 0);
+		wpa_printf(MSG_DEBUG, "P2P2: Group formation completed, First connection in progress");
+		goto out;
+	}
+
+	wpa_printf(MSG_DEBUG, "P2P: Setting up WPS for GO provisioning");
 	if (params->wps_method == WPS_PBC) {
 		wpa_supplicant_ap_wps_pbc(wpa_s, params->peer_interface_addr,
 					  params->peer_device_addr);
@@ -1987,6 +2100,7 @@  static void p2p_go_configured(void *ctx, void *data)
 	} else if (wpa_s->p2p_pin[0])
 		wpa_supplicant_ap_wps_pin(wpa_s, params->peer_interface_addr,
 					  wpa_s->p2p_pin, NULL, 0, 0);
+out:
 	os_free(wpa_s->go_params);
 	wpa_s->go_params = NULL;
 }
@@ -2069,9 +2183,9 @@  int wpas_p2p_try_edmg_channel(struct wpa_supplicant *wpa_s,
 }
 
 
-static void wpas_start_wps_go(struct wpa_supplicant *wpa_s,
+static void wpas_start_go(struct wpa_supplicant *wpa_s,
 			      struct p2p_go_neg_results *params,
-			      int group_formation)
+			      int group_formation, bool p2p2)
 {
 	struct wpa_ssid *ssid;
 
@@ -2166,6 +2280,18 @@  static void wpas_start_wps_go(struct wpa_supplicant *wpa_s,
 		wpa_config_update_psk(ssid);
 	ssid->ap_max_inactivity = wpa_s->p2pdev->conf->p2p_go_max_inactivity;
 
+	if (p2p2) {
+		if (params->akmp == WPA_KEY_MGMT_SAE)
+			ssid->auth_alg = WPA_AUTH_ALG_OPEN;
+		else
+			ssid->auth_alg |= WPA_AUTH_ALG_SAE;
+
+		ssid->key_mgmt = WPA_KEY_MGMT_SAE;
+		ssid->sae_password = os_strdup(params->password);
+		ssid->ieee80211w = MGMT_FRAME_PROTECTION_REQUIRED;
+		wpa_s->conf->sae_pwe = SAE_PWE_HASH_TO_ELEMENT;
+	}
+
 	wpa_s->ap_configured_cb = p2p_go_configured;
 	wpa_s->ap_configured_cb_ctx = wpa_s;
 	wpa_s->ap_configured_cb_data = wpa_s->go_params;
@@ -2380,6 +2506,7 @@  wpas_p2p_init_group_interface(struct wpa_supplicant *wpa_s, int go)
 	wpa_s->global->pending_group_iface_for_p2ps = 0;
 
 	wpas_p2p_clone_config(group_wpa_s, wpa_s);
+	group_wpa_s->p2p2 = wpa_s->p2p2;
 
 	if (wpa_s->conf->p2p_interface_random_mac_addr) {
 		if (wpa_drv_set_mac_addr(group_wpa_s,
@@ -2561,12 +2688,18 @@  static void wpas_go_neg_completed(void *ctx, struct p2p_go_neg_results *res)
 		os_memcpy(group_wpa_s->p2p_pin, wpa_s->p2p_pin,
 			  sizeof(group_wpa_s->p2p_pin));
 		group_wpa_s->p2p_wps_method = wpa_s->p2p_wps_method;
+		group_wpa_s->p2p2 = res->p2p2;
+		group_wpa_s->p2p_bootstrap = wpa_s->p2p_bootstrap;
 	}
+
 	if (res->role_go) {
-		wpas_start_wps_go(group_wpa_s, res, 1);
+		wpas_start_go(group_wpa_s, res, 1, res->p2p2);
 	} else {
 		os_get_reltime(&group_wpa_s->scan_min_time);
-		wpas_start_wps_enrollee(group_wpa_s, res);
+		if (res->p2p2)
+			wpas_start_gc(group_wpa_s, res);
+		else
+			wpas_start_wps_enrollee(group_wpa_s, res);
 	}
 
 	wpa_s->global->p2p_long_listen = 0;
@@ -7147,7 +7280,7 @@  int wpas_p2p_group_add(struct wpa_supplicant *wpa_s, int persistent_group,
 		return -1;
 	if (freq > 0)
 		wpa_s->p2p_go_no_pri_sec_switch = 1;
-	wpas_start_wps_go(wpa_s, &params, 0);
+	wpas_start_go(wpa_s, &params, 0, wpa_s->p2p2);
 
 	return 0;
 }
@@ -7364,7 +7497,7 @@  int wpas_p2p_group_add_persistent(struct wpa_supplicant *wpa_s,
 	p2p_channels_to_freqs(channels, params.freq_list, P2P_MAX_CHANNELS);
 
 	wpa_s->p2p_first_connection_timeout = connection_timeout;
-	wpas_start_wps_go(wpa_s, &params, 0);
+	wpas_start_go(wpa_s, &params, 0, wpa_s->p2p2);
 
 	return 0;
 }