1 files changed, 196 insertions, 7 deletions

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/ftm-responder.c b/drivers/net/wireless/intel/iwlwifi/mvm/ftm-responder.c
index 0b6c32098b5a..dd3662b9a5bc 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/ftm-responder.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/ftm-responder.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
- * Copyright (C) 2018 - 2019 Intel Corporation
+ * Copyright (C) 2018 - 2020 Intel Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -27,7 +27,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
- * Copyright (C) 2018 - 2019 Intel Corporation
+ * Copyright (C) 2018 - 2020 Intel Corporation
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -62,6 +62,18 @@
 #include "mvm.h"
 #include "constants.h"
 
+struct iwl_mvm_pasn_sta {
+	struct list_head list;
+	struct iwl_mvm_int_sta int_sta;
+	u8 addr[ETH_ALEN];
+};
+
+struct iwl_mvm_pasn_hltk_data {
+	u8 *addr;
+	u8 cipher;
+	u8 *hltk;
+};
+
 static int iwl_mvm_ftm_responder_set_bw_v1(struct cfg80211_chan_def *chandef,
 					   u8 *bw, u8 *ctrl_ch_position)
 {
@@ -137,7 +149,7 @@ iwl_mvm_ftm_responder_cmd(struct iwl_mvm *mvm,
 		.sta_id = mvmvif->bcast_sta.sta_id,
 	};
 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
-					   TOF_RESPONDER_CONFIG_CMD);
+					   TOF_RESPONDER_CONFIG_CMD, 6);
 	int err;
 
 	lockdep_assert_held(&mvm->mutex);
@@ -162,11 +174,11 @@ iwl_mvm_ftm_responder_cmd(struct iwl_mvm *mvm,
 }
 
 static int
-iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,
-				  struct ieee80211_vif *vif,
-				  struct ieee80211_ftm_responder_params *params)
+iwl_mvm_ftm_responder_dyn_cfg_v2(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_ftm_responder_params *params)
 {
-	struct iwl_tof_responder_dyn_config_cmd cmd = {
+	struct iwl_tof_responder_dyn_config_cmd_v2 cmd = {
 		.lci_len = cpu_to_le32(params->lci_len + 2),
 		.civic_len = cpu_to_le32(params->civicloc_len + 2),
 	};
@@ -207,6 +219,171 @@ iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,
 	return iwl_mvm_send_cmd(mvm, &hcmd);
 }
 
+static int
+iwl_mvm_ftm_responder_dyn_cfg_v3(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_ftm_responder_params *params,
+				 struct iwl_mvm_pasn_hltk_data *hltk_data)
+{
+	struct iwl_tof_responder_dyn_config_cmd cmd;
+	struct iwl_host_cmd hcmd = {
+		.id = iwl_cmd_id(TOF_RESPONDER_DYN_CONFIG_CMD,
+				 LOCATION_GROUP, 0),
+		.data[0] = &cmd,
+		.len[0] = sizeof(cmd),
+		/* may not be able to DMA from stack */
+		.dataflags[0] = IWL_HCMD_DFL_DUP,
+	};
+
+	lockdep_assert_held(&mvm->mutex);
+
+	cmd.valid_flags = 0;
+
+	if (params) {
+		if (params->lci_len + 2 > sizeof(cmd.lci_buf) ||
+		    params->civicloc_len + 2 > sizeof(cmd.civic_buf)) {
+			IWL_ERR(mvm,
+				"LCI/civic data too big (lci=%zd, civic=%zd)\n",
+				params->lci_len, params->civicloc_len);
+			return -ENOBUFS;
+		}
+
+		cmd.lci_buf[0] = WLAN_EID_MEASURE_REPORT;
+		cmd.lci_buf[1] = params->lci_len;
+		memcpy(cmd.lci_buf + 2, params->lci, params->lci_len);
+		cmd.lci_len = params->lci_len + 2;
+
+		cmd.civic_buf[0] = WLAN_EID_MEASURE_REPORT;
+		cmd.civic_buf[1] = params->civicloc_len;
+		memcpy(cmd.civic_buf + 2, params->civicloc,
+		       params->civicloc_len);
+		cmd.civic_len = params->civicloc_len + 2;
+
+		cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_LCI |
+			IWL_RESPONDER_DYN_CFG_VALID_CIVIC;
+	}
+
+	if (hltk_data) {
+		if (hltk_data->cipher > IWL_LOCATION_CIPHER_GCMP_256) {
+			IWL_ERR(mvm, "invalid cipher: %u\n",
+				hltk_data->cipher);
+			return -EINVAL;
+		}
+
+		cmd.cipher = hltk_data->cipher;
+		memcpy(cmd.addr, hltk_data->addr, sizeof(cmd.addr));
+		memcpy(cmd.hltk_buf, hltk_data->hltk, sizeof(cmd.hltk_buf));
+		cmd.valid_flags |= IWL_RESPONDER_DYN_CFG_VALID_PASN_STA;
+	}
+
+	return iwl_mvm_send_cmd(mvm, &hcmd);
+}
+
+static int
+iwl_mvm_ftm_responder_dyn_cfg_cmd(struct iwl_mvm *mvm,
+				  struct ieee80211_vif *vif,
+				  struct ieee80211_ftm_responder_params *params)
+{
+	int ret;
+	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
+					   TOF_RESPONDER_DYN_CONFIG_CMD, 2);
+
+	switch (cmd_ver) {
+	case 2:
+		ret = iwl_mvm_ftm_responder_dyn_cfg_v2(mvm, vif,
+						       params);
+		break;
+	case 3:
+		ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif,
+						       params, NULL);
+		break;
+	default:
+		IWL_ERR(mvm, "Unsupported DYN_CONFIG_CMD version %u\n",
+			cmd_ver);
+		ret = -ENOTSUPP;
+	}
+
+	return ret;
+}
+
+static void iwl_mvm_resp_del_pasn_sta(struct iwl_mvm *mvm,
+				      struct ieee80211_vif *vif,
+				      struct iwl_mvm_pasn_sta *sta)
+{
+	list_del(&sta->list);
+	iwl_mvm_rm_sta_id(mvm, vif, sta->int_sta.sta_id);
+	iwl_mvm_dealloc_int_sta(mvm, &sta->int_sta);
+	kfree(sta);
+}
+
+int iwl_mvm_ftm_respoder_add_pasn_sta(struct iwl_mvm *mvm,
+				      struct ieee80211_vif *vif,
+				      u8 *addr, u32 cipher, u8 *tk, u32 tk_len,
+				      u8 *hltk, u32 hltk_len)
+{
+	int ret;
+	struct iwl_mvm_pasn_sta *sta = NULL;
+	struct iwl_mvm_pasn_hltk_data hltk_data = {
+		.addr = addr,
+		.hltk = hltk,
+	};
+	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, LOCATION_GROUP,
+					   TOF_RESPONDER_DYN_CONFIG_CMD, 2);
+
+	lockdep_assert_held(&mvm->mutex);
+
+	if (cmd_ver < 3) {
+		IWL_ERR(mvm, "Adding PASN station not supported by FW\n");
+		return -ENOTSUPP;
+	}
+
+	hltk_data.cipher = iwl_mvm_cipher_to_location_cipher(cipher);
+	if (hltk_data.cipher == IWL_LOCATION_CIPHER_INVALID) {
+		IWL_ERR(mvm, "invalid cipher: %u\n", cipher);
+		return -EINVAL;
+	}
+
+	if (tk && tk_len) {
+		sta = kzalloc(sizeof(*sta), GFP_KERNEL);
+		if (!sta)
+			return -ENOBUFS;
+
+		ret = iwl_mvm_add_pasn_sta(mvm, vif, &sta->int_sta, addr,
+					   cipher, tk, tk_len);
+		if (ret) {
+			kfree(sta);
+			return ret;
+		}
+
+		memcpy(sta->addr, addr, ETH_ALEN);
+		list_add_tail(&sta->list, &mvm->resp_pasn_list);
+	}
+
+	ret = iwl_mvm_ftm_responder_dyn_cfg_v3(mvm, vif, NULL, &hltk_data);
+	if (ret && sta)
+		iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
+
+	return ret;
+}
+
+int iwl_mvm_ftm_resp_remove_pasn_sta(struct iwl_mvm *mvm,
+				     struct ieee80211_vif *vif, u8 *addr)
+{
+	struct iwl_mvm_pasn_sta *sta, *prev;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list) {
+		if (!memcmp(sta->addr, addr, ETH_ALEN)) {
+			iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
+			return 0;
+		}
+	}
+
+	IWL_ERR(mvm, "FTM: PASN station %pM not found\n", addr);
+	return -EINVAL;
+}
+
 int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -255,12 +432,24 @@ int iwl_mvm_ftm_start_responder(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	return ret;
 }
 
+void iwl_mvm_ftm_responder_clear(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_pasn_sta *sta, *prev;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	list_for_each_entry_safe(sta, prev, &mvm->resp_pasn_list, list)
+		iwl_mvm_resp_del_pasn_sta(mvm, vif, sta);
+}
+
 void iwl_mvm_ftm_restart_responder(struct iwl_mvm *mvm,
 				   struct ieee80211_vif *vif)
 {
 	if (!vif->bss_conf.ftm_responder)
 		return;
 
+	iwl_mvm_ftm_responder_clear(mvm, vif);
 	iwl_mvm_ftm_start_responder(mvm, vif);
 }