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