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

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