| /****************************************************************************** |
| * |
| * Copyright(c) 2015-2017 Intel Deutschland GmbH |
| * Copyright(c) 2018 Intel Corporation |
| * All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions |
| * are met: |
| * |
| * * Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * * Redistributions in binary form must reproduce the above copyright |
| * notice, this list of conditions and the following disclaimer in |
| * the documentation and/or other materials provided with the |
| * distribution. |
| * * Neither the name Intel Corporation nor the names of its |
| * contributors may be used to endorse or promote products derived |
| * from this software without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
| * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
| * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
| * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
| * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
| * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
| * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| * |
| *****************************************************************************/ |
| #include "fw/api/nan.h" |
| |
| #include <linux/etherdevice.h> |
| #include <net/cfg80211.h> |
| |
| #include "mvm.h" |
| |
| #define NAN_WARMUP_TIMEOUT_USEC (120000000ULL) |
| #define NAN_CHANNEL_24 (6) |
| #define NAN_CHANNEL_52 (149) |
| |
| enum srf_type { |
| SRF_BF_TYPE = BIT(0), |
| SRF_INCLUDE = BIT(1), |
| SRF_BLOOM_FILTER_IDX = BIT(2) | BIT(3), |
| }; |
| |
| static bool iwl_mvm_can_beacon(struct ieee80211_vif* vif, enum nl80211_band band, uint8_t channel) { |
| struct wiphy* wiphy = ieee80211_vif_to_wdev(vif)->wiphy; |
| int freq = ieee80211_channel_to_frequency(channel, band); |
| struct ieee80211_channel* chan = ieee80211_get_channel(wiphy, freq); |
| struct cfg80211_chan_def def; |
| |
| if (!chan) { |
| return false; |
| } |
| |
| cfg80211_chandef_create(&def, chan, NL80211_CHAN_NO_HT); |
| return cfg80211_reg_can_beacon(wiphy, &def, vif->type); |
| } |
| |
| static inline bool iwl_mvm_nan_is_ver2(struct ieee80211_hw* hw) { |
| struct iwl_mvm* mvm = IWL_MAC80211_GET_MVM(hw); |
| |
| return fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_NAN2_VER2); |
| } |
| |
| static inline size_t iwl_mvm_nan_cfg_cmd_len(struct ieee80211_hw* hw) { |
| return iwl_mvm_nan_is_ver2(hw) ? sizeof(struct iwl_nan_cfg_cmd_v2) |
| : sizeof(struct iwl_nan_cfg_cmd); |
| } |
| |
| static inline struct iwl_nan_umac_cfg* iwl_mvm_nan_get_umac_cfg(struct ieee80211_hw* hw, |
| void* nan_cfg_cmd) { |
| return iwl_mvm_nan_is_ver2(hw) ? &((struct iwl_nan_cfg_cmd_v2*)nan_cfg_cmd)->umac_cfg |
| : &((struct iwl_nan_cfg_cmd*)nan_cfg_cmd)->umac_cfg; |
| } |
| |
| static inline struct iwl_nan_testbed_cfg* iwl_mvm_nan_get_tb_cfg(struct ieee80211_hw* hw, |
| void* nan_cfg_cmd) { |
| return iwl_mvm_nan_is_ver2(hw) ? &((struct iwl_nan_cfg_cmd_v2*)nan_cfg_cmd)->tb_cfg |
| : &((struct iwl_nan_cfg_cmd*)nan_cfg_cmd)->tb_cfg; |
| } |
| |
| static inline struct iwl_nan_nan2_cfg* iwl_mvm_nan_get_nan2_cfg(struct ieee80211_hw* hw, |
| void* nan_cfg_cmd) { |
| return iwl_mvm_nan_is_ver2(hw) ? &((struct iwl_nan_cfg_cmd_v2*)nan_cfg_cmd)->nan2_cfg |
| : &((struct iwl_nan_cfg_cmd*)nan_cfg_cmd)->nan2_cfg; |
| } |
| |
| int iwl_mvm_start_nan(struct ieee80211_hw* hw, struct ieee80211_vif* vif, |
| struct cfg80211_nan_conf* conf) { |
| struct iwl_mvm_vif* mvmvif = iwl_mvm_vif_from_mac80211(vif); |
| void* cmd; |
| struct iwl_nan_umac_cfg* umac_cfg; |
| struct iwl_nan_testbed_cfg* tb_cfg; |
| struct iwl_nan_nan2_cfg* nan2_cfg; |
| struct iwl_mvm* mvm = IWL_MAC80211_GET_MVM(hw); |
| int ret = 0; |
| uint16_t cdw = 0; |
| |
| IWL_DEBUG_MAC80211(IWL_MAC80211_GET_MVM(hw), "Start NAN\n"); |
| |
| /* 2GHz is mandatory and nl80211 should make sure it is set. |
| * Warn and add 2GHz if this happens anyway. |
| */ |
| if (WARN_ON(conf->bands && !(conf->bands & BIT(NL80211_BAND_2GHZ)))) { |
| return -EINVAL; |
| } |
| |
| conf->bands |= BIT(NL80211_BAND_2GHZ); |
| cmd = kzalloc(iwl_mvm_nan_cfg_cmd_len(hw), GFP_KERNEL); |
| if (!cmd) { |
| return -ENOMEM; |
| } |
| |
| umac_cfg = iwl_mvm_nan_get_umac_cfg(hw, cmd); |
| tb_cfg = iwl_mvm_nan_get_tb_cfg(hw, cmd); |
| nan2_cfg = iwl_mvm_nan_get_nan2_cfg(hw, cmd); |
| |
| mutex_lock(&mvm->mutex); |
| |
| umac_cfg->action = cpu_to_le32(FW_CTXT_ACTION_ADD); |
| umac_cfg->tsf_id = cpu_to_le32(mvmvif->tsf_id); |
| umac_cfg->beacon_template_id = cpu_to_le32(mvmvif->id); |
| |
| ether_addr_copy(umac_cfg->node_addr, vif->addr); |
| umac_cfg->sta_id = cpu_to_le32(mvm->aux_sta.sta_id); |
| umac_cfg->master_pref = conf->master_pref; |
| |
| if (conf->bands & BIT(NL80211_BAND_2GHZ)) { |
| if (!iwl_mvm_can_beacon(vif, NL80211_BAND_2GHZ, NAN_CHANNEL_24)) { |
| IWL_ERR(mvm, "Can't beacon on %d\n", NAN_CHANNEL_24); |
| ret = -EINVAL; |
| goto out; |
| } |
| |
| tb_cfg->chan24 = NAN_CHANNEL_24; |
| cdw |= conf->cdw_2g; |
| } |
| |
| if (conf->bands & BIT(NL80211_BAND_5GHZ)) { |
| if (!iwl_mvm_can_beacon(vif, NL80211_BAND_5GHZ, NAN_CHANNEL_52)) { |
| IWL_ERR(mvm, "Can't beacon on %d\n", NAN_CHANNEL_52); |
| ret = -EINVAL; |
| goto out; |
| } |
| |
| tb_cfg->chan52 = NAN_CHANNEL_52; |
| cdw |= conf->cdw_5g << 3; |
| } |
| |
| tb_cfg->warmup_timer = cpu_to_le32(NAN_WARMUP_TIMEOUT_USEC); |
| tb_cfg->op_bands = 3; |
| nan2_cfg->cdw = cpu_to_le16(cdw); |
| |
| if ((conf->bands & BIT(NL80211_BAND_2GHZ)) && (conf->bands & BIT(NL80211_BAND_5GHZ))) { |
| umac_cfg->dual_band = cpu_to_le32(1); |
| } |
| |
| ret = iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(NAN_CONFIG_CMD, NAN_GROUP, 0), 0, |
| iwl_mvm_nan_cfg_cmd_len(hw), cmd); |
| |
| if (!ret) { |
| mvm->nan_vif = vif; |
| } |
| |
| out: |
| mutex_unlock(&mvm->mutex); |
| kfree(cmd); |
| |
| return ret; |
| } |
| |
| int iwl_mvm_stop_nan(struct ieee80211_hw* hw, struct ieee80211_vif* vif) { |
| void* cmd; |
| struct iwl_nan_umac_cfg* umac_cfg; |
| struct iwl_mvm* mvm = IWL_MAC80211_GET_MVM(hw); |
| int ret = 0; |
| |
| IWL_DEBUG_MAC80211(IWL_MAC80211_GET_MVM(hw), "Stop NAN\n"); |
| |
| cmd = kzalloc(iwl_mvm_nan_cfg_cmd_len(hw), GFP_KERNEL); |
| if (!cmd) { |
| return -ENOMEM; |
| } |
| |
| umac_cfg = iwl_mvm_nan_get_umac_cfg(hw, cmd); |
| |
| mutex_lock(&mvm->mutex); |
| umac_cfg->action = cpu_to_le32(FW_CTXT_ACTION_REMOVE); |
| |
| ret = iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(NAN_CONFIG_CMD, NAN_GROUP, 0), 0, |
| iwl_mvm_nan_cfg_cmd_len(hw), cmd); |
| |
| if (!ret) { |
| mvm->nan_vif = NULL; |
| } |
| mutex_unlock(&mvm->mutex); |
| kfree(cmd); |
| |
| return ret; |
| } |
| |
| static enum iwl_fw_nan_func_type iwl_fw_nan_func_type(enum nl80211_nan_function_type type) { |
| switch (type) { |
| case NL80211_NAN_FUNC_PUBLISH: |
| return IWL_NAN_DE_FUNC_PUBLISH; |
| case NL80211_NAN_FUNC_SUBSCRIBE: |
| return IWL_NAN_DE_FUNC_SUBSCRIBE; |
| case NL80211_NAN_FUNC_FOLLOW_UP: |
| return IWL_NAN_DE_FUNC_FOLLOW_UP; |
| default: |
| return IWL_NAN_DE_FUNC_NOT_VALID; |
| } |
| } |
| |
| static uint8_t iwl_mvm_get_match_filter_len(struct cfg80211_nan_func_filter* filters, |
| uint8_t num_filters) { |
| int i; |
| unsigned int len = 0; |
| |
| len += num_filters; |
| for (i = 0; i < num_filters; i++) { |
| len += filters[i].len; |
| } |
| |
| if (WARN_ON_ONCE(len > U8_MAX)) { |
| return 0; |
| } |
| |
| return len; |
| } |
| |
| static void iwl_mvm_copy_filters(struct cfg80211_nan_func_filter* filters, uint8_t num_filters, |
| uint8_t* cmd_data) { |
| int i; |
| uint8_t offset = 0; |
| |
| for (i = 0; i < num_filters; i++) { |
| memcpy(cmd_data + offset, &filters[i].len, sizeof(uint8_t)); |
| offset++; |
| if (filters[i].len > 0) { |
| memcpy(cmd_data + offset, filters[i].filter, filters[i].len); |
| } |
| |
| offset += filters[i].len; |
| } |
| } |
| |
| static inline size_t iwl_mvm_nan_add_func_cmd_len(struct ieee80211_hw* hw) { |
| return iwl_mvm_nan_is_ver2(hw) ? sizeof(struct iwl_nan_add_func_cmd_v2) |
| : sizeof(struct iwl_nan_add_func_cmd); |
| } |
| |
| static inline struct iwl_nan_add_func_common* iwl_mvm_nan_get_add_func_common( |
| struct ieee80211_hw* hw, void* nan_add_func_cmd) { |
| return iwl_mvm_nan_is_ver2(hw) ? &((struct iwl_nan_add_func_cmd_v2*)nan_add_func_cmd)->cmn |
| : &((struct iwl_nan_add_func_cmd*)nan_add_func_cmd)->cmn; |
| } |
| |
| static inline uint8_t* iwl_mvm_nan_get_add_func_data(struct ieee80211_hw* hw, |
| void* nan_add_func_cmd) { |
| return iwl_mvm_nan_is_ver2(hw) ? ((struct iwl_nan_add_func_cmd_v2*)nan_add_func_cmd)->data |
| : ((struct iwl_nan_add_func_cmd*)nan_add_func_cmd)->data; |
| } |
| |
| int iwl_mvm_add_nan_func(struct ieee80211_hw* hw, struct ieee80211_vif* vif, |
| const struct cfg80211_nan_func* nan_func) { |
| struct iwl_mvm* mvm = IWL_MAC80211_GET_MVM(hw); |
| void* cmd; |
| struct iwl_nan_add_func_common* cmn; |
| struct iwl_host_cmd hcmd = { |
| .id = iwl_cmd_id(NAN_DISCOVERY_FUNC_CMD, NAN_GROUP, 0), |
| .flags = CMD_WANT_SKB, |
| }; |
| struct iwl_nan_add_func_res* resp; |
| struct iwl_rx_packet* pkt; |
| uint8_t* cmd_data; |
| uint16_t flags = 0; |
| uint8_t tx_filt_len, rx_filt_len; |
| size_t cmd_len; |
| int ret = 0; |
| |
| IWL_DEBUG_MAC80211(IWL_MAC80211_GET_MVM(hw), "Add NAN func\n"); |
| |
| mutex_lock(&mvm->mutex); |
| |
| /* We assume here that mac80211 properly validated the nan_func */ |
| cmd_len = iwl_mvm_nan_add_func_cmd_len(hw) + ALIGN(nan_func->serv_spec_info_len, 4); |
| if (nan_func->srf_bf_len) { |
| cmd_len += ALIGN(nan_func->srf_bf_len + 1, 4); |
| } else if (nan_func->srf_num_macs) { |
| cmd_len += ALIGN(nan_func->srf_num_macs * ETH_ALEN + 1, 4); |
| } |
| |
| rx_filt_len = iwl_mvm_get_match_filter_len(nan_func->rx_filters, nan_func->num_rx_filters); |
| |
| tx_filt_len = iwl_mvm_get_match_filter_len(nan_func->tx_filters, nan_func->num_tx_filters); |
| |
| cmd_len += ALIGN(rx_filt_len, 4); |
| cmd_len += ALIGN(tx_filt_len, 4); |
| |
| cmd = kzalloc(cmd_len, GFP_KERNEL); |
| |
| if (!cmd) { |
| ret = -ENOBUFS; |
| goto unlock; |
| } |
| |
| hcmd.len[0] = cmd_len; |
| hcmd.data[0] = cmd; |
| |
| cmn = iwl_mvm_nan_get_add_func_common(hw, cmd); |
| |
| cmd_data = iwl_mvm_nan_get_add_func_data(hw, cmd); |
| cmn->action = cpu_to_le32(FW_CTXT_ACTION_ADD); |
| cmn->type = iwl_fw_nan_func_type(nan_func->type); |
| cmn->instance_id = nan_func->instance_id; |
| cmn->dw_interval = 1; |
| |
| memcpy(&cmn->service_id, nan_func->service_id, sizeof(cmn->service_id)); |
| |
| /* |
| * TODO: Currently we want all the events, however we might need to be |
| * able to unset this flag for solicited publish to disable "Replied" |
| * events. |
| */ |
| flags |= IWL_NAN_DE_FUNC_FLAG_RAISE_EVENTS; |
| if (nan_func->subscribe_active || nan_func->publish_type == NL80211_NAN_UNSOLICITED_PUBLISH) { |
| flags |= IWL_NAN_DE_FUNC_FLAG_UNSOLICITED_OR_ACTIVE; |
| } |
| |
| if (nan_func->close_range) { |
| flags |= IWL_NAN_DE_FUNC_FLAG_CLOSE_RANGE; |
| } |
| |
| if (nan_func->type == NL80211_NAN_FUNC_FOLLOW_UP || |
| (nan_func->type == NL80211_NAN_FUNC_PUBLISH && !nan_func->publish_bcast)) { |
| flags |= IWL_NAN_DE_FUNC_FLAG_UNICAST; |
| } |
| |
| if (nan_func->publish_type == NL80211_NAN_SOLICITED_PUBLISH) { |
| flags |= IWL_NAN_DE_FUNC_FLAG_SOLICITED; |
| } |
| |
| cmn->flags = cpu_to_le16(flags); |
| cmn->ttl = cpu_to_le32(nan_func->ttl); |
| cmn->serv_info_len = nan_func->serv_spec_info_len; |
| if (nan_func->serv_spec_info_len) { |
| memcpy(cmd_data, nan_func->serv_spec_info, nan_func->serv_spec_info_len); |
| } |
| |
| if (nan_func->type == NL80211_NAN_FUNC_FOLLOW_UP) { |
| cmn->flw_up_id = nan_func->followup_id; |
| cmn->flw_up_req_id = nan_func->followup_reqid; |
| memcpy(cmn->flw_up_addr, nan_func->followup_dest.addr, ETH_ALEN); |
| cmn->ttl = cpu_to_le32(1); |
| } |
| |
| cmd_data += ALIGN(cmn->serv_info_len, 4); |
| if (nan_func->srf_bf_len) { |
| uint8_t srf_ctl = 0; |
| |
| srf_ctl |= SRF_BF_TYPE; |
| srf_ctl |= (nan_func->srf_bf_idx << 2) & SRF_BLOOM_FILTER_IDX; |
| if (nan_func->srf_include) { |
| srf_ctl |= SRF_INCLUDE; |
| } |
| |
| cmn->srf_len = nan_func->srf_bf_len + 1; |
| memcpy(cmd_data, &srf_ctl, sizeof(srf_ctl)); |
| memcpy(cmd_data + 1, nan_func->srf_bf, nan_func->srf_bf_len); |
| } else if (nan_func->srf_num_macs) { |
| uint8_t srf_ctl = 0; |
| int i; |
| |
| if (nan_func->srf_include) { |
| srf_ctl |= SRF_INCLUDE; |
| } |
| |
| cmn->srf_len = nan_func->srf_num_macs * ETH_ALEN + 1; |
| memcpy(cmd_data, &srf_ctl, sizeof(srf_ctl)); |
| |
| for (i = 0; i < nan_func->srf_num_macs; i++) { |
| memcpy(cmd_data + 1 + i * ETH_ALEN, nan_func->srf_macs[i].addr, ETH_ALEN); |
| } |
| } |
| |
| cmd_data += ALIGN(cmn->srf_len, 4); |
| |
| if (rx_filt_len > 0) { |
| iwl_mvm_copy_filters(nan_func->rx_filters, nan_func->num_rx_filters, cmd_data); |
| } |
| |
| cmn->rx_filter_len = rx_filt_len; |
| cmd_data += ALIGN(cmn->rx_filter_len, 4); |
| |
| if (tx_filt_len > 0) { |
| iwl_mvm_copy_filters(nan_func->tx_filters, nan_func->num_tx_filters, cmd_data); |
| } |
| |
| cmn->tx_filter_len = tx_filt_len; |
| |
| ret = iwl_mvm_send_cmd(mvm, &hcmd); |
| |
| if (ret) { |
| IWL_ERR(mvm, "Couldn't send NAN_DISCOVERY_FUNC_CMD: %d\n", ret); |
| goto out_free; |
| } |
| |
| pkt = hcmd.resp_pkt; |
| |
| if (WARN_ON(iwl_rx_packet_payload_len(pkt) != sizeof(*resp))) { |
| ret = -EIO; |
| goto out_free_resp; |
| } |
| |
| resp = (void*)pkt->data; |
| |
| IWL_DEBUG_MAC80211(mvm, "Add NAN func response status: %d, instance_id: %d\n", resp->status, |
| resp->instance_id); |
| |
| if (resp->status == IWL_NAN_DE_FUNC_STATUS_INSUFFICIENT_ENTRIES || |
| resp->status == IWL_NAN_DE_FUNC_STATUS_INSUFFICIENT_MEMORY) { |
| ret = -ENOBUFS; |
| goto out_free_resp; |
| } |
| |
| if (resp->status != IWL_NAN_DE_FUNC_STATUS_SUCCESSFUL) { |
| ret = -EIO; |
| goto out_free_resp; |
| } |
| |
| if (cmn->instance_id && WARN_ON(resp->instance_id != cmn->instance_id)) { |
| ret = -EIO; |
| goto out_free_resp; |
| } |
| |
| ret = 0; |
| out_free_resp: |
| iwl_free_resp(&hcmd); |
| out_free: |
| kfree(cmd); |
| unlock: |
| mutex_unlock(&mvm->mutex); |
| return ret; |
| } |
| |
| void iwl_mvm_del_nan_func(struct ieee80211_hw* hw, struct ieee80211_vif* vif, uint8_t instance_id) { |
| struct iwl_mvm* mvm = IWL_MAC80211_GET_MVM(hw); |
| void* cmd; |
| struct iwl_nan_add_func_common* cmn; |
| int ret; |
| |
| IWL_DEBUG_MAC80211(IWL_MAC80211_GET_MVM(hw), "Remove NAN func\n"); |
| |
| cmd = kzalloc(iwl_mvm_nan_add_func_cmd_len(hw), GFP_KERNEL); |
| if (!cmd) { |
| IWL_ERR(mvm, "Failed to allocate command to remove NAN func instance_id: %d\n", instance_id); |
| return; |
| } |
| |
| cmn = iwl_mvm_nan_get_add_func_common(hw, cmd); |
| |
| mutex_lock(&mvm->mutex); |
| cmn->action = cpu_to_le32(FW_CTXT_ACTION_REMOVE); |
| cmn->instance_id = instance_id; |
| |
| ret = iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(NAN_DISCOVERY_FUNC_CMD, NAN_GROUP, 0), 0, |
| iwl_mvm_nan_add_func_cmd_len(hw), cmd); |
| if (ret) { |
| IWL_ERR(mvm, "Failed to remove NAN func instance_id: %d\n", instance_id); |
| } |
| |
| mutex_unlock(&mvm->mutex); |
| kfree(cmd); |
| } |
| |
| static uint8_t iwl_cfg_nan_func_type(uint8_t fw_type) { |
| switch (fw_type) { |
| case IWL_NAN_DE_FUNC_PUBLISH: |
| return NL80211_NAN_FUNC_PUBLISH; |
| case IWL_NAN_DE_FUNC_SUBSCRIBE: |
| return NL80211_NAN_FUNC_SUBSCRIBE; |
| case IWL_NAN_DE_FUNC_FOLLOW_UP: |
| return NL80211_NAN_FUNC_FOLLOW_UP; |
| default: |
| return NL80211_NAN_FUNC_MAX_TYPE + 1; |
| } |
| } |
| |
| static void iwl_mvm_nan_match_v1(struct iwl_mvm* mvm, struct iwl_rx_cmd_buffer* rxb) { |
| struct iwl_rx_packet* pkt = rxb_addr(rxb); |
| struct iwl_nan_disc_evt_notify_v1* ev = (void*)pkt->data; |
| struct cfg80211_nan_match_params match = {0}; |
| int len = iwl_rx_packet_payload_len(pkt); |
| |
| if (WARN_ON_ONCE(!mvm->nan_vif)) { |
| IWL_ERR(mvm, "NAN vif is NULL\n"); |
| return; |
| } |
| |
| if (WARN_ON_ONCE(len < sizeof(*ev))) { |
| IWL_ERR(mvm, "Invalid NAN match event length: %d\n", len); |
| return; |
| } |
| |
| if (WARN_ON_ONCE(len < sizeof(*ev) + ev->service_info_len)) { |
| IWL_ERR(mvm, "Invalid NAN match event length: %d, info_len: %d\n", len, ev->service_info_len); |
| return; |
| } |
| |
| match.type = iwl_cfg_nan_func_type(ev->type); |
| |
| if (WARN_ON_ONCE(match.type > NL80211_NAN_FUNC_MAX_TYPE)) { |
| IWL_ERR(mvm, "Invalid func type\n"); |
| return; |
| } |
| |
| match.inst_id = ev->instance_id; |
| match.peer_inst_id = ev->peer_instance; |
| match.addr = ev->peer_mac_addr; |
| match.info = ev->buf; |
| match.info_len = ev->service_info_len; |
| ieee80211_nan_func_match(mvm->nan_vif, &match, GFP_ATOMIC); |
| } |
| |
| static void iwl_mvm_nan_match_v2(struct iwl_mvm* mvm, struct iwl_rx_cmd_buffer* rxb) { |
| struct iwl_rx_packet* pkt = rxb_addr(rxb); |
| struct iwl_nan_disc_evt_notify_v2* ev = (void*)pkt->data; |
| uint32_t len = iwl_rx_packet_payload_len(pkt); |
| uint32_t i = 0; |
| |
| if (WARN_ONCE(!mvm->nan_vif, "NAN vif is NULL")) { |
| return; |
| } |
| |
| if (WARN_ONCE(len < sizeof(*ev), "Invalid NAN match event length=%u", len)) { |
| return; |
| } |
| |
| if (WARN_ONCE(len < sizeof(*ev) + le32_to_cpu(ev->match_len) + le32_to_cpu(ev->avail_attrs_len), |
| "Bad NAN match event: len=%u, match=%u, attrs=%u\n", len, ev->match_len, |
| ev->avail_attrs_len)) { |
| return; |
| } |
| |
| i = 0; |
| while (i < le32_to_cpu(ev->match_len)) { |
| struct cfg80211_nan_match_params match = {0}; |
| struct iwl_nan_disc_info* disc_info = (struct iwl_nan_disc_info*)(((uint8_t*)(ev + 1)) + i); |
| |
| match.type = iwl_cfg_nan_func_type(disc_info->type); |
| match.inst_id = disc_info->instance_id; |
| match.peer_inst_id = disc_info->peer_instance; |
| match.addr = ev->peer_mac_addr; |
| match.info = disc_info->buf; |
| match.info_len = disc_info->service_info_len; |
| ieee80211_nan_func_match(mvm->nan_vif, &match, GFP_ATOMIC); |
| |
| i += ALIGN(sizeof(*disc_info) + disc_info->service_info_len + |
| le16_to_cpu(disc_info->sdea_service_info_len) + |
| le16_to_cpu(disc_info->sec_ctxt_len), |
| 4); |
| } |
| } |
| |
| void iwl_mvm_nan_match(struct iwl_mvm* mvm, struct iwl_rx_cmd_buffer* rxb) { |
| if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_NAN_NOTIF_V2)) { |
| iwl_mvm_nan_match_v2(mvm, rxb); |
| } else { |
| iwl_mvm_nan_match_v1(mvm, rxb); |
| } |
| } |
| |
| void iwl_mvm_nan_de_term_notif(struct iwl_mvm* mvm, struct iwl_rx_cmd_buffer* rxb) { |
| struct iwl_rx_packet* pkt = rxb_addr(rxb); |
| struct iwl_nan_de_term* ev = (void*)pkt->data; |
| int len = iwl_rx_packet_payload_len(pkt); |
| enum nl80211_nan_func_term_reason nl_reason; |
| |
| if (WARN_ON_ONCE(!mvm->nan_vif)) { |
| IWL_ERR(mvm, "NAN vif is NULL\n"); |
| return; |
| } |
| |
| if (WARN_ON_ONCE(len != sizeof(*ev))) { |
| IWL_ERR(mvm, "NAN DE termination event bad length: %d\n", len); |
| return; |
| } |
| |
| switch (ev->reason) { |
| case IWL_NAN_DE_TERM_TTL_REACHED: |
| nl_reason = NL80211_NAN_FUNC_TERM_REASON_TTL_EXPIRED; |
| break; |
| case IWL_NAN_DE_TERM_USER_REQUEST: |
| nl_reason = NL80211_NAN_FUNC_TERM_REASON_USER_REQUEST; |
| break; |
| case IWL_NAN_DE_TERM_FAILURE: |
| nl_reason = NL80211_NAN_FUNC_TERM_REASON_ERROR; |
| break; |
| default: |
| WARN_ON_ONCE(1); |
| return; |
| } |
| |
| ieee80211_nan_func_terminated(mvm->nan_vif, ev->instance_id, nl_reason, GFP_ATOMIC); |
| } |
| |
| int iwl_mvm_nan_config_nan_faw_cmd(struct iwl_mvm* mvm, struct cfg80211_chan_def* chandef, |
| uint8_t slots) { |
| struct iwl_nan_faw_config cmd = {}; |
| struct iwl_mvm_vif* mvmvif; |
| int ret; |
| |
| if (WARN_ON(!mvm->nan_vif)) { |
| return -EINVAL; |
| } |
| |
| mutex_lock(&mvm->mutex); |
| |
| mvmvif = iwl_mvm_vif_from_mac80211(mvm->nan_vif); |
| |
| /* Set the channel info data */ |
| cmd.faw_ci.band = (chandef->chan->band == NL80211_BAND_2GHZ ? PHY_BAND_24 : PHY_BAND_5); |
| |
| cmd.faw_ci.channel = chandef->chan->hw_value; |
| cmd.faw_ci.width = iwl_mvm_get_channel_width(chandef); |
| cmd.faw_ci.ctrl_pos = iwl_mvm_get_ctrl_pos(chandef); |
| ieee80211_chandef_to_operating_class(chandef, &cmd.op_class); |
| cmd.slots = slots; |
| cmd.type = IWL_NAN_POST_NAN_ATTR_FURTHER_NAN; |
| cmd.id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color)); |
| |
| ret = |
| iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(NAN_FAW_CONFIG_CMD, NAN_GROUP, 0), 0, sizeof(cmd), &cmd); |
| |
| mutex_unlock(&mvm->mutex); |
| |
| return ret; |
| } |