From 55619fa44187b6ed841f94c2bedbc4779457e3f9 Mon Sep 17 00:00:00 2001
From: MeiChia Chiu <meichia.chiu@mediatek.com>
Date: Tue, 13 Dec 2022 15:17:43 +0800
Subject: [PATCH 1002/1015] wifi: mt76: mt7996: add mu vendor command support

Change-Id: I4599bd97917651aaea51d7ff186ffff73a07e4ce
---
 mt7996/Makefile |  3 +-
 mt7996/init.c   |  8 +++++
 mt7996/mcu.c    | 37 +++++++++++++++++++---
 mt7996/mcu.h    | 12 +++++++
 mt7996/mt7996.h |  7 +++++
 mt7996/vendor.c | 84 +++++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/vendor.h | 22 +++++++++++++
 7 files changed, 167 insertions(+), 6 deletions(-)
 create mode 100644 mt7996/vendor.c
 create mode 100644 mt7996/vendor.h

diff --git a/mt7996/Makefile b/mt7996/Makefile
index df131869..8dbbc34c 100644
--- a/mt7996/Makefile
+++ b/mt7996/Makefile
@@ -1,11 +1,12 @@
 # SPDX-License-Identifier: ISC
 EXTRA_CFLAGS += -DCONFIG_MT76_LEDS
 EXTRA_CFLAGS += -DCONFIG_MTK_DEBUG
+EXTRA_CFLAGS += -DCONFIG_MTK_VENDOR
 
 obj-$(CONFIG_MT7996E) += mt7996e.o
 
 mt7996e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
-	     debugfs.o mmio.o
+	     debugfs.o mmio.o vendor.o
 
 mt7996e-$(CONFIG_DEV_COREDUMP) += coredump.o
 
