diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index ba2c2c26b..de554124a 100644 --- a/src/p2p/p2p.c +++ b/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); diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index eab15b85f..eb57eb808 100644 --- a/src/p2p/p2p.h +++ b/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 */ diff --git a/src/p2p/p2p_build.c b/src/p2p/p2p_build.c index c582a3177..28ad239d5 100644 --- a/src/p2p/p2p_build.c +++ b/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); } diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index 782ea356a..c11aabe30 100644 --- a/src/p2p/p2p_i.h +++ b/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]; }; /** diff --git a/wpa_supplicant/config.h b/wpa_supplicant/config.h index cf22ec409..75406ef3a 100644 --- a/wpa_supplicant/config.h +++ b/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 * diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c index f1153f416..a0aa55850 100644 --- a/wpa_supplicant/p2p_supplicant.c +++ b/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];