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