From a3bc2ff25993c37117aad833b5f4cb39cbedd5bf Mon Sep 17 00:00:00 2001
From: "sujuan.chen" <sujuan.chen@mediatek.com>
Date: Wed, 28 Jun 2023 17:11:18 +0800
Subject: [PATCH 2009/2010] wifi: mt76: mt7915: wed: add per bss statistic info

Signed-off-by: sujuan.chen <sujuan.chen@mediatek.com>
---
 mt7915/init.c        |  1 +
 mt7915/mac.c         | 19 +++++++++++++++++++
 mt7915/main.c        |  2 +-
 mt7915/mcu.c         | 29 ++++++++++++++++++++++++++---
 mt7915/mmio.c        | 26 ++++++++++++++++++++++++++
 mt7915/mt7915.h      |  3 ++-
 mt7915/mtk_debugfs.c |  2 +-
 7 files changed, 76 insertions(+), 6 deletions(-)

diff --git a/mt7915/init.c b/mt7915/init.c
index 58e044ea..a2aca371 100644
--- a/mt7915/init.c
+++ b/mt7915/init.c
@@ -386,6 +386,7 @@ mt7915_init_wiphy(struct mt7915_phy *phy)
 	wiphy_ext_feature_set(wiphy, NL80211_EXT_FEATURE_FILS_DISCOVERY);
 	wiphy_ext_feature_set(wiphy, NL80211_EXT_FEATURE_ACK_SIGNAL_SUPPORT);
 	wiphy_ext_feature_set(wiphy, NL80211_EXT_FEATURE_CAN_REPLACE_PTK0);
+	wiphy_ext_feature_set(wiphy, NL80211_EXT_FEATURE_STAS_COUNT);
 
 	if (!is_mt7915(&dev->mt76))
 		wiphy_ext_feature_set(wiphy, NL80211_EXT_FEATURE_STA_TX_PWR);
diff --git a/mt7915/mac.c b/mt7915/mac.c
index debc2f9d..244dff46 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -1048,6 +1048,7 @@ static void mt7915_mac_add_txs(struct mt7915_dev *dev, void *data)
 	struct mt7915_phy *phy;
 	struct mt76_wcid *wcid;
 	__le32 *txs_data = data;
+	u64 last_bytes;
 	u16 wcidx;
 	u8 pid;
 
@@ -1066,6 +1067,7 @@ static void mt7915_mac_add_txs(struct mt7915_dev *dev, void *data)
 	if (!wcid)
 		goto out;
 
+	last_bytes = wcid->stats.tx_bytes;
 	msta = container_of(wcid, struct mt7915_sta, wcid);
 
 	if (pid == MT_PACKET_ID_WED)
@@ -1076,6 +1078,22 @@ static void mt7915_mac_add_txs(struct mt7915_dev *dev, void *data)
 	if (!wcid->sta)
 		goto out;
 
+	if (wiphy_ext_feature_isset(dev->mphy.hw->wiphy,
+				    NL80211_EXT_FEATURE_STAS_COUNT)) {
+		struct ieee80211_vif *vif;
+		struct wireless_dev *wdev;
+		struct pcpu_sw_netstats *tstats;
+
+		vif = container_of((void *)msta->vif, struct ieee80211_vif,
+				   drv_priv);
+		if (!vif)
+			goto out;
+
+		wdev = ieee80211_vif_to_wdev(vif);
+		tstats = this_cpu_ptr(wdev->netdev->tstats);
+
+		tstats->tx_bytes += wcid->stats.tx_bytes - last_bytes;
+	}
 	spin_lock_bh(&dev->mt76.sta_poll_lock);
 	if (list_empty(&msta->wcid.poll_list))
 		list_add_tail(&msta->wcid.poll_list, &dev->mt76.sta_poll_list);
@@ -2053,6 +2071,7 @@ static void mt7915_mac_sta_stats_work(struct mt7915_phy *phy)
 		spin_unlock_bh(&phy->stats_lock);
 
 		mt7915_mcu_get_tx_rate(phy, sta->wcid.idx);
+		mt7915_mcu_wed_wa_tx_stats(phy->dev, sta->wcid.idx, sta);
 
 		spin_lock_bh(&phy->stats_lock);
 	}
diff --git a/mt7915/main.c b/mt7915/main.c
index 03697bac..22d1d25b 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -1173,7 +1173,7 @@ static void mt7915_sta_statistics(struct ieee80211_hw *hw,
 		sinfo->tx_bytes = msta->wcid.stats.tx_bytes;
 		sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_BYTES64);
 
-		if (!mt7915_mcu_wed_wa_tx_stats(phy->dev, msta->wcid.idx)) {
+		if (!mt7915_mcu_wed_wa_tx_stats(phy->dev, msta->wcid.idx, msta)) {
 			sinfo->tx_packets = msta->wcid.stats.tx_packets;
 			sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_PACKETS);
 		}
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index a4225675..6f93b3b0 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -4408,7 +4408,8 @@ int mt7915_mcu_twt_agrt_update(struct mt7915_dev *dev,
 				 &req, sizeof(req), true);
 }
 
