From 15d70608c54b7e8823589850714f396b76e39783 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 4 Dec 2023 18:31:02 +0800
Subject: [PATCH 097/116] wifi: mt76: mt7996: rework mac functions for
 multi-link support

This is a preliminary patch to add MLO support for mt7996 chipsets.

Co-developed-by: Bo Jiao <Bo.Jiao@mediatek.com>
Signed-off-by: Bo Jiao <Bo.Jiao@mediatek.com>
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 mt7996/mac.c | 91 +++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 65 insertions(+), 26 deletions(-)

diff --git a/mt7996/mac.c b/mt7996/mac.c
index a32b416c4..31b916695 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -85,10 +85,11 @@ static int mt7996_reverse_frag0_hdr_trans(struct sk_buff *skb, u16 hdr_gap)
 {
 	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
 	struct ethhdr *eth_hdr = (struct ethhdr *)(skb->data + hdr_gap);
-	struct mt7996_sta *msta = (struct mt7996_sta *)status->wcid;
+	struct mt7996_link_sta *mlink = (struct mt7996_link_sta *)status->wcid;
 	__le32 *rxd = (__le32 *)skb->data;
 	struct ieee80211_sta *sta;
 	struct ieee80211_vif *vif;
+	struct ieee80211_bss_conf *conf;
 	struct ieee80211_hdr hdr;
 	u16 frame_control;
 
@@ -99,11 +100,14 @@ static int mt7996_reverse_frag0_hdr_trans(struct sk_buff *skb, u16 hdr_gap)
 	if (!(le32_to_cpu(rxd[1]) & MT_RXD1_NORMAL_GROUP_4))
 		return -EINVAL;
 
-	if (!msta || !msta->vif)
+	if (!mlink->sta || !mlink->sta->vif)
 		return -EINVAL;
 
-	sta = container_of((void *)msta, struct ieee80211_sta, drv_priv);
-	vif = container_of((void *)msta->vif, struct ieee80211_vif, drv_priv);
+	sta = wcid_to_sta(status->wcid);
+	vif = container_of((void *)mlink->sta->vif, struct ieee80211_vif, drv_priv);
+	conf = rcu_dereference(vif->link_conf[mlink->wcid.link_id]);
+	if (unlikely(!conf))
+		return -ENOLINK;
 
 	/* store the info from RXD and ethhdr to avoid being overridden */
 	frame_control = le32_get_bits(rxd[8], MT_RXD8_FRAME_CONTROL);
@@ -116,7 +120,7 @@ static int mt7996_reverse_frag0_hdr_trans(struct sk_buff *skb, u16 hdr_gap)
 	switch (frame_control & (IEEE80211_FCTL_TODS |
 				 IEEE80211_FCTL_FROMDS)) {
 	case 0:
-		ether_addr_copy(hdr.addr3, vif->bss_conf.bssid);
+		ether_addr_copy(hdr.addr3, conf->bssid);
 		break;
 	case IEEE80211_FCTL_FROMDS:
 		ether_addr_copy(hdr.addr3, eth_hdr->h_source);
@@ -955,15 +959,21 @@ u32 mt7996_wed_init_buf(void *ptr, dma_addr_t phys, int token_id)
 }
 
 static void
-mt7996_tx_check_aggr(struct ieee80211_sta *sta, struct sk_buff *skb)
+mt7996_tx_check_aggr(struct ieee80211_sta *sta, struct sk_buff *skb,
+		     struct mt76_wcid *wcid)
 {
 	struct mt7996_sta *msta;
 	struct mt7996_link_sta *mlink;
+	struct ieee80211_link_sta *link_sta;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
 	u16 fc, tid;
 
-	if (!sta || !(sta->deflink.ht_cap.ht_supported || sta->deflink.he_cap.has_he))
+	link_sta = rcu_dereference(sta->link[wcid->link_id]);
+	if (!link_sta)
+		return;
+
+	if (!sta->mlo && !(link_sta->ht_cap.ht_supported || link_sta->he_cap.has_he))
 		return;
 
 	tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
@@ -987,17 +997,17 @@ mt7996_tx_check_aggr(struct ieee80211_sta *sta, struct sk_buff *skb)
 		return;
 
 	msta = (struct mt7996_sta *)sta->drv_priv;
-	mlink = rcu_dereference(msta->link[0]);
+	mlink = rcu_dereference(msta->link[msta->pri_link]);
 	if (!test_and_set_bit(tid, &mlink->wcid.ampdu_state))
 		ieee80211_start_tx_ba_session(sta, tid, 0);
 }
 
 static void
 mt7996_txwi_free(struct mt7996_dev *dev, struct mt76_txwi_cache *t,
-		 struct ieee80211_sta *sta, struct list_head *free_list)
+		 struct ieee80211_sta *sta, struct mt76_wcid *wcid,
+		 struct list_head *free_list)
 {
 	struct mt76_dev *mdev = &dev->mt76;
-	struct mt76_wcid *wcid;
 	__le32 *txwi;
 	u16 wcid_idx;
 
@@ -1007,11 +1017,10 @@ mt7996_txwi_free(struct mt7996_dev *dev, struct mt76_txwi_cache *t,
 
 	txwi = (__le32 *)mt76_get_txwi_ptr(mdev, t);
 	if (sta) {
-		wcid = (struct mt76_wcid *)sta->drv_priv;
 		wcid_idx = wcid->idx;
 
 		if (likely(t->skb->protocol != cpu_to_be16(ETH_P_PAE)))
-			mt7996_tx_check_aggr(sta, t->skb);
+			mt7996_tx_check_aggr(sta, t->skb, wcid);
 	} else {
 		wcid_idx = le32_get_bits(txwi[9], MT_TXD9_WLAN_IDX);
 	}
@@ -1066,7 +1075,9 @@ mt7996_mac_tx_free(struct mt7996_dev *dev, void *data, int len)
 		 */
 		info = le32_to_cpu(*cur_info);
 		if (info & MT_TXFREE_INFO_PAIR) {
-			struct mt7996_link_sta *mlink;
+			struct mt7996_sta *msta;
+			unsigned long valid_links;
+			unsigned int link_id;
 			u16 idx;
 
 			idx = FIELD_GET(MT_TXFREE_INFO_WLAN_ID, info);
@@ -1075,11 +1086,17 @@ mt7996_mac_tx_free(struct mt7996_dev *dev, void *data, int len)
 			if (!sta)
 				continue;
 
-			mlink = container_of(wcid, struct mt7996_link_sta, wcid);
+			valid_links = sta->valid_links ?: BIT(0);
+			msta = (struct mt7996_sta *)sta->drv_priv;
+			/* for MLD STA, add all link's wcid to sta_poll_list */
 			spin_lock_bh(&mdev->sta_poll_lock);
-			if (list_empty(&mlink->wcid.poll_list))
-				list_add_tail(&mlink->wcid.poll_list,
-					      &mdev->sta_poll_list);
+			for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+				struct mt7996_link_sta *mlink =
+					rcu_dereference(msta->link[link_id]);
+
+				if (list_empty(&mlink->wcid.poll_list))
+					list_add_tail(&mlink->wcid.poll_list, &mdev->sta_poll_list);
+			}
 			spin_unlock_bh(&mdev->sta_poll_lock);
 			continue;
 		} else if (info & MT_TXFREE_INFO_HEADER) {
@@ -1115,7 +1132,8 @@ mt7996_mac_tx_free(struct mt7996_dev *dev, void *data, int len)
 			if (!txwi)
 				continue;
 
-			mt7996_txwi_free(dev, txwi, sta, &free_list);
+			mt7996_txwi_free(dev, txwi, sta, wcid, &free_list);
+			txwi->jiffies = 0;
 		}
 	}
 
