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