From 9f4ef5f8637f597acf5fc876d40f9159e051e512 Mon Sep 17 00:00:00 2001
From: Howard Hsu <howard-yh.hsu@mediatek.com>
Date: Fri, 24 Jun 2022 11:15:45 +0800
Subject: [PATCH] mt76: mt7915: add vendor subcmd EDCCA ctrl
 enable/threshold/compensation

Change-Id: I06a3f94d5e444be894200e2b6588d76ed38d09d0
---
 mt76_connac_mcu.h |   1 +
 mt7915/main.c     |   3 ++
 mt7915/mcu.c      |  73 +++++++++++++++++++++++++
 mt7915/mcu.h      |  21 ++++++++
 mt7915/mt7915.h   |   3 +-
 mt7915/vendor.c   | 132 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h   |  33 ++++++++++++
 7 files changed, 265 insertions(+), 1 deletion(-)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 5cef308a..65863a0d 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1205,6 +1205,7 @@ enum {
 	MCU_EXT_CMD_SMESH_CTRL = 0xae,
 	MCU_EXT_CMD_RX_STAT_USER_CTRL = 0xb3,
 	MCU_EXT_CMD_CERT_CFG = 0xb7,
+	MCU_EXT_CMD_EDCCA = 0xba,
 	MCU_EXT_CMD_CSI_CTRL = 0xc2,
 	MCU_EXT_CMD_IPI_HIST_SCAN = 0xc5,
 };
diff --git a/mt7915/main.c b/mt7915/main.c
index d1135d9f..0d01d880 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -479,6 +479,9 @@ static int mt7915_config(struct ieee80211_hw *hw, u32 changed)
 			mutex_unlock(&dev->mt76.mutex);
 		}
 #endif
+		ret = mt7915_mcu_set_edcca(phy, EDCCA_CTRL_SET_EN, NULL, 0);
+		if (ret)
+			return ret;
 		ieee80211_stop_queues(hw);
 		ret = mt7915_set_channel(phy);
 		if (ret)
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index 185b6495..85bb60c9 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -4623,3 +4623,76 @@ int mt7915_mcu_ipi_hist_scan(struct mt7915_phy *phy, void *data, u8 mode, bool w
 
 	return 0;
 }
+
+int mt7915_mcu_set_edcca(struct mt7915_phy *phy, int mode, u8 *value, s8 compensation)
+{
+	static const u8 ch_band[] = {
+		[NL80211_BAND_2GHZ] = 0,
+		[NL80211_BAND_5GHZ] = 1,
+		[NL80211_BAND_6GHZ] = 2,
+	};
+	struct mt7915_dev *dev = phy->dev;
+	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
+	struct {
+		u8 band_idx;
+		u8 cmd_idx;
+		u8 setting[3];
+		bool record_in_fw;
+		u8 region;
+		s8 thres_compensation;
+	} __packed req = {
+		.band_idx = phy->mt76->band_idx,
+		.cmd_idx = mode,
+		.record_in_fw = false,
+		.thres_compensation = compensation,
+	};
+
+	if (ch_band[chandef->chan->band] == 2 && dev->mt76.region == NL80211_DFS_FCC)
+		req.region = dev->mt76.region;
+
+	if (mode == EDCCA_CTRL_SET_EN) {
+		req.setting[0] = (!value)? EDCCA_MODE_AUTO: value[0];
+	} else if (mode == EDCCA_CTRL_SET_THERS) {
+		req.setting[0] = value[0];
+		req.setting[1] = value[1];
+		req.setting[2] = value[2];
+	} else {
+		return -EINVAL;
+	}
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EDCCA), &req, sizeof(req), true);
+}
+
+
+int mt7915_mcu_get_edcca(struct mt7915_phy *phy, u8 mode, s8 *value)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct {
+		u8 band_idx;
+		u8 cmd_idx;
+		u8 setting[3];
+		bool record_in_fw;
+		u8 region;
+		s8 thres_compensation;
+	} __packed req = {
+		.band_idx = phy->mt76->band_idx,
+		.cmd_idx = mode,
+		.record_in_fw = false,
+	};
+	struct sk_buff *skb;
+	int ret;
+	struct mt7915_mcu_edcca_info *res;
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(EDCCA), &req, sizeof(req),
+				        true, &skb);
+	if (ret)
+		return ret;
+
+	res = (struct mt7915_mcu_edcca_info *)skb->data;
+	*value++ = res->info[0];
+	*value++ = res->info[1];
+	*value = res->info[2];
+	dev_kfree_skb(skb);
+
+	return 0;
+}
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index 5c5c92fc..c87fe8e8 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -1058,6 +1058,27 @@ enum {
    MURU_DL_INIT,
    MURU_UL_INIT,
 };