@@ -1537,19 +1555,29 @@ static void
 mt7996_update_vif_beacon(void *priv, u8 *mac, struct ieee80211_vif *vif)
 {
 	struct ieee80211_hw *hw = priv;
-	struct ieee80211_bss_conf *conf = &vif->bss_conf;
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-	struct mt7996_bss_conf *mconf = &mvif->deflink;
+	unsigned long update = vif->valid_links ?: BIT(0);
+	unsigned int link_id;
 
+	mutex_lock(&dev->mt76.mutex);
 	switch (vif->type) {
 	case NL80211_IFTYPE_MESH_POINT:
 	case NL80211_IFTYPE_ADHOC:
 	case NL80211_IFTYPE_AP:
-		mt7996_mcu_add_beacon(hw, conf, mconf, conf->enable_beacon);
+		for_each_set_bit(link_id, &update, IEEE80211_MLD_MAX_NUM_LINKS) {
+			struct mt7996_bss_conf *mconf =
+					mconf_dereference_protected(mvif, link_id);
+			struct ieee80211_bss_conf *conf =
+					link_conf_dereference_protected(vif, link_id);
+
+			mt7996_mcu_add_beacon(hw, conf, mconf, conf->enable_beacon);
+		}
 		break;
 	default:
 		break;
 	}
+	mutex_unlock(&dev->mt76.mutex);
 }
 
 static void
