[PATCH 7/9] wifi: mt76: mt7925: inject the deauth for an aging-timeout SAP event

[JB Tsai <jb.tsai@mediatek.com>]

From: james-ly huang <james-ly.huang@mediatek.com>

The FW ages out inactive clients of an AP/GO and reports it with a
SAP aging-timeout event; the expected reaction is a deauth with
REASON_CODE_DISASSOC_INACTIVITY. The AP MLME here lives in
wpa_supplicant (softmac, device_ap_sme=0), so synthesize the deauth
frame the GO would have received from the GC and hand it to
userspace via cfg80211_rx_mgmt_khz().

Handle the new MCU_UNI_EVENT_SAP in p2p.c
(mt7925_p2p_mcu_sap_event) and dispatch it from the unsolicited
event handler.

Signed-off-by: james-ly huang <james-ly.huang@mediatek.com>
---
 .../wireless/mediatek/mt76/mt76_connac_mcu.h  |   1 +
 .../net/wireless/mediatek/mt76/mt7925/mcu.c   |   4 +
 .../net/wireless/mediatek/mt76/mt7925/p2p.c   | 141 ++++++++++++++++++
 .../net/wireless/mediatek/mt76/mt7925/p2p.h   |  16 ++
 4 files changed, 162 insertions(+)

