ftm-responder.c

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