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