From 1dd09d7cdf80f3982017cae52013a423f84bfa52 Mon Sep 17 00:00:00 2001
From: Yi-Chia Hsieh <Yi-Chia.Hsieh@mediatek.com>
Date: Tue, 12 Jul 2022 10:04:35 -0700
Subject: [PATCH] mt76: mt7915: add phy capability vendor command

---
 mt7915/mt7915.h |  1 +
 mt7915/vendor.c | 52 +++++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h | 25 ++++++++++++++++++++++++
 3 files changed, 78 insertions(+)

diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 5f11b3fb..fb8bd877 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -11,6 +11,7 @@
 
 #define MTK_DEBUG 1
 #define MT7915_MAX_INTERFACES		19
+#define MT7915_MAX_BSS			16
 #define MT7915_WTBL_SIZE		288
 #define MT7916_WTBL_SIZE		544
 #define MT7915_WTBL_RESERVED		(mt7915_wtbl_size(dev) - 1)
diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index c7551848..77d71e48 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -50,6 +50,18 @@ rfeature_ctrl_policy[NUM_MTK_VENDOR_ATTRS_RFEATURE_CTRL] = {
 	[MTK_VENDOR_ATTR_RFEATURE_CTRL_TRIG_TXBF] = { .type = NLA_U8 },
 };
 
+static const struct nla_policy
+phy_capa_ctrl_policy[NUM_MTK_VENDOR_ATTRS_PHY_CAPA_CTRL] = {
+	[MTK_VENDOR_ATTR_PHY_CAPA_CTRL_SET] = { .type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_PHY_CAPA_CTRL_DUMP] = { .type = NLA_NESTED },
+};
+
+static const struct nla_policy
+phy_capa_dump_policy[NUM_MTK_VENDOR_ATTRS_PHY_CAPA_DUMP] = {
+	[MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_BSS] = { .type = NLA_U16 },
+	[MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_STA] = { .type = NLA_U16 },
+};
+
 struct csi_null_tone {
 	u8 start;
 	u8 end;
@@ -974,6 +986,35 @@ static int mt7915_vendor_hemu_ctrl(struct wiphy *wiphy,
 	return 0;
 }
 
+static int
+mt7915_vendor_phy_capa_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
+			     struct sk_buff *skb, const void *data, int data_len,
+			     unsigned long *storage)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7915_phy *phy = mt7915_hw_phy(hw);
+	struct mt7915_dev *dev = phy->dev;
+	void *a;
+	int len = 0;
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	a = nla_nest_start(skb, MTK_VENDOR_ATTR_PHY_CAPA_CTRL_DUMP);
+
+	if (nla_put_u16(skb,
+	    MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_BSS, MT7915_MAX_BSS) ||
+	    nla_put_u16(skb,
+	    MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_STA, MT7915_WTBL_STA))
+		return -ENOMEM;
+	len += 2;
+
+	nla_nest_end(skb, a);
+
+	return len;
+}
+
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
 		.info = {
@@ -1031,6 +1072,17 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.doit = mt7915_vendor_hemu_ctrl,
 		.policy = hemu_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_HEMU_CTRL_MAX,
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_PHY_CAPA_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.dumpit = mt7915_vendor_phy_capa_ctrl_dump,
+		.policy = phy_capa_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_PHY_CAPA_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index a8e4ebf9..719b851f 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -9,6 +9,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_RFEATURE_CTRL = 0xc3,
 	MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL = 0xc4,
 	MTK_NL80211_VENDOR_SUBCMD_HEMU_CTRL = 0xc5,
+	MTK_NL80211_VENDOR_SUBCMD_PHY_CAPA_CTRL = 0xc6,
 };
 
 enum mtk_capi_control_changed {
@@ -149,4 +150,28 @@ enum mtk_vendor_attr_mnt_dump {
 		NUM_MTK_VENDOR_ATTRS_AMNT_DUMP - 1
 };
 
+enum mtk_vendor_attr_phy_capa_ctrl {
+	MTK_VENDOR_ATTR_PHY_CAPA_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_PHY_CAPA_CTRL_SET,
+	MTK_VENDOR_ATTR_PHY_CAPA_CTRL_DUMP,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_PHY_CAPA_CTRL,
+	MTK_VENDOR_ATTR_PHY_CAPA_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_PHY_CAPA_CTRL - 1
+};
+
+enum mtk_vendor_attr_phy_capa_dump {
+	MTK_VENDOR_ATTR_PHY_CAPA_DUMP_UNSPEC,
+
+	MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_BSS,
+	MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_STA,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_PHY_CAPA_DUMP,
+	MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX =
+		NUM_MTK_VENDOR_ATTRS_PHY_CAPA_DUMP - 1
+};
+
 #endif
-- 
2.32.0