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