1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2 /*
3  * Copyright (C) 2015-2017 Intel Deutschland GmbH
4  * Copyright (C) 2018-2022 Intel Corporation
5  */
6 #include <net/cfg80211.h>
7 #include <linux/etherdevice.h>
8 #include "mvm.h"
9 #include "constants.h"
10 
11 struct iwl_mvm_pasn_sta {
12 	struct list_head list;
13 	struct iwl_mvm_int_sta int_sta;
14 	u8 addr[ETH_ALEN];
15 };
16 
17 struct iwl_mvm_pasn_hltk_data {
18 	u8 *addr;
19 	u8 cipher;
20 	u8 *hltk;
21 };
22 
23 static int iwl_mvm_ftm_responder_set_bw_v1(struct cfg80211_chan_def *chandef,
24 					   u8 *bw, u8 *ctrl_ch_position)
25 {
26 	switch (chandef->width) {
27 	case NL80211_CHAN_WIDTH_20_NOHT:
28 		*bw = IWL_TOF_BW_20_LEGACY;
29 		break;
30 	case NL80211_CHAN_WIDTH_20:
31 		*bw = IWL_TOF_BW_20_HT;
32 		break;
33 	case NL80211_CHAN_WIDTH_40:
34 		*bw = IWL_TOF_BW_40;
35 		*ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);
36 		break;
37 	case NL80211_CHAN_WIDTH_80:
38 		*bw = IWL_TOF_BW_80;
39 		*ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);
40 		break;
41 	default:
42 		return -ENOTSUPP;
43 	}
44 
45 	return 0;
46 }
47 
48 static int iwl_mvm_ftm_responder_set_bw_v2(struct cfg80211_chan_def *chandef,
49 					   u8 *format_bw, u8 *ctrl_ch_position,
50 					   u8 cmd_ver)
51 {
52 	switch (chandef->width) {
53 	case NL80211_CHAN_WIDTH_20_NOHT:
54 		*format_bw = IWL_LOCATION_FRAME_FORMAT_LEGACY;
55 		*format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS;
56 		break;
57 	case NL80211_CHAN_WIDTH_20:
58 		*format_bw = IWL_LOCATION_FRAME_FORMAT_HT;
59 		*format_bw |= IWL_LOCATION_BW_20MHZ << LOCATION_BW_POS;
60 		break;
61 	case NL80211_CHAN_WIDTH_40:
62 		*format_bw = IWL_LOCATION_FRAME_FORMAT_HT;
63 		*format_bw |= IWL_LOCATION_BW_40MHZ << LOCATION_BW_POS;
64 		*ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);
65 		break;
66 	case NL80211_CHAN_WIDTH_80:
67 		*format_bw = IWL_LOCATION_FRAME_FORMAT_VHT;
68 		*format_bw |= IWL_LOCATION_BW_80MHZ << LOCATION_BW_POS;
69 		*ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);
70 		break;
71 	case NL80211_CHAN_WIDTH_160:
72 		if (cmd_ver >= 9) {
73 			*format_bw = IWL_LOCATION_FRAME_FORMAT_HE;
74 			*format_bw |= IWL_LOCATION_BW_160MHZ << LOCATION_BW_POS;
75 			*ctrl_ch_position = iwl_mvm_get_ctrl_pos(chandef);
76 			break;
77 		}
78 		fallthrough;
79 	default:
80 		return -ENOTSUPP;
81 	}
82 
83 	return 0;
84 }
85 
86 static void
87 iwl_mvm_ftm_responder_set_ndp(struct iwl_mvm *mvm,
88 			      struct iwl_tof_responder_config_cmd_v9 *cmd)
89 {
90 	/* Up to 2 R2I STS are allowed on the responder */
91 	u32 r2i_max_sts = IWL_MVM_FTM_R2I_MAX_STS < 2 ?
92 		IWL_MVM_FTM_R2I_MAX_STS : 1;
93 
94 	cmd->r2i_ndp_params = IWL_MVM_FTM_R2I_MAX_REP |
95 		(r2i_max_sts << IWL_RESPONDER_STS_POS) |
96 		(IWL_MVM_FTM_R2I_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);
97 	cmd->i2r_ndp_params = IWL_MVM_FTM_I2R_MAX_REP |
98 		(IWL_MVM_FTM_I2R_MAX_STS << IWL_RESPONDER_STS_POS) |
99 		(IWL_MVM_FTM_I2R_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);
100 	cmd->cmd_valid_fields |=
101 		cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_NDP_PARAMS);
102 }
103 
104 static int
105 iwl_mvm_ftm_responder_cmd(struct iwl_mvm *mvm,
106 			  struct ieee80211_vif *vif,
107 			  struct cfg80211_chan_def *chandef)
108 {
109 	u32 cmd_id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_CONFIG_CMD);
110 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
111 	/*
112 	 * The command structure is the same for versions 6, 7 and 8 (only the
113 	 * field interpretation is different), so the same struct can be use
114 	 * for all cases.
115 	 */
116 	struct iwl_tof_responder_config_cmd_v9 cmd = {
117 		.channel_num = chandef->chan->hw_value,
118 		.cmd_valid_fields =
119 			cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_CHAN_INFO |
120 				    IWL_TOF_RESPONDER_CMD_VALID_BSSID |
121 				    IWL_TOF_RESPONDER_CMD_VALID_STA_ID),
122 		.sta_id = mvmvif->deflink.bcast_sta.sta_id,
123 	};
124 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 6);
125 	int err;
126 	int cmd_size;
127 
128 	lockdep_assert_held(&mvm->mutex);
129 
130 	/* Use a default of bss_color=1 for now */
131 	if (cmd_ver == 9) {
132 		cmd.cmd_valid_fields |=
133 			cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_BSS_COLOR |
134 				    IWL_TOF_RESPONDER_CMD_VALID_MIN_MAX_TIME_BETWEEN_MSR);
135 		cmd.bss_color = 1;
136 		cmd.min_time_between_msr =
137 			cpu_to_le16(IWL_MVM_FTM_NON_TB_MIN_TIME_BETWEEN_MSR);
138 		cmd.max_time_between_msr =
139 			cpu_to_le16(IWL_MVM_FTM_NON_TB_MAX_TIME_BETWEEN_MSR);
140 		cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v9);
141 	} else {
142 		/* All versions up to version 8 have the same size */
143 		cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v8);
144 	}
145 
146 	if (cmd_ver >= 8)
147 		iwl_mvm_ftm_responder_set_ndp(mvm, &cmd);
148 
149 	if (cmd_ver >= 7)
150 		err = iwl_mvm_ftm_responder_set_bw_v2(chandef, &cmd.format_bw,
151 						      &cmd.ctrl_ch_position,
152 						      cmd_ver);
153 	else
154 		err = iwl_mvm_ftm_responder_set_bw_v1(chandef, &cmd.format_bw,
155 						      &cmd.ctrl_ch_position);
156 
157 	if (err) {
158 		IWL_ERR(mvm, "Failed to set responder bandwidth\n");
159 		return err;
160 	}
161 
162 	memcpy(cmd.bssid, vif->addr, ETH_ALEN);
163 
164 	return iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0, cmd_size, &cmd);
165 }
166 
167 static int
168 iwl_mvm_ftm_responder_dyn_cfg_v2(struct iwl_mvm *mvm,
169 				 struct ieee80211_vif *vif,
170 				 struct ieee80211_ftm_responder_params *params)
171 {
172 	struct iwl_tof_responder_dyn_config_cmd_v2 cmd = {
173 		.lci_len = cpu_to_le32(params->lci_len + 2),
174 		.civic_len = cpu_to_le32(params->civicloc_len + 2),
175 	};
176 	u8 data[IWL_LCI_CIVIC_IE_MAX_SIZE] = {0};
177 	struct iwl_host_cmd hcmd = {
178 		.id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
179 		.data[0] = &cmd,
180 		.len[0] = sizeof(cmd),
181 		.data[1] = &data,
182 		/* .len[1] set later */
183 		/* may not be able to DMA from stack */
184 		.dataflags[1] = IWL_HCMD_DFL_DUP,
185 	};
186 	u32 aligned_lci_len = ALIGN(params->lci_len + 2, 4);
187 	u32 aligned_civicloc_len = ALIGN(params->civicloc_len + 2, 4);
188 	u8 *pos = data;
189 
190 	lockdep_assert_held(&mvm->mutex);
191 
192 	if (aligned_lci_len + aligned_civicloc_len > sizeof(data)) {
193 		IWL_ERR(mvm, "LCI/civicloc data too big (%zd + %zd)\n",
194 			params->lci_len, params->civicloc_len);
195 		return -ENOBUFS;
196 	}
197 
198 	pos[0] = WLAN_EID_MEASURE_REPORT;
199 	pos[1] = params->lci_len;
200 	memcpy(pos + 2, params->lci, params->lci_len);
201 
202 	pos += aligned_lci_len;
203 	pos[0] = WLAN_EID_MEASURE_REPORT;
204 	pos[1] = params->civicloc_len;
205 	memcpy(pos + 2, params->civicloc, params->civicloc_len);
206 
207 	hcmd.len[1] = aligned_lci_len + aligned_civicloc_len;
208 
209 	return iwl_mvm_send_cmd(mvm, &hcmd);
210 }
211 
212 static int
213 iwl_mvm_ftm_responder_dyn_cfg_v3(struct iwl_mvm *mvm,
214 				 struct ieee80211_vif *vif,
215 				 struct ieee80211_ftm_responder_params *params,
216 				 struct iwl_mvm_pasn_hltk_data *hltk_data)
217 {
218 	struct iwl_tof_responder_dyn_config_cmd cmd;
219 	struct iwl_host_cmd hcmd = {
220 		.id = WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
221 		.data[0] = &cmd,
222 		.len[0] = sizeof(cmd),
223 		/* may not be able to DMA from stack */
224 		.dataflags[0] = IWL_HCMD_DFL_DUP,
225 	};
226 
227 	lockdep_assert_held(&mvm->mutex);
228 
229 	cmd.valid_flags = 0;
230 
231 	if (params) {
232 		if (params->lci_len + 2 > sizeof(cmd.lci_buf) ||
233 		    params->civicloc_len + 2 > sizeof(cmd.civic_buf)) {
234 			IWL_ERR(mvm,
235 				"LCI/civic data too big (lci=%zd, civic=%zd)\n",
236 				params->lci_len, params->civicloc_len);
237 			return -ENOBUFS;
238 		}
239 
240 		cmd.lci_buf[0] = WLAN_EID_MEASURE_REPORT;
241 		cmd.lci_buf[1] = params->lci_len;
242 		memcpy(cmd.lci_buf + 2, params->lci, params->lci_len);
243 		cmd.lci_len = params->lci_len + 2;
244 
245 		cmd.civic_buf[0] = WLAN_EID_MEASURE_REPORT;
246 		cmd.civic_buf[1] = params->civicloc_len;
247 		memcpy(cmd.civic_buf + 2, params->civicloc,
248 		       params->civicloc_len);
249 		cmd.civic_len = params->civicloc_len + 2;
250 
251 		cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_LCI |
252 			IWL_RESPONDER_DYN_CFG_VALID_CIVIC;
253 	}
254 
255 	if (hltk_data) {
256 		if (hltk_data->cipher > IWL_LOCATION_CIPHER_GCMP_256) {
257 			IWL_ERR(mvm, "invalid cipher: %u\n",
258 				hltk_data->cipher);
259 			return -EINVAL;
260 		}
261 
262 		cmd.cipher = hltk_data->cipher;
263 		memcpy(cmd.addr, hltk_data->addr, sizeof(cmd.addr));
264 		memcpy(cmd.hltk_buf, hltk_data->hltk, sizeof(cmd.hltk_buf));
265 		cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_PASN_STA;
266 	}
267 
268 	return iwl_mvm_send_cmd(mvm, &hcmd);
269 }
270 
271 static int
272 iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,
273 				  struct ieee80211_vif *vif,
274 				  struct ieee80211_ftm_responder_params *params)
275 {
276 	int ret;
277 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,
278 					   WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
279 					   2);
280 
281 	switch (cmd_ver) {
282 	case 2:
283 		ret = iwl_mvm_ftm_responder_dyn_cfg_v2(mvm, vif,
284 						       params);
285 		break;
286 	case 3:
287 		ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif,
288 						       params, NULL);
289 		break;
290 	default:
291 		IWL_ERR(mvm, "Unsupported DYN_CONFIG_CMD version %u\n",
292 			cmd_ver);
293 		ret = -ENOTSUPP;
294 	}
295 
296 	return ret;
297 }
298 
299 static void iwl_mvm_resp_del_pasn_sta(struct iwl_mvm *mvm,
300 				      struct ieee80211_vif *vif,
301 				      struct iwl_mvm_pasn_sta *sta)
302 {
303 	list_del(&sta->list);
304 	iwl_mvm_rm_sta_id(mvm, vif, sta->int_sta.sta_id);
305 	iwl_mvm_dealloc_int_sta(mvm, &sta->int_sta);
306 	kfree(sta);
307 }
308 
309 int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,
310 				      struct ieee80211_vif *vif,
311 				      u8 *addr, u32 cipher, u8 *tk, u32 tk_len,
312 				      u8 *hltk, u32 hltk_len)
313 {
314 	int ret;
315 	struct iwl_mvm_pasn_sta *sta = NULL;
316 	struct iwl_mvm_pasn_hltk_data hltk_data = {
317 		.addr = addr,
318 		.hltk = hltk,
319 	};
320 	struct iwl_mvm_pasn_hltk_data *hltk_data_ptr = NULL;
321 
322 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,
323 					   WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
324 					   2);
325 
326 	lockdep_assert_held(&mvm->mutex);
327 
328 	if (cmd_ver < 3) {
329 		IWL_ERR(mvm, "Adding PASN station not supported by FW\n");
330 		return -ENOTSUPP;
331 	}
332 
333 	if ((!hltk || !hltk_len) && (!tk || !tk_len)) {
334 		IWL_ERR(mvm, "TK and HLTK not set\n");
335 		return -EINVAL;
336 	}
337 
338 	if (hltk && hltk_len) {
339 		hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);
340 		if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {
341 			IWL_ERR(mvm, "invalid cipher: %u\n", cipher);
342 			return -EINVAL;
343 		}
344 
345 		hltk_data_ptr = &hltk_data;
346 	}
347 
348 	if (tk && tk_len) {
349 		sta = kzalloc(sizeof(*sta), GFP_KERNEL);
350 		if (!sta)
351 			return -ENOBUFS;
352 
353 		ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,
354 					   cipher, tk, tk_len);
355 		if (ret) {
356 			kfree(sta);
357 			return ret;
358 		}
359 
360 		memcpy(sta->addr, addr, ETH_ALEN);
361 		list_add_tail(&sta->list, &mvm->resp_pasn_list);
362 	}
363 
364 	ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, hltk_data_ptr);
365 	if (ret && sta)
366 		iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
367 
368 	return ret;
369 }
370 
371 int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,
372 				     struct ieee80211_vif *vif, u8 *addr)
373 {
374 	struct iwl_mvm_pasn_sta *sta, *prev;
375 
376 	lockdep_assert_held(&mvm->mutex);
377 
378 	list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {
379 		if (!memcmp(sta->addr, addr, ETH_ALEN)) {
380 			iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
381 			return 0;
382 		}
383 	}
384 
385 	IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);
386 	return -EINVAL;
387 }
388 
389 int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
390 {
391 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
392 	struct ieee80211_ftm_responder_params *params;
393 	struct ieee80211_chanctx_conf ctx, *pctx;
394 	u16 *phy_ctxt_id;
395 	struct iwl_mvm_phy_ctxt *phy_ctxt;
396 	int ret;
397 
398 	params = vif->bss_conf.ftmr_params;
399 
400 	lockdep_assert_held(&mvm->mutex);
401 
402 	if (WARN_ON_ONCE(!vif->bss_conf.ftm_responder))
403 		return -EINVAL;
404 
405 	if (vif->p2p || vif->type != NL80211_IFTYPE_AP ||
406 	    !mvmvif->ap_ibss_active) {
407 		IWL_ERR(mvm, "Cannot start responder, not in AP mode\n");
408 		return -EIO;
409 	}
410 
411 	rcu_read_lock();
412 	pctx = rcu_dereference(vif->bss_conf.chanctx_conf);
413 	/* Copy the ctx to unlock the rcu and send the phy ctxt. We don't care
414 	 * about changes in the ctx after releasing the lock because the driver
415 	 * is still protected by the mutex. */
416 	ctx = *pctx;
417 	phy_ctxt_id  = (u16 *)pctx->drv_priv;
418 	rcu_read_unlock();
419 
420 	phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
421 	ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx.def,
422 				       ctx.rx_chains_static,
423 				       ctx.rx_chains_dynamic);
424 	if (ret)
425 		return ret;
426 
427 	ret = iwl_mvm_ftm_responder_cmd(mvm, vif, &ctx.def);
428 	if (ret)
429 		return ret;
430 
431 	if (params)
432 		ret = iwl_mvm_ftm_responder_dyn_cfg_cmd(mvm, vif, params);
433 
434 	return ret;
435 }
436 
437 void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,
438 				 struct ieee80211_vif *vif)
439 {
440 	struct iwl_mvm_pasn_sta *sta, *prev;
441 
442 	lockdep_assert_held(&mvm->mutex);
443 
444 	list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)
445 		iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
446 }
447 
448 void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,
449 				   struct ieee80211_vif *vif)
450 {
451 	if (!vif->bss_conf.ftm_responder)
452 		return;
453 
454 	iwl_mvm_ftm_responder_clear(mvm, vif);
455 	iwl_mvm_ftm_start_responder(mvm, vif);
456 }
457 
458 void iwl_mvm_ftm_responder_stats(struct iwl_mvm *mvm,
459 				 struct iwl_rx_cmd_buffer *rxb)
460 {
461 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
462 	struct iwl_ftm_responder_stats *resp = (void *)pkt->data;
463 	struct cfg80211_ftm_responder_stats *stats = &mvm->ftm_resp_stats;
464 	u32 flags = le32_to_cpu(resp->flags);
465 
466 	if (resp->success_ftm == resp->ftm_per_burst)
467 		stats->success_num++;
468 	else if (resp->success_ftm >= 2)
469 		stats->partial_num++;
470 	else
471 		stats->failed_num++;
472 
473 	if ((flags & FTM_RESP_STAT_ASAP_REQ) &&
474 	    (flags & FTM_RESP_STAT_ASAP_RESP))
475 		stats->asap_num++;
476 
477 	if (flags & FTM_RESP_STAT_NON_ASAP_RESP)
478 		stats->non_asap_num++;
479 
480 	stats->total_duration_ms += le32_to_cpu(resp->duration) / USEC_PER_MSEC;
481 
482 	if (flags & FTM_RESP_STAT_TRIGGER_UNKNOWN)
483 		stats->unknown_triggers_num++;
484 
485 	if (flags & FTM_RESP_STAT_DUP)
486 		stats->reschedule_requests_num++;
487 
488 	if (flags & FTM_RESP_STAT_NON_ASAP_OUT_WIN)
489 		stats->out_of_window_triggers_num++;
490 }
491