From 72f979ee7ce94f3f28b6fc761a1aadb37ab2d63a Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 5 Jul 2023 10:00:17 +0800
Subject: [PATCH 029/116] mtk: wifi: mt76: mt7996: support eagle ZWDFS on iFEM

Fix the case that control channel is not first chan during first
interface setup.
Refactor ifem adie logic (if/else to switch, use sku_type & fem_type)

CR-Id: WCNCR00274293
Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 mt7996/main.c   | 65 ++++++++++++++++++++++++++++++++++++++++++++++---
 mt7996/mcu.c    |  6 +++--
 mt7996/mt7996.h |  1 +
 3 files changed, 67 insertions(+), 5 deletions(-)

diff --git a/mt7996/main.c b/mt7996/main.c
index 52870fa72..1a1f0242c 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -1444,6 +1444,61 @@ mt7996_twt_teardown_request(struct ieee80211_hw *hw,
 	mutex_unlock(&dev->mt76.mutex);
 }
 
+static void
+mt7996_background_radar_handle_7975_ifem(struct ieee80211_hw *hw,
+					 struct cfg80211_chan_def *user_chandef,
+					 struct cfg80211_chan_def *fw_chandef)
+{
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct cfg80211_chan_def *c = user_chandef;
+	struct ieee80211_channel *first_chan;
+	bool is_ifem_adie, expand = false;
+
+	switch (mt76_chip(&dev->mt76)) {
+	case 0x7990:
+		is_ifem_adie = dev->fem_type == MT7996_FEM_INT &&
+			       dev->chip_sku != MT7996_SKU_233;
+		break;
+	case 0x7992:
+		is_ifem_adie = dev->chip_sku == MT7992_SKU_44 &&
+			       dev->fem_type != MT7996_FEM_EXT;
+		break;
+	default:
+		return;
+	}
+
+	if (!user_chandef || !is_ifem_adie)
+		goto out;
+
+	if (user_chandef->width == NL80211_CHAN_WIDTH_160) {
+		first_chan = ieee80211_get_channel(hw->wiphy, user_chandef->center_freq1 - 70);
+		if (dev->bg_nxt_freq)
+			goto out;
+
+		if (first_chan->flags & IEEE80211_CHAN_RADAR)
+			dev->bg_nxt_freq = first_chan->center_freq;
+		else
+			c = fw_chandef;
+
+		c->chan = ieee80211_get_channel(hw->wiphy, first_chan->center_freq + 80);
+	} else {
+		if (!dev->bg_nxt_freq)
+			goto out;
+
+		c->chan = ieee80211_get_channel(hw->wiphy, dev->bg_nxt_freq);
+		dev->bg_nxt_freq = 0;
+		expand = true;
+	}
+	c->width = NL80211_CHAN_WIDTH_80;
+	c->center_freq1 = c->chan->center_freq + 30;
+
+	if (c == user_chandef)
+		cfg80211_background_radar_update_channel(hw->wiphy, c, expand);
+	return;
+out:
+	dev->bg_nxt_freq = 0;
+}
+
 static int
 mt7996_set_radar_background(struct ieee80211_hw *hw,
 			    struct cfg80211_chan_def *chandef)
@@ -1452,6 +1507,7 @@ mt7996_set_radar_background(struct ieee80211_hw *hw,
 	struct mt7996_dev *dev = phy->dev;
 	int ret = -EINVAL;
 	bool running;
+	struct cfg80211_chan_def ifem_chandef = {};
 
 	mutex_lock(&dev->mt76.mutex);
 
@@ -1464,13 +1520,14 @@ mt7996_set_radar_background(struct ieee80211_hw *hw,
 		goto out;
 	}
 
+	mt7996_background_radar_handle_7975_ifem(hw, chandef, &ifem_chandef);
+
 	/* rdd2 already configured on a radar channel */
 	running = dev->rdd2_phy &&
 		  cfg80211_chandef_valid(&dev->rdd2_chandef) &&
 		  !!(dev->rdd2_chandef.chan->flags & IEEE80211_CHAN_RADAR);
 
-	if (!chandef || running ||
-	    !(chandef->chan->flags & IEEE80211_CHAN_RADAR)) {
+	if (!chandef || running) {
 		ret = mt7996_mcu_rdd_background_enable(phy, NULL);
 		if (ret)
 			goto out;
@@ -1479,7 +1536,9 @@ mt7996_set_radar_background(struct ieee80211_hw *hw,
 			goto update_phy;
 	}
 
-	ret = mt7996_mcu_rdd_background_enable(phy, chandef);
+	ret = mt7996_mcu_rdd_background_enable(phy,
+					       ifem_chandef.chan ?
+					       &ifem_chandef : chandef);
 	if (ret)
 		goto out;
 
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 1330ff397..f375a5aba 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -372,12 +372,14 @@ mt7996_mcu_rx_radar_detected(struct mt7996_dev *dev, struct sk_buff *skb)
 	if (!mphy)
 		return;
 
-	if (r->band_idx == MT_RX_SEL2)
+	if (r->band_idx == MT_RX_SEL2) {
+		dev->bg_nxt_freq = 0;
 		cfg80211_background_radar_event(mphy->hw->wiphy,
 						&dev->rdd2_chandef,
 						GFP_ATOMIC);
-	else
+	} else {
 		ieee80211_radar_detected(mphy->hw);
+	}
 	dev->hw_pattern++;
 }
 
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 7d6ec8ba4..fd93db2fc 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -399,6 +399,7 @@ struct mt7996_dev {
 	bool testmode_enable;
 	bool bin_file_mode;
 	u8 eeprom_mode;
+	u32 bg_nxt_freq;
 
 	bool ibf;
 	u8 fw_debug_wm;
-- 
2.39.2