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