From 74544c474f70e1998043b79e8f15db6a05894bcd Mon Sep 17 00:00:00 2001
From: mtk27835 <shurong.wen@mediatek.com>
Date: Thu, 25 Aug 2022 14:25:51 -0700
Subject: [PATCH] [PATCH 1122] mt76: add ibf control vendor cmd

Signed-off-by: mtk27835 <shurong.wen@mediatek.com>
---
 mt7915/vendor.c | 69 +++++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h | 23 +++++++++++++++++
 2 files changed, 92 insertions(+)

diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index 7acb330..745266f 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -73,6 +73,16 @@ edcca_ctrl_policy[NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL] = {
        [MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE] = { .type = NLA_S8 },
 };
 
+static const struct nla_policy
+ibf_ctrl_policy[NUM_MTK_VENDOR_ATTRS_IBF_CTRL] = {
+	[MTK_VENDOR_ATTR_IBF_CTRL_ENABLE] = { .type = NLA_U8 },
+};
+
+static struct nla_policy
+ibf_dump_policy[NUM_MTK_VENDOR_ATTRS_IBF_DUMP] = {
+	[MTK_VENDOR_ATTR_IBF_DUMP_ENABLE] = { .type = NLA_U8 },
+};
+
 
 struct csi_null_tone {
 	u8 start;
@@ -1091,6 +1101,53 @@ static int mt7915_vendor_edcca_ctrl(struct wiphy *wiphy,
 	return 0;
 }
 
+static int mt7915_vendor_ibf_ctrl(struct wiphy *wiphy,
+				  struct wireless_dev *wdev,
+				  const void *data,
+				  int data_len)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7915_phy *phy = mt7915_hw_phy(hw);
+	struct mt7915_dev *dev = phy->dev;
+	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_IBF_CTRL];
+	int err;
+	u8 val;
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_IBF_CTRL_MAX, data, data_len,
+			ibf_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (tb[MTK_VENDOR_ATTR_IBF_CTRL_ENABLE]) {
+		val = nla_get_u8(tb[MTK_VENDOR_ATTR_IBF_CTRL_ENABLE]);
+
+		dev->ibf = !!val;
+
+		err = mt7915_mcu_set_txbf(dev, MT_BF_TYPE_UPDATE);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int
+mt7915_vendor_ibf_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;
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	if (nla_put_u8(skb, MTK_VENDOR_ATTR_IBF_DUMP_ENABLE, dev->ibf))
+		return -ENOMEM;
+
+	return 1;
+}
 
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
@@ -1172,6 +1229,18 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.doit = mt7915_vendor_edcca_ctrl,
 		.policy = edcca_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_EDCCA_CTRL_MAX,
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7915_vendor_ibf_ctrl,
+		.dumpit = mt7915_vendor_ibf_ctrl_dump,
+		.policy = ibf_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_IBF_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index 57f52f3..2a7e23b 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -11,6 +11,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_HEMU_CTRL = 0xc5,
 	MTK_NL80211_VENDOR_SUBCMD_PHY_CAPA_CTRL = 0xc6,
 	MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7,
+	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc8,
 };
 
 
@@ -194,4 +195,26 @@ enum mtk_vendor_attr_phy_capa_dump {
 		NUM_MTK_VENDOR_ATTRS_PHY_CAPA_DUMP - 1
 };
 
+enum mtk_vendor_attr_ibf_ctrl {
+	MTK_VENDOR_ATTR_IBF_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_IBF_CTRL_ENABLE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_IBF_CTRL,
+	MTK_VENDOR_ATTR_IBF_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_IBF_CTRL - 1
+};
+
+enum mtk_vendor_attr_ibf_dump {
+	MTK_VENDOR_ATTR_IBF_DUMP_UNSPEC,
+
+	MTK_VENDOR_ATTR_IBF_DUMP_ENABLE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_IBF_DUMP,
+	MTK_VENDOR_ATTR_IBF_DUMP_MAX =
+		NUM_MTK_VENDOR_ATTRS_IBF_DUMP - 1
+};
+
 #endif
-- 
2.32.0