From 29127cd132bbeca563f105bb43fb6e5855cc3572 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Fri, 1 Dec 2023 17:26:43 +0800
Subject: [PATCH 093/116] wifi: mt76: mt7996: rework ieee80211_ops callbacks
 for link consideration

Extend ieee80211 callback functions to support multi-link operation.
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/main.c | 313 ++++++++++++++++++++++++++++++++------------------
 1 file changed, 204 insertions(+), 109 deletions(-)

diff --git a/mt7996/main.c b/mt7996/main.c
index 435815e9a..3bd97d4aa 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -568,7 +568,6 @@ static int mt7996_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			  struct ieee80211_key_conf *key)
 {
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
-	struct mt7996_phy *phy = mt7996_hw_phy(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	struct mt7996_sta *msta = sta ? (struct mt7996_sta *)sta->drv_priv :
 				  &mvif->sta;
@@ -578,70 +577,77 @@ static int mt7996_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	u8 *wcid_keyidx;
 	int idx = key->keyidx;
 	int err = 0;
+	unsigned long add;
+	unsigned int link_id;
 
-	/* The hardware does not support per-STA RX GTK, fallback
-	 * to software mode for these.
-	 */
-	if ((vif->type == NL80211_IFTYPE_ADHOC ||
-	     vif->type == NL80211_IFTYPE_MESH_POINT) &&
-	    (key->cipher == WLAN_CIPHER_SUITE_TKIP ||
-	     key->cipher == WLAN_CIPHER_SUITE_CCMP) &&
-	    !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
-		return -EOPNOTSUPP;
+	if (key->link_id >= 0) {
+		add = BIT(key->link_id);
+	} else {
+		if (sta)
+			add = sta->valid_links ?: BIT(0);
+		else
+			add = vif->valid_links ?: BIT(0);
+	}
 
 	mutex_lock(&dev->mt76.mutex);
-	conf = link_conf_dereference_protected(vif, 0);
-	mconf = mconf_dereference_protected(mvif, 0);
-	mlink = mlink_dereference_protected(msta, 0);
-	wcid_keyidx = &mlink->wcid.hw_key_idx;
-
-	/* fall back to sw encryption for unsupported ciphers */
-	switch (key->cipher) {
-	case WLAN_CIPHER_SUITE_TKIP:
-	case WLAN_CIPHER_SUITE_CCMP:
-	case WLAN_CIPHER_SUITE_CCMP_256:
-	case WLAN_CIPHER_SUITE_GCMP:
-	case WLAN_CIPHER_SUITE_GCMP_256:
-	case WLAN_CIPHER_SUITE_SMS4:
-		break;
-	case WLAN_CIPHER_SUITE_AES_CMAC:
-	case WLAN_CIPHER_SUITE_BIP_CMAC_256:
-	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
-	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
-		if (key->keyidx == 6 || key->keyidx == 7) {
-			wcid_keyidx = &wcid->hw_key_idx2;
-			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIE;
+
+	for_each_set_bit(link_id, &add, IEEE80211_MLD_MAX_NUM_LINKS) {
+		conf = link_conf_dereference_protected(vif, link_id);
+		mconf = mconf_dereference_protected(mvif, link_id);
+		mlink = mlink_dereference_protected(msta, link_id);
+		wcid_keyidx = &mlink->wcid.hw_key_idx;
+
+		if (!conf || !mconf || !mlink)
+			continue;
+
+		/* fall back to sw encryption for unsupported ciphers */
+		switch (key->cipher) {
+		case WLAN_CIPHER_SUITE_TKIP:
+		case WLAN_CIPHER_SUITE_CCMP:
+		case WLAN_CIPHER_SUITE_CCMP_256:
+		case WLAN_CIPHER_SUITE_GCMP:
+		case WLAN_CIPHER_SUITE_GCMP_256:
+		case WLAN_CIPHER_SUITE_SMS4:
 			break;
+		case WLAN_CIPHER_SUITE_AES_CMAC:
+		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
+		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
+		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
+			if (key->keyidx == 6 || key->keyidx == 7) {
+				wcid_keyidx = &mlink->wcid.hw_key_idx2;
+				key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIE;
+				break;
+			}
+			fallthrough;
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		default:
+			mutex_unlock(&dev->mt76.mutex);
+			return -EOPNOTSUPP;
 		}
-		fallthrough;
-	case WLAN_CIPHER_SUITE_WEP40:
-	case WLAN_CIPHER_SUITE_WEP104:
-	default:
-		mutex_unlock(&dev->mt76.mutex);
-		return -EOPNOTSUPP;
-	}
 
-	if (cmd == SET_KEY && !sta && !mconf->mt76.cipher) {
-		mconf->mt76.cipher = mt76_connac_mcu_get_cipher(key->cipher);
-		mt7996_mcu_add_bss_info(phy, conf, mconf, mlink, true);
-	}
+		if (cmd == SET_KEY && !sta && !mconf->mt76.cipher) {
+			mconf->mt76.cipher = mt76_connac_mcu_get_cipher(key->cipher);
+			mt7996_mcu_add_bss_info(mconf->phy, conf, mconf, mlink, true);
+		}
 
-	if (cmd == SET_KEY) {
-		*wcid_keyidx = idx;
-	} else {
-		if (idx == *wcid_keyidx)
-			*wcid_keyidx = -1;
-		goto out;
-	}
+		if (cmd == SET_KEY) {
+			*wcid_keyidx = idx;
+		} else {
+			if (idx == *wcid_keyidx)
+				*wcid_keyidx = -1;
+			goto out;
+		}
 
-	mt76_wcid_key_setup(&dev->mt76, &mlink->wcid, key);
+		mt76_wcid_key_setup(&dev->mt76, &mlink->wcid, key);
 
-	if (key->keyidx == 6 || key->keyidx == 7)
-		err = mt7996_mcu_bcn_prot_enable(dev, conf, mconf, mlink, key);
-	else
-		err = mt7996_mcu_add_key(&dev->mt76, mconf, key,
-					 MCU_WMWA_UNI_CMD(STA_REC_UPDATE),
-					 &mlink->wcid, cmd);
+		if (key->keyidx == 6 || key->keyidx == 7)
+			err = mt7996_mcu_bcn_prot_enable(dev, conf, mconf, mlink, key);
+		else
+			err = mt7996_mcu_add_key(&dev->mt76, mconf, key,
+						 MCU_WMWA_UNI_CMD(STA_REC_UPDATE),
+						 &mlink->wcid, cmd);
+	}
 out:
 	mutex_unlock(&dev->mt76.mutex);
 
@@ -697,7 +703,11 @@ mt7996_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	};
 
 	mutex_lock(&dev->mt76.mutex);
-	mconf = mconf_dereference_protected(mvif, 0);
+	mconf = mconf_dereference_protected(mvif, link_id);
+	if (!mconf) {
+		mutex_unlock(&dev->mt76.mutex);
+		return -EINVAL;
+	}
 
 	/* firmware uses access class index */
 	mconf->queue_params[mq_to_aci[queue]] = *params;
@@ -840,19 +850,26 @@ mt7996_update_mu_group(struct ieee80211_hw *hw, struct ieee80211_bss_conf *conf,
 static void mt7996_vif_cfg_changed(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif, u64 changed)
 {
-	struct mt7996_phy *phy = mt7996_hw_phy(hw);
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 
 	mutex_lock(&dev->mt76.mutex);
 
 	if (changed & BSS_CHANGED_ASSOC && vif->cfg.assoc) {
-		struct ieee80211_bss_conf *conf = &vif->bss_conf;
 		struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-		struct mt7996_bss_conf *mconf = mconf_dereference_protected(mvif, 0);
-		struct mt7996_link_sta *mlink = mlink_dereference_protected(&mvif->sta, 0);
-
-		mt7996_mcu_add_bss_info(phy, conf, mconf, mlink, true);
-		mt7996_mcu_add_sta(dev, conf, mconf, NULL, mlink, true, false);
+		unsigned long valid_links = vif->valid_links ?: BIT(0);
+		unsigned int link_id;
+
+		for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+			struct ieee80211_bss_conf *conf =
+				link_conf_dereference_protected(vif, link_id);
+			struct mt7996_bss_conf *mconf =
+				mconf_dereference_protected(mvif, link_id);
+			struct mt7996_link_sta *mlink =
+				mlink_dereference_protected(&mvif->sta, link_id);
+
+			mt7996_mcu_add_bss_info(mconf->phy, conf, mconf, mlink, true);
+			mt7996_mcu_add_sta(dev, conf, mconf, NULL, mlink, true, false);
+		}
 	}
 
 	mutex_unlock(&dev->mt76.mutex);
@@ -871,8 +888,13 @@ static void mt7996_link_info_changed(struct ieee80211_hw *hw,
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mconf = mconf_dereference_protected(mvif, 0);
-	mlink = mlink_dereference_protected(&mvif->sta, 0);
+	mconf = mconf_dereference_protected(mvif, info->link_id);
+	mlink = mlink_dereference_protected(&mvif->sta, info->link_id);
+	if (!mconf || !mlink)
+		goto out;
+
+	if (mconf->phy)
+		phy = mconf->phy;
 	/* station mode uses BSSID to map the wlan entry to a peer,
 	 * and then peer references bss_info_rfch to set bandwidth cap.
 	 */
@@ -928,6 +950,7 @@ static void mt7996_link_info_changed(struct ieee80211_hw *hw,
 	if (changed & BSS_CHANGED_MU_GROUPS)
 		mt7996_update_mu_group(hw, info, mconf);
 
+out:
 	mutex_unlock(&dev->mt76.mutex);
 }
 
@@ -938,13 +961,22 @@ mt7996_channel_switch_beacon(struct ieee80211_hw *hw,
 {
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-	struct mt7996_bss_conf *mconf;
-	struct ieee80211_bss_conf *conf;
+	struct mt7996_phy *phy = mt7996_band_phy(hw, chandef->chan->band);
+	unsigned long valid_links = vif->valid_links ?: BIT(0);
+	unsigned int link_id;
 
 	mutex_lock(&dev->mt76.mutex);
-	mconf = mconf_dereference_protected(mvif, 0);
-	conf = link_conf_dereference_protected(vif, 0);
-	mt7996_mcu_add_beacon(hw, conf, mconf, true);
+	for_each_set_bit(link_id, &valid_links, 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);
+
+		if (!mconf || phy != mconf->phy)
+			continue;
+
+		mt7996_mcu_add_beacon(hw, conf, mconf, true);
+	}
 	mutex_unlock(&dev->mt76.mutex);
 }
 
@@ -1216,34 +1248,74 @@ void mt7996_mac_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 	mt7996_mac_sta_remove_links(dev, vif, sta, rem);
 }
 
+static void
+mt7996_sta_pre_rcu_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			  struct ieee80211_sta *sta)
+{
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
+	unsigned long rem = sta->valid_links ?: BIT(0);
+	unsigned int link_id;
+
+	mutex_lock(&dev->mt76.mutex);
+	spin_lock_bh(&dev->mt76.status_lock);
+	for_each_set_bit(link_id, &rem, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct mt7996_link_sta *mlink =
+			mlink_dereference_protected(msta, link_id);
+
+		rcu_assign_pointer(dev->mt76.wcid[mlink->wcid.idx], NULL);
+	}
+	spin_unlock_bh(&dev->mt76.status_lock);
+	mutex_unlock(&dev->mt76.mutex);
+}
+
 static void mt7996_tx(struct ieee80211_hw *hw,
 		      struct ieee80211_tx_control *control,
 		      struct sk_buff *skb)
 {
-	struct mt7996_dev *dev = mt7996_hw_dev(hw);
-	struct mt76_phy *mphy = hw->priv;
+	struct mt76_phy *mphy;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_vif *vif = info->control.vif;
-	struct mt76_wcid *wcid = &dev->mt76.global_wcid;
-	struct mt7996_link_sta *mlink;
+	struct mt76_wcid *wcid;
+	struct mt7996_vif *mvif;
+	struct mt7996_sta *msta;
 
 	if (control->sta) {
-		struct mt7996_sta *msta;
-
 		msta = (struct mt7996_sta *)control->sta->drv_priv;
-		mlink = rcu_dereference(msta->link[0]);
-		wcid = &mlink->wcid;
+		mvif = msta->vif;
+	} else if (vif) {
+		mvif = (struct mt7996_vif *)vif->drv_priv;
+		msta = &mvif->sta;
 	}
 
-	if (vif && !control->sta) {
-		struct mt7996_vif *mvif;
+	rcu_read_lock();
+	if (mvif && msta) {
+		struct mt7996_bss_conf *mconf;
+		struct mt7996_link_sta *mlink;
+
+		u8 link_id = u32_get_bits(info->control.flags,
+					  IEEE80211_TX_CTRL_MLO_LINK);
 
-		mvif = (struct mt7996_vif *)vif->drv_priv;
-		mlink = rcu_dereference(mvif->sta.link[0]);
+		if (link_id >= IEEE80211_LINK_UNSPECIFIED)
+			link_id = mvif->master_link_id;
+
+		mconf = rcu_dereference(mvif->link[link_id]);
+		mlink = rcu_dereference(msta->link[link_id]);
+
+		if (!mconf || !mlink)
+			goto unlock;
+
+		mphy = mconf->phy->mt76;
 		wcid = &mlink->wcid;
+	} else {
+		struct mt7996_dev *dev = mt7996_hw_dev(hw);
+
+		mphy = hw->priv;
+		wcid = &dev->mt76.global_wcid;
 	}
 
 	mt76_tx(mphy, control->sta, wcid, skb);
+	rcu_read_unlock();
 }
 
 static int mt7996_set_rts_threshold(struct ieee80211_hw *hw, u32 val)
@@ -1279,7 +1351,7 @@ mt7996_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	mtxq = (struct mt76_txq *)txq->drv_priv;
 
 	mutex_lock(&dev->mt76.mutex);
-	mlink = mlink_dereference_protected(msta, 0);
+	mlink = mlink_dereference_protected(msta, msta->pri_link);
 	switch (action) {
 	case IEEE80211_AMPDU_RX_START:
 		mt76_rx_aggr_start(&dev->mt76, &mlink->wcid, tid, ssn,
@@ -1494,7 +1566,7 @@ static void mt7996_sta_statistics(struct ieee80211_hw *hw,
 
 	/* TODO: support per-link rate report */
 	mutex_lock(&dev->mt76.mutex);
-	mlink = mlink_dereference_protected(msta, 0);
+	mlink = mlink_dereference_protected(msta, msta->pri_link);
 	if (!mlink)
 		goto out;
 
@@ -1554,7 +1626,7 @@ static void mt7996_sta_rc_work(void *data, struct ieee80211_sta *sta)
 	u32 *changed = data;
 
 	rcu_read_lock();
-	mlink = rcu_dereference(msta->link[0]);
+	mlink = rcu_dereference(msta->link[msta->pri_link]);
 
 	spin_lock_bh(&dev->mt76.sta_poll_lock);
 	mlink->changed |= *changed;
@@ -1621,19 +1693,26 @@ static void mt7996_sta_set_4addr(struct ieee80211_hw *hw,
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
-	struct mt7996_bss_conf *mconf;
-	struct mt7996_link_sta *mlink;
+	unsigned long valid_links = sta->valid_links ?: BIT(0);
+	unsigned int link_id;
 
 	mutex_lock(&dev->mt76.mutex);
-	mconf = mconf_dereference_protected(mvif, 0);
-	mlink = mlink_dereference_protected(msta, 0);
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct mt7996_bss_conf *mconf =
+			mconf_dereference_protected(mvif, link_id);
+		struct mt7996_link_sta *mlink =
+			mlink_dereference_protected(msta, link_id);
 
-	if (enabled)
-		set_bit(MT_WCID_FLAG_4ADDR, &mlink->wcid.flags);
-	else
-		clear_bit(MT_WCID_FLAG_4ADDR, &mlink->wcid.flags);
+		if (!mconf || !mlink)
+			continue;
+
+		if (enabled)
+			set_bit(MT_WCID_FLAG_4ADDR, &mlink->wcid.flags);
+		else
+			clear_bit(MT_WCID_FLAG_4ADDR, &mlink->wcid.flags);
 
-	mt7996_mcu_wtbl_update_hdr_trans(dev, vif, mconf, mlink);
+		mt7996_mcu_wtbl_update_hdr_trans(dev, vif, mconf, mlink);
+	}
 	mutex_unlock(&dev->mt76.mutex);
 }
 
@@ -1645,19 +1724,26 @@ static void mt7996_sta_set_decap_offload(struct ieee80211_hw *hw,
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
-	struct mt7996_bss_conf *mconf;
-	struct mt7996_link_sta *mlink;
+	unsigned long valid_links = sta->valid_links ?: BIT(0);
+	unsigned int link_id;
 
 	mutex_lock(&dev->mt76.mutex);
-	mconf = mconf_dereference_protected(mvif, 0);
-	mlink = mlink_dereference_protected(msta, 0);
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct mt7996_bss_conf *mconf =
+			mconf_dereference_protected(mvif, link_id);
+		struct mt7996_link_sta *mlink =
+			mlink_dereference_protected(msta, link_id);
 
-	if (enabled)
-		set_bit(MT_WCID_FLAG_HDR_TRANS, &mlink->wcid.flags);
-	else
-		clear_bit(MT_WCID_FLAG_HDR_TRANS, &mlink->wcid.flags);
+		if (!mconf || !mlink)
+			continue;
+
+		if (enabled)
+			set_bit(MT_WCID_FLAG_HDR_TRANS, &mlink->wcid.flags);
+		else
+			clear_bit(MT_WCID_FLAG_HDR_TRANS, &mlink->wcid.flags);
 
-	mt7996_mcu_wtbl_update_hdr_trans(dev, vif, mconf, mlink);
+		mt7996_mcu_wtbl_update_hdr_trans(dev, vif, mconf, mlink);
+	}
 	mutex_unlock(&dev->mt76.mutex);
 }
 
@@ -1790,9 +1876,13 @@ static void mt7996_ethtool_worker(void *wi_data, struct ieee80211_sta *sta)
 {
 	struct mt76_ethtool_worker_info *wi = wi_data;
 	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
-	struct mt7996_link_sta *mlink = &msta->deflink;
+	struct mt7996_link_sta *mlink;
+	struct mt7996_bss_conf *mconf;
+
+	mlink = mlink_dereference_protected(msta, msta->pri_link);
+	mconf = mconf_dereference_protected(msta->vif, msta->pri_link);
 
-	if (msta->vif->deflink.mt76.idx != wi->idx)
+	if (mconf->mt76.idx != wi->idx)
 		return;
 
 	mt76_ethtool_worker(wi, &mlink->wcid.stats, true);
@@ -2024,12 +2114,13 @@ mt7996_net_fill_forward_path(struct ieee80211_hw *hw,
 			     struct net_device_path *path)
 {
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-	struct mt7996_bss_conf *mconf = &mvif->deflink;
 	struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
-	struct mt7996_link_sta *mlink = &msta->deflink;
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_phy *phy = mt7996_hw_phy(hw);
 	struct mtk_wed_device *wed = &dev->mt76.mmio.wed;
+	struct mt7996_bss_conf *mconf;
+	struct mt7996_link_sta *mlink;
+	u8 link_id;
 
 	if (dev->hif2) {
 		switch (dev->option_type) {
@@ -2049,6 +2140,10 @@ mt7996_net_fill_forward_path(struct ieee80211_hw *hw,
 	if (!mtk_wed_device_active(wed))
 		return -ENODEV;
 
+	link_id = msta->pri_link;
+	mconf = rcu_dereference(mvif->link[link_id]);
+	mlink = rcu_dereference(msta->link[link_id]);
+
 	if (mlink->wcid.idx > MT7996_WTBL_STA)
 		return -EIO;
 
@@ -2380,7 +2475,7 @@ const struct ieee80211_ops mt7996_ops = {
 	.vif_cfg_changed = mt7996_vif_cfg_changed,
 	.link_info_changed = mt7996_link_info_changed,
 	.sta_state = mt76_sta_state,
-	.sta_pre_rcu_remove = mt76_sta_pre_rcu_remove,
+	.sta_pre_rcu_remove = mt7996_sta_pre_rcu_remove,
 	.sta_rc_update = mt7996_sta_rc_update,
 	.set_key = mt7996_set_key,
 	.ampdu_action = mt7996_ampdu_action,
-- 
2.39.2