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