P2P: Allow adding of WPS vendor extension attributes

This adds the ability to add WPS vendor extension attributes in P2P
frames, like GO Negotiation and Probe Response frames.

Signed-off-by: Jean-Michel Bachot <jean-michelx.bachot@linux.intel.com>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

src/p2p/p2p.c

@@ -1936,6 +1936,7 @@ void p2p_deinit(struct p2p_data *p2p)
 	os_free(p2p->groups);
 	wpabuf_free(p2p->sd_resp);
 	os_free(p2p->after_scan_tx);
+	p2p_remove_wps_vendor_extensions(p2p);
 	os_free(p2p);
 }
 
@@ -2019,6 +2020,40 @@ int p2p_set_sec_dev_types(struct p2p_data *p2p, const u8 dev_types[][8],
 }
 
 
+void p2p_remove_wps_vendor_extensions(struct p2p_data *p2p)
+{
+	int i;
+
+	for (i = 0; i < P2P_MAX_WPS_VENDOR_EXTENSIONS; i++) {
+		wpabuf_free(p2p->wps_vendor_ext[i]);
+		p2p->wps_vendor_ext[i] = NULL;
+	}
+}
+
+
+int p2p_add_wps_vendor_extension(struct p2p_data *p2p,
+				 const struct wpabuf *vendor_ext)
+{
+	int i;
+
+	if (vendor_ext == NULL)
+		return -1;
+
+	for (i = 0; i < P2P_MAX_WPS_VENDOR_EXTENSIONS; i++) {
+		if (p2p->wps_vendor_ext[i] == NULL)
+			break;
+	}
+	if (i >= P2P_MAX_WPS_VENDOR_EXTENSIONS)
+		return -1;
+
+	p2p->wps_vendor_ext[i] = wpabuf_dup(vendor_ext);
+	if (p2p->wps_vendor_ext[i] == NULL)
+		return -1;
+
+	return 0;
+}
+
+
 int p2p_set_country(struct p2p_data *p2p, const char *country)
 {
 	os_memcpy(p2p->cfg->country, country, 3);

src/p2p/p2p.h

@@ -1405,4 +1405,22 @@ const u8 * p2p_iterate_group_members(struct p2p_group *group, void **next);
 const struct p2p_peer_info *
 p2p_get_peer_found(struct p2p_data *p2p, const u8 *addr, int next);
 
+/**
+ * p2p_remove_wps_vendor_extensions - Remove WPS vendor extensions
+ * @p2p: P2P module context from p2p_init()
+ */
+void p2p_remove_wps_vendor_extensions(struct p2p_data *p2p);
+
+/**
+ * p2p_add_wps_vendor_extension - Add a WPS vendor extension
+ * @p2p: P2P module context from p2p_init()
+ * @vendor_ext: The vendor extensions to add
+ * Returns: 0 on success, -1 on failure
+ *
+ * The wpabuf structures in the array are owned by the P2P
+ * module after this call.
+ */
+int p2p_add_wps_vendor_extension(struct p2p_data *p2p,
+				 const struct wpabuf *vendor_ext);
+
 #endif /* P2P_H */

src/p2p/p2p_build.c

@@ -334,6 +334,8 @@ void p2p_build_wps_ie(struct p2p_data *p2p, struct wpabuf *buf, u16 pw_id,
 		      int all_attr)
 {
 	u8 *len;
+	int i;
+
 	wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC);
 	len = wpabuf_put(buf, 1);
 	wpabuf_put_be32(buf, WPS_DEV_OUI_WFA);
@@ -394,5 +396,17 @@ void p2p_build_wps_ie(struct p2p_data *p2p, struct wpabuf *buf, u16 pw_id,
 				p2p->cfg->num_sec_dev_types);
 	}
 
