From d9f388ece4275ee2ff5102825110dc9c7557b1a1 Mon Sep 17 00:00:00 2001
From: MeiChia Chiu <MeiChia.Chiu@mediatek.com>
Date: Wed, 24 Jan 2024 14:39:14 +0800
Subject: [PATCH 1048/1049] wifi: mt76: mt7915: add no_beacon vendor command
 for cert

Add the vendor command to disable/enable beacon

[Usage]
hostapd_cli -i <interface> no_beacon <value>
<value>
0: enable beacon
1: disable beacon

Signed-off-by: MeiChia Chiu <meichia.chiu@mediatek.com>
---
 mt7915/mcu.c    | 11 +++++++++++
 mt7915/mt7915.h |  1 +
 mt7915/vendor.c | 42 +++++++++++++++++++++++++++++++++++++++++-
 mt7915/vendor.h | 12 ++++++++++++
 4 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index ba47d0d0..3b214a23 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -5107,6 +5107,17 @@ int mt7915_mcu_set_rfeature_trig_type(struct mt7915_phy *phy, u8 enable, u8 trig
 		return 0;
 	}
 }
+
+void mt7915_set_beacon_vif(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
+	struct ieee80211_hw *hw = mvif->phy->mt76->hw;
+	u8 val = *((u8 *)data);
+
+	vif->bss_conf.enable_beacon = val;
+
+	mt7915_mcu_add_beacon(hw, vif, val);
+}
 #endif
 
 #ifdef MTK_DEBUG
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index 23ee118f..3a596dad 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -794,6 +794,7 @@ void mt7915_vendor_amnt_fill_rx(struct mt7915_phy *phy, struct sk_buff *skb);
 int mt7915_vendor_amnt_sta_remove(struct mt7915_phy *phy,
 				  struct ieee80211_sta *sta);
 int mt7915_mcu_set_lpi(struct mt7915_phy *phy, bool en);
+void mt7915_set_beacon_vif(void *data, u8 *mac, struct ieee80211_vif *vif);
 #endif
 int mt7915_mcu_set_edcca(struct mt7915_phy *phy, int mode, u8 *value, s8 compensation);
 int mt7915_mcu_get_edcca(struct mt7915_phy *phy, u8 mode, s8 *value);
diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index 566fec01..6154d1a0 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -113,6 +113,11 @@ txpower_ctrl_policy[NUM_MTK_VENDOR_ATTRS_TXPOWER_CTRL] = {
 	[MTK_VENDOR_ATTR_TXPOWER_CTRL_BCN_DUP] = { .type = NLA_U8 },
 };
 
+static const struct nla_policy
+beacon_ctrl_policy[NUM_MTK_VENDOR_ATTRS_BEACON_CTRL] = {
+		[MTK_VENDOR_ATTR_BEACON_CTRL_MODE] = { .type = NLA_U8 },
+};
+
 struct csi_null_tone {
 	u8 start;
 	u8 end;
@@ -1399,6 +1404,30 @@ static int mt7915_vendor_txpower_ctrl(struct wiphy *wiphy,
 	return 0;
 }
 
+static int mt7915_vendor_beacon_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_BEACON_CTRL];
+	int err;
+	u8 val8;
+
+	err = nla_parse(tb, MTK_VENDOR_ATTR_BEACON_CTRL_MAX, data, data_len,
+			beacon_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (tb[MTK_VENDOR_ATTR_BEACON_CTRL_MODE]) {
+		val8 = nla_get_u8(tb[MTK_VENDOR_ATTR_BEACON_CTRL_MODE]);
+		ieee80211_iterate_active_interfaces_atomic(hw, IEEE80211_IFACE_ITER_RESUME_ALL,
+				mt7915_set_beacon_vif, &val8);
+	}
+
+	return 0;
+}
+
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
 		.info = {
@@ -1526,7 +1555,18 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.doit = mt7915_vendor_txpower_ctrl,
 		.policy = txpower_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_TXPOWER_CTRL_MAX,
-	}
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_BEACON_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7915_vendor_beacon_ctrl,
+		.policy = beacon_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_BEACON_CTRL_MAX,
+	},
 };
 
 void mt7915_vendor_register(struct mt7915_phy *phy)
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index 5b8a1fb3..661d636f 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -16,6 +16,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8,
 	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9,
 	MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca,
+	MTK_NL80211_VENDOR_SUBCMD_BEACON_CTRL = 0xcd,
 	MTK_NL80211_VENDOR_SUBCMD_TXPOWER_CTRL = 0xce,
 };
 
@@ -289,4 +290,15 @@ enum mtk_vendor_attr_txpower_ctrl {
 		NUM_MTK_VENDOR_ATTRS_TXPOWER_CTRL - 1
 };
 
+enum mtk_vendor_attr_beacon_ctrl {
+	MTK_VENDOR_ATTR_BEACON_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_BEACON_CTRL_MODE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_BEACON_CTRL,
+	MTK_VENDOR_ATTR_BEACON_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_BEACON_CTRL - 1
+};
+
 #endif
-- 
2.18.0