[rtw-next 3/8] wifi: rtw89: mlo: handle needed H2C when link switching is requested by stack

[Ping-Ke Shih <pkshih@realtek.com>]

From: Zong-Zhe Yang <kevin_yang@realtek.com>

To switch link, FW needs H2C commands to indicate which link is on or off.
Originally, these H2C commands are considered only when the link switching
is initiated by driver. But, in some cases, e.g. ml_reconf or TTLM, link
switching would be initiated by stack. Hence, plan these H2C commands into
ieee80211_ops.

Signed-off-by: Zong-Zhe Yang <kevin_yang@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/core.c     | 18 +-----
 drivers/net/wireless/realtek/rtw89/core.h     |  8 +++
 drivers/net/wireless/realtek/rtw89/mac80211.c | 64 ++++++++++++++++++-
 3 files changed, 74 insertions(+), 16 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.c b/drivers/net/wireless/realtek/rtw89/core.c
index 8b40cada4149..a3f156bf708e 100644
--- a/drivers/net/wireless/realtek/rtw89/core.c
+++ b/drivers/net/wireless/realtek/rtw89/core.c
@@ -6010,7 +6010,7 @@ int rtw89_core_mlsr_switch(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 	struct ieee80211_vif *vif = rtwvif_to_vif(rtwvif);
 	u16 usable_links = ieee80211_vif_usable_links(vif);
 	u16 active_links = vif->active_links;
-	struct rtw89_vif_link *target, *cur;
+	struct rtw89_vif_link *target;
 	int ret;
 
 	lockdep_assert_wiphy(rtwdev->hw->wiphy);
@@ -6036,11 +6036,9 @@ int rtw89_core_mlsr_switch(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 	ieee80211_stop_queues(rtwdev->hw);
 	flush_work(&rtwdev->txq_work);
 
-	cur = rtw89_get_designated_link(rtwvif);
-
-	ret = ieee80211_set_active_links(vif, active_links | BIT(link_id));
+	ret = ieee80211_set_active_links(vif, BIT(link_id));
 	if (ret) {
-		rtw89_err(rtwdev, "%s: failed to activate link id %u\n",
+		rtw89_err(rtwdev, "%s: failed to work on link id %u\n",
 			  __func__, link_id);
 		goto wake_queue;
 	}
@@ -6055,16 +6053,6 @@ int rtw89_core_mlsr_switch(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 		goto wake_queue;
 	}
 
-	if (likely(cur))
-		rtw89_fw_h2c_mlo_link_cfg(rtwdev, cur, false);
-
-	rtw89_fw_h2c_mlo_link_cfg(rtwdev, target, true);
-
-	ret = ieee80211_set_active_links(vif, BIT(link_id));
-	if (ret)
-		rtw89_err(rtwdev, "%s: failed to inactivate links 0x%x\n",
-			  __func__, active_links);
-
 	rtw89_chip_rfk_channel(rtwdev, target);
 
 	rtwvif->mlo_mode = RTW89_MLO_MODE_MLSR;
diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index 928c8c84c964..7c74ed3cfaf7 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -5933,6 +5933,7 @@ struct rtw89_mcc_info {
 
 enum rtw89_mlo_mode {
 	RTW89_MLO_MODE_MLSR = 0,
+	RTW89_MLO_MODE_EMLSR = 1,
 
 	NUM_OF_RTW89_MLO_MODE,
 };
@@ -6097,6 +6098,12 @@ struct rtw89_link_conf_container {
 	struct ieee80211_bss_conf *link_conf[IEEE80211_MLD_MAX_NUM_LINKS];
 };
 
+struct rtw89_vif_ml_trans {
+	u16 mediate_links;
+	u16 links_to_del;
+	u16 links_to_add;
+};
+
 #define RTW89_VIF_IDLE_LINK_ID 0
 
 struct rtw89_vif {
@@ -6119,6 +6126,7 @@ struct rtw89_vif {
 	bool offchan;
 
 	enum rtw89_mlo_mode mlo_mode;
+	struct rtw89_vif_ml_trans ml_trans;
 
 	struct list_head dlink_pool;
 	u8 links_inst_valid_num;
diff --git a/drivers/net/wireless/realtek/rtw89/mac80211.c b/drivers/net/wireless/realtek/rtw89/mac80211.c
index 7b04183a3a5d..a19304ff8306 100644
--- a/drivers/net/wireless/realtek/rtw89/mac80211.c
+++ b/drivers/net/wireless/realtek/rtw89/mac80211.c
@@ -718,6 +718,17 @@ static void rtw89_ops_vif_cfg_changed(struct ieee80211_hw *hw,
 
 	if (changed & BSS_CHANGED_ARP_FILTER)
 		rtwvif->ip_addr = vif->cfg.arp_addr_list[0];
+
+	if (changed & BSS_CHANGED_MLD_VALID_LINKS) {
+		struct rtw89_vif_link *cur = rtw89_get_designated_link(rtwvif);
+
+		rtw89_chip_rfk_channel(rtwdev, cur);
+
+		if (hweight16(vif->active_links) == 1)
+			rtwvif->mlo_mode = RTW89_MLO_MODE_MLSR;
+		else
+			rtwvif->mlo_mode = RTW89_MLO_MODE_EMLSR;
+	}
 }
 
 static void rtw89_ops_link_info_changed(struct ieee80211_hw *hw,
@@ -1531,10 +1542,29 @@ static bool rtw89_ops_can_activate_links(struct ieee80211_hw *hw,
 					 u16 active_links)
 {
 	struct rtw89_dev *rtwdev = hw->priv;
+	struct rtw89_vif *rtwvif = vif_to_rtwvif(vif);
+	u16 current_links = vif->active_links;
+	struct rtw89_vif_ml_trans trans = {
+		.mediate_links = current_links | active_links,
+		.links_to_del = current_links & ~active_links,
+		.links_to_add = active_links & ~current_links,
+	};
 
 	lockdep_assert_wiphy(hw->wiphy);
 
-	return rtw89_can_work_on_links(rtwdev, vif, active_links);
+	if (!rtw89_can_work_on_links(rtwdev, vif, active_links))
+		return false;
+
+	/*
+	 * Leave LPS at the beginning of ieee80211_set_active_links().
+	 * Because the entire process takes the same lock as our track
+	 * work, LPS will not enter during ieee80211_set_active_links().
+	 */
+	rtw89_leave_lps(rtwdev);
+
+	rtwvif->ml_trans = trans;
+
+	return true;
 }
 
 static void __rtw89_ops_clr_vif_links(struct rtw89_dev *rtwdev,
@@ -1579,6 +1609,36 @@ static int __rtw89_ops_set_vif_links(struct rtw89_dev *rtwdev,
 	return 0;
 }
 
+static void rtw89_vif_cfg_fw_links(struct rtw89_dev *rtwdev,
+				   struct rtw89_vif *rtwvif,
+				   unsigned long links, bool en)
+{
+	struct rtw89_vif_link *rtwvif_link;
+	unsigned int link_id;
+
+	for_each_set_bit(link_id, &links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		rtwvif_link = rtwvif->links[link_id];
+		if (unlikely(!rtwvif_link))
+			continue;
+
+		rtw89_fw_h2c_mlo_link_cfg(rtwdev, rtwvif_link, en);
+	}
+}
+
+static void rtw89_vif_update_fw_links(struct rtw89_dev *rtwdev,
+				      struct rtw89_vif *rtwvif,
+				      u16 current_links)
+{
+	struct rtw89_vif_ml_trans *trans = &rtwvif->ml_trans;
+
+	/* Do follow-up when all updating links exist. */
+	if (current_links != trans->mediate_links)
+		return;
+
+	rtw89_vif_cfg_fw_links(rtwdev, rtwvif, trans->links_to_del, false);
+	rtw89_vif_cfg_fw_links(rtwdev, rtwvif, trans->links_to_add, true);
+}
+
 static
 int rtw89_ops_change_vif_links(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
@@ -1620,6 +1680,8 @@ int rtw89_ops_change_vif_links(struct ieee80211_hw *hw,
 	if (rtwdev->scanning)
 		rtw89_hw_scan_abort(rtwdev, rtwdev->scan_info.scanning_vif);
 
+	rtw89_vif_update_fw_links(rtwdev, rtwvif, old_links);
+
 	if (!old_links)
 		__rtw89_ops_clr_vif_links(rtwdev, rtwvif,
 					  BIT(RTW89_VIF_IDLE_LINK_ID));
-- 
2.25.1