[PATCH 3/6] wifi: mt76: mt7915: Support setting aid when in monitor mode.

[greearb@candelatech.com]

From: Ben Greear <greearb@candelatech.com>

The parser expects values in hex.
Syntax: aid color uldl enables

The enables value is a set of flags to enable any/all of the
aid, color, and/or uldl (in that order).
options.  For uldl, 0x1 means upload.
Example, capture aid 11:
echo "b 0 0 1" > /debug/ieee80211/phy0/mt76/he_sniffer_params

Note that you must also enable the group-5 fields in the rx-status
header for he-trig (and he-mu) to show up properly in a packet
capture.

Signed-off-by: Ben Greear <greearb@candelatech.com>
---
 .../wireless/mediatek/mt76/mt7915/debugfs.c   | 60 +++++++++++++++++++
 .../net/wireless/mediatek/mt76/mt7915/main.c  | 35 +++++++++++
 .../wireless/mediatek/mt76/mt7915/mt7915.h    |  8 +++
 .../net/wireless/mediatek/mt76/mt7915/regs.h  |  9 +++
 4 files changed, 112 insertions(+)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/debugfs.c b/drivers/net/wireless/mediatek/mt76/mt7915/debugfs.c
index 879884ead660..f836754103dc 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/debugfs.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/debugfs.c
@@ -247,6 +247,64 @@ mt7915_muru_debug_get(void *data, u64 *val)
 DEFINE_DEBUGFS_ATTRIBUTE(fops_muru_debug, mt7915_muru_debug_get,
 			 mt7915_muru_debug_set, "%lld\n");
 
