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