[PATCH rtw-next 02/11] wifi: rtw89: allow driver to do specific band TX for MLO

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

From: Po-Hao Huang <phhuang@realtek.com>

For data packets that can be sent on any band, fill in main mac_id
and let HW decide. For packets that we wish to transmit on specific
band, fill in sw_mld field so HW would only send it on that band.
Main mac_id is the corresponding mac_id on band 0.

Signed-off-by: Po-Hao Huang <phhuang@realtek.com>
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 | 17 ++++++++++++++---
 drivers/net/wireless/realtek/rtw89/core.h |  1 +
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.c b/drivers/net/wireless/realtek/rtw89/core.c
index 0fd902eef05b..c8b00f790829 100644
--- a/drivers/net/wireless/realtek/rtw89/core.c
+++ b/drivers/net/wireless/realtek/rtw89/core.c
@@ -658,9 +658,17 @@ static u16 rtw89_core_get_mgmt_rate(struct rtw89_dev *rtwdev,
 static u8 rtw89_core_tx_get_mac_id(struct rtw89_dev *rtwdev,
 				   struct rtw89_core_tx_request *tx_req)
 {
+	struct rtw89_tx_desc_info *desc_info = &tx_req->desc_info;
 	struct rtw89_vif_link *rtwvif_link = tx_req->rtwvif_link;
 	struct rtw89_sta_link *rtwsta_link = tx_req->rtwsta_link;
 
+	if (desc_info->mlo && !desc_info->sw_mld) {
+		if (rtwsta_link)
+			return rtw89_sta_get_main_macid(rtwsta_link->rtwsta);
+		else
+			return rtw89_vif_get_main_macid(rtwvif_link->rtwvif);
+	}
+
 	if (!rtwsta_link)
 		return rtwvif_link->mac_id;
 
@@ -1126,7 +1134,7 @@ int rtw89_h2c_tx(struct rtw89_dev *rtwdev,
 static int rtw89_core_tx_write_link(struct rtw89_dev *rtwdev,
 				    struct rtw89_vif_link *rtwvif_link,
 				    struct rtw89_sta_link *rtwsta_link,
-				    struct sk_buff *skb, int *qsel)
+				    struct sk_buff *skb, int *qsel, bool sw_mld)
 {
 	struct ieee80211_sta *sta = rtwsta_link_to_sta_safe(rtwsta_link);
 	struct ieee80211_vif *vif = rtwvif_link_to_vif(rtwvif_link);
@@ -1139,6 +1147,7 @@ static int rtw89_core_tx_write_link(struct rtw89_dev *rtwdev,
 	tx_req.sta = sta;
 	tx_req.rtwvif_link = rtwvif_link;
 	tx_req.rtwsta_link = rtwsta_link;
+	tx_req.desc_info.sw_mld = sw_mld;
 
 	rtw89_traffic_stats_accu(rtwdev, &rtwdev->stats, skb, true);
 	rtw89_traffic_stats_accu(rtwdev, &rtwvif->stats, skb, true);
@@ -1181,7 +1190,7 @@ int rtw89_core_tx_write(struct rtw89_dev *rtwdev, struct ieee80211_vif *vif,
 		}
 	}
 
-	return rtw89_core_tx_write_link(rtwdev, rtwvif_link, rtwsta_link, skb, qsel);
+	return rtw89_core_tx_write_link(rtwdev, rtwvif_link, rtwsta_link, skb, qsel, false);
 }
 
 static __le32 rtw89_build_txwd_body0(struct rtw89_tx_desc_info *desc_info)
@@ -1411,7 +1420,9 @@ static __le32 rtw89_build_txwd_body2_v2(struct rtw89_tx_desc_info *desc_info)
 
 static __le32 rtw89_build_txwd_body3_v2(struct rtw89_tx_desc_info *desc_info)
 {
-	u32 dword = FIELD_PREP(BE_TXD_BODY3_WIFI_SEQ, desc_info->seq);
+	u32 dword = FIELD_PREP(BE_TXD_BODY3_WIFI_SEQ, desc_info->seq) |
+		    FIELD_PREP(BE_TXD_BODY3_MLO_FLAG, desc_info->mlo) |
+		    FIELD_PREP(BE_TXD_BODY3_IS_MLD_SW_EN, desc_info->sw_mld);
 
 	return cpu_to_le32(dword);
 }
diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index dfc578ad8c64..4e44cbd6cc53 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -1175,6 +1175,7 @@ struct rtw89_tx_desc_info {
 	bool ldpc;
 	bool upd_wlan_hdr;
 	bool mlo;
+	bool sw_mld;
 };
 
 struct rtw89_core_tx_request {
-- 
2.25.1