drivers/net/wireless/intel/iwlwifi/mvm/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
  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         /* must be last as it followed by buffer holding the key */
  17         struct ieee80211_key_conf keyconf;
  18 };
  19
  20 struct iwl_mvm_pasn_hltk_data {
  21         u8 *addr;
  22         u8 cipher;
  23         u8 *hltk;
  24 };
  25
  26 static 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
  51 static 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
  89 static void
  90 iwl_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
 107 static int
 108 iwl_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
 176 static int
 177 iwl_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
 221 static int
 222 iwl_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
 280 static int
 281 iwl_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
 308 static 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
 327 int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,
 328                                       struct ieee80211_vif *vif,
 329                                       u8 *addr, u32 cipher, u8 *tk, u32 tk_len,
 330                                       u8 *hltk, u32 hltk_len)
 331 {
 332         int ret;
 333         struct iwl_mvm_pasn_sta *sta = NULL;
 334         struct iwl_mvm_pasn_hltk_data hltk_data = {
 335                 .addr = addr,
 336                 .hltk = hltk,
 337         };
 338         struct iwl_mvm_pasn_hltk_data *hltk_data_ptr = NULL;
 339
 340         u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,
 341                                            WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
 342                                            2);
 343
 344         lockdep_assert_held(&mvm->mutex);
 345
 346         if (cmd_ver < 3) {
 347                 IWL_ERR(mvm, "Adding PASN station not supported by FW\n");
 348                 return -EOPNOTSUPP;
 349         }
 350
 351         if ((!hltk || !hltk_len) && (!tk || !tk_len)) {
 352                 IWL_ERR(mvm, "TK and HLTK not set\n");
 353                 return -EINVAL;
 354         }
 355
 356         if (hltk && hltk_len) {
 357                 if (!fw_has_capa(&mvm->fw->ucode_capa,
 358                                  IWL_UCODE_TLV_CAPA_SECURE_LTF_SUPPORT)) {
 359                         IWL_ERR(mvm, "No support for secure LTF measurement\n");
 360                         return -EINVAL;
 361                 }
 362
 363                 hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);
 364                 if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {
 365                         IWL_ERR(mvm, "invalid cipher: %u\n", cipher);
 366                         return -EINVAL;
 367                 }
 368
 369                 hltk_data_ptr = &hltk_data;
 370         }
 371
 372         if (tk && tk_len) {
 373                 sta = kzalloc(sizeof(*sta) + tk_len, GFP_KERNEL);
 374                 if (!sta)
 375                         return -ENOBUFS;
 376
 377                 ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,
 378                                            cipher, tk, tk_len, &sta->keyconf);
 379                 if (ret) {
 380                         kfree(sta);
 381                         return ret;
 382                 }
 383
 384                 memcpy(sta->addr, addr, ETH_ALEN);
 385                 list_add_tail(&sta->list, &mvm->resp_pasn_list);
 386         }
 387
 388         ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, hltk_data_ptr);
 389         if (ret && sta)
 390                 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 391
 392         return ret;
 393 }
 394
 395 int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,
 396                                      struct ieee80211_vif *vif, u8 *addr)
 397 {
 398         struct iwl_mvm_pasn_sta *sta, *prev;
 399
 400         lockdep_assert_held(&mvm->mutex);
 401
 402         list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {
 403                 if (!memcmp(sta->addr, addr, ETH_ALEN)) {
 404                         iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 405                         return 0;
 406                 }
 407         }
 408
 409         IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);
 410         return -EINVAL;
 411 }
 412
 413 int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 414                                 struct ieee80211_bss_conf *bss_conf)
 415 {
 416         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 417         struct ieee80211_ftm_responder_params *params;
 418         struct ieee80211_chanctx_conf ctx, *pctx;
 419         u16 *phy_ctxt_id;
 420         struct iwl_mvm_phy_ctxt *phy_ctxt;
 421         int ret;
 422
 423         params = bss_conf->ftmr_params;
 424
 425         lockdep_assert_held(&mvm->mutex);
 426
 427         if (WARN_ON_ONCE(!bss_conf->ftm_responder))
 428                 return -EINVAL;
 429
 430         if (vif->p2p || vif->type != NL80211_IFTYPE_AP ||
 431             !mvmvif->ap_ibss_active) {
 432                 IWL_ERR(mvm, "Cannot start responder, not in AP mode\n");
 433                 return -EIO;
 434         }
 435
 436         rcu_read_lock();
 437         pctx = rcu_dereference(bss_conf->chanctx_conf);
 438         /* Copy the ctx to unlock the rcu and send the phy ctxt. We don't care
 439          * about changes in the ctx after releasing the lock because the driver
 440          * is still protected by the mutex. */
 441         ctx = *pctx;
 442         phy_ctxt_id  = (u16 *)pctx->drv_priv;
 443         rcu_read_unlock();
 444
 445         phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
 446         ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx.def, &ctx.ap,
 447                                        ctx.rx_chains_static,
 448                                        ctx.rx_chains_dynamic);
 449         if (ret)
 450                 return ret;
 451
 452         ret = iwl_mvm_ftm_responder_cmd(mvm, vif, &ctx.def, bss_conf);
 453         if (ret)
 454                 return ret;
 455
 456         if (params)
 457                 ret = iwl_mvm_ftm_responder_dyn_cfg_cmd(mvm, vif, params);
 458
 459         return ret;
 460 }
 461
 462 void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,
 463                                  struct ieee80211_vif *vif)
 464 {
 465         struct iwl_mvm_pasn_sta *sta, *prev;
 466
 467         lockdep_assert_held(&mvm->mutex);
 468
 469         list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)
 470                 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 471 }
 472
 473 void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,
 474                                    struct ieee80211_vif *vif,
 475                                    struct ieee80211_bss_conf *bss_conf)
 476 {
 477         if (!bss_conf->ftm_responder)
 478                 return;
 479
 480         iwl_mvm_ftm_responder_clear(mvm, vif);
 481         iwl_mvm_ftm_start_responder(mvm, vif, bss_conf);
 482 }
 483
 484 void iwl_mvm_ftm_responder_stats(struct iwl_mvm *mvm,
 485                                  struct iwl_rx_cmd_buffer *rxb)
 486 {
 487         struct iwl_rx_packet *pkt = rxb_addr(rxb);
 488         struct iwl_ftm_responder_stats *resp = (void *)pkt->data;
 489         struct cfg80211_ftm_responder_stats *stats = &mvm->ftm_resp_stats;
 490         u32 flags = le32_to_cpu(resp->flags);
 491
 492         if (resp->success_ftm == resp->ftm_per_burst)
 493                 stats->success_num++;
 494         else if (resp->success_ftm >= 2)
 495                 stats->partial_num++;
 496         else
 497                 stats->failed_num++;
 498
 499         if ((flags & FTM_RESP_STAT_ASAP_REQ) &&
 500             (flags & FTM_RESP_STAT_ASAP_RESP))
 501                 stats->asap_num++;
 502
 503         if (flags & FTM_RESP_STAT_NON_ASAP_RESP)
 504                 stats->non_asap_num++;
 505
 506         stats->total_duration_ms += le32_to_cpu(resp->duration) / USEC_PER_MSEC;
 507
 508         if (flags & FTM_RESP_STAT_TRIGGER_UNKNOWN)
 509                 stats->unknown_triggers_num++;
 510
 511         if (flags & FTM_RESP_STAT_DUP)
 512                 stats->reschedule_requests_num++;
 513
 514         if (flags & FTM_RESP_STAT_NON_ASAP_OUT_WIN)
 515                 stats->out_of_window_triggers_num++;
 516 }