diff --git a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
index bb98a6eacac4..817554ffb9d8 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
@@ -1099,6 +1099,7 @@ enum {
 	MCU_UNI_EVENT_PS_SYNC = 0x0d,
 	MCU_UNI_EVENT_SCAN_DONE = 0x0e,
 	MCU_UNI_EVENT_RDD_REPORT = 0x11,
+	MCU_UNI_EVENT_SAP = 0x1c,
 	MCU_UNI_EVENT_ROC = 0x27,
 	MCU_UNI_EVENT_MBMC = 0x28,
 	MCU_UNI_EVENT_TX_DONE = 0x2d,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
index 6ec74d7bfd82..d9a5b5f57fba 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
@@ -8,6 +8,7 @@
 #include "mcu.h"
 #include "mac.h"
 #include "nan.h"
+#include "p2p.h"
 
 #define MT_STA_BFER			BIT(0)
 #define MT_STA_BFEE			BIT(1)
@@ -734,6 +735,9 @@ mt7925_mcu_uni_rx_unsolicited_event(struct mt792x_dev *dev,
 	case MCU_UNI_EVENT_BSS_BEACON_LOSS:
 		mt7925_mcu_connection_loss_event(dev, skb);
 		break;
+	case MCU_UNI_EVENT_SAP:
+		mt7925_p2p_mcu_sap_event(dev, skb);
+		break;
 	case MCU_UNI_EVENT_RSSI_MONITOR:
 		mt7925_mcu_rssi_monitor_event(dev, skb);
 		break;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/p2p.c b/drivers/net/wireless/mediatek/mt76/mt7925/p2p.c
index f41098b324ba..d05c936d40ea 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7925/p2p.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/p2p.c
@@ -462,3 +462,144 @@ bool mt7925_p2p_chanctx_bss_programmed(struct ieee80211_vif *vif,
 	}
 }
 
+/* ------------------------------------------------------------------ */
+/* P2P-only MCU events                                                  */
+/* ------------------------------------------------------------------ */
+
+/* FW reported that a peer of our AP/GO aged out (no traffic/no ack for the FW
+ * inactivity period); the expected reaction is a deauth with
+ * REASON_CODE_DISASSOC_INACTIVITY. Here the AP MLME lives in
+ * wpa_supplicant (softmac, device_ap_sme=0), so instead of tearing the
+ * STA down in the driver we synthesize the deauth frame the GO would
+ * have received from the GC and hand it to userspace via
+ * NL80211_CMD_FRAME.
+ */
+static void
+mt7925_mcu_sap_aging_timeout(struct mt792x_dev *dev, u16 wlan_idx)
+{
+	struct {
+		struct ieee80211_hdr_3addr hdr;
+		__le16 reason_code;
+	} __packed deauth = {};
+	u8 peer[ETH_ALEN], own[ETH_ALEN], bssid[ETH_ALEN];
+	struct ieee80211_chanctx_conf *ctx;
+	struct mt792x_bss_conf *mconf;
+	struct mt792x_link_sta *mlink;
+	struct ieee80211_vif *vif;
+	struct ieee80211_sta *sta;
+	struct mt792x_vif *mvif;
+	struct mt792x_sta *msta;
+	struct mt76_wcid *wcid;
+	struct wireless_dev *wdev;
+	int freq = 0;
+	u8 link_id;
+
+	rcu_read_lock();
+
+	wcid = mt76_wcid_ptr(dev, wlan_idx);
+	if (!wcid || !wcid->sta)
+		goto unlock;
+
+	mlink = container_of(wcid, struct mt792x_link_sta, wcid);
+	msta = mlink->sta;
+	if (!msta || !msta->vif)
+		goto unlock;
+
+	vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
+	if (vif->type != NL80211_IFTYPE_AP &&
+	    vif->type != NL80211_IFTYPE_P2P_GO)
+		goto unlock;
+
+	mvif = (struct mt792x_vif *)vif->drv_priv;
+	sta = container_of((void *)msta, struct ieee80211_sta, drv_priv);
+	link_id = wcid->link_id;
+
+	ether_addr_copy(peer, sta->addr);
+	ether_addr_copy(own, vif->addr);
+	ether_addr_copy(bssid, vif->bss_conf.bssid);
+	mconf = &mvif->bss_conf;
+
+	if (ieee80211_vif_is_mld(vif) && link_id < IEEE80211_LINK_UNSPECIFIED) {
+		struct ieee80211_link_sta *link_sta;
+		struct ieee80211_bss_conf *link_conf;
+		struct mt792x_bss_conf *link_mconf;
+
+		link_sta = rcu_dereference(sta->link[link_id]);
+		if (link_sta)
+			ether_addr_copy(peer, link_sta->addr);
+
+		link_conf = rcu_dereference(vif->link_conf[link_id]);
+		if (link_conf) {
+			ether_addr_copy(own, link_conf->addr);
+			ether_addr_copy(bssid, link_conf->bssid);
+		}
+
+		link_mconf = rcu_dereference(mvif->link_conf[link_id]);
+		if (link_mconf)
+			mconf = link_mconf;
+	}
+
+	ctx = mconf->mt76.ctx;
+	if (ctx && ctx->def.chan)
+		freq = ctx->def.chan->center_freq;
+	else if (dev->mphy.chandef.chan)
+		freq = dev->mphy.chandef.chan->center_freq;
+
+	rcu_read_unlock();
+
+	deauth.hdr.frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					       IEEE80211_STYPE_DEAUTH);
+	ether_addr_copy(deauth.hdr.addr1, own);	/* DA: the GO itself */
+	ether_addr_copy(deauth.hdr.addr2, peer);	/* SA: the aged-out GC */
+	ether_addr_copy(deauth.hdr.addr3, bssid);
+	deauth.reason_code = cpu_to_le16(WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY);
+
+	wdev = ieee80211_vif_to_wdev(vif);
+	if (!wdev)
+		return;
+
+	cfg80211_rx_mgmt_khz(wdev, MHZ_TO_KHZ(freq), 0, (const u8 *)&deauth,
+			     sizeof(deauth), 0);
+
+	return;
+
+unlock:
+	rcu_read_unlock();
+}
+
+void mt7925_p2p_mcu_sap_event(struct mt792x_dev *dev, struct sk_buff *skb)
+{
+	struct mt7925_uni_sap_event *event;
+	u32 tlv_len;
+	u8 *tlv;
+
+	skb_pull(skb, sizeof(struct mt7925_mcu_rxd));
+	if (skb->len < sizeof(*event))
+		return;
+
+	event = (struct mt7925_uni_sap_event *)skb->data;
+	tlv = event->tlv;
+	tlv_len = skb->len - sizeof(*event);
+
+	while (tlv_len >= sizeof(struct tlv)) {
+		struct tlv *hdr = (struct tlv *)tlv;
+		u16 tag = le16_to_cpu(hdr->tag);
+		u16 len = le16_to_cpu(hdr->len);
+
+		if (len < sizeof(*hdr) || len > tlv_len)
+			break;
+
+		if (tag == MT7925_SAP_TAG_AGING_TIMEOUT &&
+		    len >= sizeof(struct mt7925_sap_aging_timeout_tlv)) {
+			struct mt7925_sap_aging_timeout_tlv *aging =
+				(struct mt7925_sap_aging_timeout_tlv *)tlv;
+
+			mt7925_mcu_sap_aging_timeout(dev,
+						     le16_to_cpu(aging->wlan_idx));
+		}
+
+		tlv += len;
+		tlv_len -= len;
+	}
+}
+
diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/p2p.h b/drivers/net/wireless/mediatek/mt76/mt7925/p2p.h
index f7af45ee4c1f..9afae222845f 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7925/p2p.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/p2p.h
@@ -39,6 +39,20 @@ static inline bool mt7925_is_p2p_go_probe_resp(struct sk_buff *skb)
 	return ieee80211_is_probe_resp(mgmt->frame_control);
 }
 
+/* SAP event (FW event UNI_EVENT_ID_SAP / enum ENUM_UNI_EVENT_SAP_TAG) */
+#define MT7925_SAP_TAG_AGING_TIMEOUT		0
+
+struct mt7925_sap_aging_timeout_tlv {
+	__le16 tag;
+	__le16 len;
+	__le16 wlan_idx;
+	u8 pad[2];
+} __packed;
+
+struct mt7925_uni_sap_event {
+	u8 pad[4];	/* fixed field */
+	u8 tlv[];
+} __packed;
 /* TX/RX path overrides, BMC wcid bookkeeping and the P2P-only MCU event
  * handlers.
  */
@@ -54,4 +68,6 @@ void mt7925_p2p_bmc_remove(struct mt792x_dev *dev,
 bool mt7925_p2p_chanctx_bss_programmed(struct ieee80211_vif *vif,
 				       struct ieee80211_bss_conf *link_conf);
 
+void mt7925_p2p_mcu_sap_event(struct mt792x_dev *dev, struct sk_buff *skb);
+
 #endif /* __MT7925_P2P_H */
-- 
2.45.2