diff --git a/src/ap/drv_callbacks.c b/src/ap/drv_callbacks.c index a726a6ff8..7e71e6f8c 100644 --- a/src/ap/drv_callbacks.c +++ b/src/ap/drv_callbacks.c @@ -1104,10 +1104,7 @@ static void hostapd_action_rx(struct hostapd_data *hapd, #endif /* CONFIG_IEEE80211R_AP */ #ifdef CONFIG_IEEE80211W if (mgmt->u.action.category == WLAN_ACTION_SA_QUERY && plen >= 4) { - ieee802_11_sa_query_action( - hapd, mgmt->sa, - mgmt->u.action.u.sa_query_resp.action, - mgmt->u.action.u.sa_query_resp.trans_id); + ieee802_11_sa_query_action(hapd, mgmt, drv_mgmt->frame_len); } #endif /* CONFIG_IEEE80211W */ #ifdef CONFIG_WNM_AP diff --git a/src/ap/ieee802_11.c b/src/ap/ieee802_11.c index 41de3f6c1..647b4801f 100644 --- a/src/ap/ieee802_11.c +++ b/src/ap/ieee802_11.c @@ -3760,9 +3760,7 @@ static int hostapd_sa_query_action(struct hostapd_data *hapd, return 0; } - ieee802_11_sa_query_action(hapd, mgmt->sa, - mgmt->u.action.u.sa_query_resp.action, - mgmt->u.action.u.sa_query_resp.trans_id); + ieee802_11_sa_query_action(hapd, mgmt, len); return 1; } diff --git a/src/ap/ieee802_11.h b/src/ap/ieee802_11.h index 848deaf41..b57f2d0ba 100644 --- a/src/ap/ieee802_11.h +++ b/src/ap/ieee802_11.h @@ -93,8 +93,8 @@ void ieee802_11_rx_from_unknown(struct hostapd_data *hapd, const u8 *src, u8 * hostapd_eid_assoc_comeback_time(struct hostapd_data *hapd, struct sta_info *sta, u8 *eid); void ieee802_11_sa_query_action(struct hostapd_data *hapd, - const u8 *sa, const u8 action_type, - const u8 *trans_id); + const struct ieee80211_mgmt *mgmt, + size_t len); u8 * hostapd_eid_interworking(struct hostapd_data *hapd, u8 *eid); u8 * hostapd_eid_adv_proto(struct hostapd_data *hapd, u8 *eid); u8 * hostapd_eid_roaming_consortium(struct hostapd_data *hapd, u8 *eid); diff --git a/src/ap/ieee802_11_shared.c b/src/ap/ieee802_11_shared.c index 0e41a4984..f6c655fd3 100644 --- a/src/ap/ieee802_11_shared.c +++ b/src/ap/ieee802_11_shared.c @@ -10,10 +10,12 @@ #include "utils/common.h" #include "common/ieee802_11_defs.h" +#include "common/ocv.h" #include "hostapd.h" #include "sta_info.h" #include "ap_config.h" #include "ap_drv_ops.h" +#include "wpa_auth.h" #include "ieee802_11.h" @@ -49,7 +51,12 @@ u8 * hostapd_eid_assoc_comeback_time(struct hostapd_data *hapd, void ieee802_11_send_sa_query_req(struct hostapd_data *hapd, const u8 *addr, const u8 *trans_id) { - struct ieee80211_mgmt mgmt; +#ifdef CONFIG_OCV + struct sta_info *sta; +#endif /* CONFIG_OCV */ + struct ieee80211_mgmt *mgmt; + u8 *oci_ie = NULL; + u8 oci_ie_len = 0; u8 *end; wpa_printf(MSG_DEBUG, "IEEE 802.11: Sending SA Query Request to " @@ -57,19 +64,61 @@ void ieee802_11_send_sa_query_req(struct hostapd_data *hapd, wpa_hexdump(MSG_DEBUG, "IEEE 802.11: SA Query Transaction ID", trans_id, WLAN_SA_QUERY_TR_ID_LEN); - os_memset(&mgmt, 0, sizeof(mgmt)); - mgmt.frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, - WLAN_FC_STYPE_ACTION); - os_memcpy(mgmt.da, addr, ETH_ALEN); - os_memcpy(mgmt.sa, hapd->own_addr, ETH_ALEN); - os_memcpy(mgmt.bssid, hapd->own_addr, ETH_ALEN); - mgmt.u.action.category = WLAN_ACTION_SA_QUERY; - mgmt.u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST; - os_memcpy(mgmt.u.action.u.sa_query_req.trans_id, trans_id, +#ifdef CONFIG_OCV + sta = ap_get_sta(hapd, addr); + if (sta && wpa_auth_uses_ocv(sta->wpa_sm)) { + struct wpa_channel_info ci; + + if (hostapd_drv_channel_info(hapd, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info for OCI element in SA Query Request"); + return; + } + + oci_ie_len = OCV_OCI_EXTENDED_LEN; + oci_ie = os_zalloc(oci_ie_len); + if (!oci_ie) { + wpa_printf(MSG_WARNING, + "Failed to allocate buffer for OCI element in SA Query Request"); + return; + } + + if (ocv_insert_extended_oci(&ci, oci_ie) < 0) { + os_free(oci_ie); + return; + } + } +#endif /* CONFIG_OCV */ + + mgmt = os_zalloc(sizeof(*mgmt) + oci_ie_len); + if (!mgmt) { + wpa_printf(MSG_DEBUG, + "Failed to allocate buffer for SA Query Response frame"); + os_free(oci_ie); + return; + } + + mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, + WLAN_FC_STYPE_ACTION); + os_memcpy(mgmt->da, addr, ETH_ALEN); + os_memcpy(mgmt->sa, hapd->own_addr, ETH_ALEN); + os_memcpy(mgmt->bssid, hapd->own_addr, ETH_ALEN); + mgmt->u.action.category = WLAN_ACTION_SA_QUERY; + mgmt->u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST; + os_memcpy(mgmt->u.action.u.sa_query_req.trans_id, trans_id, WLAN_SA_QUERY_TR_ID_LEN); - end = mgmt.u.action.u.sa_query_req.trans_id + WLAN_SA_QUERY_TR_ID_LEN; - if (hostapd_drv_send_mlme(hapd, &mgmt, end - (u8 *) &mgmt, 0) < 0) + end = mgmt->u.action.u.sa_query_req.variable; +#ifdef CONFIG_OCV + if (oci_ie_len > 0) { + os_memcpy(end, oci_ie, oci_ie_len); + end += oci_ie_len; + } +#endif /* CONFIG_OCV */ + if (hostapd_drv_send_mlme(hapd, mgmt, end - (u8 *) mgmt, 0) < 0) wpa_printf(MSG_INFO, "ieee802_11_send_sa_query_req: send failed"); + + os_free(mgmt); + os_free(oci_ie); } @@ -77,7 +126,9 @@ static void ieee802_11_send_sa_query_resp(struct hostapd_data *hapd, const u8 *sa, const u8 *trans_id) { struct sta_info *sta; - struct ieee80211_mgmt resp; + struct ieee80211_mgmt *resp; + u8 *oci_ie = NULL; + u8 oci_ie_len = 0; u8 *end; wpa_printf(MSG_DEBUG, "IEEE 802.11: Received SA Query Request from " @@ -92,30 +143,115 @@ static void ieee802_11_send_sa_query_resp(struct hostapd_data *hapd, return; } +#ifdef CONFIG_OCV + if (wpa_auth_uses_ocv(sta->wpa_sm)) { + struct wpa_channel_info ci; + + if (hostapd_drv_channel_info(hapd, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info for OCI element in SA Query Response"); + return; + } + + oci_ie_len = OCV_OCI_EXTENDED_LEN; + oci_ie = os_zalloc(oci_ie_len); + if (!oci_ie) { + wpa_printf(MSG_WARNING, + "Failed to allocate buffer for for OCI element in SA Query Response"); + return; + } + + if (ocv_insert_extended_oci(&ci, oci_ie) < 0) { + os_free(oci_ie); + return; + } + } +#endif /* CONFIG_OCV */ + + resp = os_zalloc(sizeof(*resp) + oci_ie_len); + if (!resp) { + wpa_printf(MSG_DEBUG, + "Failed to allocate buffer for SA Query Response frame"); + os_free(oci_ie); + return; + } + wpa_printf(MSG_DEBUG, "IEEE 802.11: Sending SA Query Response to " MACSTR, MAC2STR(sa)); - os_memset(&resp, 0, sizeof(resp)); - resp.frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, - WLAN_FC_STYPE_ACTION); - os_memcpy(resp.da, sa, ETH_ALEN); - os_memcpy(resp.sa, hapd->own_addr, ETH_ALEN); - os_memcpy(resp.bssid, hapd->own_addr, ETH_ALEN); - resp.u.action.category = WLAN_ACTION_SA_QUERY; - resp.u.action.u.sa_query_req.action = WLAN_SA_QUERY_RESPONSE; - os_memcpy(resp.u.action.u.sa_query_req.trans_id, trans_id, + resp->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, + WLAN_FC_STYPE_ACTION); + os_memcpy(resp->da, sa, ETH_ALEN); + os_memcpy(resp->sa, hapd->own_addr, ETH_ALEN); + os_memcpy(resp->bssid, hapd->own_addr, ETH_ALEN); + resp->u.action.category = WLAN_ACTION_SA_QUERY; + resp->u.action.u.sa_query_req.action = WLAN_SA_QUERY_RESPONSE; + os_memcpy(resp->u.action.u.sa_query_req.trans_id, trans_id, WLAN_SA_QUERY_TR_ID_LEN); - end = resp.u.action.u.sa_query_req.trans_id + WLAN_SA_QUERY_TR_ID_LEN; - if (hostapd_drv_send_mlme(hapd, &resp, end - (u8 *) &resp, 0) < 0) + end = resp->u.action.u.sa_query_req.variable; +#ifdef CONFIG_OCV + if (oci_ie_len > 0) { + os_memcpy(end, oci_ie, oci_ie_len); + end += oci_ie_len; + } +#endif /* CONFIG_OCV */ + if (hostapd_drv_send_mlme(hapd, resp, end - (u8 *) resp, 0) < 0) wpa_printf(MSG_INFO, "ieee80211_mgmt_sa_query_request: send failed"); + + os_free(resp); + os_free(oci_ie); } -void ieee802_11_sa_query_action(struct hostapd_data *hapd, const u8 *sa, - const u8 action_type, const u8 *trans_id) +void ieee802_11_sa_query_action(struct hostapd_data *hapd, + const struct ieee80211_mgmt *mgmt, + size_t len) { struct sta_info *sta; int i; + const u8 *sa = mgmt->sa; + const u8 action_type = mgmt->u.action.u.sa_query_resp.action; + const u8 *trans_id = mgmt->u.action.u.sa_query_resp.trans_id; + + sta = ap_get_sta(hapd, sa); + +#ifdef CONFIG_OCV + if (sta && wpa_auth_uses_ocv(sta->wpa_sm)) { + struct ieee802_11_elems elems; + struct wpa_channel_info ci; + int tx_chanwidth; + int tx_seg1_idx; + size_t ies_len; + const u8 *ies; + + ies = mgmt->u.action.u.sa_query_resp.variable; + ies_len = len - (ies - (u8 *) mgmt); + if (ieee802_11_parse_elems(ies, ies_len, &elems, 1) == + ParseFailed) { + wpa_printf(MSG_DEBUG, + "SA Query: Failed to parse elements"); + return; + } + + if (hostapd_drv_channel_info(hapd, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info to validate received OCI in SA Query Action frame"); + return; + } + + if (get_sta_tx_parameters(sta->wpa_sm, + channel_width_to_int(ci.chanwidth), + ci.seg1_idx, &tx_chanwidth, + &tx_seg1_idx) < 0) + return; + + if (ocv_verify_tx_params(elems.oci, elems.oci_len, &ci, + tx_chanwidth, tx_seg1_idx) != 0) { + wpa_printf(MSG_WARNING, "%s", ocv_errorstr); + return; + } + } +#endif /* CONFIG_OCV */ if (action_type == WLAN_SA_QUERY_REQUEST) { ieee802_11_send_sa_query_resp(hapd, sa, trans_id); @@ -135,7 +271,6 @@ void ieee802_11_sa_query_action(struct hostapd_data *hapd, const u8 *sa, /* MLME-SAQuery.confirm */ - sta = ap_get_sta(hapd, sa); if (sta == NULL || sta->sa_query_trans_id == NULL) { wpa_printf(MSG_DEBUG, "IEEE 802.11: No matching STA with " "pending SA Query request found"); diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h index 27b2edaac..1d2768bb8 100644 --- a/src/common/ieee802_11_defs.h +++ b/src/common/ieee802_11_defs.h @@ -866,10 +866,12 @@ struct ieee80211_mgmt { struct { u8 action; u8 trans_id[WLAN_SA_QUERY_TR_ID_LEN]; + u8 variable[]; /* OCI element */ } STRUCT_PACKED sa_query_req; struct { u8 action; /* */ u8 trans_id[WLAN_SA_QUERY_TR_ID_LEN]; + u8 variable[]; /* OCI element */ } STRUCT_PACKED sa_query_resp; struct { u8 action; diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c index 2de15f20d..c9dd86211 100644 --- a/src/drivers/driver_nl80211.c +++ b/src/drivers/driver_nl80211.c @@ -2233,6 +2233,11 @@ static int nl80211_mgmt_subscribe_non_ap(struct i802_bss *bss) ret = -1; #endif /* CONFIG_DPP */ #ifdef CONFIG_IEEE80211W +#ifdef CONFIG_OCV + /* SA Query Request */ + if (nl80211_register_action_frame(bss, (u8 *) "\x08\x00", 2) < 0) + ret = -1; +#endif /* CONFIG_OCV */ /* SA Query Response */ if (nl80211_register_action_frame(bss, (u8 *) "\x08\x01", 2) < 0) ret = -1; diff --git a/wpa_supplicant/sme.c b/wpa_supplicant/sme.c index becc4fa28..c4abff6eb 100644 --- a/wpa_supplicant/sme.c +++ b/wpa_supplicant/sme.c @@ -12,6 +12,7 @@ #include "utils/eloop.h" #include "common/ieee802_11_defs.h" #include "common/ieee802_11_common.h" +#include "common/ocv.h" #include "eapol_supp/eapol_supp_sm.h" #include "common/wpa_common.h" #include "common/sae.h" @@ -2242,7 +2243,9 @@ static int sme_check_sa_query_timeout(struct wpa_supplicant *wpa_s) static void sme_send_sa_query_req(struct wpa_supplicant *wpa_s, const u8 *trans_id) { - u8 req[2 + WLAN_SA_QUERY_TR_ID_LEN]; + u8 req[2 + WLAN_SA_QUERY_TR_ID_LEN + OCV_OCI_EXTENDED_LEN]; + u8 req_len = 2 + WLAN_SA_QUERY_TR_ID_LEN; + wpa_dbg(wpa_s, MSG_DEBUG, "SME: Sending SA Query Request to " MACSTR, MAC2STR(wpa_s->bssid)); wpa_hexdump(MSG_DEBUG, "SME: SA Query Transaction ID", @@ -2250,9 +2253,27 @@ static void sme_send_sa_query_req(struct wpa_supplicant *wpa_s, req[0] = WLAN_ACTION_SA_QUERY; req[1] = WLAN_SA_QUERY_REQUEST; os_memcpy(req + 2, trans_id, WLAN_SA_QUERY_TR_ID_LEN); + +#ifdef CONFIG_OCV + if (wpa_sm_ocv_enabled(wpa_s->wpa)) { + struct wpa_channel_info ci; + + if (wpa_drv_channel_info(wpa_s, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info for OCI element in SA Query Request frame"); + return; + } + + if (ocv_insert_extended_oci(&ci, req + req_len) < 0) + return; + + req_len += OCV_OCI_EXTENDED_LEN; + } +#endif /* CONFIG_OCV */ + if (wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid, wpa_s->own_addr, wpa_s->bssid, - req, sizeof(req), 0) < 0) + req, req_len, 0) < 0) wpa_msg(wpa_s, MSG_INFO, "SME: Failed to send SA Query " "Request"); } @@ -2347,15 +2368,54 @@ void sme_event_unprot_disconnect(struct wpa_supplicant *wpa_s, const u8 *sa, } -void sme_sa_query_rx(struct wpa_supplicant *wpa_s, const u8 *sa, - const u8 *data, size_t len) +static void sme_process_sa_query_request(struct wpa_supplicant *wpa_s, + const u8 *sa, const u8 *data, + size_t len) +{ + u8 resp[2 + WLAN_SA_QUERY_TR_ID_LEN + OCV_OCI_EXTENDED_LEN]; + u8 resp_len = 2 + WLAN_SA_QUERY_TR_ID_LEN; + + wpa_dbg(wpa_s, MSG_DEBUG, "SME: Sending SA Query Response to " + MACSTR, MAC2STR(wpa_s->bssid)); + + resp[0] = WLAN_ACTION_SA_QUERY; + resp[1] = WLAN_SA_QUERY_RESPONSE; + os_memcpy(resp + 2, data + 1, WLAN_SA_QUERY_TR_ID_LEN); + +#ifdef CONFIG_OCV + if (wpa_sm_ocv_enabled(wpa_s->wpa)) { + struct wpa_channel_info ci; + + if (wpa_drv_channel_info(wpa_s, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info for OCI element in SA Query Response frame"); + return; + } + + if (ocv_insert_extended_oci(&ci, resp + resp_len) < 0) + return; + + resp_len += OCV_OCI_EXTENDED_LEN; + } +#endif /* CONFIG_OCV */ + + if (wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid, + wpa_s->own_addr, wpa_s->bssid, + resp, resp_len, 0) < 0) + wpa_msg(wpa_s, MSG_INFO, + "SME: Failed to send SA Query Response"); +} + + +static void sme_process_sa_query_response(struct wpa_supplicant *wpa_s, + const u8 *sa, const u8 *data, + size_t len) { int i; - if (wpa_s->sme.sa_query_trans_id == NULL || - len < 1 + WLAN_SA_QUERY_TR_ID_LEN || - data[0] != WLAN_SA_QUERY_RESPONSE) + if (!wpa_s->sme.sa_query_trans_id) return; + wpa_dbg(wpa_s, MSG_DEBUG, "SME: Received SA Query response from " MACSTR " (trans_id %02x%02x)", MAC2STR(sa), data[1], data[2]); @@ -2380,4 +2440,48 @@ void sme_sa_query_rx(struct wpa_supplicant *wpa_s, const u8 *sa, sme_stop_sa_query(wpa_s); } + +void sme_sa_query_rx(struct wpa_supplicant *wpa_s, const u8 *sa, + const u8 *data, size_t len) +{ + if (len < 1 + WLAN_SA_QUERY_TR_ID_LEN) + return; + + wpa_dbg(wpa_s, MSG_DEBUG, "SME: Received SA Query frame from " + MACSTR " (trans_id %02x%02x)", MAC2STR(sa), data[1], data[2]); + +#ifdef CONFIG_OCV + if (wpa_sm_ocv_enabled(wpa_s->wpa)) { + struct ieee802_11_elems elems; + struct wpa_channel_info ci; + + if (ieee802_11_parse_elems(data + 1 + WLAN_SA_QUERY_TR_ID_LEN, + len - 1 - WLAN_SA_QUERY_TR_ID_LEN, + &elems, 1) == ParseFailed) { + wpa_printf(MSG_DEBUG, + "SA Query: Failed to parse elements"); + return; + } + + if (wpa_drv_channel_info(wpa_s, &ci) != 0) { + wpa_printf(MSG_WARNING, + "Failed to get channel info to validate received OCI in SA Query Action frame"); + return; + } + + if (ocv_verify_tx_params(elems.oci, elems.oci_len, &ci, + channel_width_to_int(ci.chanwidth), + ci.seg1_idx) != 0) { + wpa_printf(MSG_WARNING, "%s", ocv_errorstr); + return; + } + } +#endif /* CONFIG_OCV */ + + if (data[0] == WLAN_SA_QUERY_REQUEST) + sme_process_sa_query_request(wpa_s, sa, data, len); + else if (data[0] == WLAN_SA_QUERY_RESPONSE) + sme_process_sa_query_response(wpa_s, sa, data, len); +} + #endif /* CONFIG_IEEE80211W */