From 3df403316fc018d07aa2b212045bbcaedf0e3a02 Mon Sep 17 00:00:00 2001
From: Carter Chen <carter.chen@infineon.com>
Date: Thu, 11 Aug 2022 02:02:24 -0500
Subject: [PATCH] brcmfmac: trying to get GCMP cap before doing set it

trying to get GCMP cp to make sure the firmware supports it before set.

Signed-off-by: Carter Chen <carter.chen@infineon.com>
---
 .../broadcom/brcm80211/brcmfmac/cfg80211.c    | 45 +++++++++++++++----
 .../broadcom/brcm80211/brcmfmac/feature.c     |  1 +
 .../broadcom/brcm80211/brcmfmac/feature.h     |  4 +-
 3 files changed, 41 insertions(+), 9 deletions(-)

diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
index 38664911a..76519024d 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
@@ -2234,6 +2234,10 @@ brcmf_set_wsec_mode(struct net_device *ndev,
 			pval = AES_ENABLED;
 			break;
 		case WLAN_CIPHER_SUITE_GCMP_256:
+			if (!brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+				brcmf_err("the low layer not support GCMP\n");
+				return -EOPNOTSUPP;
+			}
 			pval = AES_ENABLED;
 			algos = KEY_ALGO_MASK(CRYPTO_ALGO_AES_GCM256);
 			mask = algos | KEY_ALGO_MASK(CRYPTO_ALGO_AES_CCM);
@@ -2260,6 +2264,10 @@ brcmf_set_wsec_mode(struct net_device *ndev,
 			gval = AES_ENABLED;
 			break;
 		case WLAN_CIPHER_SUITE_GCMP_256:
+			if (!brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+				brcmf_err("the low layer not support GCMP\n");
+				return -EOPNOTSUPP;
+			}
 			gval = AES_ENABLED;
 			algos = KEY_ALGO_MASK(CRYPTO_ALGO_AES_GCM256);
 			mask = algos | KEY_ALGO_MASK(CRYPTO_ALGO_AES_CCM);
@@ -2286,10 +2294,15 @@ brcmf_set_wsec_mode(struct net_device *ndev,
 		return err;
 	}
 
-	err = wl_set_wsec_info_algos(ifp, algos, mask);
-	if (err) {
-		bphy_err(drvr, "set wsec_info error (%d)\n", err);
-		return err;
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+		brcmf_dbg(CONN,
+			  "set_wsdec_info algos (0x%x) mask (0x%x)\n",
+			  algos, mask);
+		err = wl_set_wsec_info_algos(ifp, algos, mask);
+		if (err) {
+			brcmf_err("set wsec_info error (%d)\n", err);
+			return err;
+		}
 	}
 
 	sec = &profile->sec;
@@ -3212,6 +3225,11 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_CCMP\n");
 		break;
 	case WLAN_CIPHER_SUITE_GCMP_256:
+		if (!brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+			brcmf_err("the low layer not support GCMP\n");
+			err = -EOPNOTSUPP;
+			goto done;
+		}
 		key->algo = CRYPTO_ALGO_AES_GCM256;
 		val = AES_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_GCMP_256\n");
@@ -3219,6 +3237,11 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		mask = algos | KEY_ALGO_MASK(CRYPTO_ALGO_AES_CCM);
 		break;
 	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
+		if (!brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+			brcmf_err("the low layer not support GCMP\n");
+			err = -EOPNOTSUPP;
+			goto done;
+		}
 		key->algo = CRYPTO_ALGO_BIP_GMAC256;
 		val = AES_ENABLED;
 		algos = KEY_ALGO_MASK(CRYPTO_ALGO_BIP_GMAC256);
@@ -3247,10 +3270,16 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		goto done;
 	}
 
-	brcmf_dbg(CONN, "algos (0x%x) mask (0x%x)\n", algos, mask);
-	err = wl_set_wsec_info_algos(ifp, algos, mask);
-	if (err)
-		bphy_err(drvr, "set wsec_info error (%d)\n", err);
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GCMP)) {
+		brcmf_dbg(CONN,
+			  "set_wsdec_info algos (0x%x) mask (0x%x)\n",
+			  algos, mask);
+		err = wl_set_wsec_info_algos(ifp, algos, mask);
+		if (err) {
+			brcmf_err("set wsec_info error (%d)\n", err);
+			return err;
+		}
+	}
 
 done:
 	brcmf_dbg(TRACE, "Exit\n");
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
index 5bad7e232..030557d83 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
@@ -47,6 +47,7 @@ static const struct brcmf_feat_fwcap brcmf_fwcap_map[] = {
 	{ BRCMF_FEAT_SAE_EXT, "sae_ext " },
 	{ BRCMF_FEAT_FBT, "fbt " },
 	{ BRCMF_FEAT_OKC, "okc" },
+	{ BRCMF_FEAT_GCMP, "gcmp" },
 };
 
 #ifdef DEBUG
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
index 71bf9d806..3039d82ef 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
@@ -32,6 +32,7 @@
  * DUMP_OBSS: Firmware has capable to dump obss info to support ACS
  * SCAN_V2: Version 2 scan params
  * SAE_EXT: SAE be handled by userspace supplicant
+ * GCMP: firmware has defined GCMP or not.
  */
 #define BRCMF_FEAT_LIST \
 	BRCMF_FEAT_DEF(MBSS) \
@@ -61,7 +62,8 @@
 	BRCMF_FEAT_DEF(PMKID_V3) \
 	BRCMF_FEAT_DEF(SAE_EXT) \
 	BRCMF_FEAT_DEF(FBT) \
-	BRCMF_FEAT_DEF(OKC)
+	BRCMF_FEAT_DEF(OKC) \
+	BRCMF_FEAT_DEF(GCMP)
 
 /*
  * Quirks:
-- 
2.39.5 (Apple Git-154)

