1 files changed, 77 insertions, 27 deletions

diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index c64d864799c..994c8c263dc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -61,6 +61,7 @@
  *
  *****************************************************************************/
 
+#include <linux/etherdevice.h>
 #include <net/cfg80211.h>
 #include <net/ipv6.h>
 #include "iwl-modparams.h"
@@ -192,6 +193,11 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 					   sizeof(wkc), &wkc);
 		data->error = ret != 0;
 
+		mvm->ptk_ivlen = key->iv_len;
+		mvm->ptk_icvlen = key->icv_len;
+		mvm->gtk_ivlen = key->iv_len;
+		mvm->gtk_icvlen = key->icv_len;
+
 		/* don't upload key again */
 		goto out_unlock;
 	}
@@ -304,9 +310,13 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 	 */
 	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
 		key->hw_key_idx = 0;
+		mvm->ptk_ivlen = key->iv_len;
+		mvm->ptk_icvlen = key->icv_len;
 	} else {
 		data->gtk_key_idx++;
 		key->hw_key_idx = data->gtk_key_idx;
+		mvm->gtk_ivlen = key->iv_len;
+		mvm->gtk_icvlen = key->icv_len;
 	}
 
 	ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, true);
@@ -649,6 +659,11 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 	/* We reprogram keys and shouldn't allocate new key indices */
 	memset(mvm->fw_key_table, 0, sizeof(mvm->fw_key_table));
 
+	mvm->ptk_ivlen = 0;
+	mvm->ptk_icvlen = 0;
+	mvm->ptk_ivlen = 0;
+	mvm->ptk_icvlen = 0;
+
 	/*
 	 * The D3 firmware still hardcodes the AP station ID for the
 	 * BSS we're associated with as 0. As a result, we have to move
@@ -783,7 +798,6 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
 	struct iwl_wowlan_status *status;
 	u32 reasons;
 	int ret, len;
-	bool pkt8023 = false;
 	struct sk_buff *pkt = NULL;
 
 	iwl_trans_read_mem_bytes(mvm->trans, base,
@@ -824,7 +838,8 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
 	status = (void *)cmd.resp_pkt->data;
 
 	if (len - sizeof(struct iwl_cmd_header) !=
-	    sizeof(*status) + le32_to_cpu(status->wake_packet_bufsize)) {
+	    sizeof(*status) +
+	    ALIGN(le32_to_cpu(status->wake_packet_bufsize), 4)) {
 		IWL_ERR(mvm, "Invalid WoWLAN status response!\n");
 		goto out;
 	}
@@ -836,61 +851,96 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
 		goto report;
 	}
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_MAGIC_PACKET) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_MAGIC_PACKET)
 		wakeup.magic_pkt = true;
-		pkt8023 = true;
-	}
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_PATTERN) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_PATTERN)
 		wakeup.pattern_idx =
 			le16_to_cpu(status->pattern_number);
-		pkt8023 = true;
-	}
 
 	if (reasons & (IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_MISSED_BEACON |
 		       IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH))
 		wakeup.disconnect = true;
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_GTK_REKEY_FAILURE) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_GTK_REKEY_FAILURE)
 		wakeup.gtk_rekey_failure = true;
-		pkt8023 = true;
-	}
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_RFKILL_DEASSERTED) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_RFKILL_DEASSERTED)
 		wakeup.rfkill_release = true;
-		pkt8023 = true;
-	}
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_EAPOL_REQUEST) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_EAPOL_REQUEST)
 		wakeup.eap_identity_req = true;
-		pkt8023 = true;
-	}
 
-	if (reasons & IWL_WOWLAN_WAKEUP_BY_FOUR_WAY_HANDSHAKE) {
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_FOUR_WAY_HANDSHAKE)
 		wakeup.four_way_handshake = true;
-		pkt8023 = true;
-	}
 
 	if (status->wake_packet_bufsize) {
-		u32 pktsize = le32_to_cpu(status->wake_packet_bufsize);
-		u32 pktlen = le32_to_cpu(status->wake_packet_length);
+		int pktsize = le32_to_cpu(status->wake_packet_bufsize);
+		int pktlen = le32_to_cpu(status->wake_packet_length);
+		const u8 *pktdata = status->wake_packet;
+		struct ieee80211_hdr *hdr = (void *)pktdata;
+		int truncated = pktlen - pktsize;
+
+		/* this would be a firmware bug */
+		if (WARN_ON_ONCE(truncated < 0))
+			truncated = 0;
+
+		if (ieee80211_is_data(hdr->frame_control)) {
+			int hdrlen = ieee80211_hdrlen(hdr->frame_control);
+			int ivlen = 0, icvlen = 4; /* also FCS */
 
-		if (pkt8023) {
 			pkt = alloc_skb(pktsize, GFP_KERNEL);
 			if (!pkt)
 				goto report;
-			memcpy(skb_put(pkt, pktsize), status->wake_packet,
-			       pktsize);
+
+			memcpy(skb_put(pkt, hdrlen), pktdata, hdrlen);
+			pktdata += hdrlen;
+			pktsize -= hdrlen;
+
+			if (ieee80211_has_protected(hdr->frame_control)) {
+				if (is_multicast_ether_addr(hdr->addr1)) {
+					ivlen = mvm->gtk_ivlen;
+					icvlen += mvm->gtk_icvlen;
+				} else {
+					ivlen = mvm->ptk_ivlen;
+					icvlen += mvm->ptk_icvlen;
+				}
+			}
+
+			/* if truncated, FCS/ICV is (partially) gone */
+			if (truncated >= icvlen) {
+				icvlen = 0;
+				truncated -= icvlen;
+			} else {
+				icvlen -= truncated;
+				truncated = 0;
+			}
+
+			pktsize -= ivlen + icvlen;
+			pktdata += ivlen;
+
+			memcpy(skb_put(pkt, pktsize), pktdata, pktsize);
+
 			if (ieee80211_data_to_8023(pkt, vif->addr, vif->type))
 				goto report;
 			wakeup.packet = pkt->data;
 			wakeup.packet_present_len = pkt->len;
-			wakeup.packet_len = pkt->len - (pktlen - pktsize);
+			wakeup.packet_len = pkt->len - truncated;
 			wakeup.packet_80211 = false;
 		} else {
+			int fcslen = 4;
+
+			if (truncated >= 4) {
+				truncated -= 4;
+				fcslen = 0;
+			} else {
+				fcslen -= truncated;
+				truncated = 0;
+			}
+			pktsize -= fcslen;
 			wakeup.packet = status->wake_packet;
 			wakeup.packet_present_len = pktsize;
-			wakeup.packet_len = pktlen;
+			wakeup.packet_len = pktlen - truncated;
 			wakeup.packet_80211 = true;
 		}
 	}