1 /****************************************************************************** 2 * 3 * This file is provided under a dual BSD/GPLv2 license. When using or 4 * redistributing this file, you may do so under either license. 5 * 6 * GPL LICENSE SUMMARY 7 * 8 * Copyright(c) 2015 - 2017 Intel Deutschland GmbH 9 * Copyright (C) 2018 Intel Corporation 10 * Copyright (C) 2019 Intel Corporation 11 * Copyright (C) 2020 Intel Corporation 12 * 13 * This program is free software; you can redistribute it and/or modify 14 * it under the terms of version 2 of the GNU General Public License as 15 * published by the Free Software Foundation. 16 * 17 * This program is distributed in the hope that it will be useful, but 18 * WITHOUT ANY WARRANTY; without even the implied warranty of 19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 20 * General Public License for more details. 21 * 22 * The full GNU General Public License is included in this distribution 23 * in the file called COPYING. 24 * 25 * Contact Information: 26 * Intel Linux Wireless <linuxwifi@intel.com> 27 * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497 28 * 29 * BSD LICENSE 30 * 31 * Copyright(c) 2015 - 2017 Intel Deutschland GmbH 32 * Copyright (C) 2018 Intel Corporation 33 * Copyright (C) 2019 Intel Corporation 34 * Copyright (C) 2020 Intel Corporation 35 * All rights reserved. 36 * 37 * Redistribution and use in source and binary forms, with or without 38 * modification, are permitted provided that the following conditions 39 * are met: 40 * 41 * * Redistributions of source code must retain the above copyright 42 * notice, this list of conditions and the following disclaimer. 43 * * Redistributions in binary form must reproduce the above copyright 44 * notice, this list of conditions and the following disclaimer in 45 * the documentation and/or other materials provided with the 46 * distribution. 47 * * Neither the name Intel Corporation nor the names of its 48 * contributors may be used to endorse or promote products derived 49 * from this software without specific prior written permission. 50 * 51 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 52 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 53 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 54 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 55 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 56 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 57 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 58 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 59 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 60 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 61 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 62 * 63 *****************************************************************************/ 64 #include <linux/etherdevice.h> 65 #include <linux/math64.h> 66 #include <net/cfg80211.h> 67 #include "mvm.h" 68 #include "iwl-io.h" 69 #include "iwl-prph.h" 70 #include "constants.h" 71 72 struct iwl_mvm_loc_entry { 73 struct list_head list; 74 u8 addr[ETH_ALEN]; 75 u8 lci_len, civic_len; 76 u8 buf[]; 77 }; 78 79 struct iwl_mvm_smooth_entry { 80 struct list_head list; 81 u8 addr[ETH_ALEN]; 82 s64 rtt_avg; 83 u64 host_time; 84 }; 85 86 struct iwl_mvm_ftm_pasn_entry { 87 struct list_head list; 88 u8 addr[ETH_ALEN]; 89 u8 hltk[HLTK_11AZ_LEN]; 90 u8 tk[TK_11AZ_LEN]; 91 u8 cipher; 92 u8 tx_pn[IEEE80211_CCMP_PN_LEN]; 93 u8 rx_pn[IEEE80211_CCMP_PN_LEN]; 94 }; 95 96 int iwl_mvm_ftm_add_pasn_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 97 u8 *addr, u32 cipher, u8 *tk, u32 tk_len, 98 u8 *hltk, u32 hltk_len) 99 { 100 struct iwl_mvm_ftm_pasn_entry *pasn = kzalloc(sizeof(*pasn), 101 GFP_KERNEL); 102 u32 expected_tk_len; 103 104 lockdep_assert_held(&mvm->mutex); 105 106 if (!pasn) 107 return -ENOBUFS; 108 109 pasn->cipher = iwl_mvm_cipher_to_location_cipher(cipher); 110 111 switch (pasn->cipher) { 112 case IWL_LOCATION_CIPHER_CCMP_128: 113 case IWL_LOCATION_CIPHER_GCMP_128: 114 expected_tk_len = WLAN_KEY_LEN_CCMP; 115 break; 116 case IWL_LOCATION_CIPHER_GCMP_256: 117 expected_tk_len = WLAN_KEY_LEN_GCMP_256; 118 break; 119 default: 120 goto out; 121 } 122 123 /* 124 * If associated to this AP and already have security context, 125 * the TK is already configured for this station, so it 126 * shouldn't be set again here. 127 */ 128 if (vif->bss_conf.assoc && 129 !memcmp(addr, vif->bss_conf.bssid, ETH_ALEN)) { 130 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 131 struct ieee80211_sta *sta; 132 133 rcu_read_lock(); 134 sta = rcu_dereference(mvm->fw_id_to_mac_id[mvmvif->ap_sta_id]); 135 if (!IS_ERR_OR_NULL(sta) && sta->mfp) 136 expected_tk_len = 0; 137 rcu_read_unlock(); 138 } 139 140 if (tk_len != expected_tk_len || hltk_len != sizeof(pasn->hltk)) { 141 IWL_ERR(mvm, "Invalid key length: tk_len=%u hltk_len=%u\n", 142 tk_len, hltk_len); 143 goto out; 144 } 145 146 memcpy(pasn->addr, addr, sizeof(pasn->addr)); 147 memcpy(pasn->hltk, hltk, sizeof(pasn->hltk)); 148 149 if (tk && tk_len) 150 memcpy(pasn->tk, tk, sizeof(pasn->tk)); 151 152 list_add_tail(&pasn->list, &mvm->ftm_initiator.pasn_list); 153 return 0; 154 out: 155 kfree(pasn); 156 return -EINVAL; 157 } 158 159 void iwl_mvm_ftm_remove_pasn_sta(struct iwl_mvm *mvm, u8 *addr) 160 { 161 struct iwl_mvm_ftm_pasn_entry *entry, *prev; 162 163 lockdep_assert_held(&mvm->mutex); 164 165 list_for_each_entry_safe(entry, prev, &mvm->ftm_initiator.pasn_list, 166 list) { 167 if (memcmp(entry->addr, addr, sizeof(entry->addr))) 168 continue; 169 170 list_del(&entry->list); 171 kfree(entry); 172 return; 173 } 174 } 175 176 static void iwl_mvm_ftm_reset(struct iwl_mvm *mvm) 177 { 178 struct iwl_mvm_loc_entry *e, *t; 179 180 mvm->ftm_initiator.req = NULL; 181 mvm->ftm_initiator.req_wdev = NULL; 182 memset(mvm->ftm_initiator.responses, 0, 183 sizeof(mvm->ftm_initiator.responses)); 184 185 list_for_each_entry_safe(e, t, &mvm->ftm_initiator.loc_list, list) { 186 list_del(&e->list); 187 kfree(e); 188 } 189 } 190 191 void iwl_mvm_ftm_restart(struct iwl_mvm *mvm) 192 { 193 struct cfg80211_pmsr_result result = { 194 .status = NL80211_PMSR_STATUS_FAILURE, 195 .final = 1, 196 .host_time = ktime_get_boottime_ns(), 197 .type = NL80211_PMSR_TYPE_FTM, 198 }; 199 int i; 200 201 lockdep_assert_held(&mvm->mutex); 202 203 if (!mvm->ftm_initiator.req) 204 return; 205 206 for (i = 0; i < mvm->ftm_initiator.req->n_peers; i++) { 207 memcpy(result.addr, mvm->ftm_initiator.req->peers[i].addr, 208 ETH_ALEN); 209 result.ftm.burst_index = mvm->ftm_initiator.responses[i]; 210 211 cfg80211_pmsr_report(mvm->ftm_initiator.req_wdev, 212 mvm->ftm_initiator.req, 213 &result, GFP_KERNEL); 214 } 215 216 cfg80211_pmsr_complete(mvm->ftm_initiator.req_wdev, 217 mvm->ftm_initiator.req, GFP_KERNEL); 218 iwl_mvm_ftm_reset(mvm); 219 } 220 221 void iwl_mvm_ftm_initiator_smooth_config(struct iwl_mvm *mvm) 222 { 223 INIT_LIST_HEAD(&mvm->ftm_initiator.smooth.resp); 224 225 IWL_DEBUG_INFO(mvm, 226 "enable=%u, alpha=%u, age_jiffies=%u, thresh=(%u:%u)\n", 227 IWL_MVM_FTM_INITIATOR_ENABLE_SMOOTH, 228 IWL_MVM_FTM_INITIATOR_SMOOTH_ALPHA, 229 IWL_MVM_FTM_INITIATOR_SMOOTH_AGE_SEC * HZ, 230 IWL_MVM_FTM_INITIATOR_SMOOTH_OVERSHOOT, 231 IWL_MVM_FTM_INITIATOR_SMOOTH_UNDERSHOOT); 232 } 233 234 void iwl_mvm_ftm_initiator_smooth_stop(struct iwl_mvm *mvm) 235 { 236 struct iwl_mvm_smooth_entry *se, *st; 237 238 list_for_each_entry_safe(se, st, &mvm->ftm_initiator.smooth.resp, 239 list) { 240 list_del(&se->list); 241 kfree(se); 242 } 243 } 244 245 static int 246 iwl_ftm_range_request_status_to_err(enum iwl_tof_range_request_status s) 247 { 248 switch (s) { 249 case IWL_TOF_RANGE_REQUEST_STATUS_SUCCESS: 250 return 0; 251 case IWL_TOF_RANGE_REQUEST_STATUS_BUSY: 252 return -EBUSY; 253 default: 254 WARN_ON_ONCE(1); 255 return -EIO; 256 } 257 } 258 259 static void iwl_mvm_ftm_cmd_v5(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 260 struct iwl_tof_range_req_cmd_v5 *cmd, 261 struct cfg80211_pmsr_request *req) 262 { 263 int i; 264 265 cmd->request_id = req->cookie; 266 cmd->num_of_ap = req->n_peers; 267 268 /* use maximum for "no timeout" or bigger than what we can do */ 269 if (!req->timeout || req->timeout > 255 * 100) 270 cmd->req_timeout = 255; 271 else 272 cmd->req_timeout = DIV_ROUND_UP(req->timeout, 100); 273 274 /* 275 * We treat it always as random, since if not we'll 276 * have filled our local address there instead. 277 */ 278 cmd->macaddr_random = 1; 279 memcpy(cmd->macaddr_template, req->mac_addr, ETH_ALEN); 280 for (i = 0; i < ETH_ALEN; i++) 281 cmd->macaddr_mask[i] = ~req->mac_addr_mask[i]; 282 283 if (vif->bss_conf.assoc) 284 memcpy(cmd->range_req_bssid, vif->bss_conf.bssid, ETH_ALEN); 285 else 286 eth_broadcast_addr(cmd->range_req_bssid); 287 } 288 289 static void iwl_mvm_ftm_cmd_common(struct iwl_mvm *mvm, 290 struct ieee80211_vif *vif, 291 struct iwl_tof_range_req_cmd_v9 *cmd, 292 struct cfg80211_pmsr_request *req) 293 { 294 int i; 295 296 cmd->initiator_flags = 297 cpu_to_le32(IWL_TOF_INITIATOR_FLAGS_MACADDR_RANDOM | 298 IWL_TOF_INITIATOR_FLAGS_NON_ASAP_SUPPORT); 299 cmd->request_id = req->cookie; 300 cmd->num_of_ap = req->n_peers; 301 302 /* 303 * Use a large value for "no timeout". Don't use the maximum value 304 * because of fw limitations. 305 */ 306 if (req->timeout) 307 cmd->req_timeout_ms = cpu_to_le32(req->timeout); 308 else 309 cmd->req_timeout_ms = cpu_to_le32(0xfffff); 310 311 memcpy(cmd->macaddr_template, req->mac_addr, ETH_ALEN); 312 for (i = 0; i < ETH_ALEN; i++) 313 cmd->macaddr_mask[i] = ~req->mac_addr_mask[i]; 314 315 if (vif->bss_conf.assoc) { 316 memcpy(cmd->range_req_bssid, vif->bss_conf.bssid, ETH_ALEN); 317 318 /* AP's TSF is only relevant if associated */ 319 for (i = 0; i < req->n_peers; i++) { 320 if (req->peers[i].report_ap_tsf) { 321 struct iwl_mvm_vif *mvmvif = 322 iwl_mvm_vif_from_mac80211(vif); 323 324 cmd->tsf_mac_id = cpu_to_le32(mvmvif->id); 325 return; 326 } 327 } 328 } else { 329 eth_broadcast_addr(cmd->range_req_bssid); 330 } 331 332 /* Don't report AP's TSF */ 333 cmd->tsf_mac_id = cpu_to_le32(0xff); 334 } 335 336 static void iwl_mvm_ftm_cmd_v8(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 337 struct iwl_tof_range_req_cmd_v8 *cmd, 338 struct cfg80211_pmsr_request *req) 339 { 340 iwl_mvm_ftm_cmd_common(mvm, vif, (void *)cmd, req); 341 } 342 343 static int 344 iwl_mvm_ftm_target_chandef_v1(struct iwl_mvm *mvm, 345 struct cfg80211_pmsr_request_peer *peer, 346 u8 *channel, u8 *bandwidth, 347 u8 *ctrl_ch_position) 348 { 349 u32 freq = peer->chandef.chan->center_freq; 350 351 *channel = ieee80211_frequency_to_channel(freq); 352 353 switch (peer->chandef.width) { 354 case NL80211_CHAN_WIDTH_20_NOHT: 355 *bandwidth = IWL_TOF_BW_20_LEGACY; 356 break; 357 case NL80211_CHAN_WIDTH_20: 358 *bandwidth = IWL_TOF_BW_20_HT; 359 break; 360 case NL80211_CHAN_WIDTH_40: 361 *bandwidth = IWL_TOF_BW_40; 362 break; 363 case NL80211_CHAN_WIDTH_80: 364 *bandwidth = IWL_TOF_BW_80; 365 break; 366 default: 367 IWL_ERR(mvm, "Unsupported BW in FTM request (%d)\n", 368 peer->chandef.width); 369 return -EINVAL; 370 } 371 372 *ctrl_ch_position = (peer->chandef.width > NL80211_CHAN_WIDTH_20) ? 373 iwl_mvm_get_ctrl_pos(&peer->chandef) : 0; 374 375 return 0; 376 } 377 378 static int 379 iwl_mvm_ftm_target_chandef_v2(struct iwl_mvm *mvm, 380 struct cfg80211_pmsr_request_peer *peer, 381 u8 *channel, u8 *format_bw, 382 u8 *ctrl_ch_position) 383 { 384 u32 freq = peer->chandef.chan->center_freq; 385 386 *channel = ieee80211_frequency_to_channel(freq); 387 388 switch (peer->chandef.width) { 389 case NL80211_CHAN_WIDTH_20_NOHT: 390 *format_bw = IWL_LOCATION_FRAME_FORMAT_LEGACY; 391 *format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS; 392 break; 393 case NL80211_CHAN_WIDTH_20: 394 *format_bw = IWL_LOCATION_FRAME_FORMAT_HT; 395 *format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS; 396 break; 397 case NL80211_CHAN_WIDTH_40: 398 *format_bw = IWL_LOCATION_FRAME_FORMAT_HT; 399 *format_bw |= IWL_LOCATION_BW_40MHZ << LOCATION_BW_POS; 400 break; 401 case NL80211_CHAN_WIDTH_80: 402 *format_bw = IWL_LOCATION_FRAME_FORMAT_VHT; 403 *format_bw |= IWL_LOCATION_BW_80MHZ << LOCATION_BW_POS; 404 break; 405 default: 406 IWL_ERR(mvm, "Unsupported BW in FTM request (%d)\n", 407 peer->chandef.width); 408 return -EINVAL; 409 } 410 411 /* non EDCA based measurement must use HE preamble */ 412 if (peer->ftm.trigger_based || peer->ftm.non_trigger_based) 413 *format_bw |= IWL_LOCATION_FRAME_FORMAT_HE; 414 415 *ctrl_ch_position = (peer->chandef.width > NL80211_CHAN_WIDTH_20) ? 416 iwl_mvm_get_ctrl_pos(&peer->chandef) : 0; 417 418 return 0; 419 } 420 421 static int 422 iwl_mvm_ftm_put_target_v2(struct iwl_mvm *mvm, 423 struct cfg80211_pmsr_request_peer *peer, 424 struct iwl_tof_range_req_ap_entry_v2 *target) 425 { 426 int ret; 427 428 ret = iwl_mvm_ftm_target_chandef_v1(mvm, peer, &target->channel_num, 429 &target->bandwidth, 430 &target->ctrl_ch_position); 431 if (ret) 432 return ret; 433 434 memcpy(target->bssid, peer->addr, ETH_ALEN); 435 target->burst_period = 436 cpu_to_le16(peer->ftm.burst_period); 437 target->samples_per_burst = peer->ftm.ftms_per_burst; 438 target->num_of_bursts = peer->ftm.num_bursts_exp; 439 target->measure_type = 0; /* regular two-sided FTM */ 440 target->retries_per_sample = peer->ftm.ftmr_retries; 441 target->asap_mode = peer->ftm.asap; 442 target->enable_dyn_ack = IWL_MVM_FTM_INITIATOR_DYNACK; 443 444 if (peer->ftm.request_lci) 445 target->location_req |= IWL_TOF_LOC_LCI; 446 if (peer->ftm.request_civicloc) 447 target->location_req |= IWL_TOF_LOC_CIVIC; 448 449 target->algo_type = IWL_MVM_FTM_INITIATOR_ALGO; 450 451 return 0; 452 } 453 454 #define FTM_PUT_FLAG(flag) (target->initiator_ap_flags |= \ 455 cpu_to_le32(IWL_INITIATOR_AP_FLAGS_##flag)) 456 457 static void 458 iwl_mvm_ftm_put_target_common(struct iwl_mvm *mvm, 459 struct cfg80211_pmsr_request_peer *peer, 460 struct iwl_tof_range_req_ap_entry_v6 *target) 461 { 462 memcpy(target->bssid, peer->addr, ETH_ALEN); 463 target->burst_period = 464 cpu_to_le16(peer->ftm.burst_period); 465 target->samples_per_burst = peer->ftm.ftms_per_burst; 466 target->num_of_bursts = peer->ftm.num_bursts_exp; 467 target->ftmr_max_retries = peer->ftm.ftmr_retries; 468 target->initiator_ap_flags = cpu_to_le32(0); 469 470 if (peer->ftm.asap) 471 FTM_PUT_FLAG(ASAP); 472 473 if (peer->ftm.request_lci) 474 FTM_PUT_FLAG(LCI_REQUEST); 475 476 if (peer->ftm.request_civicloc) 477 FTM_PUT_FLAG(CIVIC_REQUEST); 478 479 if (IWL_MVM_FTM_INITIATOR_DYNACK) 480 FTM_PUT_FLAG(DYN_ACK); 481 482 if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_LINEAR_REG) 483 FTM_PUT_FLAG(ALGO_LR); 484 else if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_FFT) 485 FTM_PUT_FLAG(ALGO_FFT); 486 487 if (peer->ftm.trigger_based) 488 FTM_PUT_FLAG(TB); 489 else if (peer->ftm.non_trigger_based) 490 FTM_PUT_FLAG(NON_TB); 491 } 492 493 static int 494 iwl_mvm_ftm_put_target_v3(struct iwl_mvm *mvm, 495 struct cfg80211_pmsr_request_peer *peer, 496 struct iwl_tof_range_req_ap_entry_v3 *target) 497 { 498 int ret; 499 500 ret = iwl_mvm_ftm_target_chandef_v1(mvm, peer, &target->channel_num, 501 &target->bandwidth, 502 &target->ctrl_ch_position); 503 if (ret) 504 return ret; 505 506 /* 507 * Versions 3 and 4 has some common fields, so 508 * iwl_mvm_ftm_put_target_common() can be used for version 7 too. 509 */ 510 iwl_mvm_ftm_put_target_common(mvm, peer, (void *)target); 511 512 return 0; 513 } 514 515 static int 516 iwl_mvm_ftm_put_target_v4(struct iwl_mvm *mvm, 517 struct cfg80211_pmsr_request_peer *peer, 518 struct iwl_tof_range_req_ap_entry_v4 *target) 519 { 520 int ret; 521 522 ret = iwl_mvm_ftm_target_chandef_v2(mvm, peer, &target->channel_num, 523 &target->format_bw, 524 &target->ctrl_ch_position); 525 if (ret) 526 return ret; 527 528 iwl_mvm_ftm_put_target_common(mvm, peer, (void *)target); 529 530 return 0; 531 } 532 533 static int 534 iwl_mvm_ftm_put_target(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 535 struct cfg80211_pmsr_request_peer *peer, 536 struct iwl_tof_range_req_ap_entry_v6 *target) 537 { 538 int ret; 539 540 ret = iwl_mvm_ftm_target_chandef_v2(mvm, peer, &target->channel_num, 541 &target->format_bw, 542 &target->ctrl_ch_position); 543 if (ret) 544 return ret; 545 546 iwl_mvm_ftm_put_target_common(mvm, peer, target); 547 548 if (vif->bss_conf.assoc && 549 !memcmp(peer->addr, vif->bss_conf.bssid, ETH_ALEN)) { 550 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 551 552 target->sta_id = mvmvif->ap_sta_id; 553 } else { 554 target->sta_id = IWL_MVM_INVALID_STA; 555 } 556 557 /* 558 * TODO: Beacon interval is currently unknown, so use the common value 559 * of 100 TUs. 560 */ 561 target->beacon_interval = cpu_to_le16(100); 562 return 0; 563 } 564 565 static int iwl_mvm_ftm_send_cmd(struct iwl_mvm *mvm, struct iwl_host_cmd *hcmd) 566 { 567 u32 status; 568 int err = iwl_mvm_send_cmd_status(mvm, hcmd, &status); 569 570 if (!err && status) { 571 IWL_ERR(mvm, "FTM range request command failure, status: %u\n", 572 status); 573 err = iwl_ftm_range_request_status_to_err(status); 574 } 575 576 return err; 577 } 578 579 static int iwl_mvm_ftm_start_v5(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 580 struct cfg80211_pmsr_request *req) 581 { 582 struct iwl_tof_range_req_cmd_v5 cmd_v5; 583 struct iwl_host_cmd hcmd = { 584 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0), 585 .dataflags[0] = IWL_HCMD_DFL_DUP, 586 .data[0] = &cmd_v5, 587 .len[0] = sizeof(cmd_v5), 588 }; 589 u8 i; 590 int err; 591 592 iwl_mvm_ftm_cmd_v5(mvm, vif, &cmd_v5, req); 593 594 for (i = 0; i < cmd_v5.num_of_ap; i++) { 595 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 596 597 err = iwl_mvm_ftm_put_target_v2(mvm, peer, &cmd_v5.ap[i]); 598 if (err) 599 return err; 600 } 601 602 return iwl_mvm_ftm_send_cmd(mvm, &hcmd); 603 } 604 605 static int iwl_mvm_ftm_start_v7(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 606 struct cfg80211_pmsr_request *req) 607 { 608 struct iwl_tof_range_req_cmd_v7 cmd_v7; 609 struct iwl_host_cmd hcmd = { 610 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0), 611 .dataflags[0] = IWL_HCMD_DFL_DUP, 612 .data[0] = &cmd_v7, 613 .len[0] = sizeof(cmd_v7), 614 }; 615 u8 i; 616 int err; 617 618 /* 619 * Versions 7 and 8 has the same structure except from the responders 620 * list, so iwl_mvm_ftm_cmd() can be used for version 7 too. 621 */ 622 iwl_mvm_ftm_cmd_v8(mvm, vif, (void *)&cmd_v7, req); 623 624 for (i = 0; i < cmd_v7.num_of_ap; i++) { 625 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 626 627 err = iwl_mvm_ftm_put_target_v3(mvm, peer, &cmd_v7.ap[i]); 628 if (err) 629 return err; 630 } 631 632 return iwl_mvm_ftm_send_cmd(mvm, &hcmd); 633 } 634 635 static int iwl_mvm_ftm_start_v8(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 636 struct cfg80211_pmsr_request *req) 637 { 638 struct iwl_tof_range_req_cmd_v8 cmd; 639 struct iwl_host_cmd hcmd = { 640 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0), 641 .dataflags[0] = IWL_HCMD_DFL_DUP, 642 .data[0] = &cmd, 643 .len[0] = sizeof(cmd), 644 }; 645 u8 i; 646 int err; 647 648 iwl_mvm_ftm_cmd_v8(mvm, vif, (void *)&cmd, req); 649 650 for (i = 0; i < cmd.num_of_ap; i++) { 651 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 652 653 err = iwl_mvm_ftm_put_target_v4(mvm, peer, &cmd.ap[i]); 654 if (err) 655 return err; 656 } 657 658 return iwl_mvm_ftm_send_cmd(mvm, &hcmd); 659 } 660 661 static int iwl_mvm_ftm_start_v9(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 662 struct cfg80211_pmsr_request *req) 663 { 664 struct iwl_tof_range_req_cmd_v9 cmd; 665 struct iwl_host_cmd hcmd = { 666 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0), 667 .dataflags[0] = IWL_HCMD_DFL_DUP, 668 .data[0] = &cmd, 669 .len[0] = sizeof(cmd), 670 }; 671 u8 i; 672 int err; 673 674 iwl_mvm_ftm_cmd_common(mvm, vif, &cmd, req); 675 676 for (i = 0; i < cmd.num_of_ap; i++) { 677 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 678 struct iwl_tof_range_req_ap_entry_v6 *target = &cmd.ap[i]; 679 680 err = iwl_mvm_ftm_put_target(mvm, vif, peer, target); 681 if (err) 682 return err; 683 } 684 685 return iwl_mvm_ftm_send_cmd(mvm, &hcmd); 686 } 687 688 static void iter(struct ieee80211_hw *hw, 689 struct ieee80211_vif *vif, 690 struct ieee80211_sta *sta, 691 struct ieee80211_key_conf *key, 692 void *data) 693 { 694 struct iwl_tof_range_req_ap_entry_v6 *target = data; 695 696 if (!sta || memcmp(sta->addr, target->bssid, ETH_ALEN)) 697 return; 698 699 WARN_ON(!sta->mfp); 700 701 if (WARN_ON(key->keylen > sizeof(target->tk))) 702 return; 703 704 memcpy(target->tk, key->key, key->keylen); 705 target->cipher = iwl_mvm_cipher_to_location_cipher(key->cipher); 706 WARN_ON(target->cipher == IWL_LOCATION_CIPHER_INVALID); 707 } 708 709 static void 710 iwl_mvm_ftm_set_secured_ranging(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 711 struct iwl_tof_range_req_ap_entry_v7 *target) 712 { 713 struct iwl_mvm_ftm_pasn_entry *entry; 714 u32 flags = le32_to_cpu(target->initiator_ap_flags); 715 716 if (!(flags & (IWL_INITIATOR_AP_FLAGS_NON_TB | 717 IWL_INITIATOR_AP_FLAGS_TB))) 718 return; 719 720 lockdep_assert_held(&mvm->mutex); 721 722 list_for_each_entry(entry, &mvm->ftm_initiator.pasn_list, list) { 723 if (memcmp(entry->addr, target->bssid, sizeof(entry->addr))) 724 continue; 725 726 target->cipher = entry->cipher; 727 memcpy(target->hltk, entry->hltk, sizeof(target->hltk)); 728 729 if (vif->bss_conf.assoc && 730 !memcmp(vif->bss_conf.bssid, target->bssid, 731 sizeof(target->bssid))) 732 ieee80211_iter_keys(mvm->hw, vif, iter, target); 733 else 734 memcpy(target->tk, entry->tk, sizeof(target->tk)); 735 736 memcpy(target->rx_pn, entry->rx_pn, sizeof(target->rx_pn)); 737 memcpy(target->tx_pn, entry->tx_pn, sizeof(target->tx_pn)); 738 739 target->initiator_ap_flags |= 740 cpu_to_le32(IWL_INITIATOR_AP_FLAGS_SECURED); 741 return; 742 } 743 } 744 745 static int iwl_mvm_ftm_start_v11(struct iwl_mvm *mvm, 746 struct ieee80211_vif *vif, 747 struct cfg80211_pmsr_request *req) 748 { 749 struct iwl_tof_range_req_cmd_v11 cmd; 750 struct iwl_host_cmd hcmd = { 751 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0), 752 .dataflags[0] = IWL_HCMD_DFL_DUP, 753 .data[0] = &cmd, 754 .len[0] = sizeof(cmd), 755 }; 756 u8 i; 757 int err; 758 759 iwl_mvm_ftm_cmd_common(mvm, vif, (void *)&cmd, req); 760 761 for (i = 0; i < cmd.num_of_ap; i++) { 762 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 763 struct iwl_tof_range_req_ap_entry_v7 *target = &cmd.ap[i]; 764 765 err = iwl_mvm_ftm_put_target(mvm, vif, peer, (void *)target); 766 if (err) 767 return err; 768 769 iwl_mvm_ftm_set_secured_ranging(mvm, vif, target); 770 } 771 772 return iwl_mvm_ftm_send_cmd(mvm, &hcmd); 773 } 774 775 int iwl_mvm_ftm_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 776 struct cfg80211_pmsr_request *req) 777 { 778 bool new_api = fw_has_api(&mvm->fw->ucode_capa, 779 IWL_UCODE_TLV_API_FTM_NEW_RANGE_REQ); 780 int err; 781 782 lockdep_assert_held(&mvm->mutex); 783 784 if (mvm->ftm_initiator.req) 785 return -EBUSY; 786 787 if (new_api) { 788 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP, 789 TOF_RANGE_REQ_CMD, 790 IWL_FW_CMD_VER_UNKNOWN); 791 792 switch (cmd_ver) { 793 case 11: 794 err = iwl_mvm_ftm_start_v11(mvm, vif, req); 795 break; 796 case 9: 797 case 10: 798 err = iwl_mvm_ftm_start_v9(mvm, vif, req); 799 break; 800 case 8: 801 err = iwl_mvm_ftm_start_v8(mvm, vif, req); 802 break; 803 default: 804 err = iwl_mvm_ftm_start_v7(mvm, vif, req); 805 break; 806 } 807 } else { 808 err = iwl_mvm_ftm_start_v5(mvm, vif, req); 809 } 810 811 if (!err) { 812 mvm->ftm_initiator.req = req; 813 mvm->ftm_initiator.req_wdev = ieee80211_vif_to_wdev(vif); 814 } 815 816 return err; 817 } 818 819 void iwl_mvm_ftm_abort(struct iwl_mvm *mvm, struct cfg80211_pmsr_request *req) 820 { 821 struct iwl_tof_range_abort_cmd cmd = { 822 .request_id = req->cookie, 823 }; 824 825 lockdep_assert_held(&mvm->mutex); 826 827 if (req != mvm->ftm_initiator.req) 828 return; 829 830 iwl_mvm_ftm_reset(mvm); 831 832 if (iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(TOF_RANGE_ABORT_CMD, 833 LOCATION_GROUP, 0), 834 0, sizeof(cmd), &cmd)) 835 IWL_ERR(mvm, "failed to abort FTM process\n"); 836 } 837 838 static int iwl_mvm_ftm_find_peer(struct cfg80211_pmsr_request *req, 839 const u8 *addr) 840 { 841 int i; 842 843 for (i = 0; i < req->n_peers; i++) { 844 struct cfg80211_pmsr_request_peer *peer = &req->peers[i]; 845 846 if (ether_addr_equal_unaligned(peer->addr, addr)) 847 return i; 848 } 849 850 return -ENOENT; 851 } 852 853 static u64 iwl_mvm_ftm_get_host_time(struct iwl_mvm *mvm, __le32 fw_gp2_ts) 854 { 855 u32 gp2_ts = le32_to_cpu(fw_gp2_ts); 856 u32 curr_gp2, diff; 857 u64 now_from_boot_ns; 858 859 iwl_mvm_get_sync_time(mvm, &curr_gp2, &now_from_boot_ns); 860 861 if (curr_gp2 >= gp2_ts) 862 diff = curr_gp2 - gp2_ts; 863 else 864 diff = curr_gp2 + (U32_MAX - gp2_ts + 1); 865 866 return now_from_boot_ns - (u64)diff * 1000; 867 } 868 869 static void iwl_mvm_ftm_get_lci_civic(struct iwl_mvm *mvm, 870 struct cfg80211_pmsr_result *res) 871 { 872 struct iwl_mvm_loc_entry *entry; 873 874 list_for_each_entry(entry, &mvm->ftm_initiator.loc_list, list) { 875 if (!ether_addr_equal_unaligned(res->addr, entry->addr)) 876 continue; 877 878 if (entry->lci_len) { 879 res->ftm.lci_len = entry->lci_len; 880 res->ftm.lci = entry->buf; 881 } 882 883 if (entry->civic_len) { 884 res->ftm.civicloc_len = entry->civic_len; 885 res->ftm.civicloc = entry->buf + entry->lci_len; 886 } 887 888 /* we found the entry we needed */ 889 break; 890 } 891 } 892 893 static int iwl_mvm_ftm_range_resp_valid(struct iwl_mvm *mvm, u8 request_id, 894 u8 num_of_aps) 895 { 896 lockdep_assert_held(&mvm->mutex); 897 898 if (request_id != (u8)mvm->ftm_initiator.req->cookie) { 899 IWL_ERR(mvm, "Request ID mismatch, got %u, active %u\n", 900 request_id, (u8)mvm->ftm_initiator.req->cookie); 901 return -EINVAL; 902 } 903 904 if (num_of_aps > mvm->ftm_initiator.req->n_peers) { 905 IWL_ERR(mvm, "FTM range response invalid\n"); 906 return -EINVAL; 907 } 908 909 return 0; 910 } 911 912 static void iwl_mvm_ftm_rtt_smoothing(struct iwl_mvm *mvm, 913 struct cfg80211_pmsr_result *res) 914 { 915 struct iwl_mvm_smooth_entry *resp; 916 s64 rtt_avg, rtt = res->ftm.rtt_avg; 917 u32 undershoot, overshoot; 918 u8 alpha; 919 bool found; 920 921 if (!IWL_MVM_FTM_INITIATOR_ENABLE_SMOOTH) 922 return; 923 924 WARN_ON(rtt < 0); 925 926 if (res->status != NL80211_PMSR_STATUS_SUCCESS) { 927 IWL_DEBUG_INFO(mvm, 928 ": %pM: ignore failed measurement. Status=%u\n", 929 res->addr, res->status); 930 return; 931 } 932 933 found = false; 934 list_for_each_entry(resp, &mvm->ftm_initiator.smooth.resp, list) { 935 if (!memcmp(res->addr, resp->addr, ETH_ALEN)) { 936 found = true; 937 break; 938 } 939 } 940 941 if (!found) { 942 resp = kzalloc(sizeof(*resp), GFP_KERNEL); 943 if (!resp) 944 return; 945 946 memcpy(resp->addr, res->addr, ETH_ALEN); 947 list_add_tail(&resp->list, &mvm->ftm_initiator.smooth.resp); 948 949 resp->rtt_avg = rtt; 950 951 IWL_DEBUG_INFO(mvm, "new: %pM: rtt_avg=%lld\n", 952 resp->addr, resp->rtt_avg); 953 goto update_time; 954 } 955 956 if (res->host_time - resp->host_time > 957 IWL_MVM_FTM_INITIATOR_SMOOTH_AGE_SEC * 1000000000) { 958 resp->rtt_avg = rtt; 959 960 IWL_DEBUG_INFO(mvm, "expired: %pM: rtt_avg=%lld\n", 961 resp->addr, resp->rtt_avg); 962 goto update_time; 963 } 964 965 /* Smooth the results based on the tracked RTT average */ 966 undershoot = IWL_MVM_FTM_INITIATOR_SMOOTH_UNDERSHOOT; 967 overshoot = IWL_MVM_FTM_INITIATOR_SMOOTH_OVERSHOOT; 968 alpha = IWL_MVM_FTM_INITIATOR_SMOOTH_ALPHA; 969 970 rtt_avg = (alpha * rtt + (100 - alpha) * resp->rtt_avg) / 100; 971 972 IWL_DEBUG_INFO(mvm, 973 "%pM: prev rtt_avg=%lld, new rtt_avg=%lld, rtt=%lld\n", 974 resp->addr, resp->rtt_avg, rtt_avg, rtt); 975 976 /* 977 * update the responder's average RTT results regardless of 978 * the under/over shoot logic below 979 */ 980 resp->rtt_avg = rtt_avg; 981 982 /* smooth the results */ 983 if (rtt_avg > rtt && (rtt_avg - rtt) > undershoot) { 984 res->ftm.rtt_avg = rtt_avg; 985 986 IWL_DEBUG_INFO(mvm, 987 "undershoot: val=%lld\n", 988 (rtt_avg - rtt)); 989 } else if (rtt_avg < rtt && (rtt - rtt_avg) > 990 overshoot) { 991 res->ftm.rtt_avg = rtt_avg; 992 IWL_DEBUG_INFO(mvm, 993 "overshoot: val=%lld\n", 994 (rtt - rtt_avg)); 995 } 996 997 update_time: 998 resp->host_time = res->host_time; 999 } 1000 1001 static void iwl_mvm_debug_range_resp(struct iwl_mvm *mvm, u8 index, 1002 struct cfg80211_pmsr_result *res) 1003 { 1004 s64 rtt_avg = div_s64(res->ftm.rtt_avg * 100, 6666); 1005 1006 IWL_DEBUG_INFO(mvm, "entry %d\n", index); 1007 IWL_DEBUG_INFO(mvm, "\tstatus: %d\n", res->status); 1008 IWL_DEBUG_INFO(mvm, "\tBSSID: %pM\n", res->addr); 1009 IWL_DEBUG_INFO(mvm, "\thost time: %llu\n", res->host_time); 1010 IWL_DEBUG_INFO(mvm, "\tburst index: %hhu\n", res->ftm.burst_index); 1011 IWL_DEBUG_INFO(mvm, "\tsuccess num: %u\n", res->ftm.num_ftmr_successes); 1012 IWL_DEBUG_INFO(mvm, "\trssi: %d\n", res->ftm.rssi_avg); 1013 IWL_DEBUG_INFO(mvm, "\trssi spread: %hhu\n", res->ftm.rssi_spread); 1014 IWL_DEBUG_INFO(mvm, "\trtt: %lld\n", res->ftm.rtt_avg); 1015 IWL_DEBUG_INFO(mvm, "\trtt var: %llu\n", res->ftm.rtt_variance); 1016 IWL_DEBUG_INFO(mvm, "\trtt spread: %llu\n", res->ftm.rtt_spread); 1017 IWL_DEBUG_INFO(mvm, "\tdistance: %lld\n", rtt_avg); 1018 } 1019 1020 static void 1021 iwl_mvm_ftm_pasn_update_pn(struct iwl_mvm *mvm, 1022 struct iwl_tof_range_rsp_ap_entry_ntfy_v6 *fw_ap) 1023 { 1024 struct iwl_mvm_ftm_pasn_entry *entry; 1025 1026 lockdep_assert_held(&mvm->mutex); 1027 1028 list_for_each_entry(entry, &mvm->ftm_initiator.pasn_list, list) { 1029 if (memcmp(fw_ap->bssid, entry->addr, sizeof(entry->addr))) 1030 continue; 1031 1032 memcpy(entry->rx_pn, fw_ap->rx_pn, sizeof(entry->rx_pn)); 1033 memcpy(entry->tx_pn, fw_ap->tx_pn, sizeof(entry->tx_pn)); 1034 return; 1035 } 1036 } 1037 1038 void iwl_mvm_ftm_range_resp(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb) 1039 { 1040 struct iwl_rx_packet *pkt = rxb_addr(rxb); 1041 struct iwl_tof_range_rsp_ntfy_v5 *fw_resp_v5 = (void *)pkt->data; 1042 struct iwl_tof_range_rsp_ntfy_v6 *fw_resp_v6 = (void *)pkt->data; 1043 struct iwl_tof_range_rsp_ntfy_v7 *fw_resp_v7 = (void *)pkt->data; 1044 struct iwl_tof_range_rsp_ntfy_v8 *fw_resp_v8 = (void *)pkt->data; 1045 int i; 1046 bool new_api = fw_has_api(&mvm->fw->ucode_capa, 1047 IWL_UCODE_TLV_API_FTM_NEW_RANGE_REQ); 1048 u8 num_of_aps, last_in_batch; 1049 1050 lockdep_assert_held(&mvm->mutex); 1051 1052 if (!mvm->ftm_initiator.req) { 1053 return; 1054 } 1055 1056 if (new_api) { 1057 if (iwl_mvm_ftm_range_resp_valid(mvm, fw_resp_v8->request_id, 1058 fw_resp_v8->num_of_aps)) 1059 return; 1060 1061 num_of_aps = fw_resp_v8->num_of_aps; 1062 last_in_batch = fw_resp_v8->last_report; 1063 } else { 1064 if (iwl_mvm_ftm_range_resp_valid(mvm, fw_resp_v5->request_id, 1065 fw_resp_v5->num_of_aps)) 1066 return; 1067 1068 num_of_aps = fw_resp_v5->num_of_aps; 1069 last_in_batch = fw_resp_v5->last_in_batch; 1070 } 1071 1072 IWL_DEBUG_INFO(mvm, "Range response received\n"); 1073 IWL_DEBUG_INFO(mvm, "request id: %lld, num of entries: %hhu\n", 1074 mvm->ftm_initiator.req->cookie, num_of_aps); 1075 1076 for (i = 0; i < num_of_aps && i < IWL_MVM_TOF_MAX_APS; i++) { 1077 struct cfg80211_pmsr_result result = {}; 1078 struct iwl_tof_range_rsp_ap_entry_ntfy_v6 *fw_ap; 1079 int peer_idx; 1080 1081 if (new_api) { 1082 if (mvm->cmd_ver.range_resp == 8) { 1083 fw_ap = &fw_resp_v8->ap[i]; 1084 iwl_mvm_ftm_pasn_update_pn(mvm, fw_ap); 1085 } else if (fw_has_api(&mvm->fw->ucode_capa, 1086 IWL_UCODE_TLV_API_FTM_RTT_ACCURACY)) { 1087 fw_ap = (void *)&fw_resp_v7->ap[i]; 1088 } else { 1089 fw_ap = (void *)&fw_resp_v6->ap[i]; 1090 } 1091 1092 result.final = fw_ap->last_burst; 1093 result.ap_tsf = le32_to_cpu(fw_ap->start_tsf); 1094 result.ap_tsf_valid = 1; 1095 } else { 1096 /* the first part is the same for old and new APIs */ 1097 fw_ap = (void *)&fw_resp_v5->ap[i]; 1098 /* 1099 * FIXME: the firmware needs to report this, we don't 1100 * even know the number of bursts the responder picked 1101 * (if we asked it to) 1102 */ 1103 result.final = 0; 1104 } 1105 1106 peer_idx = iwl_mvm_ftm_find_peer(mvm->ftm_initiator.req, 1107 fw_ap->bssid); 1108 if (peer_idx < 0) { 1109 IWL_WARN(mvm, 1110 "Unknown address (%pM, target #%d) in FTM response\n", 1111 fw_ap->bssid, i); 1112 continue; 1113 } 1114 1115 switch (fw_ap->measure_status) { 1116 case IWL_TOF_ENTRY_SUCCESS: 1117 result.status = NL80211_PMSR_STATUS_SUCCESS; 1118 break; 1119 case IWL_TOF_ENTRY_TIMING_MEASURE_TIMEOUT: 1120 result.status = NL80211_PMSR_STATUS_TIMEOUT; 1121 break; 1122 case IWL_TOF_ENTRY_NO_RESPONSE: 1123 result.status = NL80211_PMSR_STATUS_FAILURE; 1124 result.ftm.failure_reason = 1125 NL80211_PMSR_FTM_FAILURE_NO_RESPONSE; 1126 break; 1127 case IWL_TOF_ENTRY_REQUEST_REJECTED: 1128 result.status = NL80211_PMSR_STATUS_FAILURE; 1129 result.ftm.failure_reason = 1130 NL80211_PMSR_FTM_FAILURE_PEER_BUSY; 1131 result.ftm.busy_retry_time = fw_ap->refusal_period; 1132 break; 1133 default: 1134 result.status = NL80211_PMSR_STATUS_FAILURE; 1135 result.ftm.failure_reason = 1136 NL80211_PMSR_FTM_FAILURE_UNSPECIFIED; 1137 break; 1138 } 1139 memcpy(result.addr, fw_ap->bssid, ETH_ALEN); 1140 result.host_time = iwl_mvm_ftm_get_host_time(mvm, 1141 fw_ap->timestamp); 1142 result.type = NL80211_PMSR_TYPE_FTM; 1143 result.ftm.burst_index = mvm->ftm_initiator.responses[peer_idx]; 1144 mvm->ftm_initiator.responses[peer_idx]++; 1145 result.ftm.rssi_avg = fw_ap->rssi; 1146 result.ftm.rssi_avg_valid = 1; 1147 result.ftm.rssi_spread = fw_ap->rssi_spread; 1148 result.ftm.rssi_spread_valid = 1; 1149 result.ftm.rtt_avg = (s32)le32_to_cpu(fw_ap->rtt); 1150 result.ftm.rtt_avg_valid = 1; 1151 result.ftm.rtt_variance = le32_to_cpu(fw_ap->rtt_variance); 1152 result.ftm.rtt_variance_valid = 1; 1153 result.ftm.rtt_spread = le32_to_cpu(fw_ap->rtt_spread); 1154 result.ftm.rtt_spread_valid = 1; 1155 1156 iwl_mvm_ftm_get_lci_civic(mvm, &result); 1157 1158 iwl_mvm_ftm_rtt_smoothing(mvm, &result); 1159 1160 cfg80211_pmsr_report(mvm->ftm_initiator.req_wdev, 1161 mvm->ftm_initiator.req, 1162 &result, GFP_KERNEL); 1163 1164 if (fw_has_api(&mvm->fw->ucode_capa, 1165 IWL_UCODE_TLV_API_FTM_RTT_ACCURACY)) 1166 IWL_DEBUG_INFO(mvm, "RTT confidence: %hhu\n", 1167 fw_ap->rttConfidence); 1168 1169 iwl_mvm_debug_range_resp(mvm, i, &result); 1170 } 1171 1172 if (last_in_batch) { 1173 cfg80211_pmsr_complete(mvm->ftm_initiator.req_wdev, 1174 mvm->ftm_initiator.req, 1175 GFP_KERNEL); 1176 iwl_mvm_ftm_reset(mvm); 1177 } 1178 } 1179 1180 void iwl_mvm_ftm_lc_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb) 1181 { 1182 struct iwl_rx_packet *pkt = rxb_addr(rxb); 1183 const struct ieee80211_mgmt *mgmt = (void *)pkt->data; 1184 size_t len = iwl_rx_packet_payload_len(pkt); 1185 struct iwl_mvm_loc_entry *entry; 1186 const u8 *ies, *lci, *civic, *msr_ie; 1187 size_t ies_len, lci_len = 0, civic_len = 0; 1188 size_t baselen = IEEE80211_MIN_ACTION_SIZE + 1189 sizeof(mgmt->u.action.u.ftm); 1190 static const u8 rprt_type_lci = IEEE80211_SPCT_MSR_RPRT_TYPE_LCI; 1191 static const u8 rprt_type_civic = IEEE80211_SPCT_MSR_RPRT_TYPE_CIVIC; 1192 1193 if (len <= baselen) 1194 return; 1195 1196 lockdep_assert_held(&mvm->mutex); 1197 1198 ies = mgmt->u.action.u.ftm.variable; 1199 ies_len = len - baselen; 1200 1201 msr_ie = cfg80211_find_ie_match(WLAN_EID_MEASURE_REPORT, ies, ies_len, 1202 &rprt_type_lci, 1, 4); 1203 if (msr_ie) { 1204 lci = msr_ie + 2; 1205 lci_len = msr_ie[1]; 1206 } 1207 1208 msr_ie = cfg80211_find_ie_match(WLAN_EID_MEASURE_REPORT, ies, ies_len, 1209 &rprt_type_civic, 1, 4); 1210 if (msr_ie) { 1211 civic = msr_ie + 2; 1212 civic_len = msr_ie[1]; 1213 } 1214 1215 entry = kmalloc(sizeof(*entry) + lci_len + civic_len, GFP_KERNEL); 1216 if (!entry) 1217 return; 1218 1219 memcpy(entry->addr, mgmt->bssid, ETH_ALEN); 1220 1221 entry->lci_len = lci_len; 1222 if (lci_len) 1223 memcpy(entry->buf, lci, lci_len); 1224 1225 entry->civic_len = civic_len; 1226 if (civic_len) 1227 memcpy(entry->buf + lci_len, civic, civic_len); 1228 1229 list_add_tail(&entry->list, &mvm->ftm_initiator.loc_list); 1230 } 1231