517 lines · c
1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause2/*3 * Copyright (C) 2015-2017 Intel Deutschland GmbH4 * Copyright (C) 2018-2024 Intel Corporation5 */6#include <net/cfg80211.h>7#include <linux/etherdevice.h>8#include "mvm.h"9#include "constants.h"10 11struct iwl_mvm_pasn_sta {12 struct list_head list;13 struct iwl_mvm_int_sta int_sta;14 u8 addr[ETH_ALEN];15 16 /* must be last as it followed by buffer holding the key */17 struct ieee80211_key_conf keyconf;18};19 20struct iwl_mvm_pasn_hltk_data {21 u8 *addr;22 u8 cipher;23 u8 *hltk;24};25 26static int iwl_mvm_ftm_responder_set_bw_v1(struct cfg80211_chan_def *chandef,27 u8 *bw, u8 *ctrl_ch_position)28{29 switch (chandef->width) {30 case NL80211_CHAN_WIDTH_20_NOHT:31 *bw = IWL_TOF_BW_20_LEGACY;32 break;33 case NL80211_CHAN_WIDTH_20:34 *bw = IWL_TOF_BW_20_HT;35 break;36 case NL80211_CHAN_WIDTH_40:37 *bw = IWL_TOF_BW_40;38 *ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);39 break;40 case NL80211_CHAN_WIDTH_80:41 *bw = IWL_TOF_BW_80;42 *ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);43 break;44 default:45 return -EOPNOTSUPP;46 }47 48 return 0;49}50 51static int iwl_mvm_ftm_responder_set_bw_v2(struct cfg80211_chan_def *chandef,52 u8 *format_bw, u8 *ctrl_ch_position,53 u8 cmd_ver)54{55 switch (chandef->width) {56 case NL80211_CHAN_WIDTH_20_NOHT:57 *format_bw = IWL_LOCATION_FRAME_FORMAT_LEGACY;58 *format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS;59 break;60 case NL80211_CHAN_WIDTH_20:61 *format_bw = IWL_LOCATION_FRAME_FORMAT_HT;62 *format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS;63 break;64 case NL80211_CHAN_WIDTH_40:65 *format_bw = IWL_LOCATION_FRAME_FORMAT_HT;66 *format_bw |= IWL_LOCATION_BW_40MHZ << LOCATION_BW_POS;67 *ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);68 break;69 case NL80211_CHAN_WIDTH_80:70 *format_bw = IWL_LOCATION_FRAME_FORMAT_VHT;71 *format_bw |= IWL_LOCATION_BW_80MHZ << LOCATION_BW_POS;72 *ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);73 break;74 case NL80211_CHAN_WIDTH_160:75 if (cmd_ver >= 9) {76 *format_bw = IWL_LOCATION_FRAME_FORMAT_HE;77 *format_bw |= IWL_LOCATION_BW_160MHZ << LOCATION_BW_POS;78 *ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);79 break;80 }81 fallthrough;82 default:83 return -EOPNOTSUPP;84 }85 86 return 0;87}88 89static void90iwl_mvm_ftm_responder_set_ndp(struct iwl_mvm *mvm,91 struct iwl_tof_responder_config_cmd *cmd)92{93 /* Up to 2 R2I STS are allowed on the responder */94 u32 r2i_max_sts = IWL_MVM_FTM_R2I_MAX_STS < 2 ?95 IWL_MVM_FTM_R2I_MAX_STS : 1;96 97 cmd->r2i_ndp_params = IWL_MVM_FTM_R2I_MAX_REP |98 (r2i_max_sts << IWL_RESPONDER_STS_POS) |99 (IWL_MVM_FTM_R2I_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);100 cmd->i2r_ndp_params = IWL_MVM_FTM_I2R_MAX_REP |101 (IWL_MVM_FTM_I2R_MAX_STS << IWL_RESPONDER_STS_POS) |102 (IWL_MVM_FTM_I2R_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);103 cmd->cmd_valid_fields |=104 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_NDP_PARAMS);105}106 107static int108iwl_mvm_ftm_responder_cmd(struct iwl_mvm *mvm,109 struct ieee80211_vif *vif,110 struct cfg80211_chan_def *chandef,111 struct ieee80211_bss_conf *link_conf)112{113 u32 cmd_id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_CONFIG_CMD);114 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);115 /*116 * The command structure is the same for versions 6, 7 and 8 (only the117 * field interpretation is different), so the same struct can be use118 * for all cases.119 */120 struct iwl_tof_responder_config_cmd cmd = {121 .channel_num = chandef->chan->hw_value,122 .cmd_valid_fields =123 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_CHAN_INFO |124 IWL_TOF_RESPONDER_CMD_VALID_BSSID |125 IWL_TOF_RESPONDER_CMD_VALID_STA_ID),126 .sta_id = mvmvif->link[link_conf->link_id]->bcast_sta.sta_id,127 };128 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 6);129 int err;130 int cmd_size;131 132 lockdep_assert_held(&mvm->mutex);133 134 if (cmd_ver == 10) {135 cmd.band =136 iwl_mvm_phy_band_from_nl80211(chandef->chan->band);137 }138 139 /* Use a default of bss_color=1 for now */140 if (cmd_ver >= 9) {141 cmd.cmd_valid_fields |=142 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_BSS_COLOR |143 IWL_TOF_RESPONDER_CMD_VALID_MIN_MAX_TIME_BETWEEN_MSR);144 cmd.bss_color = 1;145 cmd.min_time_between_msr =146 cpu_to_le16(IWL_MVM_FTM_NON_TB_MIN_TIME_BETWEEN_MSR);147 cmd.max_time_between_msr =148 cpu_to_le16(IWL_MVM_FTM_NON_TB_MAX_TIME_BETWEEN_MSR);149 cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v9);150 } else {151 /* All versions up to version 8 have the same size */152 cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v8);153 }154 155 if (cmd_ver >= 8)156 iwl_mvm_ftm_responder_set_ndp(mvm, (void *)&cmd);157 158 if (cmd_ver >= 7)159 err = iwl_mvm_ftm_responder_set_bw_v2(chandef, &cmd.format_bw,160 &cmd.ctrl_ch_position,161 cmd_ver);162 else163 err = iwl_mvm_ftm_responder_set_bw_v1(chandef, &cmd.format_bw,164 &cmd.ctrl_ch_position);165 166 if (err) {167 IWL_ERR(mvm, "Failed to set responder bandwidth\n");168 return err;169 }170 171 memcpy(cmd.bssid, vif->addr, ETH_ALEN);172 173 return iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0, cmd_size, &cmd);174}175 176static int177iwl_mvm_ftm_responder_dyn_cfg_v2(struct iwl_mvm *mvm,178 struct ieee80211_vif *vif,179 struct ieee80211_ftm_responder_params *params)180{181 struct iwl_tof_responder_dyn_config_cmd_v2 cmd = {182 .lci_len = cpu_to_le32(params->lci_len + 2),183 .civic_len = cpu_to_le32(params->civicloc_len + 2),184 };185 u8 data[IWL_LCI_CIVIC_IE_MAX_SIZE] = {0};186 struct iwl_host_cmd hcmd = {187 .id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),188 .data[0] = &cmd,189 .len[0] = sizeof(cmd),190 .data[1] = &data,191 /* .len[1] set later */192 /* may not be able to DMA from stack */193 .dataflags[1] = IWL_HCMD_DFL_DUP,194 };195 u32 aligned_lci_len = ALIGN(params->lci_len + 2, 4);196 u32 aligned_civicloc_len = ALIGN(params->civicloc_len + 2, 4);197 u8 *pos = data;198 199 lockdep_assert_held(&mvm->mutex);200 201 if (aligned_lci_len + aligned_civicloc_len > sizeof(data)) {202 IWL_ERR(mvm, "LCI/civicloc data too big (%zd + %zd)\n",203 params->lci_len, params->civicloc_len);204 return -ENOBUFS;205 }206 207 pos[0] = WLAN_EID_MEASURE_REPORT;208 pos[1] = params->lci_len;209 memcpy(pos + 2, params->lci, params->lci_len);210 211 pos += aligned_lci_len;212 pos[0] = WLAN_EID_MEASURE_REPORT;213 pos[1] = params->civicloc_len;214 memcpy(pos + 2, params->civicloc, params->civicloc_len);215 216 hcmd.len[1] = aligned_lci_len + aligned_civicloc_len;217 218 return iwl_mvm_send_cmd(mvm, &hcmd);219}220 221static int222iwl_mvm_ftm_responder_dyn_cfg_v3(struct iwl_mvm *mvm,223 struct ieee80211_vif *vif,224 struct ieee80211_ftm_responder_params *params,225 struct iwl_mvm_pasn_hltk_data *hltk_data)226{227 struct iwl_tof_responder_dyn_config_cmd cmd;228 struct iwl_host_cmd hcmd = {229 .id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),230 .data[0] = &cmd,231 .len[0] = sizeof(cmd),232 /* may not be able to DMA from stack */233 .dataflags[0] = IWL_HCMD_DFL_DUP,234 };235 236 lockdep_assert_held(&mvm->mutex);237 238 cmd.valid_flags = 0;239 240 if (params) {241 if (params->lci_len + 2 > sizeof(cmd.lci_buf) ||242 params->civicloc_len + 2 > sizeof(cmd.civic_buf)) {243 IWL_ERR(mvm,244 "LCI/civic data too big (lci=%zd, civic=%zd)\n",245 params->lci_len, params->civicloc_len);246 return -ENOBUFS;247 }248 249 cmd.lci_buf[0] = WLAN_EID_MEASURE_REPORT;250 cmd.lci_buf[1] = params->lci_len;251 memcpy(cmd.lci_buf + 2, params->lci, params->lci_len);252 cmd.lci_len = params->lci_len + 2;253 254 cmd.civic_buf[0] = WLAN_EID_MEASURE_REPORT;255 cmd.civic_buf[1] = params->civicloc_len;256 memcpy(cmd.civic_buf + 2, params->civicloc,257 params->civicloc_len);258 cmd.civic_len = params->civicloc_len + 2;259 260 cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_LCI |261 IWL_RESPONDER_DYN_CFG_VALID_CIVIC;262 }263 264 if (hltk_data) {265 if (hltk_data->cipher > IWL_LOCATION_CIPHER_GCMP_256) {266 IWL_ERR(mvm, "invalid cipher: %u\n",267 hltk_data->cipher);268 return -EINVAL;269 }270 271 cmd.cipher = hltk_data->cipher;272 memcpy(cmd.addr, hltk_data->addr, sizeof(cmd.addr));273 memcpy(cmd.hltk_buf, hltk_data->hltk, sizeof(cmd.hltk_buf));274 cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_PASN_STA;275 }276 277 return iwl_mvm_send_cmd(mvm, &hcmd);278}279 280static int281iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,282 struct ieee80211_vif *vif,283 struct ieee80211_ftm_responder_params *params)284{285 int ret;286 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,287 WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),288 2);289 290 switch (cmd_ver) {291 case 2:292 ret = iwl_mvm_ftm_responder_dyn_cfg_v2(mvm, vif,293 params);294 break;295 case 3:296 ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif,297 params, NULL);298 break;299 default:300 IWL_ERR(mvm, "Unsupported DYN_CONFIG_CMD version %u\n",301 cmd_ver);302 ret = -EOPNOTSUPP;303 }304 305 return ret;306}307 308static void iwl_mvm_resp_del_pasn_sta(struct iwl_mvm *mvm,309 struct ieee80211_vif *vif,310 struct iwl_mvm_pasn_sta *sta)311{312 list_del(&sta->list);313 314 if (sta->keyconf.keylen)315 iwl_mvm_sec_key_del_pasn(mvm, vif, BIT(sta->int_sta.sta_id),316 &sta->keyconf);317 318 if (iwl_mvm_has_mld_api(mvm->fw))319 iwl_mvm_mld_rm_sta_id(mvm, sta->int_sta.sta_id);320 else321 iwl_mvm_rm_sta_id(mvm, vif, sta->int_sta.sta_id);322 323 iwl_mvm_dealloc_int_sta(mvm, &sta->int_sta);324 kfree(sta);325}326 327int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,328 struct ieee80211_vif *vif,329 u8 *addr, u32 cipher, u8 *tk, u32 tk_len,330 u8 *hltk, u32 hltk_len)331{332 int ret;333 struct iwl_mvm_pasn_sta *sta = NULL;334 struct iwl_mvm_pasn_hltk_data hltk_data = {335 .addr = addr,336 .hltk = hltk,337 };338 struct iwl_mvm_pasn_hltk_data *hltk_data_ptr = NULL;339 340 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,341 WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),342 2);343 344 lockdep_assert_held(&mvm->mutex);345 346 if (cmd_ver < 3) {347 IWL_ERR(mvm, "Adding PASN station not supported by FW\n");348 return -EOPNOTSUPP;349 }350 351 if ((!hltk || !hltk_len) && (!tk || !tk_len)) {352 IWL_ERR(mvm, "TK and HLTK not set\n");353 return -EINVAL;354 }355 356 if (hltk && hltk_len) {357 if (!fw_has_capa(&mvm->fw->ucode_capa,358 IWL_UCODE_TLV_CAPA_SECURE_LTF_SUPPORT)) {359 IWL_ERR(mvm, "No support for secure LTF measurement\n");360 return -EINVAL;361 }362 363 hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);364 if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {365 IWL_ERR(mvm, "invalid cipher: %u\n", cipher);366 return -EINVAL;367 }368 369 hltk_data_ptr = &hltk_data;370 }371 372 if (tk && tk_len) {373 sta = kzalloc(sizeof(*sta) + tk_len, GFP_KERNEL);374 if (!sta)375 return -ENOBUFS;376 377 ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,378 cipher, tk, tk_len, &sta->keyconf);379 if (ret) {380 kfree(sta);381 return ret;382 }383 384 memcpy(sta->addr, addr, ETH_ALEN);385 list_add_tail(&sta->list, &mvm->resp_pasn_list);386 }387 388 ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, hltk_data_ptr);389 if (ret && sta)390 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);391 392 return ret;393}394 395int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,396 struct ieee80211_vif *vif, u8 *addr)397{398 struct iwl_mvm_pasn_sta *sta, *prev;399 400 lockdep_assert_held(&mvm->mutex);401 402 list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {403 if (!memcmp(sta->addr, addr, ETH_ALEN)) {404 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);405 return 0;406 }407 }408 409 IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);410 return -EINVAL;411}412 413int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif,414 struct ieee80211_bss_conf *bss_conf)415{416 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);417 struct ieee80211_ftm_responder_params *params;418 struct ieee80211_chanctx_conf ctx, *pctx;419 u16 *phy_ctxt_id;420 struct iwl_mvm_phy_ctxt *phy_ctxt;421 int ret;422 423 params = bss_conf->ftmr_params;424 425 lockdep_assert_held(&mvm->mutex);426 427 if (WARN_ON_ONCE(!bss_conf->ftm_responder))428 return -EINVAL;429 430 if (vif->p2p || vif->type != NL80211_IFTYPE_AP ||431 !mvmvif->ap_ibss_active) {432 IWL_ERR(mvm, "Cannot start responder, not in AP mode\n");433 return -EIO;434 }435 436 rcu_read_lock();437 pctx = rcu_dereference(bss_conf->chanctx_conf);438 /* Copy the ctx to unlock the rcu and send the phy ctxt. We don't care439 * about changes in the ctx after releasing the lock because the driver440 * is still protected by the mutex. */441 ctx = *pctx;442 phy_ctxt_id = (u16 *)pctx->drv_priv;443 rcu_read_unlock();444 445 phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];446 ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx.def, &ctx.ap,447 ctx.rx_chains_static,448 ctx.rx_chains_dynamic);449 if (ret)450 return ret;451 452 ret = iwl_mvm_ftm_responder_cmd(mvm, vif, &ctx.def, bss_conf);453 if (ret)454 return ret;455 456 if (params)457 ret = iwl_mvm_ftm_responder_dyn_cfg_cmd(mvm, vif, params);458 459 return ret;460}461 462void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,463 struct ieee80211_vif *vif)464{465 struct iwl_mvm_pasn_sta *sta, *prev;466 467 lockdep_assert_held(&mvm->mutex);468 469 list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)470 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);471}472 473void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,474 struct ieee80211_vif *vif,475 struct ieee80211_bss_conf *bss_conf)476{477 if (!bss_conf->ftm_responder)478 return;479 480 iwl_mvm_ftm_responder_clear(mvm, vif);481 iwl_mvm_ftm_start_responder(mvm, vif, bss_conf);482}483 484void iwl_mvm_ftm_responder_stats(struct iwl_mvm *mvm,485 struct iwl_rx_cmd_buffer *rxb)486{487 struct iwl_rx_packet *pkt = rxb_addr(rxb);488 struct iwl_ftm_responder_stats *resp = (void *)pkt->data;489 struct cfg80211_ftm_responder_stats *stats = &mvm->ftm_resp_stats;490 u32 flags = le32_to_cpu(resp->flags);491 492 if (resp->success_ftm == resp->ftm_per_burst)493 stats->success_num++;494 else if (resp->success_ftm >= 2)495 stats->partial_num++;496 else497 stats->failed_num++;498 499 if ((flags & FTM_RESP_STAT_ASAP_REQ) &&500 (flags & FTM_RESP_STAT_ASAP_RESP))501 stats->asap_num++;502 503 if (flags & FTM_RESP_STAT_NON_ASAP_RESP)504 stats->non_asap_num++;505 506 stats->total_duration_ms += le32_to_cpu(resp->duration) / USEC_PER_MSEC;507 508 if (flags & FTM_RESP_STAT_TRIGGER_UNKNOWN)509 stats->unknown_triggers_num++;510 511 if (flags & FTM_RESP_STAT_DUP)512 stats->reschedule_requests_num++;513 514 if (flags & FTM_RESP_STAT_NON_ASAP_OUT_WIN)515 stats->out_of_window_triggers_num++;516}517