-int mt7915_mcu_wed_wa_tx_stats(struct mt7915_dev *dev, u16 wlan_idx)
+int mt7915_mcu_wed_wa_tx_stats(struct mt7915_dev *dev, u16 wlan_idx,
+			       struct mt7915_sta *sta)
 {
 	struct {
 		__le32 cmd;
@@ -4452,11 +4453,33 @@ int mt7915_mcu_wed_wa_tx_stats(struct mt7915_dev *dev, u16 wlan_idx)
 	rcu_read_lock();
 
 	wcid = rcu_dereference(dev->mt76.wcid[wlan_idx]);
-	if (wcid)
+	if (wcid) {
+		struct ieee80211_vif *vif;
+		struct wireless_dev *wdev;
+		struct pcpu_sw_netstats *tstats;
+
 		wcid->stats.tx_packets += le32_to_cpu(res->tx_packets);
-	else
+
+		if (!wiphy_ext_feature_isset(dev->mphy.hw->wiphy,
+					     NL80211_EXT_FEATURE_STAS_COUNT) ||
+		    !sta)
+			 goto unlock;
+
+		vif = container_of((void *)sta->vif,
+				   struct ieee80211_vif,
+				   drv_priv);
+		if (!vif)
+			goto unlock;
+
+		wdev = ieee80211_vif_to_wdev(vif);
+
+		tstats = this_cpu_ptr(wdev->netdev->tstats);
+		tstats->tx_packets += le32_to_cpu(res->tx_packets);
+	} else {
 		ret = -EINVAL;
+	}
 
+unlock:
 	rcu_read_unlock();
 out:
 	dev_kfree_skb(skb);
diff --git a/mt7915/mmio.c b/mt7915/mmio.c
index 89523a52..00c190c7 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -704,8 +704,34 @@ static void mt7915_mmio_wed_update_rx_stats(struct mtk_wed_device *wed,
 		wcid->stats.rx_packets += le32_to_cpu(stats->rx_pkt_cnt);
 		wcid->stats.rx_errors += le32_to_cpu(stats->rx_err_cnt);
 		wcid->stats.rx_drops += le32_to_cpu(stats->rx_drop_cnt);
+
+		if (wiphy_ext_feature_isset(dev->mphy.hw->wiphy,
+					    NL80211_EXT_FEATURE_STAS_COUNT)) {
+			struct mt7915_sta *msta;
+			struct ieee80211_vif *vif;
+			struct wireless_dev *wdev;
+			struct pcpu_sw_netstats *tstats;
+
+			msta = container_of(wcid, struct mt7915_sta, wcid);
+
+			if (!msta || !msta->vif)
+				goto unlock;
+
+			vif = container_of((void *)msta->vif,
+					   struct ieee80211_vif,
+					   drv_priv);
+			if (!vif)
+				goto unlock;
+
+			wdev = ieee80211_vif_to_wdev(vif);
+			tstats = this_cpu_ptr(wdev->netdev->tstats);
+
+			tstats->rx_packets += le32_to_cpu(stats->rx_pkt_cnt);
+			tstats->rx_bytes += le32_to_cpu(stats->rx_byte_cnt);
+		}
 	}
 
+unlock:
 	rcu_read_unlock();
 }
 
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 70e92734..8171ea8a 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -673,7 +673,8 @@ int mt7915_mcu_get_rx_rate(struct mt7915_phy *phy, struct ieee80211_vif *vif,
 			   struct ieee80211_sta *sta, struct rate_info *rate);
 int mt7915_mcu_rdd_background_enable(struct mt7915_phy *phy,
 				     struct cfg80211_chan_def *chandef);
-int mt7915_mcu_wed_wa_tx_stats(struct mt7915_dev *dev, u16 wcid);
+int mt7915_mcu_wed_wa_tx_stats(struct mt7915_dev *dev, u16 wcid,
+			       struct mt7915_sta *sta);
 int mt7915_get_tx_stat(struct mt7915_phy *phy, u16 wlan_idx);
 int mt7915_mcu_rf_regval(struct mt7915_dev *dev, u32 regidx, u32 *val, bool set);
 int mt7915_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3);
diff --git a/mt7915/mtk_debugfs.c b/mt7915/mtk_debugfs.c
index 401b4dd6..7a641b10 100644
--- a/mt7915/mtk_debugfs.c
+++ b/mt7915/mtk_debugfs.c
@@ -3797,7 +3797,7 @@ static int mt7915_reset_counter(void *data, u64 val)
 	struct mt76_wcid *wcid;
 
 	/* Clear the firmware counters */
-	mt7915_mcu_wed_wa_tx_stats(dev, dev->wlan_idx);
+	mt7915_mcu_wed_wa_tx_stats(dev, dev->wlan_idx, NULL);
 	mt7915_get_tx_stat(phy, dev->wlan_idx);
 
 	rcu_read_lock();
-- 
2.39.2