1 files changed, 193 insertions, 66 deletions

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c b/drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c
index 72a3d71f46f0..afd90a52d4ec 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/ftm-initiator.c
@@ -40,6 +40,12 @@ struct iwl_mvm_ftm_pasn_entry {
 	u32 flags;
 };
 
+struct iwl_mvm_ftm_iter_data {
+	u8 *cipher;
+	u8 *bssid;
+	u8 *tk;
+};
+
 int iwl_mvm_ftm_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)
@@ -431,47 +437,55 @@ iwl_mvm_ftm_put_target_v2(struct iwl_mvm *mvm,
 	return 0;
 }
 
-#define FTM_PUT_FLAG(flag)	(target->initiator_ap_flags |= \
+#define FTM_SET_FLAG(flag)	(*flags |= \
 				 cpu_to_le32(IWL_INITIATOR_AP_FLAGS_##flag))
 
 static void
-iwl_mvm_ftm_put_target_common(struct iwl_mvm *mvm,
-			      struct cfg80211_pmsr_request_peer *peer,
-			      struct iwl_tof_range_req_ap_entry_v6 *target)
+iwl_mvm_ftm_set_target_flags(struct iwl_mvm *mvm,
+			     struct cfg80211_pmsr_request_peer *peer,
+			     __le32 *flags)
 {
-	memcpy(target->bssid, peer->addr, ETH_ALEN);
-	target->burst_period =
-		cpu_to_le16(peer->ftm.burst_period);
-	target->samples_per_burst = peer->ftm.ftms_per_burst;
-	target->num_of_bursts = peer->ftm.num_bursts_exp;
-	target->ftmr_max_retries = peer->ftm.ftmr_retries;
-	target->initiator_ap_flags = cpu_to_le32(0);
+	*flags = cpu_to_le32(0);
 
 	if (peer->ftm.asap)
-		FTM_PUT_FLAG(ASAP);
+		FTM_SET_FLAG(ASAP);
 
 	if (peer->ftm.request_lci)
-		FTM_PUT_FLAG(LCI_REQUEST);
+		FTM_SET_FLAG(LCI_REQUEST);
 
 	if (peer->ftm.request_civicloc)
-		FTM_PUT_FLAG(CIVIC_REQUEST);
+		FTM_SET_FLAG(CIVIC_REQUEST);
 
 	if (IWL_MVM_FTM_INITIATOR_DYNACK)
-		FTM_PUT_FLAG(DYN_ACK);
+		FTM_SET_FLAG(DYN_ACK);
 
 	if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_LINEAR_REG)
-		FTM_PUT_FLAG(ALGO_LR);
+		FTM_SET_FLAG(ALGO_LR);
 	else if (IWL_MVM_FTM_INITIATOR_ALGO == IWL_TOF_ALGO_TYPE_FFT)
-		FTM_PUT_FLAG(ALGO_FFT);
+		FTM_SET_FLAG(ALGO_FFT);
 
 	if (peer->ftm.trigger_based)
-		FTM_PUT_FLAG(TB);
+		FTM_SET_FLAG(TB);
 	else if (peer->ftm.non_trigger_based)
-		FTM_PUT_FLAG(NON_TB);
+		FTM_SET_FLAG(NON_TB);
 
 	if ((peer->ftm.trigger_based || peer->ftm.non_trigger_based) &&
 	    peer->ftm.lmr_feedback)
-		FTM_PUT_FLAG(LMR_FEEDBACK);
+		FTM_SET_FLAG(LMR_FEEDBACK);
+}
+
+static void
+iwl_mvm_ftm_put_target_common(struct iwl_mvm *mvm,
+			      struct cfg80211_pmsr_request_peer *peer,
+			      struct iwl_tof_range_req_ap_entry_v6 *target)
+{
+	memcpy(target->bssid, peer->addr, ETH_ALEN);
+	target->burst_period =
+		cpu_to_le16(peer->ftm.burst_period);
+	target->samples_per_burst = peer->ftm.ftms_per_burst;
+	target->num_of_bursts = peer->ftm.num_bursts_exp;
+	target->ftmr_max_retries = peer->ftm.ftmr_retries;
+	iwl_mvm_ftm_set_target_flags(mvm, peer, &target->initiator_ap_flags);
 }
 
 static int
@@ -514,21 +528,10 @@ iwl_mvm_ftm_put_target_v4(struct iwl_mvm *mvm,
 	return 0;
 }
 
