sys/contrib/dev/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-2021 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->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 #if defined(__linux__)
 310 int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,
 311                                       struct ieee80211_vif *vif,
 312                                       u8 *addr, u32 cipher, u8 *tk, u32 tk_len,
 313                                       u8 *hltk, u32 hltk_len)
 314 {
 315         int ret;
 316         struct iwl_mvm_pasn_sta *sta = NULL;
 317         struct iwl_mvm_pasn_hltk_data hltk_data = {
 318                 .addr = addr,
 319                 .hltk = hltk,
 320         };
 321         u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw,
 322                                            WIDE_ID(LOCATION_GROUP, TOF_RESPONDER_DYN_CONFIG_CMD),
 323                                            2);
 324
 325         lockdep_assert_held(&mvm->mutex);
 326
 327         if (cmd_ver < 3) {
 328                 IWL_ERR(mvm, "Adding PASN station not supported by FW\n");
 329                 return -ENOTSUPP;
 330         }
 331
 332         hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);
 333         if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {
 334                 IWL_ERR(mvm, "invalid cipher: %u\n", cipher);
 335                 return -EINVAL;
 336         }
 337
 338         if (tk && tk_len) {
 339                 sta = kzalloc(sizeof(*sta), GFP_KERNEL);
 340                 if (!sta)
 341                         return -ENOBUFS;
 342
 343                 ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,
 344                                            cipher, tk, tk_len);
 345                 if (ret) {
 346                         kfree(sta);
 347                         return ret;
 348                 }
 349
 350                 memcpy(sta->addr, addr, ETH_ALEN);
 351                 list_add_tail(&sta->list, &mvm->resp_pasn_list);
 352         }
 353
 354         ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, &hltk_data);
 355         if (ret && sta)
 356                 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 357
 358         return ret;
 359 }
 360
 361 int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,
 362                                      struct ieee80211_vif *vif, u8 *addr)
 363 {
 364         struct iwl_mvm_pasn_sta *sta, *prev;
 365
 366         lockdep_assert_held(&mvm->mutex);
 367
 368         list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {
 369                 if (!memcmp(sta->addr, addr, ETH_ALEN)) {
 370                         iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 371                         return 0;
 372                 }
 373         }
 374
 375         IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);
 376         return -EINVAL;
 377 }
 378 #endif
 379
 380 int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 381 {
 382         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 383         struct ieee80211_ftm_responder_params *params;
 384         struct ieee80211_chanctx_conf ctx, *pctx;
 385         u16 *phy_ctxt_id;
 386         struct iwl_mvm_phy_ctxt *phy_ctxt;
 387         int ret;
 388
 389         params = vif->bss_conf.ftmr_params;
 390
 391         lockdep_assert_held(&mvm->mutex);
 392
 393         if (WARN_ON_ONCE(!vif->bss_conf.ftm_responder))
 394                 return -EINVAL;
 395
 396         if (vif->p2p || vif->type != NL80211_IFTYPE_AP ||
 397             !mvmvif->ap_ibss_active) {
 398                 IWL_ERR(mvm, "Cannot start responder, not in AP mode\n");
 399                 return -EIO;
 400         }
 401
 402         rcu_read_lock();
 403         pctx = rcu_dereference(vif->chanctx_conf);
 404         /* Copy the ctx to unlock the rcu and send the phy ctxt. We don't care
 405          * about changes in the ctx after releasing the lock because the driver
 406          * is still protected by the mutex. */
 407         ctx = *pctx;
 408         phy_ctxt_id  = (u16 *)pctx->drv_priv;
 409         rcu_read_unlock();
 410
 411         phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
 412         ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx.def,
 413                                        ctx.rx_chains_static,
 414                                        ctx.rx_chains_dynamic);
 415         if (ret)
 416                 return ret;
 417
 418         ret = iwl_mvm_ftm_responder_cmd(mvm, vif, &ctx.def);
 419         if (ret)
 420                 return ret;
 421
 422         if (params)
 423                 ret = iwl_mvm_ftm_responder_dyn_cfg_cmd(mvm, vif, params);
 424
 425         return ret;
 426 }
 427
 428 void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,
 429                                  struct ieee80211_vif *vif)
 430 {
 431         struct iwl_mvm_pasn_sta *sta, *prev;
 432
 433         lockdep_assert_held(&mvm->mutex);
 434
 435         list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)
 436                 iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
 437 }
 438
 439 void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,
 440                                    struct ieee80211_vif *vif)
 441 {
 442         if (!vif->bss_conf.ftm_responder)
 443                 return;
 444
 445         iwl_mvm_ftm_responder_clear(mvm, vif);
 446         iwl_mvm_ftm_start_responder(mvm, vif);
 447 }
 448
 449 void iwl_mvm_ftm_responder_stats(struct iwl_mvm *mvm,
 450                                  struct iwl_rx_cmd_buffer *rxb)
 451 {
 452         struct iwl_rx_packet *pkt = rxb_addr(rxb);
 453         struct iwl_ftm_responder_stats *resp = (void *)pkt->data;
 454         struct cfg80211_ftm_responder_stats *stats = &mvm->ftm_resp_stats;
 455         u32 flags = le32_to_cpu(resp->flags);
 456
 457         if (resp->success_ftm == resp->ftm_per_burst)
 458                 stats->success_num++;
 459         else if (resp->success_ftm >= 2)
 460                 stats->partial_num++;
 461         else
 462                 stats->failed_num++;
 463
 464         if ((flags & FTM_RESP_STAT_ASAP_REQ) &&
 465             (flags & FTM_RESP_STAT_ASAP_RESP))
 466                 stats->asap_num++;
 467
 468         if (flags & FTM_RESP_STAT_NON_ASAP_RESP)
 469                 stats->non_asap_num++;
 470
 471         stats->total_duration_ms += le32_to_cpu(resp->duration) / USEC_PER_MSEC;
 472
 473         if (flags & FTM_RESP_STAT_TRIGGER_UNKNOWN)
 474                 stats->unknown_triggers_num++;
 475
 476         if (flags & FTM_RESP_STAT_DUP)
 477                 stats->reschedule_requests_num++;
 478
 479         if (flags & FTM_RESP_STAT_NON_ASAP_OUT_WIN)
 480                 stats->out_of_window_triggers_num++;
 481 }