From a6dfbd040e26d0fa429545d37ca9d3dc8a80cf3b Mon Sep 17 00:00:00 2001
From: Johannes Berg <johannes.berg@intel.com>
Date: Mon, 2 Aug 2021 17:28:19 +0300
Subject: [PATCH] iwlwifi: mvm: set replay counter on key install
Git-commit: a6dfbd040e26d0fa429545d37ca9d3dc8a80cf3b
Patch-mainline: v5.15-rc1
References: jsc#SLE-19360

When installing a (new) key, set the replay counter so that
after FW restart the firmware has the correct value of the
replay counters.

This doesn't have a large effect - for frames that reach
the driver, it will do a replay check, and when installing
a new key, the counter is normally zero to start with (not
for GTK though, if joining the BSS for the first time).

Since this only affects frames handled entirely by the FW,
and that's restricted to a few unicast management frames,
the only affect here is for those after a firmware restart.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: Luca Coelho <luciano.coelho@intel.com>
Link: https://lore.kernel.org/r/iwlwifi.20210802172232.1cedf2ca7bb6.I2e609c28eaa301436e6740f4f1beca838f69a96a@changeid
Acked-by: Takashi Iwai <tiwai@suse.de>

---
 drivers/net/wireless/intel/iwlwifi/mvm/sta.c | 53 ++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/sta.c b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
index 9c45a64c5009..0ec84d8ff9e6 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
@@ -3190,6 +3190,20 @@ static struct iwl_mvm_sta *iwl_mvm_get_key_sta(struct iwl_mvm *mvm,
 	return NULL;
 }
 
+static int iwl_mvm_pn_cmp(const u8 *pn1, const u8 *pn2, int len)
+{
+	int i;
+
+	for (i = len - 1; i >= 0; i--) {
+		if (pn1[i] > pn2[i])
+			return 1;
+		if (pn1[i] < pn2[i])
+			return -1;
+	}
+
+	return 0;
+}
+
 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
 				u32 sta_id,
 				struct ieee80211_key_conf *key, bool mcast,
@@ -3274,6 +3288,45 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
 	u.cmd.common.key_flags = key_flags;
 	u.cmd.common.sta_id = sta_id;
 
+	if (key->cipher == WLAN_CIPHER_SUITE_TKIP)
+		i = 0;
+	else
+		i = -1;
+
+	for (; i < IEEE80211_NUM_TIDS; i++) {
+		struct ieee80211_key_seq seq = {};
+		u8 _rx_pn[IEEE80211_MAX_PN_LEN] = {}, *rx_pn = _rx_pn;
+		int rx_pn_len = 8;
+
+		ieee80211_get_key_rx_seq(key, i, &seq);
+
+		if (key->cipher == WLAN_CIPHER_SUITE_TKIP) {
+			rx_pn[0] = seq.tkip.iv16;
+			rx_pn[1] = seq.tkip.iv16 >> 8;
+			/* hole at 2/3 in FW format */
+			rx_pn[4] = seq.tkip.iv32;
+			rx_pn[5] = seq.tkip.iv32 >> 8;
+			rx_pn[6] = seq.tkip.iv32 >> 16;
+			rx_pn[7] = seq.tkip.iv32 >> 24;
+		} else if (key_flags & cpu_to_le16(STA_KEY_FLG_EXT)) {
+			rx_pn = seq.hw.seq;
+			rx_pn_len = seq.hw.seq_len;
+		} else {
+			rx_pn[0] = seq.ccmp.pn[0];
+			rx_pn[1] = seq.ccmp.pn[1];
+			/* hole at 2/3 in FW format */
+			rx_pn[4] = seq.ccmp.pn[2];
+			rx_pn[5] = seq.ccmp.pn[3];
+			rx_pn[6] = seq.ccmp.pn[4];
+			rx_pn[7] = seq.ccmp.pn[5];
+		}
+
+		if (iwl_mvm_pn_cmp(rx_pn, (u8 *)&u.cmd.common.rx_secur_seq_cnt,
+				   rx_pn_len) > 0)
+			memcpy(&u.cmd.common.rx_secur_seq_cnt, rx_pn,
+			       rx_pn_len);
+	}
+
 	if (new_api) {
 		u.cmd.transmit_seq_cnt = cpu_to_le64(pn);
 		size = sizeof(u.cmd);
-- 
2.26.2