+	/* Add the WPS vendor extensions */
+	for (i = 0; i < P2P_MAX_WPS_VENDOR_EXTENSIONS; i++) {
+		if (p2p->wps_vendor_ext[i] == NULL)
+			break;
+		if (wpabuf_tailroom(buf) <
+		    4 + wpabuf_len(p2p->wps_vendor_ext[i]))
+			continue;
+		wpabuf_put_be16(buf, ATTR_VENDOR_EXT);
+		wpabuf_put_be16(buf, wpabuf_len(p2p->wps_vendor_ext[i]));
+		wpabuf_put_buf(buf, p2p->wps_vendor_ext[i]);
+	}
+
 	p2p_buf_update_ie_hdr(buf, len);
 }

src/p2p/p2p_i.h

@@ -384,6 +384,12 @@ struct p2p_data {
 	int best_freq_24;
 	int best_freq_5;
 	int best_freq_overall;
+
+#define P2P_MAX_WPS_VENDOR_EXTENSIONS 10
+	/**
+	 * wps_vendor_ext - WPS Vendor Extensions to add
+	 */
+	struct wpabuf *wps_vendor_ext[P2P_MAX_WPS_VENDOR_EXTENSIONS];
 };
 
 /**

wpa_supplicant/config.h

@@ -41,6 +41,7 @@
 #define CFG_CHANGED_P2P_SSID_POSTFIX BIT(7)
 #define CFG_CHANGED_WPS_STRING BIT(8)
 #define CFG_CHANGED_P2P_INTRA_BSS BIT(9)
+#define CFG_CHANGED_VENDOR_EXTENSION BIT(10)
 
 /**
  * struct wpa_config - wpa_supplicant configuration data
@@ -357,6 +358,12 @@ struct wpa_config {
 	int persistent_reconnect;
 	int p2p_intra_bss;
 
+#define MAX_WPS_VENDOR_EXT 10
+	/**
+	 * wps_vendor_ext - Vendor extension attributes in WPS
+	 */
+	struct wpabuf *wps_vendor_ext[MAX_WPS_VENDOR_EXT];
+
 	/**
 	 * p2p_group_idle - Maximum idle time in seconds for P2P group
 	 *

wpa_supplicant/p2p_supplicant.c

@@ -2295,6 +2295,7 @@ int wpas_p2p_init(struct wpa_global *global, struct wpa_supplicant *wpa_s)
 {
 	struct p2p_config p2p;
 	unsigned int r;
+	int i;
 
 	if (!(wpa_s->drv_flags & WPA_DRIVER_FLAGS_P2P_CAPABLE))
 		return 0;
@@ -2433,6 +2434,13 @@ int wpas_p2p_init(struct wpa_global *global, struct wpa_supplicant *wpa_s)
 	if (global->p2p == NULL)
 		return -1;
 
+	for (i = 0; i < MAX_WPS_VENDOR_EXT; i++) {
+		if (wpa_s->conf->wps_vendor_ext[i] == NULL)
+			continue;
+		p2p_add_wps_vendor_extension(
+			global->p2p, wpa_s->conf->wps_vendor_ext[i]);
+	}
+
 	return 0;
 }
 
@@ -3883,6 +3891,17 @@ void wpas_p2p_update_config(struct wpa_supplicant *wpa_s)
 				      (void *) wpa_s->conf->sec_device_type,
 				      wpa_s->conf->num_sec_device_types);
 
+	if (wpa_s->conf->changed_parameters & CFG_CHANGED_VENDOR_EXTENSION) {
+		int i;
+		p2p_remove_wps_vendor_extensions(p2p);
+		for (i = 0; i < MAX_WPS_VENDOR_EXT; i++) {
+			if (wpa_s->conf->wps_vendor_ext[i] == NULL)
+				continue;
+			p2p_add_wps_vendor_extension(
+				p2p, wpa_s->conf->wps_vendor_ext[i]);
+		}
+	}
+
 	if ((wpa_s->conf->changed_parameters & CFG_CHANGED_COUNTRY) &&
 	    wpa_s->conf->country[0] && wpa_s->conf->country[1]) {
 		char country[3];