drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c

   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  *
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of version 2 of the GNU General Public License as
  14  * published by the Free Software Foundation.
  15  *
  16  * This program is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19  * General Public License for more details.
  20  *
  21  * The full GNU General Public License is included in this distribution
  22  * in the file called COPYING.
  23  *
  24  * Contact Information:
  25  * Intel Linux Wireless <linuxwifi@intel.com>
  26  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  27  *
  28  * BSD LICENSE
  29  *
  30  * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  31  * Copyright (C) 2018 Intel Corporation
  32  * Copyright (C) 2019 Intel Corporation
  33  * All rights reserved.
  34  *
  35  * Redistribution and use in source and binary forms, with or without
  36  * modification, are permitted provided that the following conditions
  37  * are met:
  38  *
  39  *  * Redistributions of source code must retain the above copyright
  40  *    notice, this list of conditions and the following disclaimer.
  41  *  * Redistributions in binary form must reproduce the above copyright
  42  *    notice, this list of conditions and the following disclaimer in
  43  *    the documentation and/or other materials provided with the
  44  *    distribution.
  45  *  * Neither the name Intel Corporation nor the names of its
  46  *    contributors may be used to endorse or promote products derived
  47  *    from this software without specific prior written permission.
  48  *
  49  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  50  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  51  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  52  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  53  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  54  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  55  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  56  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  57  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  58  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  59  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  60  *
  61  *****************************************************************************/
  62 #include <linux/etherdevice.h>
  63 #include <linux/math64.h>
  64 #include <net/cfg80211.h>
  65 #include "mvm.h"
  66 #include "iwl-io.h"
  67 #include "iwl-prph.h"
  68 #include "constants.h"
  69
  70 struct iwl_mvm_loc_entry {
  71         struct list_head list;
  72         u8 addr[ETH_ALEN];
  73         u8 lci_len, civic_len;
  74         u8 buf[];
  75 };
  76
  77 static void iwl_mvm_ftm_reset(struct iwl_mvm *mvm)
  78 {
  79         struct iwl_mvm_loc_entry *e, *t;
  80
  81         mvm->ftm_initiator.req = NULL;
  82         mvm->ftm_initiator.req_wdev = NULL;
  83         memset(mvm->ftm_initiator.responses, 0,
  84                sizeof(mvm->ftm_initiator.responses));
  85         list_for_each_entry_safe(e, t, &mvm->ftm_initiator.loc_list, list) {
  86                 list_del(&e->list);
  87                 kfree(e);
  88         }
  89 }
  90
  91 void iwl_mvm_ftm_restart(struct iwl_mvm *mvm)
  92 {
  93         struct cfg80211_pmsr_result result = {
  94                 .status = NL80211_PMSR_STATUS_FAILURE,
  95                 .final = 1,
  96                 .host_time = ktime_get_boot_ns(),
  97                 .type = NL80211_PMSR_TYPE_FTM,
  98         };
  99         int i;
 100
 101         lockdep_assert_held(&mvm->mutex);
 102
 103         if (!mvm->ftm_initiator.req)
 104                 return;
 105
 106         for (i = 0; i < mvm->ftm_initiator.req->n_peers; i++) {
 107                 memcpy(result.addr, mvm->ftm_initiator.req->peers[i].addr,
 108                        ETH_ALEN);
 109                 result.ftm.burst_index = mvm->ftm_initiator.responses[i];
 110
 111                 cfg80211_pmsr_report(mvm->ftm_initiator.req_wdev,
 112                                      mvm->ftm_initiator.req,
 113                                      &result, GFP_KERNEL);
 114         }
 115
 116         cfg80211_pmsr_complete(mvm->ftm_initiator.req_wdev,
 117                                mvm->ftm_initiator.req, GFP_KERNEL);
 118         iwl_mvm_ftm_reset(mvm);
 119 }
 120
 121 static int
 122 iwl_ftm_range_request_status_to_err(enum iwl_tof_range_request_status s)
 123 {
 124         switch (s) {
 125         case IWL_TOF_RANGE_REQUEST_STATUS_SUCCESS:
 126                 return 0;
 127         case IWL_TOF_RANGE_REQUEST_STATUS_BUSY:
 128                 return -EBUSY;
 129         default:
 130                 WARN_ON_ONCE(1);
 131                 return -EIO;
 132         }
 133 }
 134
 135 static void iwl_mvm_ftm_cmd_v5(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 136                                struct iwl_tof_range_req_cmd_v5 *cmd,
 137                                struct cfg80211_pmsr_request *req)
 138 {
 139         int i;
 140
 141         cmd->request_id = req->cookie;
 142         cmd->num_of_ap = req->n_peers;
 143
 144         /* use maximum for "no timeout" or bigger than what we can do */
 145         if (!req->timeout || req->timeout > 255 * 100)
 146                 cmd->req_timeout = 255;
 147         else
 148                 cmd->req_timeout = DIV_ROUND_UP(req->timeout, 100);
 149
 150         /*
 151          * We treat it always as random, since if not we'll
 152          * have filled our local address there instead.
 153          */
 154         cmd->macaddr_random = 1;
 155         memcpy(cmd->macaddr_template, req->mac_addr, ETH_ALEN);
 156         for (i = 0; i < ETH_ALEN; i++)
 157                 cmd->macaddr_mask[i] = ~req->mac_addr_mask[i];
 158
 159         if (vif->bss_conf.assoc)
 160                 memcpy(cmd->range_req_bssid, vif->bss_conf.bssid, ETH_ALEN);
 161         else
 162                 eth_broadcast_addr(cmd->range_req_bssid);
 163 }
 164
 165 static void iwl_mvm_ftm_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 166                             struct iwl_tof_range_req_cmd *cmd,
 167                             struct cfg80211_pmsr_request *req)
 168 {
 169         int i;
 170
 171         cmd->initiator_flags =
 172                 cpu_to_le32(IWL_TOF_INITIATOR_FLAGS_MACADDR_RANDOM |
 173                             IWL_TOF_INITIATOR_FLAGS_NON_ASAP_SUPPORT);
 174         cmd->request_id = req->cookie;
 175         cmd->num_of_ap = req->n_peers;
 176
 177         /*
 178          * Use a large value for "no timeout". Don't use the maximum value
 179          * because of fw limitations.
 180          */
 181         if (req->timeout)
 182                 cmd->req_timeout_ms = cpu_to_le32(req->timeout);
 183         else
 184                 cmd->req_timeout_ms = cpu_to_le32(0xfffff);
 185
 186         memcpy(cmd->macaddr_template, req->mac_addr, ETH_ALEN);
 187         for (i = 0; i < ETH_ALEN; i++)
 188                 cmd->macaddr_mask[i] = ~req->mac_addr_mask[i];
 189
 190         if (vif->bss_conf.assoc)
 191                 memcpy(cmd->range_req_bssid, vif->bss_conf.bssid, ETH_ALEN);
 192         else
 193                 eth_broadcast_addr(cmd->range_req_bssid);
 194
 195         /* TODO: fill in tsf_mac_id if needed */
 196         cmd->tsf_mac_id = cpu_to_le32(0xff);
 197 }
 198
 199 static int iwl_mvm_ftm_target_chandef(struct iwl_mvm *mvm,
 200                                       struct cfg80211_pmsr_request_peer *peer,
 201                                       u8 *channel, u8 *bandwidth,
 202                                       u8 *ctrl_ch_position)
 203 {
 204         u32 freq = peer->chandef.chan->center_freq;
 205
 206         *channel = ieee80211_frequency_to_channel(freq);
 207
 208         switch (peer->chandef.width) {
 209         case NL80211_CHAN_WIDTH_20_NOHT:
 210                 *bandwidth = IWL_TOF_BW_20_LEGACY;
 211                 break;
 212         case NL80211_CHAN_WIDTH_20:
 213                 *bandwidth = IWL_TOF_BW_20_HT;
 214                 break;
 215         case NL80211_CHAN_WIDTH_40:
 216                 *bandwidth = IWL_TOF_BW_40;
 217                 break;
 218         case NL80211_CHAN_WIDTH_80:
 219                 *bandwidth = IWL_TOF_BW_80;
 220                 break;
 221         default:
 222                 IWL_ERR(mvm, "Unsupported BW in FTM request (%d)\n",
 223                         peer->chandef.width);
 224                 return -EINVAL;
 225         }
 226
 227         *ctrl_ch_position = (peer->chandef.width > NL80211_CHAN_WIDTH_20) ?
 228                 iwl_mvm_get_ctrl_pos(&peer->chandef) : 0;
 229
 230         return 0;
 231 }
 232
 233 static int
 234 iwl_mvm_ftm_put_target_v2(struct iwl_mvm *mvm,
 235                           struct cfg80211_pmsr_request_peer *peer,
 236                           struct iwl_tof_range_req_ap_entry_v2 *target)
 237 {
 238         int ret;
 239
 240         ret = iwl_mvm_ftm_target_chandef(mvm, peer, &target->channel_num,
 241                                          &target->bandwidth,
 242                                          &target->ctrl_ch_position);
 243         if (ret)
 244                 return ret;
 245
 246         memcpy(target->bssid, peer->addr, ETH_ALEN);
 247         target->burst_period =
 248                 cpu_to_le16(peer->ftm.burst_period);
 249         target->samples_per_burst = peer->ftm.ftms_per_burst;
 250         target->num_of_bursts = peer->ftm.num_bursts_exp;
 251         target->measure_type = 0; /* regular two-sided FTM */
 252         target->retries_per_sample = peer->ftm.ftmr_retries;
 253         target->asap_mode = peer->ftm.asap;
 254         target->enable_dyn_ack = IWL_MVM_FTM_INITIATOR_DYNACK;
 255
 256         if (peer->ftm.request_lci)
 257                 target->location_req |= IWL_TOF_LOC_LCI;
 258         if (peer->ftm.request_civicloc)
 259                 target->location_req |= IWL_TOF_LOC_CIVIC;
 260
 261         target->algo_type = IWL_MVM_FTM_INITIATOR_ALGO;
 262
 263         return 0;
 264 }
 265
 266 #define FTM_PUT_FLAG(flag)      (target->initiator_ap_flags |= \
 267                                  cpu_to_le32(IWL_INITIATOR_AP_FLAGS_##flag))
 268
 269 static int iwl_mvm_ftm_put_target(struct iwl_mvm *mvm,
 270                                   struct cfg80211_pmsr_request_peer *peer,
 271                                   struct iwl_tof_range_req_ap_entry *target)
 272 {
 273         int ret;
 274
 275         ret = iwl_mvm_ftm_target_chandef(mvm, peer, &target->channel_num,
 276                                          &target->bandwidth,
 277                                          &target->ctrl_ch_position);
 278         if (ret)
 279                 return ret;
 280
 281         memcpy(target->bssid, peer->addr, ETH_ALEN);
 282         target->burst_period =
 283                 cpu_to_le16(peer->ftm.burst_period);
 284         target->samples_per_burst = peer->ftm.ftms_per_burst;
 285         target->num_of_bursts = peer->ftm.num_bursts_exp;
 286         target->ftmr_max_retries = peer->ftm.ftmr_retries;
 287         target->initiator_ap_flags = cpu_to_le32(0);
 288
 289         if (peer->ftm.asap)
 290                 FTM_PUT_FLAG(ASAP);
 291
 292         if (peer->ftm.request_lci)
 293                 FTM_PUT_FLAG(LCI_REQUEST);
 294
 295         if (peer->ftm.request_civicloc)
 296                 FTM_PUT_FLAG(CIVIC_REQUEST);
 297
 298         if (IWL_MVM_FTM_INITIATOR_DYNACK)
 299                 FTM_PUT_FLAG(DYN_ACK);
 300
 301         if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_LINEAR_REG)
 302                 FTM_PUT_FLAG(ALGO_LR);
 303         else if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_FFT)
 304                 FTM_PUT_FLAG(ALGO_FFT);
 305
 306         return 0;
 307 }
 308
 309 int iwl_mvm_ftm_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 310                       struct cfg80211_pmsr_request *req)
 311 {
 312         struct iwl_tof_range_req_cmd_v5 cmd_v5;
 313         struct iwl_tof_range_req_cmd cmd;
 314         bool new_api = fw_has_api(&mvm->fw->ucode_capa,
 315                                   IWL_UCODE_TLV_API_FTM_NEW_RANGE_REQ);
 316         u8 num_of_ap;
 317         struct iwl_host_cmd hcmd = {
 318                 .id = iwl_cmd_id(TOF_RANGE_REQ_CMD, LOCATION_GROUP, 0),
 319                 .dataflags[0] = IWL_HCMD_DFL_DUP,
 320         };
 321         u32 status = 0;
 322         int err, i;
 323
 324         lockdep_assert_held(&mvm->mutex);
 325
 326         if (mvm->ftm_initiator.req)
 327                 return -EBUSY;
 328
 329         if (new_api) {
 330                 iwl_mvm_ftm_cmd(mvm, vif, &cmd, req);
 331                 hcmd.data[0] = &cmd;
 332                 hcmd.len[0] = sizeof(cmd);
 333                 num_of_ap = cmd.num_of_ap;
 334         } else {
 335                 iwl_mvm_ftm_cmd_v5(mvm, vif, &cmd_v5, req);
 336                 hcmd.data[0] = &cmd_v5;
 337                 hcmd.len[0] = sizeof(cmd_v5);
 338                 num_of_ap = cmd_v5.num_of_ap;
 339         }
 340
 341         for (i = 0; i < num_of_ap; i++) {
 342                 struct cfg80211_pmsr_request_peer *peer = &req->peers[i];
 343
 344                 if (new_api)
 345                         err = iwl_mvm_ftm_put_target(mvm, peer, &cmd.ap[i]);
 346                 else
 347                         err = iwl_mvm_ftm_put_target_v2(mvm, peer,
 348                                                         &cmd_v5.ap[i]);
 349
 350                 if (err)
 351                         return err;
 352         }
 353
 354         err = iwl_mvm_send_cmd_status(mvm, &hcmd, &status);
 355         if (!err && status) {
 356                 IWL_ERR(mvm, "FTM range request command failure, status: %u\n",
 357                         status);
 358                 err = iwl_ftm_range_request_status_to_err(status);
 359         }
 360
 361         if (!err) {
 362                 mvm->ftm_initiator.req = req;
 363                 mvm->ftm_initiator.req_wdev = ieee80211_vif_to_wdev(vif);
 364         }
 365
 366         return err;
 367 }
 368
 369 void iwl_mvm_ftm_abort(struct iwl_mvm *mvm, struct cfg80211_pmsr_request *req)
 370 {
 371         struct iwl_tof_range_abort_cmd cmd = {
 372                 .request_id = req->cookie,
 373         };
 374
 375         lockdep_assert_held(&mvm->mutex);
 376
 377         if (req != mvm->ftm_initiator.req)
 378                 return;
 379
 380         if (iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(TOF_RANGE_ABORT_CMD,
 381                                                  LOCATION_GROUP, 0),
 382                                  0, sizeof(cmd), &cmd))
 383                 IWL_ERR(mvm, "failed to abort FTM process\n");
 384 }
 385
 386 static int iwl_mvm_ftm_find_peer(struct cfg80211_pmsr_request *req,
 387                                  const u8 *addr)
 388 {
 389         int i;
 390
 391         for (i = 0; i < req->n_peers; i++) {
 392                 struct cfg80211_pmsr_request_peer *peer = &req->peers[i];
 393
 394                 if (ether_addr_equal_unaligned(peer->addr, addr))
 395                         return i;
 396         }
 397
 398         return -ENOENT;
 399 }
 400
 401 static u64 iwl_mvm_ftm_get_host_time(struct iwl_mvm *mvm, __le32 fw_gp2_ts)
 402 {
 403         u32 gp2_ts = le32_to_cpu(fw_gp2_ts);
 404         u32 curr_gp2, diff;
 405         u64 now_from_boot_ns;
 406
 407         iwl_mvm_get_sync_time(mvm, &curr_gp2, &now_from_boot_ns);
 408
 409         if (curr_gp2 >= gp2_ts)
 410                 diff = curr_gp2 - gp2_ts;
 411         else
 412                 diff = curr_gp2 + (U32_MAX - gp2_ts + 1);
 413
 414         return now_from_boot_ns - (u64)diff * 1000;
 415 }
 416
 417 static void iwl_mvm_ftm_get_lci_civic(struct iwl_mvm *mvm,
 418                                       struct cfg80211_pmsr_result *res)
 419 {
 420         struct iwl_mvm_loc_entry *entry;
 421
 422         list_for_each_entry(entry, &mvm->ftm_initiator.loc_list, list) {
 423                 if (!ether_addr_equal_unaligned(res->addr, entry->addr))
 424                         continue;
 425
 426                 if (entry->lci_len) {
 427                         res->ftm.lci_len = entry->lci_len;
 428                         res->ftm.lci = entry->buf;
 429                 }
 430
 431                 if (entry->civic_len) {
 432                         res->ftm.civicloc_len = entry->civic_len;
 433                         res->ftm.civicloc = entry->buf + entry->lci_len;
 434                 }
 435
 436                 /* we found the entry we needed */
 437                 break;
 438         }
 439 }
 440
 441 static int iwl_mvm_ftm_range_resp_valid(struct iwl_mvm *mvm, u8 request_id,
 442                                         u8 num_of_aps)
 443 {
 444         lockdep_assert_held(&mvm->mutex);
 445
 446         if (request_id != (u8)mvm->ftm_initiator.req->cookie) {
 447                 IWL_ERR(mvm, "Request ID mismatch, got %u, active %u\n",
 448                         request_id, (u8)mvm->ftm_initiator.req->cookie);
 449                 return -EINVAL;
 450         }
 451
 452         if (num_of_aps > mvm->ftm_initiator.req->n_peers) {
 453                 IWL_ERR(mvm, "FTM range response invalid\n");
 454                 return -EINVAL;
 455         }
 456
 457         return 0;
 458 }
 459
 460 static void iwl_mvm_debug_range_resp(struct iwl_mvm *mvm, u8 index,
 461                                      struct cfg80211_pmsr_result *res)
 462 {
 463         s64 rtt_avg = div_s64(res->ftm.rtt_avg * 100, 6666);
 464
 465         IWL_DEBUG_INFO(mvm, "entry %d\n", index);
 466         IWL_DEBUG_INFO(mvm, "\tstatus: %d\n", res->status);
 467         IWL_DEBUG_INFO(mvm, "\tBSSID: %pM\n", res->addr);
 468         IWL_DEBUG_INFO(mvm, "\thost time: %llu\n", res->host_time);
 469         IWL_DEBUG_INFO(mvm, "\tburst index: %hhu\n", res->ftm.burst_index);
 470         IWL_DEBUG_INFO(mvm, "\tsuccess num: %u\n", res->ftm.num_ftmr_successes);
 471         IWL_DEBUG_INFO(mvm, "\trssi: %d\n", res->ftm.rssi_avg);
 472         IWL_DEBUG_INFO(mvm, "\trssi spread: %hhu\n", res->ftm.rssi_spread);
 473         IWL_DEBUG_INFO(mvm, "\trtt: %lld\n", res->ftm.rtt_avg);
 474         IWL_DEBUG_INFO(mvm, "\trtt var: %llu\n", res->ftm.rtt_variance);
 475         IWL_DEBUG_INFO(mvm, "\trtt spread: %llu\n", res->ftm.rtt_spread);
 476         IWL_DEBUG_INFO(mvm, "\tdistance: %lld\n", rtt_avg);
 477 }
 478
 479 void iwl_mvm_ftm_range_resp(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
 480 {
 481         struct iwl_rx_packet *pkt = rxb_addr(rxb);
 482         struct iwl_tof_range_rsp_ntfy_v5 *fw_resp_v5 = (void *)pkt->data;
 483         struct iwl_tof_range_rsp_ntfy *fw_resp = (void *)pkt->data;
 484         int i;
 485         bool new_api = fw_has_api(&mvm->fw->ucode_capa,
 486                                   IWL_UCODE_TLV_API_FTM_NEW_RANGE_REQ);
 487         u8 num_of_aps, last_in_batch;
 488
 489         lockdep_assert_held(&mvm->mutex);
 490
 491         if (!mvm->ftm_initiator.req) {
 492                 IWL_ERR(mvm, "Got FTM response but have no request?\n");
 493                 return;
 494         }
 495
 496         if (new_api) {
 497                 if (iwl_mvm_ftm_range_resp_valid(mvm, fw_resp->request_id,
 498                                                  fw_resp->num_of_aps))
 499                         return;
 500
 501                 num_of_aps = fw_resp->num_of_aps;
 502                 last_in_batch = fw_resp->last_report;
 503         } else {
 504                 if (iwl_mvm_ftm_range_resp_valid(mvm, fw_resp_v5->request_id,
 505                                                  fw_resp_v5->num_of_aps))
 506                         return;
 507
 508                 num_of_aps = fw_resp_v5->num_of_aps;
 509                 last_in_batch = fw_resp_v5->last_in_batch;
 510         }
 511
 512         IWL_DEBUG_INFO(mvm, "Range response received\n");
 513         IWL_DEBUG_INFO(mvm, "request id: %lld, num of entries: %hhu\n",
 514                        mvm->ftm_initiator.req->cookie, num_of_aps);
 515
 516         for (i = 0; i < num_of_aps && i < IWL_MVM_TOF_MAX_APS; i++) {
 517                 struct cfg80211_pmsr_result result = {};
 518                 struct iwl_tof_range_rsp_ap_entry_ntfy *fw_ap;
 519                 int peer_idx;
 520
 521                 if (new_api) {
 522                         fw_ap = &fw_resp->ap[i];
 523                         result.final = fw_resp->ap[i].last_burst;
 524                 } else {
 525                         /* the first part is the same for old and new APIs */
 526                         fw_ap = (void *)&fw_resp_v5->ap[i];
 527                         /*
 528                          * FIXME: the firmware needs to report this, we don't
 529                          * even know the number of bursts the responder picked
 530                          * (if we asked it to)
 531                          */
 532                         result.final = 0;
 533                 }
 534
 535                 peer_idx = iwl_mvm_ftm_find_peer(mvm->ftm_initiator.req,
 536                                                  fw_ap->bssid);
 537                 if (peer_idx < 0) {
 538                         IWL_WARN(mvm,
 539                                  "Unknown address (%pM, target #%d) in FTM response\n",
 540                                  fw_ap->bssid, i);
 541                         continue;
 542                 }
 543
 544                 switch (fw_ap->measure_status) {
 545                 case IWL_TOF_ENTRY_SUCCESS:
 546                         result.status = NL80211_PMSR_STATUS_SUCCESS;
 547                         break;
 548                 case IWL_TOF_ENTRY_TIMING_MEASURE_TIMEOUT:
 549                         result.status = NL80211_PMSR_STATUS_TIMEOUT;
 550                         break;
 551                 case IWL_TOF_ENTRY_NO_RESPONSE:
 552                         result.status = NL80211_PMSR_STATUS_FAILURE;
 553                         result.ftm.failure_reason =
 554                                 NL80211_PMSR_FTM_FAILURE_NO_RESPONSE;
 555                         break;
 556                 case IWL_TOF_ENTRY_REQUEST_REJECTED:
 557                         result.status = NL80211_PMSR_STATUS_FAILURE;
 558                         result.ftm.failure_reason =
 559                                 NL80211_PMSR_FTM_FAILURE_PEER_BUSY;
 560                         result.ftm.busy_retry_time = fw_ap->refusal_period;
 561                         break;
 562                 default:
 563                         result.status = NL80211_PMSR_STATUS_FAILURE;
 564                         result.ftm.failure_reason =
 565                                 NL80211_PMSR_FTM_FAILURE_UNSPECIFIED;
 566                         break;
 567                 }
 568                 memcpy(result.addr, fw_ap->bssid, ETH_ALEN);
 569                 result.host_time = iwl_mvm_ftm_get_host_time(mvm,
 570                                                              fw_ap->timestamp);
 571                 result.type = NL80211_PMSR_TYPE_FTM;
 572                 result.ftm.burst_index = mvm->ftm_initiator.responses[peer_idx];
 573                 mvm->ftm_initiator.responses[peer_idx]++;
 574                 result.ftm.rssi_avg = fw_ap->rssi;
 575                 result.ftm.rssi_avg_valid = 1;
 576                 result.ftm.rssi_spread = fw_ap->rssi_spread;
 577                 result.ftm.rssi_spread_valid = 1;
 578                 result.ftm.rtt_avg = (s32)le32_to_cpu(fw_ap->rtt);
 579                 result.ftm.rtt_avg_valid = 1;
 580                 result.ftm.rtt_variance = le32_to_cpu(fw_ap->rtt_variance);
 581                 result.ftm.rtt_variance_valid = 1;
 582                 result.ftm.rtt_spread = le32_to_cpu(fw_ap->rtt_spread);
 583                 result.ftm.rtt_spread_valid = 1;
 584
 585                 iwl_mvm_ftm_get_lci_civic(mvm, &result);
 586
 587                 cfg80211_pmsr_report(mvm->ftm_initiator.req_wdev,
 588                                      mvm->ftm_initiator.req,
 589                                      &result, GFP_KERNEL);
 590
 591                 iwl_mvm_debug_range_resp(mvm, i, &result);
 592         }
 593
 594         if (last_in_batch) {
 595                 cfg80211_pmsr_complete(mvm->ftm_initiator.req_wdev,
 596                                        mvm->ftm_initiator.req,
 597                                        GFP_KERNEL);
 598                 iwl_mvm_ftm_reset(mvm);
 599         }
 600 }
 601
 602 void iwl_mvm_ftm_lc_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
 603 {
 604         struct iwl_rx_packet *pkt = rxb_addr(rxb);
 605         const struct ieee80211_mgmt *mgmt = (void *)pkt->data;
 606         size_t len = iwl_rx_packet_payload_len(pkt);
 607         struct iwl_mvm_loc_entry *entry;
 608         const u8 *ies, *lci, *civic, *msr_ie;
 609         size_t ies_len, lci_len = 0, civic_len = 0;
 610         size_t baselen = IEEE80211_MIN_ACTION_SIZE +
 611                          sizeof(mgmt->u.action.u.ftm);
 612         static const u8 rprt_type_lci = IEEE80211_SPCT_MSR_RPRT_TYPE_LCI;
 613         static const u8 rprt_type_civic = IEEE80211_SPCT_MSR_RPRT_TYPE_CIVIC;
 614
 615         if (len <= baselen)
 616                 return;
 617
 618         lockdep_assert_held(&mvm->mutex);
 619
 620         ies = mgmt->u.action.u.ftm.variable;
 621         ies_len = len - baselen;
 622
 623         msr_ie = cfg80211_find_ie_match(WLAN_EID_MEASURE_REPORT, ies, ies_len,
 624                                         &rprt_type_lci, 1, 4);
 625         if (msr_ie) {
 626                 lci = msr_ie + 2;
 627                 lci_len = msr_ie[1];
 628         }
 629
 630         msr_ie = cfg80211_find_ie_match(WLAN_EID_MEASURE_REPORT, ies, ies_len,
 631                                         &rprt_type_civic, 1, 4);
 632         if (msr_ie) {
 633                 civic = msr_ie + 2;
 634                 civic_len = msr_ie[1];
 635         }
 636
 637         entry = kmalloc(sizeof(*entry) + lci_len + civic_len, GFP_KERNEL);
 638         if (!entry)
 639                 return;
 640
 641         memcpy(entry->addr, mgmt->bssid, ETH_ALEN);
 642
 643         entry->lci_len = lci_len;
 644         if (lci_len)
 645                 memcpy(entry->buf, lci, lci_len);
 646
 647         entry->civic_len = civic_len;
 648         if (civic_len)
 649                 memcpy(entry->buf + lci_len, civic, civic_len);
 650
 651         list_add_tail(&entry->list, &mvm->ftm_initiator.loc_list);
 652 }