From e5d1346bdc27c267911ec5d2a6815350c43cccb8 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

Change-Id: I92dabf8be9c5a7ecec78f35325bc5645af8d15ab
---
 mt76_connac_mcu.h |  1 +
 mt7915/main.c     |  3 +++
 mt7915/mcu.c      | 38 ++++++++++++++++++++++++++++
 mt7915/mcu.h      | 12 +++++++++
 mt7915/mt7915.h   |  2 ++
 mt7915/vendor.c   | 63 +++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h   | 19 ++++++++++++++
 7 files changed, 138 insertions(+)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index b14f931b..6143d8e4 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1157,6 +1157,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,
 };
 
diff --git a/mt7915/main.c b/mt7915/main.c
index 85e9cd60..bad0fe1e 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -456,6 +456,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 6d67e9e1..523cc6da 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -4272,3 +4272,41 @@ int mt7915_mcu_set_amsdu_algo(struct mt7915_dev *dev, u16 wcid, u8 enable)
 	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(MEC_CTRL), &req, sizeof(req), true);
 }
 #endif
+
+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->band_idx,
+		.cmd_idx = mode,
+		.record_in_fw = false,
+		.region = dev->mt76.region,
+		.thres_compensation = compensation,
+	};
+
+	if (ch_band[chandef->chan->band] != 2)
+		return 0;
+
+	if (mode == EDCCA_CTRL_SET_EN) {
+		if (!value)
+			req.setting[0] = EDCCA_MODE_AUTO;
+		else
+			req.setting[0] = value[0];
+	}
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EDCCA), &req, sizeof(req), true);
+}
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index ad85e56c..b8a433e5 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -789,4 +789,16 @@ enum {
 };
 #endif
 
+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,
+};
 #endif
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index b750660d..a5abcc56 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -723,6 +723,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);
 
 #ifdef MTK_DEBUG
 int mt7915_mtk_init_debugfs(struct mt7915_phy *phy, struct dentry *dir);
diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index 77d71e48..5a28a554 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -62,6 +62,17 @@ 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 },
+};
+
+
 struct csi_null_tone {
 	u8 start;
 	u8 end;
@@ -1015,6 +1026,47 @@ 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;
+
+	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) {
+		u8 edcca_value[3] = {0};
+		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;
+	}
+	return 0;
+}
+
+
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
 		.info = {
@@ -1083,6 +1135,17 @@ 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,
+		.policy = edcca_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_EDCCA_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index 719b851f..83c41bc1 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -10,8 +10,27 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL = 0xc4,
 	MTK_NL80211_VENDOR_SUBCMD_HEMU_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_capi_control_changed {
 	CAPI_RFEATURE_CHANGED		= BIT(16),
 	CAPI_WIRELESS_CHANGED		= BIT(17),
-- 
2.18.0