+static ssize_t
+mt7915_he_monitor_set(struct file *file, const char __user *user_buf,
+		      size_t count, loff_t *ppos)
+{
+	struct mt7915_phy *phy = file->private_data;
+	char buf[64] = {0};
+	u32 aid, bss_color, uldl, enables;
+	int ret;
+	struct mt7915_dev *dev = phy->dev;
+
+	if (count >= sizeof(buf))
+		return -EINVAL;
+
+	if (copy_from_user(buf, user_buf, count))
+		return -EFAULT;
+
+	ret = sscanf(buf, "%x %x %x %x",
+		     &aid, &bss_color, &uldl, &enables);
+	if (ret != 4)
+		return -EINVAL;
+
+	phy->monitor_cur_aid = aid;
+	phy->monitor_cur_color = bss_color;
+	phy->monitor_cur_uldl = uldl;
+	phy->monitor_cur_enables = enables;
+
+	mutex_lock(&dev->mt76.mutex);
+	mt7915_check_apply_monitor_config(phy);
+	mutex_unlock(&dev->mt76.mutex);
+
+	return count;
+}
+
+static ssize_t
+mt7915_he_monitor_get(struct file *file, char __user *user_buf,
+		      size_t count, loff_t *ppos)
+{
+	struct mt7915_phy *phy = file->private_data;
+	u8 buf[32];
+	int len;
+
+	len = scnprintf(buf, sizeof(buf),
+			"aid: 0x%x bss-color: 0x%x  uldl: 0x%x  enables: 0x%x\n"
+			"  ULDL:  0 is download, 1 is upload\n"
+			"  Enable-bits: 1: AID  2: Color  4: ULDL\n",
+			phy->monitor_cur_aid, phy->monitor_cur_color,
+			phy->monitor_cur_uldl, phy->monitor_cur_enables);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static const struct file_operations mt7915_he_sniffer_ops = {
+	.write = mt7915_he_monitor_set,
+	.read = mt7915_he_monitor_get,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
 static int mt7915_muru_stats_show(struct seq_file *file, void *data)
 {
 	struct mt7915_phy *phy = file->private;
@@ -1230,6 +1288,8 @@ int mt7915_init_debugfs(struct mt7915_phy *phy)
 	debugfs_create_file("tx_stats", 0400, dir, phy, &mt7915_tx_stats_fops);
 	debugfs_create_file("sys_recovery", 0600, dir, phy,
 			    &mt7915_sys_recovery_ops);
+	debugfs_create_file("he_sniffer_params", 0600, dir, phy,
+			    &mt7915_he_sniffer_ops);
 	debugfs_create_file("fw_debug_wm", 0600, dir, dev, &fops_fw_debug_wm);
 	debugfs_create_file("fw_debug_wa", 0600, dir, dev, &fops_fw_debug_wa);
 	debugfs_create_file("fw_debug_bin", 0600, dir, dev, &fops_fw_debug_bin);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/main.c b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
index 7ec9f45cfa15..fec30cf26855 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
@@ -516,6 +516,40 @@ mt7915_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	return 0;
 }
 
+void mt7915_check_apply_monitor_config(struct mt7915_phy *phy)
+{
+	/* I first thought that this would not work well when combined
+	 * with STA mode, but now I think it will not matter since it appears
+	 * the HEMU rule 1 is only used after normal HE MU operations
+	 * have happened. --Ben
+	 */
+	struct mt7915_dev *dev = phy->dev;
+	u32 reg = mt76_rr(dev, MT_WF_HEMU_RULE1);
+
+	reg &= ~(MT_WF_HEMU_RULE1_AID |
+		 MT_WF_HEMU_RULE1_COLOR |
+		 MT_WF_HEMU_RULE1_ULDL |
+		 MT_WF_HEMU_RULE1_AID_ENABLE |
+		 MT_WF_HEMU_RULE1_BSS_COLOR_ENABLE |
+		 MT_WF_HEMU_RULE1_ULDL_ENABLE |
+		 MT_WF_HEMU_RULE1_PRIORITY);
+	reg |= phy->monitor_cur_aid;
+	reg |= (phy->monitor_cur_color << 10) & 0x3f;
+	if (phy->monitor_cur_uldl)
+		reg |= MT_WF_HEMU_RULE1_ULDL;
+	if (phy->monitor_cur_enables) {
+		if (phy->monitor_cur_enables & 0x1)
+			reg |= MT_WF_HEMU_RULE1_AID_ENABLE;
+		if (phy->monitor_cur_enables & 0x2)
+			reg |= MT_WF_HEMU_RULE1_BSS_COLOR_ENABLE;
+		if (phy->monitor_cur_enables & 0x4)
+			reg |= MT_WF_HEMU_RULE1_ULDL_ENABLE;
+		reg |= MT_WF_HEMU_RULE1_PRIORITY; /* set priority to 7 */
+	}
+
+	mt76_wr(dev, MT_WF_HEMU_RULE1, reg);
+}
+
 static void __mt7915_configure_filter(struct ieee80211_hw *hw,
 				      unsigned int changed_flags,
 				      unsigned int *total_flags,
@@ -575,6 +609,7 @@ static void __mt7915_configure_filter(struct ieee80211_hw *hw,
 		mt76_clear(dev, MT_WF_RFCR1(band), ctl_flags);
 		mt76_set(dev, MT_WF_RMAC_TOP_TF_PARSER(band),
 			 MT_WF_RMAC_TOP_TF_SNIFFER);
+		mt7915_check_apply_monitor_config(phy);
 	} else {
 		mt76_set(dev, MT_WF_RFCR1(band), ctl_flags);
 		mt76_clear(dev, MT_WF_RMAC_TOP_TF_PARSER(band),
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
index 06f98e5cd95e..084001647aaa 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
@@ -242,6 +242,13 @@ struct mt7915_phy {
 	struct ieee80211_sband_iftype_data iftype[NUM_NL80211_BANDS][NUM_NL80211_IFTYPES];
 
 	struct ieee80211_vif *monitor_vif;
+	u16 monitor_cur_aid; /* aid to be used in monitor mode to capture HE trigger frames */
+	/* bss-color to be used in monitor mode to capture HE trigger frames */
+	u8 monitor_cur_color;
+	/* upload/download to be used in monitor mode to capture HE trigger frames */
+	u8 monitor_cur_uldl;
+	/* Specifies which of the above are used:  0x1 is AID, 0x2 is color, 0x3 is uldl */
+	u8 monitor_cur_enables;
 
 	struct thermal_cooling_device *cdev;
 	u32 mac80211_rxfilter_flags;
@@ -628,6 +635,7 @@ int mt7915_mcu_muru_debug_set(struct mt7915_dev *dev, bool enable);
 int mt7915_mcu_muru_debug_get(struct mt7915_phy *phy, void *ms);
 int mt7915_mcu_wed_enable_rx_stats(struct mt7915_dev *dev);
 int mt7915_init_debugfs(struct mt7915_phy *phy);
+void mt7915_check_apply_monitor_config(struct mt7915_phy *phy);
 void mt7915_debugfs_rx_fw_monitor(struct mt7915_dev *dev, const void *data, int len);
 bool mt7915_debugfs_rx_log(struct mt7915_dev *dev, const void *data, int len);
 #ifdef CONFIG_MAC80211_DEBUGFS
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/regs.h b/drivers/net/wireless/mediatek/mt76/mt7915/regs.h
index 0118fdaa96b3..252e5f1405cf 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/regs.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/regs.h
@@ -1217,4 +1217,13 @@ enum offs_rev {
 #define MT_MCU_WM_CIRQ_EINT_MASK_CLR_ADDR	MT_MCU_WM_CIRQ(0x108)
 #define MT_MCU_WM_CIRQ_EINT_SOFT_ADDR		MT_MCU_WM_CIRQ(0x118)
 
+#define MT_WF_HEMU_RULE1                       (MT_WF_PHY_BASE + 0x10e0)
+#define MT_WF_HEMU_RULE1_AID                   GENMASK(10, 0)
+#define MT_WF_HEMU_RULE1_COLOR                 GENMASK(21, 16)
+#define MT_WF_HEMU_RULE1_ULDL                  (BIT(22)) /* 0 dl, 1 ul */
+#define MT_WF_HEMU_RULE1_AID_ENABLE            (BIT(24))
+#define MT_WF_HEMU_RULE1_BSS_COLOR_ENABLE      (BIT(25))
+#define MT_WF_HEMU_RULE1_ULDL_ENABLE           (BIT(26))
+#define MT_WF_HEMU_RULE1_PRIORITY              GENMASK(30, 28) /* 0 disable, 7 is highest */
+
 #endif
-- 
2.40.0