]> Git Repo - linux.git/blob - drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c
Linux 6.14-rc3
[linux.git] / 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 }
This page took 0.129284 seconds and 4 git commands to generate.