Received-SPF: pass (google.com: domain of linux-wireless+bounces-3189-linux.lists.archive=gmail.com@vger.kernel.org designates 147.75.199.223 as permitted sender) client-ip=147.75.199.223;
From: Ping-Ke Shih <pkshih@realtek.com>
To: <kvalo@kernel.org>
CC: <kevin_yang@realtek.com>, <linux-wireless@vger.kernel.org>
Subject: [PATCH 2/6] wifi: rtw89: chan: add sub-entity swap function to cover replacing
Date: Tue, 6 Feb 2024 11:06:20 +0800
Message-ID: <20240206030624.23382-3-pkshih@realtek.com>
In-Reply-To: <20240206030624.23382-1-pkshih@realtek.com>
References: <20240206030624.23382-1-pkshih@realtek.com>
Precedence: bulk
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Content-Type: text/plain

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

Originally, we replaced sub-entity of index 0 with another one in some
cases. However, we will need a swap here in following implementations.
So, we introduce it ahead and change code from replacing to swapping.

Signed-off-by: Zong-Zhe Yang <kevin_yang@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/chan.c | 47 +++++++++++++++++------
 1 file changed, 36 insertions(+), 11 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw89/chan.c b/drivers/net/wireless/realtek/rtw89/chan.c
index 2a95f9db83f9..11d46878f51e 100644
--- a/drivers/net/wireless/realtek/rtw89/chan.c
+++ b/drivers/net/wireless/realtek/rtw89/chan.c
@@ -1893,6 +1893,41 @@ void rtw89_chanctx_proceed(struct rtw89_dev *rtwdev)
 	rtw89_queue_chanctx_work(rtwdev);
 }
 
+static void rtw89_swap_sub_entity(struct rtw89_dev *rtwdev,
+				  enum rtw89_sub_entity_idx idx1,
+				  enum rtw89_sub_entity_idx idx2)
+{
+	struct rtw89_hal *hal = &rtwdev->hal;
+	struct rtw89_sub_entity tmp;
+	struct rtw89_vif *rtwvif;
+	u8 cur;
+
+	if (idx1 == idx2)
+		return;
+
+	hal->sub[idx1].cfg->idx = idx2;
+	hal->sub[idx2].cfg->idx = idx1;
+
+	tmp = hal->sub[idx1];
+	hal->sub[idx1] = hal->sub[idx2];
+	hal->sub[idx2] = tmp;
+
+	rtw89_for_each_rtwvif(rtwdev, rtwvif) {
+		if (!rtwvif->chanctx_assigned)
+			continue;
+		if (rtwvif->sub_entity_idx == idx1)
+			rtwvif->sub_entity_idx = idx2;
+		else if (rtwvif->sub_entity_idx == idx2)
+			rtwvif->sub_entity_idx = idx1;
+	}
+
+	cur = atomic_read(&hal->roc_entity_idx);
+	if (cur == idx1)
+		atomic_set(&hal->roc_entity_idx, idx2);
+	else if (cur == idx2)
+		atomic_set(&hal->roc_entity_idx, idx1);
+}
+
 int rtw89_chanctx_ops_add(struct rtw89_dev *rtwdev,
 			  struct ieee80211_chanctx_conf *ctx)
 {
@@ -1918,7 +1953,6 @@ void rtw89_chanctx_ops_remove(struct rtw89_dev *rtwdev,
 	struct rtw89_hal *hal = &rtwdev->hal;
 	struct rtw89_chanctx_cfg *cfg = (struct rtw89_chanctx_cfg *)ctx->drv_priv;
 	enum rtw89_entity_mode mode;
-	struct rtw89_vif *rtwvif;
 	u8 drop, roll;
 
 	drop = cfg->idx;
@@ -1934,16 +1968,7 @@ void rtw89_chanctx_ops_remove(struct rtw89_dev *rtwdev,
 	/* RTW89_SUB_ENTITY_0 is going to release, and another exists.
 	 * Make another roll down to RTW89_SUB_ENTITY_0 to replace.
 	 */
-	hal->sub[roll].cfg->idx = RTW89_SUB_ENTITY_0;
-	hal->sub[RTW89_SUB_ENTITY_0] = hal->sub[roll];
-
-	rtw89_for_each_rtwvif(rtwdev, rtwvif) {
-		if (rtwvif->sub_entity_idx == roll)
-			rtwvif->sub_entity_idx = RTW89_SUB_ENTITY_0;
-	}
-
-	atomic_cmpxchg(&hal->roc_entity_idx, roll, RTW89_SUB_ENTITY_0);
-
+	rtw89_swap_sub_entity(rtwdev, RTW89_SUB_ENTITY_0, roll);
 	drop = roll;
 
 out:
-- 
2.25.1