+
+enum {
+   EDCCA_CTRL_SET_EN = 0,
+   EDCCA_CTRL_SET_THERS,
+   EDCCA_CTRL_GET_EN,
+   EDCCA_CTRL_GET_THERS,
+   EDCCA_CTRL_NUM,
+};
+
+enum {
+   EDCCA_MODE_FORCE_DISABLE,
+   EDCCA_MODE_AUTO,
+};
+
+struct mt7915_mcu_edcca_info {
+	u8 cmd_idx;
+	u8 band_idx;
+	u8 info[3];
+	u8 fginit;
+	u8 rsv[2];
+};
 #endif
 
 #endif
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index f6b7241d..582887d4 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -763,7 +763,8 @@ void mt7915_vendor_amnt_fill_rx(struct mt7915_phy *phy, struct sk_buff *skb);
 int mt7915_vendor_amnt_sta_remove(struct mt7915_phy *phy,
 				  struct ieee80211_sta *sta);
 #endif
-
+int mt7915_mcu_set_edcca(struct mt7915_phy *phy, int mode, u8 *value, s8 compensation);
+int mt7915_mcu_get_edcca(struct mt7915_phy *phy, u8 mode, s8 *value);
 int mt7915_mcu_ipi_hist_ctrl(struct mt7915_phy *phy, void *data, u8 cmd, bool wait_resp);
 int mt7915_mcu_ipi_hist_scan(struct mt7915_phy *phy, void *data, u8 mode, bool wait_resp);
 
diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index 526eb6b2..f712d67c 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -62,6 +62,24 @@ phy_capa_dump_policy[NUM_MTK_VENDOR_ATTRS_PHY_CAPA_DUMP] = {
 	[MTK_VENDOR_ATTR_PHY_CAPA_DUMP_MAX_SUPPORTED_STA] = { .type = NLA_U16 },
 };
 
+static const struct nla_policy
+edcca_ctrl_policy[NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL] = {
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_MODE] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_SEC20_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_SEC40_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_SEC80_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE] = { .type = NLA_S8 },
+};
+
+static const struct nla_policy
+edcca_dump_policy[NUM_MTK_VENDOR_ATTRS_EDCCA_DUMP] = {
+       [MTK_VENDOR_ATTR_EDCCA_DUMP_MODE] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_DUMP_PRI20_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_DUMP_SEC40_VAL] = { .type = NLA_U8 },
+       [MTK_VENDOR_ATTR_EDCCA_DUMP_SEC80_VAL] = { .type = NLA_U8 },
+};
+
 struct csi_null_tone {
 	u8 start;
 	u8 end;
@@ -1015,6 +1033,108 @@ mt7915_vendor_phy_capa_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
 	return len;
 }
 
