From 18f990de206c3301d3ae72cfeef40dfb3b361fb0 Mon Sep 17 00:00:00 2001
From: Howard Hsu <howard-yh.hsu@mediatek.com>
Date: Mon, 8 May 2023 09:03:50 +0800
Subject: [PATCH 36/39] wifi: mt76: mt7996: enable SCS feature for mt7996
 driver

Enable Smart Carrier Sense algorithn by default to improve performance
in a noisy environment.

Signed-off-by: Howard Hsu <howard-yh.hsu@mediatek.com>
---
 mt76_connac_mcu.h |   1 +
 mt7996/init.c     |   1 +
 mt7996/main.c     |   7 +++
 mt7996/mcu.c      | 123 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/mcu.h      |   6 +++
 mt7996/mt7996.h   |  16 ++++++
 6 files changed, 154 insertions(+)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 97f874b..bfec420 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1213,6 +1213,7 @@ enum {
 	MCU_UNI_CMD_GET_MIB_INFO = 0x22,
 	MCU_UNI_CMD_SNIFFER = 0x24,
 	MCU_UNI_CMD_SR = 0x25,
+	MCU_UNI_CMD_SCS = 0x26,
 	MCU_UNI_CMD_ROC = 0x27,
 	MCU_UNI_CMD_TXPOWER = 0x2b,
 	MCU_UNI_CMD_EFUSE_CTRL = 0x2d,
diff --git a/mt7996/init.c b/mt7996/init.c
index 0825e0b..1072874 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -1058,6 +1058,7 @@ int mt7996_register_device(struct mt7996_dev *dev)
 	dev->mt76.phy.priv = &dev->phy;
 	INIT_WORK(&dev->rc_work, mt7996_mac_sta_rc_work);
 	INIT_DELAYED_WORK(&dev->mphy.mac_work, mt7996_mac_work);
+	INIT_DELAYED_WORK(&dev->scs_work, mt7996_mcu_scs_sta_poll);
 	INIT_LIST_HEAD(&dev->sta_rc_list);
 	INIT_LIST_HEAD(&dev->sta_poll_list);
 	INIT_LIST_HEAD(&dev->twt_list);
diff --git a/mt7996/main.c b/mt7996/main.c
index 520f250..20b89a7 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -73,11 +73,17 @@ int mt7996_run(struct ieee80211_hw *hw)
 	if (ret)
 		goto out;
 
+	ret = mt7996_mcu_set_scs(phy, SCS_ENABLE);
+	if (ret)
+		goto out;
+
 	set_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 
 	ieee80211_queue_delayed_work(hw, &phy->mt76->mac_work,
 				     MT7996_WATCHDOG_TIME);
 
+	ieee80211_queue_delayed_work(mt76_hw(dev), &dev->scs_work, HZ);
+
 	if (!running)
 		mt7996_mac_reset_counters(phy);
 
@@ -105,6 +111,7 @@ static void mt7996_stop(struct ieee80211_hw *hw)
 	struct mt7996_phy *phy = mt7996_hw_phy(hw);
 
 	cancel_delayed_work_sync(&phy->mt76->mac_work);
+	cancel_delayed_work_sync(&dev->scs_work);
 
 	mutex_lock(&dev->mt76.mutex);
 
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 9fb800a..a5c473a 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -4190,3 +4190,126 @@ int mt7996_mcu_set_tx_power_ctrl(struct mt7996_phy *phy, u8 power_ctrl_id, u8 da
 	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TXPOWER),
 				 &req, sizeof(req), false);
 }
