From 7c512e82c05fe1e5e00a409ca69f3e15094f1bb6 Mon Sep 17 00:00:00 2001
From: Benjamin Lin <benjamin-jw.lin@mediatek.com>
Date: Mon, 29 Jan 2024 11:28:41 +0800
Subject: [PATCH 10/15] wifi: mt76: mt7915: add support for WMM PBC
 configuration

---
 mt76_connac_mcu.h |  2 ++
 mt7915/init.c     |  2 ++
 mt7915/mac.c      | 14 ++++++++
 mt7915/mcu.c      | 90 +++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/mcu.h      | 15 ++++++++
 mt7915/mt7915.h   |  4 +++
 6 files changed, 127 insertions(+)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 2a4aa796..8d516e40 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1018,6 +1018,7 @@ enum {
 	MCU_EXT_EVENT_ASSERT_DUMP = 0x23,
 	MCU_EXT_EVENT_RDD_REPORT = 0x3a,
 	MCU_EXT_EVENT_CSA_NOTIFY = 0x4f,
+	MCU_EXT_EVENT_BSS_ACQ_PKT_CNT = 0x52,
 	MCU_EXT_EVENT_WA_TX_STAT = 0x74,
 	MCU_EXT_EVENT_BCC_NOTIFY = 0x75,
 	MCU_EXT_EVENT_MURU_CTRL = 0x9f,
@@ -1213,6 +1214,7 @@ enum {
 	MCU_EXT_CMD_TXDPD_CAL = 0x60,
 	MCU_EXT_CMD_CAL_CACHE = 0x67,
 	MCU_EXT_CMD_RED_ENABLE = 0x68,
+	MCU_EXT_CMD_PKT_BUDGET_CTRL = 0x6c,
 	MCU_EXT_CMD_SET_RADAR_TH = 0x7c,
 	MCU_EXT_CMD_SET_RDD_PATTERN = 0x7d,
 	MCU_EXT_CMD_MWDS_SUPPORT = 0x80,
diff --git a/mt7915/init.c b/mt7915/init.c
index d5d9cbf5..2fc1f3ce 100644
--- a/mt7915/init.c
+++ b/mt7915/init.c
@@ -1217,6 +1217,8 @@ int mt7915_register_device(struct mt7915_dev *dev)
 	INIT_WORK(&dev->dump_work, mt7915_mac_dump_work);
 	mutex_init(&dev->dump_mutex);
 
+	INIT_WORK(&dev->wmm_pbc_work, mt7915_mcu_wmm_pbc_work);
+
 	dev->dbdc_support = mt7915_band_config(dev);
 
 	phy2 = mt7915_alloc_ext_phy(dev);
diff --git a/mt7915/mac.c b/mt7915/mac.c
index 37368536..762159b4 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -2027,6 +2027,8 @@ void mt7915_mac_work(struct work_struct *work)
 
 	mt76_update_survey(mphy);
 	if (++mphy->mac_work_count == 5) {
+		int i;
+
 		mphy->mac_work_count = 0;
 
 		mt7915_mac_update_stats(phy);
@@ -2034,6 +2036,18 @@ void mt7915_mac_work(struct work_struct *work)
 
 		if (phy->dev->muru_debug)
 			mt7915_mcu_muru_debug_get(phy);
+
+		/* Update DEV-wise information only in
+		 * the MAC work of the first band running.
+		 */
+		for (i = MT_BAND0; i <= mphy->band_idx; ++i) {
+			if (i == mphy->band_idx) {
+				if (mt7915_mcu_wa_cmd(phy->dev, MCU_WA_PARAM_CMD(QUERY), MCU_WA_PARAM_BSS_ACQ_PKT_CNT,
+				                      BSS_ACQ_PKT_CNT_BSS_BITMAP_ALL | BSS_ACQ_PKT_CNT_READ_CLR, 0))
+					dev_err(mphy->dev->dev, "Failed to query per-AC-queue packet counts.\n");
+			} else if (test_bit(MT76_STATE_RUNNING, &mphy->dev->phys[i]->state))
+				break;
+		}
 	}
 
 	if (++phy->stats_work_count == 10) {
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index 7a2a537d..d101cbf4 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -354,6 +354,93 @@ mt7915_mcu_rx_bcc_notify(struct mt7915_dev *dev, struct sk_buff *skb)
 			mt7915_mcu_cca_finish, mphy->hw);
 }
 
+void mt7915_mcu_wmm_pbc_work(struct work_struct *work)
+{
+#define WMM_PBC_QUEUE_NUM		5
+#define WMM_PBC_BSS_ALL			0xff
+#define WMM_PBC_WLAN_IDX_ALL		0xffff
+#define WMM_PBC_BOUND_DEFAULT		0xffff
+#define WMM_PBC_UP_BOUND_BAND0_VO	950
+#define WMM_PBC_UP_BOUND_BAND0_VI	950
+#define WMM_PBC_UP_BOUND_BAND0_BE	750
+#define WMM_PBC_UP_BOUND_BAND0_BK	450
+#define WMM_PBC_UP_BOUND_BAND1_VO	1900
+#define WMM_PBC_UP_BOUND_BAND1_VI	1900
+#define WMM_PBC_UP_BOUND_BAND1_BE	1500
+#define WMM_PBC_UP_BOUND_BAND1_BK	900
+#define WMM_PBC_UP_BOUND_MGMT		32
+	struct mt7915_dev *dev = container_of(work, struct mt7915_dev, wmm_pbc_work);
+	struct {
+		u8 bss_idx;
+		u8 queue_num;
+		__le16 wlan_idx;
+		u8 __rsv[4];
+		struct {
+			__le16 low;
+			__le16 up;
+		} __packed bound[WMM_PBC_QUEUE_NUM * 2];
+	} __packed req = {
+		.bss_idx = WMM_PBC_BSS_ALL,
+		.queue_num = WMM_PBC_QUEUE_NUM * 2,
+		.wlan_idx = cpu_to_le16(WMM_PBC_WLAN_IDX_ALL),
+	};
+	int i;
+
+#define pbc_acq_up_bound_config(_band, _ac, _bound)									\
+	req.bound[_band * WMM_PBC_QUEUE_NUM + mt76_connac_lmac_mapping(_ac)].up = dev->wmm_pbc_enable			\
+	                                                                        ? cpu_to_le16(_bound)			\
+	                                                                        : cpu_to_le16(WMM_PBC_BOUND_DEFAULT)
+	pbc_acq_up_bound_config(MT_BAND0, IEEE80211_AC_VO, WMM_PBC_UP_BOUND_BAND0_VO);
+	pbc_acq_up_bound_config(MT_BAND0, IEEE80211_AC_VI, WMM_PBC_UP_BOUND_BAND0_VI);
+	pbc_acq_up_bound_config(MT_BAND0, IEEE80211_AC_BE, WMM_PBC_UP_BOUND_BAND0_BE);
+	pbc_acq_up_bound_config(MT_BAND0, IEEE80211_AC_BK, WMM_PBC_UP_BOUND_BAND0_BK);
+	req.bound[MT_BAND0 * WMM_PBC_QUEUE_NUM + 4].up = dev->wmm_pbc_enable
+	                                               ? cpu_to_le16(WMM_PBC_UP_BOUND_MGMT)
+	                                               : cpu_to_le16(WMM_PBC_BOUND_DEFAULT);
+	pbc_acq_up_bound_config(MT_BAND1, IEEE80211_AC_VO, WMM_PBC_UP_BOUND_BAND1_VO);
+	pbc_acq_up_bound_config(MT_BAND1, IEEE80211_AC_VI, WMM_PBC_UP_BOUND_BAND1_VI);
+	pbc_acq_up_bound_config(MT_BAND1, IEEE80211_AC_BE, WMM_PBC_UP_BOUND_BAND1_BE);
+	pbc_acq_up_bound_config(MT_BAND1, IEEE80211_AC_BK, WMM_PBC_UP_BOUND_BAND1_BK);
+	req.bound[MT_BAND1 * WMM_PBC_QUEUE_NUM + 4].up = dev->wmm_pbc_enable
+	                                               ? cpu_to_le16(WMM_PBC_UP_BOUND_MGMT)
+	                                               : cpu_to_le16(WMM_PBC_BOUND_DEFAULT);
+
+	for (i = 0; i < WMM_PBC_QUEUE_NUM * 2; ++i)
+		req.bound[i].low = cpu_to_le16(WMM_PBC_BOUND_DEFAULT);
+
+	if (mt76_mcu_send_msg(&dev->mt76, MCU_WA_EXT_CMD(PKT_BUDGET_CTRL),
+	                      &req, sizeof(req), true))
+		dev_err(dev->mt76.dev, "Failed to configure WMM PBC.\n");
+}
+
+static void
+mt7915_mcu_rx_bss_acq_pkt_cnt(struct mt7915_dev *dev, struct sk_buff * skb)
+{
+	struct mt7915_mcu_bss_acq_pkt_cnt_event *event = (struct mt7915_mcu_bss_acq_pkt_cnt_event *)skb->data;
+	u32 bitmap = le32_to_cpu(event->bss_bitmap);
+	u64 sum[IEEE80211_NUM_ACS] = {0};
+	u8 ac_cnt = 0;
+	int i, j;
+
+	for (i = 0; (i < BSS_ACQ_PKT_CNT_BSS_NUM) && (bitmap & (1 << i)); ++i) {
+		for (j = IEEE80211_AC_VO; j < IEEE80211_NUM_ACS; ++j)
+			sum[j] += le32_to_cpu(event->bss[i].cnt[mt76_connac_lmac_mapping(j)]);
+	}
+
+	for (i = IEEE80211_AC_VO; i < IEEE80211_NUM_ACS; ++i) {
+		if (sum[i] > WMM_PKT_THRESHOLD)
+			++ac_cnt;
+	}
+
+	if (ac_cnt > 1 && !dev->wmm_pbc_enable) {
+		dev->wmm_pbc_enable = true;
+		queue_work(dev->mt76.wq, &dev->wmm_pbc_work);
+	} else if (ac_cnt <= 1 && dev->wmm_pbc_enable) {
+		dev->wmm_pbc_enable = false;
+		queue_work(dev->mt76.wq, &dev->wmm_pbc_work);
+	}
+}
+
 static void
 mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
 {
@@ -376,6 +463,9 @@ mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
 	case MCU_EXT_EVENT_BCC_NOTIFY:
 		mt7915_mcu_rx_bcc_notify(dev, skb);
 		break;
+	case MCU_EXT_EVENT_BSS_ACQ_PKT_CNT:
+		mt7915_mcu_rx_bss_acq_pkt_cnt(dev, skb);
+		break;
 	default:
 		break;
 	}
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index 8f365461..fa0847d5 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -329,10 +329,25 @@ enum {
 	MCU_WA_PARAM_CMD_DEBUG,
 };
 
+#define BSS_ACQ_PKT_CNT_BSS_NUM		24
+#define BSS_ACQ_PKT_CNT_BSS_BITMAP_ALL	0x00ffffff
+#define BSS_ACQ_PKT_CNT_READ_CLR	BIT(31)
+#define WMM_PKT_THRESHOLD		50
+
+struct mt7915_mcu_bss_acq_pkt_cnt_event {
+	struct mt76_connac2_mcu_rxd rxd;
+
+	__le32 bss_bitmap;
+	struct {
+		__le32 cnt[IEEE80211_NUM_ACS];
+	} __packed bss[BSS_ACQ_PKT_CNT_BSS_NUM];
+} __packed;
+
 enum {
 	MCU_WA_PARAM_PDMA_RX = 0x04,
 	MCU_WA_PARAM_CPU_UTIL = 0x0b,
 	MCU_WA_PARAM_RED = 0x0e,
+	MCU_WA_PARAM_BSS_ACQ_PKT_CNT = 0x12,
 	MCU_WA_PARAM_RED_SETTING = 0x40,
 };
 
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 1b797331..874d5318 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -322,6 +322,9 @@ struct mt7915_dev {
 	struct reset_control *rstc;
 	void __iomem *dcm;
 	void __iomem *sku;
+
+	bool wmm_pbc_enable;
+	struct work_struct wmm_pbc_work;
 };
 
 enum {
@@ -512,6 +515,7 @@ int mt7915_mcu_fw_log_2_host(struct mt7915_dev *dev, u8 type, u8 ctrl);
 int mt7915_mcu_fw_dbg_ctrl(struct mt7915_dev *dev, u32 module, u8 level);
 void mt7915_mcu_rx_event(struct mt7915_dev *dev, struct sk_buff *skb);
 void mt7915_mcu_exit(struct mt7915_dev *dev);
+void mt7915_mcu_wmm_pbc_work(struct work_struct *work);
 
 static inline u16 mt7915_wtbl_size(struct mt7915_dev *dev)
 {
-- 
2.18.0