@@ -1585,7 +1613,7 @@ void mt7996_tx_token_put(struct mt7996_dev *dev)
 
 	spin_lock_bh(&dev->mt76.token_lock);
 	idr_for_each_entry(&dev->mt76.token, txwi, id) {
-		mt7996_txwi_free(dev, txwi, NULL, NULL);
+		mt7996_txwi_free(dev, txwi, NULL, NULL, NULL);
 		dev->mt76.token_count--;
 	}
 	spin_unlock_bh(&dev->mt76.token_lock);
@@ -2266,21 +2294,31 @@ void mt7996_mac_sta_rc_work(struct work_struct *work)
 	u32 changed;
 	LIST_HEAD(list);
 
+	rcu_read_lock();
 	spin_lock_bh(&dev->mt76.sta_poll_lock);
 	list_splice_init(&dev->sta_rc_list, &list);
 
 	while (!list_empty(&list)) {
+		u8 link_id;
+
 		mlink = list_first_entry(&list, struct mt7996_link_sta, rc_list);
+		link_id = mlink->wcid.link_id;
+
 		list_del_init(&mlink->rc_list);
 		changed = mlink->changed;
 		mlink->changed = 0;
 		spin_unlock_bh(&dev->mt76.sta_poll_lock);
 
-		sta = container_of((void *)mlink->sta, struct ieee80211_sta, drv_priv);
-		link_sta = &sta->deflink;
 		vif = container_of((void *)mlink->sta->vif, struct ieee80211_vif, drv_priv);
-		conf = &vif->bss_conf;
-		mconf = &mlink->sta->vif->deflink;
+		conf = rcu_dereference(vif->link_conf[link_id]);
+		mconf = rcu_dereference(mlink->sta->vif->link[link_id]);
+		sta = wcid_to_sta(&mlink->wcid);
+		link_sta = rcu_dereference(sta->link[link_id]);
+
+		if (unlikely(!conf || !mconf || !link_sta)) {
+			spin_lock_bh(&dev->mt76.sta_poll_lock);
+			continue;
+		}
 
 		if (changed & (IEEE80211_RC_SUPP_RATES_CHANGED |
 			       IEEE80211_RC_NSS_CHANGED |
@@ -2295,6 +2333,7 @@ void mt7996_mac_sta_rc_work(struct work_struct *work)
 	}
 
 	spin_unlock_bh(&dev->mt76.sta_poll_lock);
+	rcu_read_unlock();
 }
 
 void mt7996_mac_work(struct work_struct *work)
-- 
2.39.2