From 86bd36f0d5b3d359075c356d68977b4d2e7c9f71 Mon Sep 17 00:00:00 2001 From: Jouni Malinen Date: Fri, 4 Jul 2014 18:23:43 +0300 Subject: [PATCH] Add generic mechanism for adding vendor elements into frames This adds following new control interface commands to allow arbitrary vendor elements to be added into number of frames: VENDOR_ELEM_ADD VENDOR_ELEM_GET VENDOR_ELEM_REMOVE VENDOR_ELEM_REMOVE * The following frames are supported in this commit (additional frames can be added in the future): 0 = Probe Request frame in P2P device discovery 1 = Probe Response frame from P2P Device role 2 = Probe Response frame from P2P GO 3 = Beacon frame from P2P GO 4 = PD Req 5 = PD Resp 6 = GO Neg Req 7 = GO Neg Resp 8 = GO Neg Conf 9 = Invitation Request 10 = Invitation Response 11 = P2P Association Request 12 = P2P Association Response One or more vendor element can be added/removed with the commands. The hexdump of the element(s) needs to contain the full element (id, len, payload) and the buffer needs to pass IE parsing requirements to be accepted. Signed-off-by: Jouni Malinen --- src/common/wpa_ctrl.h | 19 +++ src/p2p/p2p.c | 29 ++++ src/p2p/p2p.h | 2 + src/p2p/p2p_go_neg.c | 18 +++ src/p2p/p2p_group.c | 26 ++++ src/p2p/p2p_i.h | 2 + src/p2p/p2p_invitation.c | 7 + src/p2p/p2p_pd.c | 13 ++ wpa_supplicant/ctrl_iface.c | 213 ++++++++++++++++++++++++++++++ wpa_supplicant/wpa_supplicant.c | 7 + wpa_supplicant/wpa_supplicant_i.h | 3 + 11 files changed, 339 insertions(+) diff --git a/src/common/wpa_ctrl.h b/src/common/wpa_ctrl.h index 534bc997c..d91594e31 100644 --- a/src/common/wpa_ctrl.h +++ b/src/common/wpa_ctrl.h @@ -239,6 +239,25 @@ extern "C" { #define WPA_BSS_MASK_DELIM BIT(17) +/* VENDOR_ELEM_* frame id values */ +enum wpa_vendor_elem_frame { + VENDOR_ELEM_PROBE_REQ_P2P = 0, + VENDOR_ELEM_PROBE_RESP_P2P = 1, + VENDOR_ELEM_PROBE_RESP_P2P_GO = 2, + VENDOR_ELEM_BEACON_P2P_GO = 3, + VENDOR_ELEM_P2P_PD_REQ = 4, + VENDOR_ELEM_P2P_PD_RESP = 5, + VENDOR_ELEM_P2P_GO_NEG_REQ = 6, + VENDOR_ELEM_P2P_GO_NEG_RESP = 7, + VENDOR_ELEM_P2P_GO_NEG_CONF = 8, + VENDOR_ELEM_P2P_INV_REQ = 9, + VENDOR_ELEM_P2P_INV_RESP = 10, + VENDOR_ELEM_P2P_ASSOC_REQ = 11, + VENDOR_ELEM_P2P_ASSOC_RESP = 12, + NUM_VENDOR_ELEM_FRAMES +}; + + /* wpa_supplicant/hostapd control interface access */ /** diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index 48f3aa652..69ee0d028 100644 --- a/src/p2p/p2p.c +++ b/src/p2p/p2p.c @@ -12,6 +12,7 @@ #include "eloop.h" #include "common/ieee802_11_defs.h" #include "common/ieee802_11_common.h" +#include "common/wpa_ctrl.h" #include "wps/wps_i.h" #include "p2p_i.h" #include "p2p.h" @@ -1957,6 +1958,9 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) extra = wpabuf_len(p2p->wfd_ie_probe_resp); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -1977,6 +1981,10 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) wpabuf_put_buf(buf, p2p->wfd_ie_probe_resp); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]) + wpabuf_put_buf(buf, + p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P]); + /* P2P IE */ len = p2p_buf_add_ie_hdr(buf); p2p_buf_add_capability(buf, p2p->dev_capab & @@ -2252,6 +2260,9 @@ int p2p_assoc_req_ie(struct p2p_data *p2p, const u8 *bssid, u8 *buf, extra = wpabuf_len(p2p->wfd_ie_assoc_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_REQ]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_REQ]); + /* * (Re)Association Request - P2P IE * P2P Capability attribute (shall be present) @@ -2267,6 +2278,10 @@ int p2p_assoc_req_ie(struct p2p_data *p2p, const u8 *bssid, u8 *buf, wpabuf_put_buf(tmp, p2p->wfd_ie_assoc_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_REQ]) + wpabuf_put_buf(tmp, + p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_REQ]); + peer = bssid ? p2p_get_device(p2p, bssid) : NULL; lpos = p2p_buf_add_ie_hdr(tmp); @@ -2854,6 +2869,10 @@ void p2p_scan_ie(struct p2p_data *p2p, struct wpabuf *ies, const u8 *dev_id) wpabuf_put_buf(ies, p2p->wfd_ie_probe_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_PROBE_REQ_P2P]) + wpabuf_put_buf(ies, + p2p->vendor_elem[VENDOR_ELEM_PROBE_REQ_P2P]); + len = p2p_buf_add_ie_hdr(ies); p2p_buf_add_capability(ies, p2p->dev_capab & ~P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY, 0); @@ -2880,6 +2899,10 @@ size_t p2p_scan_ie_buf_len(struct p2p_data *p2p) len += wpabuf_len(p2p->wfd_ie_probe_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p && p2p->vendor_elem && + p2p->vendor_elem[VENDOR_ELEM_PROBE_REQ_P2P]) + len += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_PROBE_REQ_P2P]); + return len; } @@ -4742,3 +4765,9 @@ int p2p_set_passphrase_len(struct p2p_data *p2p, unsigned int len) p2p->cfg->passphrase_len = len; return 0; } + + +void p2p_set_vendor_elems(struct p2p_data *p2p, struct wpabuf **vendor_elem) +{ + p2p->vendor_elem = vendor_elem; +} diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index 16500a807..c4bc0d235 100644 --- a/src/p2p/p2p.h +++ b/src/p2p/p2p.h @@ -1995,4 +1995,6 @@ void p2p_loop_on_known_peers(struct p2p_data *p2p, void *user_data), void *user_data); +void p2p_set_vendor_elems(struct p2p_data *p2p, struct wpabuf **vendor_elem); + #endif /* P2P_H */ diff --git a/src/p2p/p2p_go_neg.c b/src/p2p/p2p_go_neg.c index f24fe2365..bd7a2cf51 100644 --- a/src/p2p/p2p_go_neg.c +++ b/src/p2p/p2p_go_neg.c @@ -10,6 +10,7 @@ #include "common.h" #include "common/ieee802_11_defs.h" +#include "common/wpa_ctrl.h" #include "wps/wps_defs.h" #include "p2p_i.h" #include "p2p.h" @@ -142,6 +143,9 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p, extra = wpabuf_len(p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_REQ]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_REQ]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -191,6 +195,9 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p, wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_REQ]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_REQ]); + return buf; } @@ -268,6 +275,9 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p, extra = wpabuf_len(p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_RESP]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_RESP]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -336,6 +346,8 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p, wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_RESP]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_RESP]); return buf; } @@ -844,6 +856,9 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p, extra = wpabuf_len(p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_CONF]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_CONF]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -888,6 +903,9 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p, wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_CONF]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_GO_NEG_CONF]); + return buf; } diff --git a/src/p2p/p2p_group.c b/src/p2p/p2p_group.c index aa075bdb2..da8588aad 100644 --- a/src/p2p/p2p_group.c +++ b/src/p2p/p2p_group.c @@ -11,6 +11,7 @@ #include "common.h" #include "common/ieee802_11_defs.h" #include "common/ieee802_11_common.h" +#include "common/wpa_ctrl.h" #include "wps/wps_defs.h" #include "wps/wps_i.h" #include "p2p_i.h" @@ -214,6 +215,10 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group) extra = wpabuf_len(group->p2p->wfd_ie_beacon); #endif /* CONFIG_WIFI_DISPLAY */ + if (group->p2p->vendor_elem && + group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]) + extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]); + ie = wpabuf_alloc(257 + extra); if (ie == NULL) return NULL; @@ -223,6 +228,11 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group) wpabuf_put_buf(ie, group->p2p->wfd_ie_beacon); #endif /* CONFIG_WIFI_DISPLAY */ + if (group->p2p->vendor_elem && + group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]) + wpabuf_put_buf(ie, + group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]); + len = p2p_buf_add_ie_hdr(ie); p2p_group_add_common_ies(group, ie); p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr); @@ -448,6 +458,13 @@ static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group) ie = p2p_group_encaps_probe_resp(p2p_subelems); wpabuf_free(p2p_subelems); + if (group->p2p->vendor_elem && + group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]) { + struct wpabuf *extra; + extra = wpabuf_dup(group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]); + ie = wpabuf_concat(extra, ie); + } + #ifdef CONFIG_WIFI_DISPLAY if (group->wfd_ie) { struct wpabuf *wfd = wpabuf_dup(group->wfd_ie); @@ -634,6 +651,10 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status) extra = wpabuf_len(group->wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (group->p2p->vendor_elem && + group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]) + extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]); + /* * (Re)Association Response - P2P IE * Status attribute (shall be present when association request is @@ -649,6 +670,11 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status) wpabuf_put_buf(resp, group->wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (group->p2p->vendor_elem && + group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]) + wpabuf_put_buf(resp, + group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]); + rlen = p2p_buf_add_ie_hdr(resp); if (status != P2P_SC_SUCCESS) p2p_buf_add_status(resp, status); diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index 39a927a31..3b60582bb 100644 --- a/src/p2p/p2p_i.h +++ b/src/p2p/p2p_i.h @@ -499,6 +499,8 @@ struct p2p_data { #endif /* CONFIG_WIFI_DISPLAY */ u16 authorized_oob_dev_pw_id; + + struct wpabuf **vendor_elem; }; /** diff --git a/src/p2p/p2p_invitation.c b/src/p2p/p2p_invitation.c index a36898e99..ef01a668d 100644 --- a/src/p2p/p2p_invitation.c +++ b/src/p2p/p2p_invitation.c @@ -10,6 +10,7 @@ #include "common.h" #include "common/ieee802_11_defs.h" +#include "common/wpa_ctrl.h" #include "p2p_i.h" #include "p2p.h" @@ -45,6 +46,9 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p, extra = wpabuf_len(wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_INV_REQ]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_INV_REQ]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -86,6 +90,9 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p, wpabuf_put_buf(buf, wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_INV_REQ]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_INV_REQ]); + if (dev_pw_id >= 0) { /* WSC IE in Invitation Request for NFC static handover */ p2p_build_wps_ie(p2p, buf, dev_pw_id, 0); diff --git a/src/p2p/p2p_pd.c b/src/p2p/p2p_pd.c index 68d79d239..e1013672a 100644 --- a/src/p2p/p2p_pd.c +++ b/src/p2p/p2p_pd.c @@ -10,6 +10,7 @@ #include "common.h" #include "common/ieee802_11_defs.h" +#include "common/wpa_ctrl.h" #include "wps/wps_defs.h" #include "p2p_i.h" #include "p2p.h" @@ -53,6 +54,9 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p, extra = wpabuf_len(p2p->wfd_ie_prov_disc_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_PD_REQ]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_PD_REQ]); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -77,6 +81,9 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p, wpabuf_put_buf(buf, p2p->wfd_ie_prov_disc_req); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_PD_REQ]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_PD_REQ]); + return buf; } @@ -111,6 +118,9 @@ static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p, extra = wpabuf_len(wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_PD_RESP]) + extra += wpabuf_len(p2p->vendor_elem[VENDOR_ELEM_P2P_PD_RESP]); + buf = wpabuf_alloc(100 + extra); if (buf == NULL) return NULL; @@ -125,6 +135,9 @@ static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p, wpabuf_put_buf(buf, wfd_ie); #endif /* CONFIG_WIFI_DISPLAY */ + if (p2p->vendor_elem && p2p->vendor_elem[VENDOR_ELEM_P2P_PD_RESP]) + wpabuf_put_buf(buf, p2p->vendor_elem[VENDOR_ELEM_P2P_PD_RESP]); + return buf; } diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c index 244fd2d0e..143c854cd 100644 --- a/wpa_supplicant/ctrl_iface.c +++ b/wpa_supplicant/ctrl_iface.c @@ -6227,6 +6227,210 @@ static int wpas_ctrl_iface_driver_event(struct wpa_supplicant *wpa_s, char *cmd) #endif /* CONFIG_TESTING_OPTIONS */ +static void wpas_ctrl_vendor_elem_update(struct wpa_supplicant *wpa_s) +{ + unsigned int i; + char buf[30]; + + wpa_printf(MSG_DEBUG, "Update vendor elements"); + + for (i = 0; i < NUM_VENDOR_ELEM_FRAMES; i++) { + if (wpa_s->vendor_elem[i]) { + os_snprintf(buf, sizeof(buf), "frame[%u]", i); + wpa_hexdump_buf(MSG_DEBUG, buf, wpa_s->vendor_elem[i]); + } + } + +#ifdef CONFIG_P2P + if (wpa_s->parent == wpa_s && + wpa_s->global->p2p && + !wpa_s->global->p2p_disabled) + p2p_set_vendor_elems(wpa_s->global->p2p, wpa_s->vendor_elem); +#endif /* CONFIG_P2P */ +} + + +static struct wpa_supplicant * +wpas_ctrl_vendor_elem_iface(struct wpa_supplicant *wpa_s, + enum wpa_vendor_elem_frame frame) +{ + switch (frame) { +#ifdef CONFIG_P2P + case VENDOR_ELEM_PROBE_REQ_P2P: + case VENDOR_ELEM_PROBE_RESP_P2P: + case VENDOR_ELEM_PROBE_RESP_P2P_GO: + case VENDOR_ELEM_BEACON_P2P_GO: + case VENDOR_ELEM_P2P_PD_REQ: + case VENDOR_ELEM_P2P_PD_RESP: + case VENDOR_ELEM_P2P_GO_NEG_REQ: + case VENDOR_ELEM_P2P_GO_NEG_RESP: + case VENDOR_ELEM_P2P_GO_NEG_CONF: + case VENDOR_ELEM_P2P_INV_REQ: + case VENDOR_ELEM_P2P_INV_RESP: + case VENDOR_ELEM_P2P_ASSOC_REQ: + return wpa_s->parent; +#endif /* CONFIG_P2P */ + default: + return wpa_s; + } +} + + +static int wpas_ctrl_vendor_elem_add(struct wpa_supplicant *wpa_s, char *cmd) +{ + char *pos = cmd; + int frame; + size_t len; + struct wpabuf *buf; + struct ieee802_11_elems elems; + + frame = atoi(pos); + if (frame < 0 || frame >= NUM_VENDOR_ELEM_FRAMES) + return -1; + wpa_s = wpas_ctrl_vendor_elem_iface(wpa_s, frame); + + pos = os_strchr(pos, ' '); + if (pos == NULL) + return -1; + pos++; + + len = os_strlen(pos); + if (len == 0) + return 0; + if (len & 1) + return -1; + len /= 2; + + buf = wpabuf_alloc(len); + if (buf == NULL) + return -1; + + if (hexstr2bin(pos, wpabuf_put(buf, len), len) < 0) { + wpabuf_free(buf); + return -1; + } + + if (ieee802_11_parse_elems(wpabuf_head_u8(buf), len, &elems, 0) == + ParseFailed) { + wpabuf_free(buf); + return -1; + } + + if (wpa_s->vendor_elem[frame] == NULL) { + wpa_s->vendor_elem[frame] = buf; + wpas_ctrl_vendor_elem_update(wpa_s); + return 0; + } + + if (wpabuf_resize(&wpa_s->vendor_elem[frame], len) < 0) { + wpabuf_free(buf); + return -1; + } + + wpabuf_put_buf(wpa_s->vendor_elem[frame], buf); + wpabuf_free(buf); + wpas_ctrl_vendor_elem_update(wpa_s); + + return 0; +} + + +static int wpas_ctrl_vendor_elem_get(struct wpa_supplicant *wpa_s, char *cmd, + char *buf, size_t buflen) +{ + int frame = atoi(cmd); + + if (frame < 0 || frame >= NUM_VENDOR_ELEM_FRAMES) + return -1; + wpa_s = wpas_ctrl_vendor_elem_iface(wpa_s, frame); + + if (wpa_s->vendor_elem[frame] == NULL) + return 0; + + return wpa_snprintf_hex(buf, buflen, + wpabuf_head_u8(wpa_s->vendor_elem[frame]), + wpabuf_len(wpa_s->vendor_elem[frame])); +} + + +static int wpas_ctrl_vendor_elem_remove(struct wpa_supplicant *wpa_s, char *cmd) +{ + char *pos = cmd; + int frame; + size_t len; + u8 *buf; + struct ieee802_11_elems elems; + u8 *ie, *end; + + frame = atoi(pos); + if (frame < 0 || frame >= NUM_VENDOR_ELEM_FRAMES) + return -1; + wpa_s = wpas_ctrl_vendor_elem_iface(wpa_s, frame); + + pos = os_strchr(pos, ' '); + if (pos == NULL) + return -1; + pos++; + + if (*pos == '*') { + wpabuf_free(wpa_s->vendor_elem[frame]); + wpa_s->vendor_elem[frame] = NULL; + wpas_ctrl_vendor_elem_update(wpa_s); + return 0; + } + + if (wpa_s->vendor_elem[frame] == NULL) + return -1; + + len = os_strlen(pos); + if (len == 0) + return 0; + if (len & 1) + return -1; + len /= 2; + + buf = os_malloc(len); + if (buf == NULL) + return -1; + + if (hexstr2bin(pos, buf, len) < 0) { + os_free(buf); + return -1; + } + + if (ieee802_11_parse_elems(buf, len, &elems, 0) == ParseFailed) { + os_free(buf); + return -1; + } + + ie = wpabuf_mhead_u8(wpa_s->vendor_elem[frame]); + end = ie + wpabuf_len(wpa_s->vendor_elem[frame]); + + for (; ie + 1 < end; ie += 2 + ie[1]) { + if (ie + len > end) + break; + if (os_memcmp(ie, buf, len) != 0) + continue; + + if (wpabuf_len(wpa_s->vendor_elem[frame]) == len) { + wpabuf_free(wpa_s->vendor_elem[frame]); + wpa_s->vendor_elem[frame] = NULL; + } else { + os_memmove(ie, ie + len, + wpabuf_len(wpa_s->vendor_elem[frame]) - len); + wpa_s->vendor_elem[frame]->used -= len; + } + os_free(buf); + wpas_ctrl_vendor_elem_update(wpa_s); + return 0; + } + + os_free(buf); + + return -1; +} + + char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s, char *buf, size_t *resp_len) { @@ -6792,6 +6996,15 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s, if (wpas_ctrl_iface_driver_event(wpa_s, buf + 13) < 0) reply_len = -1; #endif /* CONFIG_TESTING_OPTIONS */ + } else if (os_strncmp(buf, "VENDOR_ELEM_ADD ", 16) == 0) { + if (wpas_ctrl_vendor_elem_add(wpa_s, buf + 16) < 0) + reply_len = -1; + } else if (os_strncmp(buf, "VENDOR_ELEM_GET ", 16) == 0) { + reply_len = wpas_ctrl_vendor_elem_get(wpa_s, buf + 16, reply, + reply_size); + } else if (os_strncmp(buf, "VENDOR_ELEM_REMOVE ", 19) == 0) { + if (wpas_ctrl_vendor_elem_remove(wpa_s, buf + 19) < 0) + reply_len = -1; } else { os_memcpy(reply, "UNKNOWN COMMAND\n", 16); reply_len = 16; diff --git a/wpa_supplicant/wpa_supplicant.c b/wpa_supplicant/wpa_supplicant.c index 55570de6b..9414e8f32 100644 --- a/wpa_supplicant/wpa_supplicant.c +++ b/wpa_supplicant/wpa_supplicant.c @@ -375,6 +375,8 @@ void free_hw_features(struct wpa_supplicant *wpa_s) static void wpa_supplicant_cleanup(struct wpa_supplicant *wpa_s) { + int i; + bgscan_deinit(wpa_s); autoscan_deinit(wpa_s); scard_deinit(wpa_s->scard); @@ -498,6 +500,11 @@ static void wpa_supplicant_cleanup(struct wpa_supplicant *wpa_s) #ifdef CONFIG_HS20 hs20_deinit(wpa_s); #endif /* CONFIG_HS20 */ + + for (i = 0; i < NUM_VENDOR_ELEM_FRAMES; i++) { + wpabuf_free(wpa_s->vendor_elem[i]); + wpa_s->vendor_elem[i] = NULL; + } } diff --git a/wpa_supplicant/wpa_supplicant_i.h b/wpa_supplicant/wpa_supplicant_i.h index 8a9ca979f..bf3d19df0 100644 --- a/wpa_supplicant/wpa_supplicant_i.h +++ b/wpa_supplicant/wpa_supplicant_i.h @@ -12,6 +12,7 @@ #include "utils/list.h" #include "common/defs.h" #include "common/sae.h" +#include "common/wpa_ctrl.h" #include "wps/wps_defs.h" #include "config_ssid.h" @@ -860,6 +861,8 @@ struct wpa_supplicant { struct wpa_radio_work *connect_work; unsigned int ext_work_id; + + struct wpabuf *vendor_elem[NUM_VENDOR_ELEM_FRAMES]; };