-static int
-iwl_mvm_ftm_put_target(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-		       struct cfg80211_pmsr_request_peer *peer,
-		       struct iwl_tof_range_req_ap_entry_v6 *target)
+static int iwl_mvm_ftm_set_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			       struct cfg80211_pmsr_request_peer *peer,
+			       u8 *sta_id, __le32 *flags)
 {
-	int ret;
-
-	ret = iwl_mvm_ftm_target_chandef_v2(mvm, peer, &target->channel_num,
-					    &target->format_bw,
-					    &target->ctrl_ch_position);
-	if (ret)
-		return ret;
-
-	iwl_mvm_ftm_put_target_common(mvm, peer, target);
-
 	if (vif->cfg.assoc) {
 		struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 		struct ieee80211_sta *sta;
@@ -540,8 +543,8 @@ iwl_mvm_ftm_put_target(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			if (memcmp(peer->addr, link_conf->bssid, ETH_ALEN))
 				continue;
 
-			target->sta_id = mvmvif->link[link_id]->ap_sta_id;
-			sta = rcu_dereference(mvm->fw_id_to_mac_id[target->sta_id]);
+			*sta_id = mvmvif->link[link_id]->ap_sta_id;
+			sta = rcu_dereference(mvm->fw_id_to_mac_id[*sta_id]);
 			if (WARN_ON_ONCE(IS_ERR_OR_NULL(sta))) {
 				rcu_read_unlock();
 				return PTR_ERR_OR_ZERO(sta);
@@ -549,23 +552,42 @@ iwl_mvm_ftm_put_target(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 			if (sta->mfp && (peer->ftm.trigger_based ||
 					 peer->ftm.non_trigger_based))
-				FTM_PUT_FLAG(PMF);
+				FTM_SET_FLAG(PMF);
 			break;
 		}
 		rcu_read_unlock();
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 		if (mvmvif->ftm_unprotected) {
-			target->sta_id = IWL_MVM_INVALID_STA;
-			target->initiator_ap_flags &=
-				~cpu_to_le32(IWL_INITIATOR_AP_FLAGS_PMF);
+			*sta_id = IWL_MVM_INVALID_STA;
+			*flags &= ~cpu_to_le32(IWL_INITIATOR_AP_FLAGS_PMF);
 		}
-
 #endif
 	} else {
-		target->sta_id = IWL_MVM_INVALID_STA;
+		*sta_id = IWL_MVM_INVALID_STA;
 	}
 
+	return 0;
+}
+
+static int
+iwl_mvm_ftm_put_target(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+		       struct cfg80211_pmsr_request_peer *peer,
+		       struct iwl_tof_range_req_ap_entry_v6 *target)
+{
+	int ret;
+
+	ret = iwl_mvm_ftm_target_chandef_v2(mvm, peer, &target->channel_num,
+					    &target->format_bw,
+					    &target->ctrl_ch_position);
+	if (ret)
+		return ret;
+
+	iwl_mvm_ftm_put_target_common(mvm, peer, target);
+
+	iwl_mvm_ftm_set_sta(mvm, vif, peer, &target->sta_id,
+			    &target->initiator_ap_flags);
+
 	/*
 	 * TODO: Beacon interval is currently unknown, so use the common value
 	 * of 100 TUs.
@@ -703,27 +725,24 @@ static void iter(struct ieee80211_hw *hw,
 		 struct ieee80211_key_conf *key,
 		 void *data)
 {
-	struct iwl_tof_range_req_ap_entry_v6 *target = data;
+	struct iwl_mvm_ftm_iter_data *target = data;
 
 	if (!sta || memcmp(sta->addr, target->bssid, ETH_ALEN))
 		return;
 
 	WARN_ON(!sta->mfp);
 
-	if (WARN_ON(key->keylen > sizeof(target->tk)))
-		return;
-
-	memcpy(target->tk, key->key, key->keylen);
-	target->cipher = iwl_mvm_cipher_to_location_cipher(key->cipher);
-	WARN_ON(target->cipher == IWL_LOCATION_CIPHER_INVALID);
+	target->tk = key->key;
+	*target->cipher = iwl_mvm_cipher_to_location_cipher(key->cipher);
+	WARN_ON(*target->cipher == IWL_LOCATION_CIPHER_INVALID);
 }
 
 static void
 iwl_mvm_ftm_set_secured_ranging(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-				struct iwl_tof_range_req_ap_entry_v7 *target)
+				u8 *bssid, u8 *cipher, u8 *hltk, u8 *tk,
+				u8 *rx_pn, u8 *tx_pn, __le32 *flags)
 {
 	struct iwl_mvm_ftm_pasn_entry *entry;
-	u32 flags = le32_to_cpu(target->initiator_ap_flags);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
@@ -731,35 +750,37 @@ iwl_mvm_ftm_set_secured_ranging(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		return;
 #endif
 
-	if (!(flags & (IWL_INITIATOR_AP_FLAGS_NON_TB |
+	if (!(le32_to_cpu(*flags) & (IWL_INITIATOR_AP_FLAGS_NON_TB |
 		       IWL_INITIATOR_AP_FLAGS_TB)))
 		return;
 
 	lockdep_assert_held(&mvm->mutex);
 
 	list_for_each_entry(entry, &mvm->ftm_initiator.pasn_list, list) {
-		if (memcmp(entry->addr, target->bssid, sizeof(entry->addr)))
+		if (memcmp(entry->addr, bssid, sizeof(entry->addr)))
 			continue;
 
-		target->cipher = entry->cipher;
+		*cipher = entry->cipher;
 
 		if (entry->flags & IWL_MVM_PASN_FLAG_HAS_HLTK)
-			memcpy(target->hltk, entry->hltk, sizeof(target->hltk));
+			memcpy(hltk, entry->hltk, sizeof(entry->hltk));
 		else
-			memset(target->hltk, 0, sizeof(target->hltk));
+			memset(hltk, 0, sizeof(entry->hltk));
 
 		if (vif->cfg.assoc &&
-		    !memcmp(vif->bss_conf.bssid, target->bssid,
-			    sizeof(target->bssid)))
-			ieee80211_iter_keys(mvm->hw, vif, iter, target);
-		else
-			memcpy(target->tk, entry->tk, sizeof(target->tk));
+		    !memcmp(vif->bss_conf.bssid, bssid, ETH_ALEN)) {
+			struct iwl_mvm_ftm_iter_data target;
+
+			target.bssid = bssid;
+			ieee80211_iter_keys(mvm->hw, vif, iter, &target);
+		} else {
+			memcpy(tk, entry->tk, sizeof(entry->tk));
+		}
 
-		memcpy(target->rx_pn, entry->rx_pn, sizeof(target->rx_pn));
-		memcpy(target->tx_pn, entry->tx_pn, sizeof(target->tx_pn));
+		memcpy(rx_pn, entry->rx_pn, sizeof(entry->rx_pn));
+		memcpy(tx_pn, entry->tx_pn, sizeof(entry->tx_pn));
 
-		target->initiator_ap_flags |=
-			cpu_to_le32(IWL_INITIATOR_AP_FLAGS_SECURED);
+		FTM_SET_FLAG(SECURED);
 		return;
 	}
 }
@@ -773,7 +794,11 @@ iwl_mvm_ftm_put_target_v7(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	if (err)
 		return err;
 
-	iwl_mvm_ftm_set_secured_ranging(mvm, vif, target);
+	iwl_mvm_ftm_set_secured_ranging(mvm, vif, target->bssid,
+					&target->cipher, target->hltk,
+					target->tk, target->rx_pn,
+					target->tx_pn,
+					&target->initiator_ap_flags);
 	return err;
 }
 
@@ -920,6 +945,105 @@ static int iwl_mvm_ftm_start_v13(struct iwl_mvm *mvm,
 	return iwl_mvm_ftm_send_cmd(mvm, &hcmd);
 }
 
+static int
+iwl_mvm_ftm_put_target_v10(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			   struct cfg80211_pmsr_request_peer *peer,
+			   struct iwl_tof_range_req_ap_entry_v10 *target)
+{
+	u32 i2r_max_sts, flags;
+	int ret;
+
+	ret = iwl_mvm_ftm_target_chandef_v2(mvm, peer, &target->channel_num,
+					    &target->format_bw,
+					    &target->ctrl_ch_position);
+	if (ret)
+		return ret;
+
+	memcpy(target->bssid, peer->addr, ETH_ALEN);
+	target->burst_period =
+		cpu_to_le16(peer->ftm.burst_period);
+	target->samples_per_burst = peer->ftm.ftms_per_burst;
+	target->num_of_bursts = peer->ftm.num_bursts_exp;
+	iwl_mvm_ftm_set_target_flags(mvm, peer, &target->initiator_ap_flags);
+	iwl_mvm_ftm_set_sta(mvm, vif, peer, &target->sta_id,
+			    &target->initiator_ap_flags);
+	iwl_mvm_ftm_set_secured_ranging(mvm, vif, target->bssid,
+					&target->cipher, target->hltk,
+					target->tk, target->rx_pn,
+					target->tx_pn,
+					&target->initiator_ap_flags);
+
+	i2r_max_sts = IWL_MVM_FTM_I2R_MAX_STS > 1 ? 1 :
+		IWL_MVM_FTM_I2R_MAX_STS;
+
+	target->r2i_ndp_params = IWL_MVM_FTM_R2I_MAX_REP |
+		(IWL_MVM_FTM_R2I_MAX_STS << IWL_LOCATION_MAX_STS_POS) |
+		(IWL_MVM_FTM_R2I_MAX_TOTAL_LTF << IWL_LOCATION_TOTAL_LTF_POS);
+	target->i2r_ndp_params = IWL_MVM_FTM_I2R_MAX_REP |
+		(i2r_max_sts << IWL_LOCATION_MAX_STS_POS) |
+		(IWL_MVM_FTM_I2R_MAX_TOTAL_LTF << IWL_LOCATION_TOTAL_LTF_POS);
+
+	if (peer->ftm.non_trigger_based) {
+		target->min_time_between_msr =
+			cpu_to_le16(IWL_MVM_FTM_NON_TB_MIN_TIME_BETWEEN_MSR);
+		target->burst_period =
+			cpu_to_le16(IWL_MVM_FTM_NON_TB_MAX_TIME_BETWEEN_MSR);
+	} else {
+		target->min_time_between_msr = cpu_to_le16(0);
+	}
+
+	target->band =
+		iwl_mvm_phy_band_from_nl80211(peer->chandef.chan->band);
+
+	/*
+	 * TODO: Beacon interval is currently unknown, so use the common value
+	 * of 100 TUs.
+	 */
+	target->beacon_interval = cpu_to_le16(100);
+
+	/*
+	 * If secure LTF is turned off, replace the flag with PMF only
+	 */
+	flags = le32_to_cpu(target->initiator_ap_flags);
+	if (flags & IWL_INITIATOR_AP_FLAGS_SECURED) {
+		if (!IWL_MVM_FTM_INITIATOR_SECURE_LTF)
+			flags &= ~IWL_INITIATOR_AP_FLAGS_SECURED;
+
+		flags |= IWL_INITIATOR_AP_FLAGS_PMF;
+		target->initiator_ap_flags = cpu_to_le32(flags);
+	}
+
+	return 0;
+}
+
+static int iwl_mvm_ftm_start_v14(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif,
+				 struct cfg80211_pmsr_request *req)
+{
+	struct iwl_tof_range_req_cmd_v14 cmd;
+	struct iwl_host_cmd hcmd = {
+		.id = WIDE_ID(LOCATION_GROUP, TOF_RANGE_REQ_CMD),
+		.dataflags[0] = IWL_HCMD_DFL_DUP,
+		.data[0] = &cmd,
+		.len[0] = sizeof(cmd),
+	};
+	u8 i;
+	int err;
+
+	iwl_mvm_ftm_cmd_common(mvm, vif, (void *)&cmd, req);
+
+	for (i = 0; i < cmd.num_of_ap; i++) {
+		struct cfg80211_pmsr_request_peer *peer = &req->peers[i];
+		struct iwl_tof_range_req_ap_entry_v10 *target = &cmd.ap[i];
+
+		err = iwl_mvm_ftm_put_target_v10(mvm, vif, peer, target);
+		if (err)
+			return err;
+	}
+
+	return iwl_mvm_ftm_send_cmd(mvm, &hcmd);
+}
+
 int iwl_mvm_ftm_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		      struct cfg80211_pmsr_request *req)
 {
@@ -938,6 +1062,9 @@ int iwl_mvm_ftm_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 						   IWL_FW_CMD_VER_UNKNOWN);
 
 		switch (cmd_ver) {
+		case 14:
+			err = iwl_mvm_ftm_start_v14(mvm, vif, req);
+			break;
 		case 13:
 			err = iwl_mvm_ftm_start_v13(mvm, vif, req);
 			break;