diff --git a/mt7996/init.c b/mt7996/init.c
index 96c4bb01..1d4359f0 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -579,6 +579,10 @@ static int mt7996_register_phy(struct mt7996_dev *dev, struct mt7996_phy *phy,
 	if (ret)
 		goto error;
 
+#ifdef CONFIG_MTK_VENDOR
+	mt7996_vendor_register(phy);
+#endif
+
 	ret = mt76_register_phy(mphy, true, mt76_rates,
 				ARRAY_SIZE(mt76_rates));
 	if (ret)
@@ -1082,6 +1086,10 @@ int mt7996_register_device(struct mt7996_dev *dev)
 	dev->mt76.test_ops = &mt7996_testmode_ops;
 #endif
 
+#ifdef CONFIG_MTK_VENDOR
+	mt7996_vendor_register(&dev->phy);
+#endif
+
 	ret = mt76_register_device(&dev->mt76, true, mt76_rates,
 				   ARRAY_SIZE(mt76_rates));
 	if (ret)
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 58893348..91f3103a 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -1158,6 +1158,8 @@ static void
 mt7996_mcu_sta_muru_tlv(struct mt7996_dev *dev, struct sk_buff *skb,
 			struct ieee80211_vif *vif, struct ieee80211_sta *sta)
 {
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_phy *phy = mvif->phy;
 	struct ieee80211_he_cap_elem *elem = &sta->deflink.he_cap.he_cap_elem;
 	struct sta_rec_muru *muru;
 	struct tlv *tlv;
@@ -1169,11 +1171,14 @@ mt7996_mcu_sta_muru_tlv(struct mt7996_dev *dev, struct sk_buff *skb,
 	tlv = mt76_connac_mcu_add_tlv(skb, STA_REC_MURU, sizeof(*muru));
 
 	muru = (struct sta_rec_muru *)tlv;
-	muru->cfg.mimo_dl_en = vif->bss_conf.eht_mu_beamformer ||
-			       vif->bss_conf.he_mu_beamformer ||
-			       vif->bss_conf.vht_mu_beamformer ||
-			       vif->bss_conf.vht_mu_beamformee;
-	muru->cfg.ofdma_dl_en = true;
+	muru->cfg.mimo_dl_en = (vif->bss_conf.eht_mu_beamformer ||
+				vif->bss_conf.he_mu_beamformer ||
+				vif->bss_conf.vht_mu_beamformer ||
+				vif->bss_conf.vht_mu_beamformee) &&
+			       !!(phy->muru_onoff & MUMIMO_DL);
+	muru->cfg.mimo_ul_en = !!(phy->muru_onoff & MUMIMO_UL);
+	muru->cfg.ofdma_dl_en = !!(phy->muru_onoff & OFDMA_DL);
+	muru->cfg.ofdma_ul_en = !!(phy->muru_onoff & OFDMA_UL);
 
 	if (sta->deflink.vht_cap.vht_supported)
 		muru->mimo_dl.vht_mu_bfee =
@@ -4353,3 +4358,25 @@ int mt7996_mcu_set_scs(struct mt7996_phy *phy, u8 enable)
 	return mt76_mcu_send_msg(&phy->dev->mt76, MCU_WM_UNI_CMD(SCS),
 				 &req, sizeof(req), false);
 }
+
+#ifdef CONFIG_MTK_VENDOR
+void mt7996_set_wireless_vif(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	u8 mode, val;
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_phy *phy =  mvif->phy;
+
+	mode = FIELD_GET(RATE_CFG_MODE, *((u32 *)data));
+	val = FIELD_GET(RATE_CFG_VAL, *((u32 *)data));
+
+	switch (mode) {
+	case RATE_PARAM_AUTO_MU:
+		if (val < 0 || val > 15) {
+			printk("Wrong value! The value is between 0-15.\n");
+			break;
+		}
+		phy->muru_onoff = val;
+		break;
+	}
+}
+#endif
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index 5d52b0b7..baffbcd7 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -568,8 +568,20 @@ enum {
 	RATE_PARAM_FIXED_MCS,
 	RATE_PARAM_FIXED_GI = 11,
 	RATE_PARAM_AUTO = 20,
+#ifdef CONFIG_MTK_VENDOR
+	RATE_PARAM_AUTO_MU = 32,
+#endif
 };
 
+#define RATE_CFG_MODE	GENMASK(15, 8)
+#define RATE_CFG_VAL	GENMASK(7, 0)
+
+/* MURU */
+#define OFDMA_DL                       BIT(0)
+#define OFDMA_UL                       BIT(1)
+#define MUMIMO_DL                      BIT(2)
+#define MUMIMO_UL                      BIT(3)
+
 enum {
 	BF_SOUNDING_ON = 1,
 	BF_HW_EN_UPDATE = 17,
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 085307ab..b9f3dd8e 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -263,6 +263,8 @@ struct mt7996_phy {
 	u32 rx_ampdu_ts;
 	u32 ampdu_ref;
 
+	u8 muru_onoff;
+
 	struct mib_stats mib;
 	struct mt76_channel_state state_ts;
 
@@ -672,6 +674,11 @@ void mt7996_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta, struct dentry *dir);
 #endif
 
+#ifdef CONFIG_MTK_VENDOR
+void mt7996_set_wireless_vif(void *data, u8 *mac, struct ieee80211_vif *vif);
+void mt7996_vendor_register(struct mt7996_phy *phy);
+#endif
+
 #ifdef CONFIG_MTK_DEBUG
 int mt7996_mtk_init_debugfs(struct mt7996_phy *phy, struct dentry *dir);
 #endif
diff --git a/mt7996/vendor.c b/mt7996/vendor.c
new file mode 100644
index 00000000..08ecc2b3
--- /dev/null
+++ b/mt7996/vendor.c
@@ -0,0 +1,84 @@
+// SPDX-License-Identifier: ISC
+/*
+ * Copyright (C) 2020, MediaTek Inc. All rights reserved.
+ */
+
+#include <net/netlink.h>
+
+#include "mt7996.h"
+#include "mcu.h"
+#include "vendor.h"
+
+static const struct nla_policy
+mu_ctrl_policy[NUM_MTK_VENDOR_ATTRS_MU_CTRL] = {
+	[MTK_VENDOR_ATTR_MU_CTRL_ONOFF] = {.type = NLA_U8 },
+	[MTK_VENDOR_ATTR_MU_CTRL_DUMP] = {.type = NLA_U8 },
+};
+
+static int mt7996_vendor_mu_ctrl(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 const void *data,
+				 int data_len)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct nlattr *tb[NUM_MTK_VENDOR_ATTRS_MU_CTRL];
+	int err;
+	u8 val8;
+	u32 val32 = 0;
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_MU_CTRL_MAX, data, data_len,
+			mu_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (tb[MTK_VENDOR_ATTR_MU_CTRL_ONOFF]) {
+		val8 = nla_get_u8(tb[MTK_VENDOR_ATTR_MU_CTRL_ONOFF]);
+		val32 |= FIELD_PREP(RATE_CFG_MODE, RATE_PARAM_AUTO_MU) |
+			 FIELD_PREP(RATE_CFG_VAL, val8);
+		ieee80211_iterate_active_interfaces_atomic(hw, IEEE80211_IFACE_ITER_RESUME_ALL,
+							   mt7996_set_wireless_vif, &val32);
+	}
+
+	return 0;
+}
+
+static int
+mt7996_vendor_mu_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
+			   struct sk_buff *skb, const void *data, int data_len,
+			   unsigned long *storage)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7996_phy *phy = mt7996_hw_phy(hw);
+	int len = 0;
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	if (nla_put_u8(skb, MTK_VENDOR_ATTR_MU_CTRL_DUMP, phy->muru_onoff))
+		return -ENOMEM;
+	len += 1;
+
+	return len;
+}
+
+static const struct wiphy_vendor_command mt7996_vendor_commands[] = {
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_MU_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7996_vendor_mu_ctrl,
+		.dumpit = mt7996_vendor_mu_ctrl_dump,
+		.policy = mu_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_MU_CTRL_MAX,
+	},
+};
+
+void mt7996_vendor_register(struct mt7996_phy *phy)
+{
+	phy->mt76->hw->wiphy->vendor_commands = mt7996_vendor_commands;
+	phy->mt76->hw->wiphy->n_vendor_commands = ARRAY_SIZE(mt7996_vendor_commands);
+}
diff --git a/mt7996/vendor.h b/mt7996/vendor.h
new file mode 100644
index 00000000..8ac3ba8e
--- /dev/null
+++ b/mt7996/vendor.h
@@ -0,0 +1,22 @@
+#ifndef __MT7996_VENDOR_H
+#define __MT7996_VENDOR_H
+
+#define MTK_NL80211_VENDOR_ID	0x0ce7
+
+enum mtk_nl80211_vendor_subcmds {
+	MTK_NL80211_VENDOR_SUBCMD_MU_CTRL = 0xc5,
+};
+
+enum mtk_vendor_attr_mu_ctrl {
+	MTK_VENDOR_ATTR_MU_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_MU_CTRL_ONOFF,
+	MTK_VENDOR_ATTR_MU_CTRL_DUMP,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_MU_CTRL,
+	MTK_VENDOR_ATTR_MU_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_MU_CTRL - 1
+};
+
+#endif
-- 
2.39.2