+
+int mt7996_mcu_set_scs_stats(struct mt7996_phy *phy)
+{
+	struct mt7996_scs_ctrl ctrl = phy->scs_ctrl;
+	struct {
+		u8 band_idx;
+		u8 _rsv[3];
+
+		__le16 tag;
+		__le16 len;
+
+		u8 _rsv2[6];
+		s8 min_rssi;
+		u8 _rsv3;
+	} __packed req = {
+		.band_idx = phy->mt76->band_idx,
+		.tag = cpu_to_le16(UNI_CMD_SCS_SEND_DATA),
+		.len = cpu_to_le16(sizeof(req) - 4),
+
+		.min_rssi = ctrl.sta_min_rssi,
+	};
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(SCS),
+				 &req, sizeof(req), false);
+}
+
+void mt7996_sta_rssi_work(void *data, struct ieee80211_sta *sta)
+{
+	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
+	struct mt7996_dev *dev = (struct mt7996_dev *)data;
+	struct mt7996_phy *poll_phy;
+	u8 band_idx = msta->wcid.phy_idx;
+
+	switch (band_idx) {
+	case MT_BAND0:
+		poll_phy = dev->mphy.priv;
+		break;
+	case MT_BAND1:
+		poll_phy = mt7996_phy2(dev);
+		break;
+	case MT_BAND2:
+		poll_phy = mt7996_phy3(dev);
+		break;
+	default:
+		poll_phy = NULL;
+		break;
+	}
+
+	if (!poll_phy->scs_ctrl.scs_enable)
+		return;
+
+	if (poll_phy->scs_ctrl.sta_min_rssi > msta->ack_signal)
+		poll_phy->scs_ctrl.sta_min_rssi = msta->ack_signal;
+}
+
+void mt7996_mcu_scs_sta_poll(struct work_struct *work)
+{
+	struct mt7996_dev *dev = container_of(work, struct mt7996_dev,
+				 scs_work.work);
+	bool scs_enable_flag = false;
+	u8 i;
+
+	ieee80211_iterate_stations_atomic(dev->mphy.hw, mt7996_sta_rssi_work, dev);
+
+	for (i = 0; i < __MT_MAX_BAND; i++) {
+		struct mt7996_phy *phy;
+
+		switch (i) {
+		case MT_BAND0:
+			phy = dev->mphy.priv;
+			break;
+		case MT_BAND1:
+			phy = mt7996_phy2(dev);
+			break;
+		case MT_BAND2:
+			phy = mt7996_phy3(dev);
+			break;
+		default:
+			phy = NULL;
+			break;
+		}
+
+		if (phy && test_bit(MT76_STATE_RUNNING, &phy->mt76->state) &&
+		    phy->scs_ctrl.scs_enable) {
+			scs_enable_flag = true;
+			if (mt7996_mcu_set_scs_stats(phy))
+				dev_err(dev->mt76.dev, "Failed to send scs mcu cmd\n");
+			phy->scs_ctrl.sta_min_rssi = 0;
+		}
+	}
+
+	if (scs_enable_flag)
+		ieee80211_queue_delayed_work(mt76_hw(dev), &dev->scs_work, HZ);
+}
+
+
+int mt7996_mcu_set_scs(struct mt7996_phy *phy, u8 enable)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct {
+		u8 band_idx;
+		u8 _rsv[3];
+
+		__le16 tag;
+		__le16 len;
+
+		u8 scs_enable;
+		u8 _rsv2[3];
+	} __packed req = {
+		.band_idx = phy->mt76->band_idx,
+		.tag = cpu_to_le16(UNI_CMD_SCS_ENABLE),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.scs_enable = enable,
+	};
+
+	phy->scs_ctrl.scs_enable = enable;
+
+	if (enable == SCS_ENABLE)
+		ieee80211_queue_delayed_work(mt76_hw(dev), &dev->scs_work, HZ);
+
+	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(SCS),
+				 &req, sizeof(req), false);
+}
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index eed7371..eb63441 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -757,6 +757,12 @@ enum {
 	MT7996_SEC_MODE_MAX,
 };
 
+enum {
+	UNI_CMD_SCS_SEND_DATA,
+	UNI_CMD_SCS_SET_PD_THR_RANGE = 2,
+	UNI_CMD_SCS_ENABLE,
+};
+
 #define MT7996_PATCH_SEC		GENMASK(31, 24)
 #define MT7996_PATCH_SCRAMBLE_KEY	GENMASK(15, 8)
 #define MT7996_PATCH_AES_KEY		GENMASK(7, 0)
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 488f59c..f78f1fd 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -222,6 +222,17 @@ struct mt7996_hif {
 	int irq;
 };
 
+
+struct mt7996_scs_ctrl {
+	u8 scs_enable;
+	s8 sta_min_rssi;
+};
+
+enum {
+	SCS_DISABLE = 0,
+	SCS_ENABLE,
+};
+
 struct mt7996_phy {
 	struct mt76_phy *mt76;
 	struct mt7996_dev *dev;
@@ -253,6 +264,8 @@ struct mt7996_phy {
 
 	bool has_aux_rx;
 
+	struct mt7996_scs_ctrl scs_ctrl;
+
 #ifdef CONFIG_NL80211_TESTMODE
 	struct {
 		u32 *reg_backup;
@@ -295,6 +308,7 @@ struct mt7996_dev {
 	struct work_struct rc_work;
 	struct work_struct dump_work;
 	struct work_struct reset_work;
+	struct delayed_work scs_work;
 	wait_queue_head_t reset_wait;
 	struct {
 		u32 state;
@@ -535,6 +549,8 @@ int mt7996_mcu_trigger_assert(struct mt7996_dev *dev);
 void mt7996_mcu_rx_event(struct mt7996_dev *dev, struct sk_buff *skb);
 void mt7996_mcu_exit(struct mt7996_dev *dev);
 int mt7996_mcu_set_tx_power_ctrl(struct mt7996_phy *phy, u8 power_ctrl_id, u8 data);
+int mt7996_mcu_set_scs(struct mt7996_phy *phy, u8 enable);
+void mt7996_mcu_scs_sta_poll(struct work_struct *work);
 
 static inline u8 mt7996_max_interface_num(struct mt7996_dev *dev)
 {
-- 
2.18.0