+static int mt7915_vendor_edcca_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 nlattr *tb[NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL];
+	int err;
+	u8 edcca_mode;
+	s8 edcca_compensation;
+	u8 edcca_value[EDCCA_THRES_NUM] = {0};
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_EDCCA_CTRL_MAX, data, data_len,
+			edcca_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (!tb[MTK_VENDOR_ATTR_EDCCA_CTRL_MODE])
+		return -EINVAL;
+
+	edcca_mode = nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_MODE]);
+	if (edcca_mode == EDCCA_CTRL_SET_EN) {
+		if (!tb[MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL] ||
+			!tb[MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE]) {
+			return -EINVAL;
+		}
+		edcca_value[0] =
+			nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL]);
+		edcca_compensation =
+			nla_get_s8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE]);
+
+		err = mt7915_mcu_set_edcca(phy, edcca_mode, edcca_value,
+					   edcca_compensation);
+		if (err)
+			return err;
+	} else if (edcca_mode == EDCCA_CTRL_SET_THERS) {
+		if (!tb[MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL] ||
+		    !tb[MTK_VENDOR_ATTR_EDCCA_CTRL_SEC40_VAL] ||
+		    !tb[MTK_VENDOR_ATTR_EDCCA_CTRL_SEC80_VAL]) {
+			return -EINVAL;
+		}
+		edcca_value[0] =
+			nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL]);
+		edcca_value[1] =
+			nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_SEC40_VAL]);
+		edcca_value[2] =
+			nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_SEC80_VAL]);
+		err = mt7915_mcu_set_edcca(phy, edcca_mode, edcca_value,
+					   edcca_compensation);
+		if (err)
+			return err;
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+mt7915_vendor_edcca_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 nlattr *tb[NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL];
+	int len = EDCCA_THRES_NUM;
+	int err;
+	u8 edcca_mode;
+	s8 value[EDCCA_THRES_NUM];
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_EDCCA_CTRL_MAX, data, data_len,
+			edcca_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (!tb[MTK_VENDOR_ATTR_EDCCA_CTRL_MODE])
+		return -EINVAL;
+
+	edcca_mode = nla_get_u8(tb[MTK_VENDOR_ATTR_EDCCA_CTRL_MODE]);
+	if (edcca_mode ==  EDCCA_CTRL_GET_EN || edcca_mode == EDCCA_CTRL_GET_THERS) {
+		err = mt7915_mcu_get_edcca(phy, edcca_mode, value);
+	} else {
+		return -EINVAL;
+	}
+
+	if (err)
+		return err;
+
+	if (nla_put_u8(skb, MTK_VENDOR_ATTR_EDCCA_DUMP_PRI20_VAL, value[0]) ||
+	    nla_put_u8(skb, MTK_VENDOR_ATTR_EDCCA_DUMP_SEC40_VAL, value[1]) ||
+	    nla_put_u8(skb, MTK_VENDOR_ATTR_EDCCA_DUMP_SEC80_VAL, value[2]))
+		return -ENOMEM;
+
+	return len;
+}
+
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
 		.info = {
@@ -1083,6 +1203,18 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.dumpit = mt7915_vendor_phy_capa_ctrl_dump,
 		.policy = phy_capa_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_PHY_CAPA_CTRL_MAX,
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7915_vendor_edcca_ctrl,
+		.dumpit = mt7915_vendor_edcca_ctrl_dump,
+		.policy = edcca_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_EDCCA_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index 719b851f..72319717 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -2,6 +2,7 @@
 #define __MT7915_VENDOR_H
 
 #define MTK_NL80211_VENDOR_ID	0x0ce7
+#define EDCCA_THRES_NUM 3
 
 enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_AMNT_CTRL = 0xae,
@@ -10,6 +11,38 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL = 0xc4,
 	MTK_NL80211_VENDOR_SUBCMD_MU_CTRL = 0xc5,
 	MTK_NL80211_VENDOR_SUBCMD_PHY_CAPA_CTRL = 0xc6,
+	MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7,
+};
+
+
+enum mtk_vendor_attr_edcca_ctrl {
+        MTK_VENDOR_ATTR_EDCCA_THRESHOLD_INVALID = 0,
+
+        MTK_VENDOR_ATTR_EDCCA_CTRL_MODE,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_PRI20_VAL,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_SEC20_VAL,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_SEC40_VAL,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_SEC80_VAL,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE,
+
+        /* keep last */
+        NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL,
+        MTK_VENDOR_ATTR_EDCCA_CTRL_MAX =
+                NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL - 1
+};
+
+enum mtk_vendor_attr_edcca_dump {
+        MTK_VENDOR_ATTR_EDCCA_DUMP_UNSPEC = 0,
+
+        MTK_VENDOR_ATTR_EDCCA_DUMP_MODE,
+        MTK_VENDOR_ATTR_EDCCA_DUMP_PRI20_VAL,
+        MTK_VENDOR_ATTR_EDCCA_DUMP_SEC40_VAL,
+        MTK_VENDOR_ATTR_EDCCA_DUMP_SEC80_VAL,
+
+        /* keep last */
+        NUM_MTK_VENDOR_ATTRS_EDCCA_DUMP,
+        MTK_VENDOR_ATTR_EDCCA_DUMP_MAX =
+                NUM_MTK_VENDOR_ATTRS_EDCCA_DUMP - 1
 };
 
 enum mtk_capi_control_changed {
-- 
2.18.0