1
2
3
4
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
17struct iwl_mvm_pasn_hltk_data {
18 u8 *addr;
19 u8 cipher;
20 u8 *hltk;
21};
22
23static 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
48static int iwl_mvm_ftm_responder_set_bw_v2(struct cfg80211_chan_def *chandef,
49 u8 *format_bw,
50 u8 *ctrl_ch_position)
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 default:
72 return -ENOTSUPP;
73 }
74
75 return 0;
76}
77
78static void
79iwl_mvm_ftm_responder_set_ndp(struct iwl_mvm *mvm,
80 struct iwl_tof_responder_config_cmd_v9 *cmd)
81{
82
83 u32 r2i_max_sts = IWL_MVM_FTM_R2I_MAX_STS < 2 ?
84 IWL_MVM_FTM_R2I_MAX_STS : 1;
85
86 cmd->r2i_ndp_params = IWL_MVM_FTM_R2I_MAX_REP |
87 (r2i_max_sts << IWL_RESPONDER_STS_POS) |
88 (IWL_MVM_FTM_R2I_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);
89 cmd->i2r_ndp_params = IWL_MVM_FTM_I2R_MAX_REP |
90 (IWL_MVM_FTM_I2R_MAX_STS << IWL_RESPONDER_STS_POS) |
91 (IWL_MVM_FTM_I2R_MAX_TOTAL_LTF << IWL_RESPONDER_TOTAL_LTF_POS);
92 cmd->cmd_valid_fields |=
93 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_NDP_PARAMS);
94}
95
96static int
97iwl_mvm_ftm_responder_cmd(struct iwl_mvm *mvm,
98 struct ieee80211_vif *vif,
99 struct cfg80211_chan_def *chandef)
100{
101 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
102
103
104
105
106
107 struct iwl_tof_responder_config_cmd_v9 cmd = {
108 .channel_num = chandef->chan->hw_value,
109 .cmd_valid_fields =
110 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_CHAN_INFO |
111 IWL_TOF_RESPONDER_CMD_VALID_BSSID |
112 IWL_TOF_RESPONDER_CMD_VALID_STA_ID),
113 .sta_id = mvmvif->bcast_sta.sta_id,
114 };
115 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
116 TOF_RESPONDER_CONFIG_CMD, 6);
117 int err;
118 int cmd_size;
119
120 lockdep_assert_held(&mvm->mutex);
121
122
123 if (cmd_ver == 9) {
124 cmd.cmd_valid_fields |=
125 cpu_to_le32(IWL_TOF_RESPONDER_CMD_VALID_BSS_COLOR |
126 IWL_TOF_RESPONDER_CMD_VALID_MIN_MAX_TIME_BETWEEN_MSR);
127 cmd.bss_color = 1;
128 cmd.min_time_between_msr =
129 cpu_to_le16(IWL_MVM_FTM_NON_TB_MIN_TIME_BETWEEN_MSR);
130 cmd.max_time_between_msr =
131 cpu_to_le16(IWL_MVM_FTM_NON_TB_MAX_TIME_BETWEEN_MSR);
132 cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v9);
133 } else {
134
135 cmd_size = sizeof(struct iwl_tof_responder_config_cmd_v8);
136 }
137
138 if (cmd_ver >= 8)
139 iwl_mvm_ftm_responder_set_ndp(mvm, &cmd);
140
141 if (cmd_ver >= 7)
142 err = iwl_mvm_ftm_responder_set_bw_v2(chandef, &cmd.format_bw,
143 &cmd.ctrl_ch_position);
144 else
145 err = iwl_mvm_ftm_responder_set_bw_v1(chandef, &cmd.format_bw,
146 &cmd.ctrl_ch_position);
147
148 if (err) {
149 IWL_ERR(mvm, "Failed to set responder bandwidth\n");
150 return err;
151 }
152
153 memcpy(cmd.bssid, vif->addr, ETH_ALEN);
154
155 return iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(TOF_RESPONDER_CONFIG_CMD,
156 LOCATION_GROUP, 0),
157 0, cmd_size, &cmd);
158}
159
160static int
161iwl_mvm_ftm_responder_dyn_cfg_v2(struct iwl_mvm *mvm,
162 struct ieee80211_vif *vif,
163 struct ieee80211_ftm_responder_params *params)
164{
165 struct iwl_tof_responder_dyn_config_cmd_v2 cmd = {
166 .lci_len = cpu_to_le32(params->lci_len + 2),
167 .civic_len = cpu_to_le32(params->civicloc_len + 2),
168 };
169 u8 data[IWL_LCI_CIVIC_IE_MAX_SIZE] = {0};
170 struct iwl_host_cmd hcmd = {
171 .id = iwl_cmd_id(TOF_RESPONDER_DYN_CONFIG_CMD,
172 LOCATION_GROUP, 0),
173 .data[0] = &cmd,
174 .len[0] = sizeof(cmd),
175 .data[1] = &data,
176
177
178 .dataflags[1] = IWL_HCMD_DFL_DUP,
179 };
180 u32 aligned_lci_len = ALIGN(params->lci_len + 2, 4);
181 u32 aligned_civicloc_len = ALIGN(params->civicloc_len + 2, 4);
182 u8 *pos = data;
183
184 lockdep_assert_held(&mvm->mutex);
185
186 if (aligned_lci_len + aligned_civicloc_len > sizeof(data)) {
187 IWL_ERR(mvm, "LCI/civicloc data too big (%zd + %zd)\n",
188 params->lci_len, params->civicloc_len);
189 return -ENOBUFS;
190 }
191
192 pos[0] = WLAN_EID_MEASURE_REPORT;
193 pos[1] = params->lci_len;
194 memcpy(pos + 2, params->lci, params->lci_len);
195
196 pos += aligned_lci_len;
197 pos[0] = WLAN_EID_MEASURE_REPORT;
198 pos[1] = params->civicloc_len;
199 memcpy(pos + 2, params->civicloc, params->civicloc_len);
200
201 hcmd.len[1] = aligned_lci_len + aligned_civicloc_len;
202
203 return iwl_mvm_send_cmd(mvm, &hcmd);
204}
205
206static int
207iwl_mvm_ftm_responder_dyn_cfg_v3(struct iwl_mvm *mvm,
208 struct ieee80211_vif *vif,
209 struct ieee80211_ftm_responder_params *params,
210 struct iwl_mvm_pasn_hltk_data *hltk_data)
211{
212 struct iwl_tof_responder_dyn_config_cmd cmd;
213 struct iwl_host_cmd hcmd = {
214 .id = iwl_cmd_id(TOF_RESPONDER_DYN_CONFIG_CMD,
215 LOCATION_GROUP, 0),
216 .data[0] = &cmd,
217 .len[0] = sizeof(cmd),
218
219 .dataflags[0] = IWL_HCMD_DFL_DUP,
220 };
221
222 lockdep_assert_held(&mvm->mutex);
223
224 cmd.valid_flags = 0;
225
226 if (params) {
227 if (params->lci_len + 2 > sizeof(cmd.lci_buf) ||
228 params->civicloc_len + 2 > sizeof(cmd.civic_buf)) {
229 IWL_ERR(mvm,
230 "LCI/civic data too big (lci=%zd, civic=%zd)\n",
231 params->lci_len, params->civicloc_len);
232 return -ENOBUFS;
233 }
234
235 cmd.lci_buf[0] = WLAN_EID_MEASURE_REPORT;
236 cmd.lci_buf[1] = params->lci_len;
237 memcpy(cmd.lci_buf + 2, params->lci, params->lci_len);
238 cmd.lci_len = params->lci_len + 2;
239
240 cmd.civic_buf[0] = WLAN_EID_MEASURE_REPORT;
241 cmd.civic_buf[1] = params->civicloc_len;
242 memcpy(cmd.civic_buf + 2, params->civicloc,
243 params->civicloc_len);
244 cmd.civic_len = params->civicloc_len + 2;
245
246 cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_LCI |
247 IWL_RESPONDER_DYN_CFG_VALID_CIVIC;
248 }
249
250 if (hltk_data) {
251 if (hltk_data->cipher > IWL_LOCATION_CIPHER_GCMP_256) {
252 IWL_ERR(mvm, "invalid cipher: %u\n",
253 hltk_data->cipher);
254 return -EINVAL;
255 }
256
257 cmd.cipher = hltk_data->cipher;
258 memcpy(cmd.addr, hltk_data->addr, sizeof(cmd.addr));
259 memcpy(cmd.hltk_buf, hltk_data->hltk, sizeof(cmd.hltk_buf));
260 cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_PASN_STA;
261 }
262
263 return iwl_mvm_send_cmd(mvm, &hcmd);
264}
265
266static int
267iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,
268 struct ieee80211_vif *vif,
269 struct ieee80211_ftm_responder_params *params)
270{
271 int ret;
272 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
273 TOF_RESPONDER_DYN_CONFIG_CMD, 2);
274
275 switch (cmd_ver) {
276 case 2:
277 ret = iwl_mvm_ftm_responder_dyn_cfg_v2(mvm, vif,
278 params);
279 break;
280 case 3:
281 ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif,
282 params, NULL);
283 break;
284 default:
285 IWL_ERR(mvm, "Unsupported DYN_CONFIG_CMD version %u\n",
286 cmd_ver);
287 ret = -ENOTSUPP;
288 }
289
290 return ret;
291}
292
293static void iwl_mvm_resp_del_pasn_sta(struct iwl_mvm *mvm,
294 struct ieee80211_vif *vif,
295 struct iwl_mvm_pasn_sta *sta)
296{
297 list_del(&sta->list);
298 iwl_mvm_rm_sta_id(mvm, vif, sta->int_sta.sta_id);
299 iwl_mvm_dealloc_int_sta(mvm, &sta->int_sta);
300 kfree(sta);
301}
302
303int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,
304 struct ieee80211_vif *vif,
305 u8 *addr, u32 cipher, u8 *tk, u32 tk_len,
306 u8 *hltk, u32 hltk_len)
307{
308 int ret;
309 struct iwl_mvm_pasn_sta *sta = NULL;
310 struct iwl_mvm_pasn_hltk_data hltk_data = {
311 .addr = addr,
312 .hltk = hltk,
313 };
314 u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
315 TOF_RESPONDER_DYN_CONFIG_CMD, 2);
316
317 lockdep_assert_held(&mvm->mutex);
318
319 if (cmd_ver < 3) {
320 IWL_ERR(mvm, "Adding PASN station not supported by FW\n");
321 return -ENOTSUPP;
322 }
323
324 hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);
325 if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {
326 IWL_ERR(mvm, "invalid cipher: %u\n", cipher);
327 return -EINVAL;
328 }
329
330 if (tk && tk_len) {
331 sta = kzalloc(sizeof(*sta), GFP_KERNEL);
332 if (!sta)
333 return -ENOBUFS;
334
335 ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,
336 cipher, tk, tk_len);
337 if (ret) {
338 kfree(sta);
339 return ret;
340 }
341
342 memcpy(sta->addr, addr, ETH_ALEN);
343 list_add_tail(&sta->list, &mvm->resp_pasn_list);
344 }
345
346 ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, &hltk_data);
347 if (ret && sta)
348 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
349
350 return ret;
351}
352
353int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,
354 struct ieee80211_vif *vif, u8 *addr)
355{
356 struct iwl_mvm_pasn_sta *sta, *prev;
357
358 lockdep_assert_held(&mvm->mutex);
359
360 list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {
361 if (!memcmp(sta->addr, addr, ETH_ALEN)) {
362 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
363 return 0;
364 }
365 }
366
367 IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);
368 return -EINVAL;
369}
370
371int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
372{
373 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
374 struct ieee80211_ftm_responder_params *params;
375 struct ieee80211_chanctx_conf ctx, *pctx;
376 u16 *phy_ctxt_id;
377 struct iwl_mvm_phy_ctxt *phy_ctxt;
378 int ret;
379
380 params = vif->bss_conf.ftmr_params;
381
382 lockdep_assert_held(&mvm->mutex);
383
384 if (WARN_ON_ONCE(!vif->bss_conf.ftm_responder))
385 return -EINVAL;
386
387 if (vif->p2p || vif->type != NL80211_IFTYPE_AP ||
388 !mvmvif->ap_ibss_active) {
389 IWL_ERR(mvm, "Cannot start responder, not in AP mode\n");
390 return -EIO;
391 }
392
393 rcu_read_lock();
394 pctx = rcu_dereference(vif->chanctx_conf);
395
396
397
398 ctx = *pctx;
399 phy_ctxt_id = (u16 *)pctx->drv_priv;
400 rcu_read_unlock();
401
402 phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
403 ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx.def,
404 ctx.rx_chains_static,
405 ctx.rx_chains_dynamic);
406 if (ret)
407 return ret;
408
409 ret = iwl_mvm_ftm_responder_cmd(mvm, vif, &ctx.def);
410 if (ret)
411 return ret;
412
413 if (params)
414 ret = iwl_mvm_ftm_responder_dyn_cfg_cmd(mvm, vif, params);
415
416 return ret;
417}
418
419void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,
420 struct ieee80211_vif *vif)
421{
422 struct iwl_mvm_pasn_sta *sta, *prev;
423
424 lockdep_assert_held(&mvm->mutex);
425
426 list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)
427 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
428}
429
430void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,
431 struct ieee80211_vif *vif)
432{
433 if (!vif->bss_conf.ftm_responder)
434 return;
435
436 iwl_mvm_ftm_responder_clear(mvm, vif);
437 iwl_mvm_ftm_start_responder(mvm, vif);
438}
439
440void iwl_mvm_ftm_responder_stats(struct iwl_mvm *mvm,
441 struct iwl_rx_cmd_buffer *rxb)
442{
443 struct iwl_rx_packet *pkt = rxb_addr(rxb);
444 struct iwl_ftm_responder_stats *resp = (void *)pkt->data;
445 struct cfg80211_ftm_responder_stats *stats = &mvm->ftm_resp_stats;
446 u32 flags = le32_to_cpu(resp->flags);
447
448 if (resp->success_ftm == resp->ftm_per_burst)
449 stats->success_num++;
450 else if (resp->success_ftm >= 2)
451 stats->partial_num++;
452 else
453 stats->failed_num++;
454
455 if ((flags & FTM_RESP_STAT_ASAP_REQ) &&
456 (flags & FTM_RESP_STAT_ASAP_RESP))
457 stats->asap_num++;
458
459 if (flags & FTM_RESP_STAT_NON_ASAP_RESP)
460 stats->non_asap_num++;
461
462 stats->total_duration_ms += le32_to_cpu(resp->duration) / USEC_PER_MSEC;
463
464 if (flags & FTM_RESP_STAT_TRIGGER_UNKNOWN)
465 stats->unknown_triggers_num++;
466
467 if (flags & FTM_RESP_STAT_DUP)
468 stats->reschedule_requests_num++;
469
470 if (flags & FTM_RESP_STAT_NON_ASAP_OUT_WIN)
471 stats->out_of_window_triggers_num++;
472}
473
