mac80211: rt2x00: experimental improvements for MT7620 wifi

Serge Vasilugin reports:

To improve mt7620 built-in wifi performance some changes:
1. Correct BW20/BW40 switching (see comments with mark (1))
2. Correct TX_SW_CFG1 MAC reg from v3 of vendor driver see
	https://gitlab.com/dm38/padavan-ng/-/blob/master/trunk/proprietary/rt_wifi/rtpci/3.0.X.X/mt76x2/chips/rt6352.c#L531
3. Set bbp66 for all chains.
4. US_CYC_CNT init based on Programming guide, default value was 33 (pci),
   set chipset bus clock with fallback to cpu clock/3.
5. Don't overwrite default values for mt7620.
6. Correct some typos.
7. Add support for external LNA:
    a) RF and BBP regs never be corrected for this mode
    b) eLNA is driven the same way as ePA with mt7620's pin PA
	but vendor driver explicitly pin PA to gpio mode (for forrect calibration?)
	so I'm not sure that request for pa_pin in dts-file will be enough

First 5 changes (really 2) improve performance for boards w/o eLNA/ePA.
Changes 7 add support for eLNA

Configuration w/o eLAN/ePA and with eLNA show results
tx/rx (from router point of view) for each stream:
 35-40/30-35 Mbps for HT20
 65-70/60-65 Mbps for HT40

Yes. Max results for 2T2R client is 140-145/135-140
with peaks 160/150, It correspond to mediatek driver results.
Boards with ePA untested.

Reported-by: Serge Vasilugin <vasilugin@yandex.ru>
Signed-off-by: Daniel Golle <daniel@makrotopia.org>
[directly include v3 of the patchset submitted upstream]
(cherry picked from commit 31a6605de04218e1c04bd5c2436c24d7d1c07506)
(cherry picked from commit e785ca05e9f0502894772f5df92192b816ba5d7c)
(cherry picked from commit 412fcf3d4400f84551f3ead0514834c62d94a251)

1 files changed, 982 insertions, 0 deletions

diff --git a/package/kernel/mac80211/patches/rt2x00/010-rt2x00-add-TX-LOFT-calibration-for-MT7620.patch b/package/kernel/mac80211/patches/rt2x00/010-rt2x00-add-TX-LOFT-calibration-for-MT7620.patch
new file mode 100644
index 0000000000..bb01ff1dca
--- /dev/null
+++ b/package/kernel/mac80211/patches/rt2x00/010-rt2x00-add-TX-LOFT-calibration-for-MT7620.patch
@@ -0,0 +1,982 @@
+From patchwork Sat Sep 17 20:28:43 2022
+Content-Type: text/plain; charset="utf-8"
+MIME-Version: 1.0
+Content-Transfer-Encoding: 8bit
+X-Patchwork-Submitter: Daniel Golle <daniel@makrotopia.org>
+X-Patchwork-Id: 12979251
+X-Patchwork-Delegate: kvalo@adurom.com
+Return-Path: <linux-wireless-owner@kernel.org>
+Date: Sat, 17 Sep 2022 21:28:43 +0100
+From: Daniel Golle <daniel@makrotopia.org>
+To: linux-wireless@vger.kernel.org, Stanislaw Gruszka <stf_xl@wp.pl>,
+        Helmut Schaa <helmut.schaa@googlemail.com>
+Cc: Kalle Valo <kvalo@kernel.org>,
+        "David S. Miller" <davem@davemloft.net>,
+        Eric Dumazet <edumazet@google.com>,
+        Jakub Kicinski <kuba@kernel.org>,
+        Paolo Abeni <pabeni@redhat.com>,
+        Johannes Berg <johannes.berg@intel.com>
+Subject: [PATCH v3 10/16] rt2x00: add TX LOFT calibration for MT7620
+Message-ID: 
+ <d9295a9138a1f552b648aacb84e1419d38f5c896.1663445157.git.daniel@makrotopia.org>
+References: <cover.1663445157.git.daniel@makrotopia.org>
+MIME-Version: 1.0
+Content-Disposition: inline
+In-Reply-To: <cover.1663445157.git.daniel@makrotopia.org>
+Precedence: bulk
+List-ID: <linux-wireless.vger.kernel.org>
+X-Mailing-List: linux-wireless@vger.kernel.org
+
+From: Tomislav Požega <pozega.tomislav@gmail.com>
+
+Add TX LOFT calibration from mtk driver.
+
+Signed-off-by: Tomislav Požega <pozega.tomislav@gmail.com>
+Signed-off-by: Daniel Golle <daniel@makrotopia.org>
+---
+v2: use helper functions, make tables static const, remove useless
+    debug prints
+v3: don't export function not used anywhere else
+Reported-by: kernel test robot <lkp@intel.com>
+
+ .../net/wireless/ralink/rt2x00/rt2800lib.c    | 902 ++++++++++++++++++
+ .../net/wireless/ralink/rt2x00/rt2800lib.h    |  10 +
+ 2 files changed, 912 insertions(+)
+
+--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
++++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
+@@ -9041,6 +9041,907 @@ restore_value:
+ 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
+ }
+ 
++static void rt2800_rf_configstore(struct rt2x00_dev *rt2x00dev,
++				  struct rf_reg_pair rf_reg_record[][13], u8 chain)
++{
++	u8 rfvalue = 0;
++
++	if (chain == CHAIN_0) {
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 1);
++		rf_reg_record[CHAIN_0][0].bank = 0;
++		rf_reg_record[CHAIN_0][0].reg = 1;
++		rf_reg_record[CHAIN_0][0].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 2);
++		rf_reg_record[CHAIN_0][1].bank = 0;
++		rf_reg_record[CHAIN_0][1].reg = 2;
++		rf_reg_record[CHAIN_0][1].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 35);
++		rf_reg_record[CHAIN_0][2].bank = 0;
++		rf_reg_record[CHAIN_0][2].reg = 35;
++		rf_reg_record[CHAIN_0][2].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
++		rf_reg_record[CHAIN_0][3].bank = 0;
++		rf_reg_record[CHAIN_0][3].reg = 42;
++		rf_reg_record[CHAIN_0][3].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 0);
++		rf_reg_record[CHAIN_0][4].bank = 4;
++		rf_reg_record[CHAIN_0][4].reg = 0;
++		rf_reg_record[CHAIN_0][4].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 2);
++		rf_reg_record[CHAIN_0][5].bank = 4;
++		rf_reg_record[CHAIN_0][5].reg = 2;
++		rf_reg_record[CHAIN_0][5].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 34);
++		rf_reg_record[CHAIN_0][6].bank = 4;
++		rf_reg_record[CHAIN_0][6].reg = 34;
++		rf_reg_record[CHAIN_0][6].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 3);
++		rf_reg_record[CHAIN_0][7].bank = 5;
++		rf_reg_record[CHAIN_0][7].reg = 3;
++		rf_reg_record[CHAIN_0][7].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
++		rf_reg_record[CHAIN_0][8].bank = 5;
++		rf_reg_record[CHAIN_0][8].reg = 4;
++		rf_reg_record[CHAIN_0][8].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 17);
++		rf_reg_record[CHAIN_0][9].bank = 5;
++		rf_reg_record[CHAIN_0][9].reg = 17;
++		rf_reg_record[CHAIN_0][9].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 18);
++		rf_reg_record[CHAIN_0][10].bank = 5;
++		rf_reg_record[CHAIN_0][10].reg = 18;
++		rf_reg_record[CHAIN_0][10].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 19);
++		rf_reg_record[CHAIN_0][11].bank = 5;
++		rf_reg_record[CHAIN_0][11].reg = 19;
++		rf_reg_record[CHAIN_0][11].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 20);
++		rf_reg_record[CHAIN_0][12].bank = 5;
++		rf_reg_record[CHAIN_0][12].reg = 20;
++		rf_reg_record[CHAIN_0][12].value = rfvalue;
++	} else if (chain == CHAIN_1) {
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 1);
++		rf_reg_record[CHAIN_1][0].bank = 0;
++		rf_reg_record[CHAIN_1][0].reg = 1;
++		rf_reg_record[CHAIN_1][0].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 2);
++		rf_reg_record[CHAIN_1][1].bank = 0;
++		rf_reg_record[CHAIN_1][1].reg = 2;
++		rf_reg_record[CHAIN_1][1].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 35);
++		rf_reg_record[CHAIN_1][2].bank = 0;
++		rf_reg_record[CHAIN_1][2].reg = 35;
++		rf_reg_record[CHAIN_1][2].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
++		rf_reg_record[CHAIN_1][3].bank = 0;
++		rf_reg_record[CHAIN_1][3].reg = 42;
++		rf_reg_record[CHAIN_1][3].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 0);
++		rf_reg_record[CHAIN_1][4].bank = 6;
++		rf_reg_record[CHAIN_1][4].reg = 0;
++		rf_reg_record[CHAIN_1][4].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 2);
++		rf_reg_record[CHAIN_1][5].bank = 6;
++		rf_reg_record[CHAIN_1][5].reg = 2;
++		rf_reg_record[CHAIN_1][5].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 34);
++		rf_reg_record[CHAIN_1][6].bank = 6;
++		rf_reg_record[CHAIN_1][6].reg = 34;
++		rf_reg_record[CHAIN_1][6].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 3);
++		rf_reg_record[CHAIN_1][7].bank = 7;
++		rf_reg_record[CHAIN_1][7].reg = 3;
++		rf_reg_record[CHAIN_1][7].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 4);
++		rf_reg_record[CHAIN_1][8].bank = 7;
++		rf_reg_record[CHAIN_1][8].reg = 4;
++		rf_reg_record[CHAIN_1][8].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 17);
++		rf_reg_record[CHAIN_1][9].bank = 7;
++		rf_reg_record[CHAIN_1][9].reg = 17;
++		rf_reg_record[CHAIN_1][9].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 18);
++		rf_reg_record[CHAIN_1][10].bank = 7;
++		rf_reg_record[CHAIN_1][10].reg = 18;
++		rf_reg_record[CHAIN_1][10].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 19);
++		rf_reg_record[CHAIN_1][11].bank = 7;
++		rf_reg_record[CHAIN_1][11].reg = 19;
++		rf_reg_record[CHAIN_1][11].value = rfvalue;
++		rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 20);
++		rf_reg_record[CHAIN_1][12].bank = 7;
++		rf_reg_record[CHAIN_1][12].reg = 20;
++		rf_reg_record[CHAIN_1][12].value = rfvalue;
++	} else {
++		rt2x00_warn(rt2x00dev, "Unknown chain = %u\n", chain);
++	}
++}
++
++static void rt2800_rf_configrecover(struct rt2x00_dev *rt2x00dev,
++				    struct rf_reg_pair rf_record[][13])
++{
++	u8 chain_index = 0, record_index = 0;
++	u8 bank = 0, rf_register = 0, value = 0;
++
++	for (chain_index = 0; chain_index < 2; chain_index++) {
++		for (record_index = 0; record_index < 13; record_index++) {
++			bank = rf_record[chain_index][record_index].bank;
++			rf_register = rf_record[chain_index][record_index].reg;
++			value = rf_record[chain_index][record_index].value;
++			rt2800_rfcsr_write_bank(rt2x00dev, bank, rf_register, value);
++			rt2x00_dbg(rt2x00dev, "bank: %d, rf_register: %d, value: %x\n",
++				   bank, rf_register, value);
++		}
++	}
++}
++
++static void rt2800_setbbptonegenerator(struct rt2x00_dev *rt2x00dev)
++{
++	rt2800_bbp_write(rt2x00dev, 158, 0xAA);
++	rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xAB);
++	rt2800_bbp_write(rt2x00dev, 159, 0x0A);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xAC);
++	rt2800_bbp_write(rt2x00dev, 159, 0x3F);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xAD);
++	rt2800_bbp_write(rt2x00dev, 159, 0x3F);
++
++	rt2800_bbp_write(rt2x00dev, 244, 0x40);
++}
++
++static u32 rt2800_do_fft_accumulation(struct rt2x00_dev *rt2x00dev, u8 tidx, u8 read_neg)
++{
++	u32 macvalue = 0;
++	int fftout_i = 0, fftout_q = 0;
++	u32 ptmp = 0, pint = 0;
++	u8 bbp = 0;
++	u8 tidxi;
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x00);
++	rt2800_bbp_write(rt2x00dev, 159, 0x9b);
++
++	bbp = 0x9b;
++
++	while (bbp == 0x9b) {
++		usleep_range(10, 50);
++		bbp = rt2800_bbp_read(rt2x00dev, 159);
++		bbp = bbp & 0xff;
++	}
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xba);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++
++	macvalue = rt2800_register_read(rt2x00dev, 0x057C);
++
++	fftout_i = (macvalue >> 16);
++	fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
++	fftout_q = (macvalue & 0xffff);
++	fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
++	ptmp = (fftout_i * fftout_i);
++	ptmp = ptmp + (fftout_q * fftout_q);
++	pint = ptmp;
++	rt2x00_dbg(rt2x00dev, "I = %d,  Q = %d, power = %x\n", fftout_i, fftout_q, pint);
++	if (read_neg) {
++		pint = pint >> 1;
++		tidxi = 0x40 - tidx;
++		tidxi = tidxi & 0x3f;
++
++		rt2800_bbp_write(rt2x00dev, 158, 0xba);
++		rt2800_bbp_write(rt2x00dev, 159, tidxi);
++		rt2800_bbp_write(rt2x00dev, 159, tidxi);
++		rt2800_bbp_write(rt2x00dev, 159, tidxi);
++
++		macvalue = rt2800_register_read(rt2x00dev, 0x057C);
++
++		fftout_i = (macvalue >> 16);
++		fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
++		fftout_q = (macvalue & 0xffff);
++		fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
++		ptmp = (fftout_i * fftout_i);
++		ptmp = ptmp + (fftout_q * fftout_q);
++		ptmp = ptmp >> 1;
++		pint = pint + ptmp;
++	}
++
++	return pint;
++}
++
++static u32 rt2800_read_fft_accumulation(struct rt2x00_dev *rt2x00dev, u8 tidx)
++{
++	u32 macvalue = 0;
++	int fftout_i = 0, fftout_q = 0;
++	u32 ptmp = 0, pint = 0;
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xBA);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++	rt2800_bbp_write(rt2x00dev, 159, tidx);
++
++	macvalue = rt2800_register_read(rt2x00dev, 0x057C);
++
++	fftout_i = (macvalue >> 16);
++	fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
++	fftout_q = (macvalue & 0xffff);
++	fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
++	ptmp = (fftout_i * fftout_i);
++	ptmp = ptmp + (fftout_q * fftout_q);
++	pint = ptmp;
++
++	return pint;
++}
++
++static void rt2800_write_dc(struct rt2x00_dev *rt2x00dev, u8 ch_idx, u8 alc, u8 iorq, u8 dc)
++{
++	u8 bbp = 0;
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xb0);
++	bbp = alc | 0x80;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++
++	if (ch_idx == 0)
++		bbp = (iorq == 0) ? 0xb1 : 0xb2;
++	else
++		bbp = (iorq == 0) ? 0xb8 : 0xb9;
++
++	rt2800_bbp_write(rt2x00dev, 158, bbp);
++	bbp = dc;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++}
++
++static void rt2800_loft_search(struct rt2x00_dev *rt2x00dev, u8 ch_idx,
++			       u8 alc_idx, u8 dc_result[][RF_ALC_NUM][2])
++{
++	u32 p0 = 0, p1 = 0, pf = 0;
++	char idx0 = 0, idx1 = 0;
++	u8 idxf[] = {0x00, 0x00};
++	u8 ibit = 0x20;
++	u8 iorq;
++	char bidx;
++
++	rt2800_bbp_write(rt2x00dev, 158, 0xb0);
++	rt2800_bbp_write(rt2x00dev, 159, 0x80);
++
++	for (bidx = 5; bidx >= 0; bidx--) {
++		for (iorq = 0; iorq <= 1; iorq++) {
++			if (idxf[iorq] == 0x20) {
++				idx0 = 0x20;
++				p0 = pf;
++			} else {
++				idx0 = idxf[iorq] - ibit;
++				idx0 = idx0 & 0x3F;
++				rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idx0);
++				p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
++			}
++
++			idx1 = idxf[iorq] + (bidx == 5 ? 0 : ibit);
++			idx1 = idx1 & 0x3F;
++			rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idx1);
++			p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
++
++			rt2x00_dbg(rt2x00dev, "alc=%u, IorQ=%u, idx_final=%2x\n",
++				   alc_idx, iorq, idxf[iorq]);
++			rt2x00_dbg(rt2x00dev, "p0=%x, p1=%x, pf=%x, idx_0=%x, idx_1=%x, ibit=%x\n",
++				   p0, p1, pf, idx0, idx1, ibit);
++
++			if (bidx != 5 && pf <= p0 && pf < p1) {
++				idxf[iorq] = idxf[iorq];
++			} else if (p0 < p1) {
++				pf = p0;
++				idxf[iorq] = idx0 & 0x3F;
++			} else {
++				pf = p1;
++				idxf[iorq] = idx1 & 0x3F;
++			}
++			rt2x00_dbg(rt2x00dev, "IorQ=%u, idx_final[%u]:%x, pf:%8x\n",
++				   iorq, iorq, idxf[iorq], pf);
++
++			rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idxf[iorq]);
++		}
++		ibit = ibit >> 1;
++	}
++	dc_result[ch_idx][alc_idx][0] = idxf[0];
++	dc_result[ch_idx][alc_idx][1] = idxf[1];
++}
++
++static void rt2800_iq_search(struct rt2x00_dev *rt2x00dev, u8 ch_idx, u8 *ges, u8 *pes)
++{
++	u32 p0 = 0, p1 = 0, pf = 0;
++	char perr = 0, gerr = 0, iq_err = 0;
++	char pef = 0, gef = 0;
++	char psta, pend;
++	char gsta, gend;
++
++	u8 ibit = 0x20;
++	u8 first_search = 0x00, touch_neg_max = 0x00;
++	char idx0 = 0, idx1 = 0;
++	u8 gop;
++	u8 bbp = 0;
++	char bidx;
++
++	for (bidx = 5; bidx >= 1; bidx--) {
++		for (gop = 0; gop < 2; gop++) {
++			if (gop == 1 || bidx < 4) {
++				if (gop == 0)
++					iq_err = gerr;
++				else
++					iq_err = perr;
++
++				first_search = (gop == 0) ? (bidx == 3) : (bidx == 5);
++				touch_neg_max = (gop) ? ((iq_err & 0x0F) == 0x08) :
++							((iq_err & 0x3F) == 0x20);
++
++				if (touch_neg_max) {
++					p0 = pf;
++					idx0 = iq_err;
++				} else {
++					idx0 = iq_err - ibit;
++					bbp = (ch_idx == 0) ? ((gop == 0) ? 0x28 : 0x29) :
++							      ((gop == 0) ? 0x46 : 0x47);
++
++					rt2800_bbp_write(rt2x00dev, 158, bbp);
++					rt2800_bbp_write(rt2x00dev, 159, idx0);
++
++					p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
++				}
++
++				idx1 = iq_err + (first_search ? 0 : ibit);
++				idx1 = (gop == 0) ? (idx1 & 0x0F) : (idx1 & 0x3F);
++
++				bbp = (ch_idx == 0) ? (gop == 0) ? 0x28 : 0x29 :
++				      (gop == 0) ? 0x46 : 0x47;
++
++				rt2800_bbp_write(rt2x00dev, 158, bbp);
++				rt2800_bbp_write(rt2x00dev, 159, idx1);
++
++				p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
++
++				rt2x00_dbg(rt2x00dev,
++					   "p0=%x, p1=%x, pwer_final=%x, idx0=%x, idx1=%x, iq_err=%x, gop=%d, ibit=%x\n",
++					   p0, p1, pf, idx0, idx1, iq_err, gop, ibit);
++
++				if (!(!first_search && pf <= p0 && pf < p1)) {
++					if (p0 < p1) {
++						pf = p0;
++						iq_err = idx0;
++					} else {
++						pf = p1;
++						iq_err = idx1;
++					}
++				}
++
++				bbp = (ch_idx == 0) ? (gop == 0) ? 0x28 : 0x29 :
++						      (gop == 0) ? 0x46 : 0x47;
++
++				rt2800_bbp_write(rt2x00dev, 158, bbp);
++				rt2800_bbp_write(rt2x00dev, 159, iq_err);
++
++				if (gop == 0)
++					gerr = iq_err;
++				else
++					perr = iq_err;
++
++				rt2x00_dbg(rt2x00dev, "IQCalibration pf=%8x (%2x, %2x) !\n",
++					   pf, gerr & 0x0F, perr & 0x3F);
++			}
++		}
++
++		if (bidx > 0)
++			ibit = (ibit >> 1);
++	}
++	gerr = (gerr & 0x08) ? (gerr & 0x0F) - 0x10 : (gerr & 0x0F);
++	perr = (perr & 0x20) ? (perr & 0x3F) - 0x40 : (perr & 0x3F);
++
++	gerr = (gerr < -0x07) ? -0x07 : (gerr > 0x05) ? 0x05 : gerr;
++	gsta = gerr - 1;
++	gend = gerr + 2;
++
++	perr = (perr < -0x1f) ? -0x1f : (perr > 0x1d) ? 0x1d : perr;
++	psta = perr - 1;
++	pend = perr + 2;
++
++	for (gef = gsta; gef <= gend; gef = gef + 1)
++		for (pef = psta; pef <= pend; pef = pef + 1) {
++			bbp = (ch_idx == 0) ? 0x28 : 0x46;
++			rt2800_bbp_write(rt2x00dev, 158, bbp);
++			rt2800_bbp_write(rt2x00dev, 159, gef & 0x0F);
++
++			bbp = (ch_idx == 0) ? 0x29 : 0x47;
++			rt2800_bbp_write(rt2x00dev, 158, bbp);
++			rt2800_bbp_write(rt2x00dev, 159, pef & 0x3F);
++
++			p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
++			if (gef == gsta && pef == psta) {
++				pf = p1;
++				gerr = gef;
++				perr = pef;
++			} else if (pf > p1) {
++				pf = p1;
++				gerr = gef;
++				perr = pef;
++			}
++			rt2x00_dbg(rt2x00dev, "Fine IQCalibration p1=%8x pf=%8x (%2x, %2x) !\n",
++				   p1, pf, gef & 0x0F, pef & 0x3F);
++		}
++
++	ges[ch_idx] = gerr & 0x0F;
++	pes[ch_idx] = perr & 0x3F;
++}
++
++static void rt2800_rf_aux_tx0_loopback(struct rt2x00_dev *rt2x00dev)
++{
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, 0x21);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 2, 0x10);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x00);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x1b);
++	rt2800_rfcsr_write_bank(rt2x00dev, 4, 0, 0x81);
++	rt2800_rfcsr_write_bank(rt2x00dev, 4, 2, 0x81);
++	rt2800_rfcsr_write_bank(rt2x00dev, 4, 34, 0xee);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 3, 0x2d);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, 0x2d);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, 0x80);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, 0xd7);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, 0xa2);
++	rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, 0x20);
++}
++
++static void rt2800_rf_aux_tx1_loopback(struct rt2x00_dev *rt2x00dev)
++{
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, 0x22);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 2, 0x20);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x00);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x4b);
++	rt2800_rfcsr_write_bank(rt2x00dev, 6, 0, 0x81);
++	rt2800_rfcsr_write_bank(rt2x00dev, 6, 2, 0x81);
++	rt2800_rfcsr_write_bank(rt2x00dev, 6, 34, 0xee);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 3, 0x2d);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 4, 0x2d);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 17, 0x80);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 18, 0xd7);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 19, 0xa2);
++	rt2800_rfcsr_write_bank(rt2x00dev, 7, 20, 0x20);
++}
++
++static void rt2800_loft_iq_calibration(struct rt2x00_dev *rt2x00dev)
++{
++	struct rf_reg_pair rf_store[CHAIN_NUM][13];
++	u32 macorg1 = 0;
++	u32 macorg2 = 0;
++	u32 macorg3 = 0;
++	u32 macorg4 = 0;
++	u32 macorg5 = 0;
++	u32 orig528 = 0;
++	u32 orig52c = 0;
++
++	u32 savemacsysctrl = 0;
++	u32 macvalue = 0;
++	u32 mac13b8 = 0;
++	u32 p0 = 0, p1 = 0;
++	u32 p0_idx10 = 0, p1_idx10 = 0;
++
++	u8 rfvalue;
++	u8 loft_dc_search_result[CHAIN_NUM][RF_ALC_NUM][2];
++	u8 ger[CHAIN_NUM], per[CHAIN_NUM];
++
++	u8 vga_gain[] = {14, 14};
++	u8 bbp = 0, ch_idx = 0, rf_alc_idx = 0, idx = 0;
++	u8 bbpr30, rfb0r39, rfb0r42;
++	u8 bbpr1;
++	u8 bbpr4;
++	u8 bbpr241, bbpr242;
++	u8 count_step;
++
++	static const u8 rf_gain[] = {0x00, 0x01, 0x02, 0x04, 0x08, 0x0c};
++	static const u8 rfvga_gain_table[] = {0x24, 0x25, 0x26, 0x27, 0x28, 0x2c, 0x2d, 0x2e, 0x2f, 0x30,
++					      0x31, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3F};
++	static const u8 bbp_2324gain[] = {0x16, 0x14, 0x12, 0x10, 0x0c, 0x08};
++
++	savemacsysctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macorg1 = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
++	macorg2 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
++	macorg3 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
++	macorg4 = rt2800_register_read(rt2x00dev, RF_CONTROL3);
++	macorg5 = rt2800_register_read(rt2x00dev, RF_BYPASS3);
++	mac13b8 = rt2800_register_read(rt2x00dev, 0x13b8);
++	orig528 = rt2800_register_read(rt2x00dev, RF_CONTROL2);
++	orig52c = rt2800_register_read(rt2x00dev, RF_BYPASS2);
++
++	macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macvalue &= (~0x04);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
++
++	if (unlikely(rt2800_wait_bbp_rf_ready(rt2x00dev, MAC_STATUS_CFG_BBP_RF_BUSY_TX)))
++		rt2x00_warn(rt2x00dev, "RF TX busy in LOFT IQ calibration\n");
++
++	macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macvalue &= (~0x08);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
++
++	if (unlikely(rt2800_wait_bbp_rf_ready(rt2x00dev, MAC_STATUS_CFG_BBP_RF_BUSY_RX)))
++		rt2x00_warn(rt2x00dev, "RF RX busy in LOFT IQ calibration\n");
++
++	for (ch_idx = 0; ch_idx < 2; ch_idx++)
++		rt2800_rf_configstore(rt2x00dev, rf_store, ch_idx);
++
++	bbpr30 = rt2800_bbp_read(rt2x00dev, 30);
++	rfb0r39 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 39);
++	rfb0r42 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
++
++	rt2800_bbp_write(rt2x00dev, 30, 0x1F);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 39, 0x80);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x5B);
++
++	rt2800_bbp_write(rt2x00dev, 23, 0x00);
++	rt2800_bbp_write(rt2x00dev, 24, 0x00);
++
++	rt2800_setbbptonegenerator(rt2x00dev);
++
++	for (ch_idx = 0; ch_idx < 2; ch_idx++) {
++		rt2800_bbp_write(rt2x00dev, 23, 0x00);
++		rt2800_bbp_write(rt2x00dev, 24, 0x00);
++		rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00);
++		rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0x0000000F);
++		rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00000004);
++		rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00003306);
++		rt2800_register_write(rt2x00dev, 0x13b8, 0x10);
++		udelay(1);
++
++		if (ch_idx == 0)
++			rt2800_rf_aux_tx0_loopback(rt2x00dev);
++		else
++			rt2800_rf_aux_tx1_loopback(rt2x00dev);
++
++		udelay(1);
++
++		if (ch_idx == 0)
++			rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00001004);
++		else
++			rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00002004);
++
++		rt2800_bbp_write(rt2x00dev, 158, 0x05);
++		rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++		rt2800_bbp_write(rt2x00dev, 158, 0x01);
++		if (ch_idx == 0)
++			rt2800_bbp_write(rt2x00dev, 159, 0x00);
++		else
++			rt2800_bbp_write(rt2x00dev, 159, 0x01);
++
++		vga_gain[ch_idx] = 18;
++		for (rf_alc_idx = 0; rf_alc_idx < 3; rf_alc_idx++) {
++			rt2800_bbp_write(rt2x00dev, 23, bbp_2324gain[rf_alc_idx]);
++			rt2800_bbp_write(rt2x00dev, 24, bbp_2324gain[rf_alc_idx]);
++
++			macvalue = rt2800_register_read(rt2x00dev, RF_CONTROL3);
++			macvalue &= (~0x0000F1F1);
++			macvalue |= (rf_gain[rf_alc_idx] << 4);
++			macvalue |= (rf_gain[rf_alc_idx] << 12);
++			rt2800_register_write(rt2x00dev, RF_CONTROL3, macvalue);
++			macvalue = (0x0000F1F1);
++			rt2800_register_write(rt2x00dev, RF_BYPASS3, macvalue);
++
++			if (rf_alc_idx == 0) {
++				rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x21);
++				for (; vga_gain[ch_idx] > 0;
++				     vga_gain[ch_idx] = vga_gain[ch_idx] - 2) {
++					rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
++					rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
++					rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
++					rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x00);
++					rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x00);
++					p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
++					rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x21);
++					p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
++					rt2x00_dbg(rt2x00dev, "LOFT AGC %d %d\n", p0, p1);
++					if ((p0 < 7000 * 7000) && (p1 < (7000 * 7000)))
++						break;
++				}
++
++				rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x00);
++				rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x00);
++
++				rt2x00_dbg(rt2x00dev, "Used VGA %d %x\n", vga_gain[ch_idx],
++					   rfvga_gain_table[vga_gain[ch_idx]]);
++
++				if (vga_gain[ch_idx] < 0)
++					vga_gain[ch_idx] = 0;
++			}
++
++			rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
++
++			rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
++			rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
++
++			rt2800_loft_search(rt2x00dev, ch_idx, rf_alc_idx, loft_dc_search_result);
++		}
++	}
++
++	for (rf_alc_idx = 0; rf_alc_idx < 3; rf_alc_idx++) {
++		for (idx = 0; idx < 4; idx++) {
++			rt2800_bbp_write(rt2x00dev, 158, 0xB0);
++			bbp = (idx << 2) + rf_alc_idx;
++			rt2800_bbp_write(rt2x00dev, 159, bbp);
++			rt2x00_dbg(rt2x00dev, " ALC %2x,", bbp);
++
++			rt2800_bbp_write(rt2x00dev, 158, 0xb1);
++			bbp = loft_dc_search_result[CHAIN_0][rf_alc_idx][0x00];
++			bbp = bbp & 0x3F;
++			rt2800_bbp_write(rt2x00dev, 159, bbp);
++			rt2x00_dbg(rt2x00dev, " I0 %2x,", bbp);
++
++			rt2800_bbp_write(rt2x00dev, 158, 0xb2);
++			bbp = loft_dc_search_result[CHAIN_0][rf_alc_idx][0x01];
++			bbp = bbp & 0x3F;
++			rt2800_bbp_write(rt2x00dev, 159, bbp);
++			rt2x00_dbg(rt2x00dev, " Q0 %2x,", bbp);
++
++			rt2800_bbp_write(rt2x00dev, 158, 0xb8);
++			bbp = loft_dc_search_result[CHAIN_1][rf_alc_idx][0x00];
++			bbp = bbp & 0x3F;
++			rt2800_bbp_write(rt2x00dev, 159, bbp);
++			rt2x00_dbg(rt2x00dev, " I1 %2x,", bbp);
++
++			rt2800_bbp_write(rt2x00dev, 158, 0xb9);
++			bbp = loft_dc_search_result[CHAIN_1][rf_alc_idx][0x01];
++			bbp = bbp & 0x3F;
++			rt2800_bbp_write(rt2x00dev, 159, bbp);
++			rt2x00_dbg(rt2x00dev, " Q1 %2x\n", bbp);
++		}
++	}
++
++	rt2800_bbp_write(rt2x00dev, 23, 0x00);
++	rt2800_bbp_write(rt2x00dev, 24, 0x00);
++
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x00);
++	rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++	bbp = 0x00;
++	rt2800_bbp_write(rt2x00dev, 244, 0x00);
++
++	rt2800_bbp_write(rt2x00dev, 21, 0x01);
++	udelay(1);
++	rt2800_bbp_write(rt2x00dev, 21, 0x00);
++
++	rt2800_rf_configrecover(rt2x00dev, rf_store);
++
++	rt2800_register_write(rt2x00dev, TX_PIN_CFG, macorg1);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00);
++	rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, macorg2);
++	udelay(1);
++	rt2800_register_write(rt2x00dev, RF_BYPASS0, macorg3);
++	rt2800_register_write(rt2x00dev, RF_CONTROL3, macorg4);
++	rt2800_register_write(rt2x00dev, RF_BYPASS3, macorg5);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
++	rt2800_register_write(rt2x00dev, RF_CONTROL2, orig528);
++	rt2800_register_write(rt2x00dev, RF_BYPASS2, orig52c);
++	rt2800_register_write(rt2x00dev, 0x13b8, mac13b8);
++
++	savemacsysctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macorg1 = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
++	macorg2 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
++	macorg3 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
++	macorg4 = rt2800_register_read(rt2x00dev, RF_CONTROL3);
++	macorg5 = rt2800_register_read(rt2x00dev, RF_BYPASS3);
++
++	bbpr1 = rt2800_bbp_read(rt2x00dev, 1);
++	bbpr4 = rt2800_bbp_read(rt2x00dev, 4);
++	bbpr241 = rt2800_bbp_read(rt2x00dev, 241);
++	bbpr242 = rt2800_bbp_read(rt2x00dev, 242);
++	mac13b8 = rt2800_register_read(rt2x00dev, 0x13b8);
++
++	macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macvalue &= (~0x04);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
++
++	if (unlikely(rt2800_wait_bbp_rf_ready(rt2x00dev, MAC_STATUS_CFG_BBP_RF_BUSY_TX)))
++		rt2x00_warn(rt2x00dev, "RF TX busy in LOFT IQ calibration\n");
++
++	macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
++	macvalue &= (~0x08);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
++
++	if (unlikely(rt2800_wait_bbp_rf_ready(rt2x00dev, MAC_STATUS_CFG_BBP_RF_BUSY_RX)))
++		rt2x00_warn(rt2x00dev, "RF RX busy in LOFT IQ calibration\n");
++
++	if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++		rt2800_register_write(rt2x00dev, RF_CONTROL3, 0x00000101);
++		rt2800_register_write(rt2x00dev, RF_BYPASS3, 0x0000F1F1);
++	}
++
++	rt2800_bbp_write(rt2x00dev, 23, 0x00);
++	rt2800_bbp_write(rt2x00dev, 24, 0x00);
++
++	if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++		rt2800_bbp_write(rt2x00dev, 4, bbpr4 & (~0x18));
++		rt2800_bbp_write(rt2x00dev, 21, 0x01);
++		udelay(1);
++		rt2800_bbp_write(rt2x00dev, 21, 0x00);
++
++		rt2800_bbp_write(rt2x00dev, 241, 0x14);
++		rt2800_bbp_write(rt2x00dev, 242, 0x80);
++		rt2800_bbp_write(rt2x00dev, 244, 0x31);
++	} else {
++		rt2800_setbbptonegenerator(rt2x00dev);
++	}
++
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00000004);
++	rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00003306);
++	udelay(1);
++
++	rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0x0000000F);
++
++	if (!test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++		rt2800_register_write(rt2x00dev, RF_CONTROL3, 0x00000000);
++		rt2800_register_write(rt2x00dev, RF_BYPASS3, 0x0000F1F1);
++	}
++
++	rt2800_register_write(rt2x00dev, 0x13b8, 0x00000010);
++
++	for (ch_idx = 0; ch_idx < 2; ch_idx++)
++		rt2800_rf_configstore(rt2x00dev, rf_store, ch_idx);
++
++	rt2800_rfcsr_write_dccal(rt2x00dev, 3, 0x3B);
++	rt2800_rfcsr_write_dccal(rt2x00dev, 4, 0x3B);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x03);
++	rt2800_bbp_write(rt2x00dev, 159, 0x60);
++	rt2800_bbp_write(rt2x00dev, 158, 0xB0);
++	rt2800_bbp_write(rt2x00dev, 159, 0x80);
++
++	for (ch_idx = 0; ch_idx < 2; ch_idx++) {
++		rt2800_bbp_write(rt2x00dev, 23, 0x00);
++		rt2800_bbp_write(rt2x00dev, 24, 0x00);
++
++		if (ch_idx == 0) {
++			rt2800_bbp_write(rt2x00dev, 158, 0x01);
++			rt2800_bbp_write(rt2x00dev, 159, 0x00);
++			if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++				bbp = bbpr1 & (~0x18);
++				bbp = bbp | 0x00;
++				rt2800_bbp_write(rt2x00dev, 1, bbp);
++			}
++			rt2800_rf_aux_tx0_loopback(rt2x00dev);
++			rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00001004);
++		} else {
++			rt2800_bbp_write(rt2x00dev, 158, 0x01);
++			rt2800_bbp_write(rt2x00dev, 159, 0x01);
++			if (test_bit(CAPABILITY_EXTERNAL_PA_TX1, &rt2x00dev->cap_flags)) {
++				bbp = bbpr1 & (~0x18);
++				bbp = bbp | 0x08;
++				rt2800_bbp_write(rt2x00dev, 1, bbp);
++			}
++			rt2800_rf_aux_tx1_loopback(rt2x00dev);
++			rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00002004);
++		}
++
++		rt2800_bbp_write(rt2x00dev, 158, 0x05);
++		rt2800_bbp_write(rt2x00dev, 159, 0x04);
++
++		bbp = (ch_idx == 0) ? 0x28 : 0x46;
++		rt2800_bbp_write(rt2x00dev, 158, bbp);
++		rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++		if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++			rt2800_bbp_write(rt2x00dev, 23, 0x06);
++			rt2800_bbp_write(rt2x00dev, 24, 0x06);
++			count_step = 1;
++		} else {
++			rt2800_bbp_write(rt2x00dev, 23, 0x1F);
++			rt2800_bbp_write(rt2x00dev, 24, 0x1F);
++			count_step = 2;
++		}
++
++		for (; vga_gain[ch_idx] < 19; vga_gain[ch_idx] = (vga_gain[ch_idx] + count_step)) {
++			rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
++			rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
++			rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
++
++			bbp = (ch_idx == 0) ? 0x29 : 0x47;
++			rt2800_bbp_write(rt2x00dev, 158, bbp);
++			rt2800_bbp_write(rt2x00dev, 159, 0x00);
++			p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 0);
++			if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags))
++				p0_idx10 = rt2800_read_fft_accumulation(rt2x00dev, 0x0A);
++
++			bbp = (ch_idx == 0) ? 0x29 : 0x47;
++			rt2800_bbp_write(rt2x00dev, 158, bbp);
++			rt2800_bbp_write(rt2x00dev, 159, 0x21);
++			p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 0);
++			if (test_bit(CAPABILITY_EXTERNAL_PA_TX1, &rt2x00dev->cap_flags))
++				p1_idx10 = rt2800_read_fft_accumulation(rt2x00dev, 0x0A);
++
++			rt2x00_dbg(rt2x00dev, "IQ AGC %d %d\n", p0, p1);
++
++			if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++				rt2x00_dbg(rt2x00dev, "IQ AGC IDX 10 %d %d\n", p0_idx10, p1_idx10);
++				if ((p0_idx10 > 7000 * 7000) || (p1_idx10 > 7000 * 7000)) {
++					if (vga_gain[ch_idx] != 0)
++						vga_gain[ch_idx] = vga_gain[ch_idx] - 1;
++					break;
++				}
++			}
++
++			if ((p0 > 2500 * 2500) || (p1 > 2500 * 2500))
++				break;
++		}
++
++		if (vga_gain[ch_idx] > 18)
++			vga_gain[ch_idx] = 18;
++		rt2x00_dbg(rt2x00dev, "Used VGA %d %x\n", vga_gain[ch_idx],
++			   rfvga_gain_table[vga_gain[ch_idx]]);
++
++		bbp = (ch_idx == 0) ? 0x29 : 0x47;
++		rt2800_bbp_write(rt2x00dev, 158, bbp);
++		rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++		rt2800_iq_search(rt2x00dev, ch_idx, ger, per);
++	}
++
++	rt2800_bbp_write(rt2x00dev, 23, 0x00);
++	rt2800_bbp_write(rt2x00dev, 24, 0x00);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x28);
++	bbp = ger[CHAIN_0] & 0x0F;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x29);
++	bbp = per[CHAIN_0] & 0x3F;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x46);
++	bbp = ger[CHAIN_1] & 0x0F;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x47);
++	bbp = per[CHAIN_1] & 0x3F;
++	rt2800_bbp_write(rt2x00dev, 159, bbp);
++
++	if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
++		rt2800_bbp_write(rt2x00dev, 1, bbpr1);
++		rt2800_bbp_write(rt2x00dev, 241, bbpr241);
++		rt2800_bbp_write(rt2x00dev, 242, bbpr242);
++	}
++	rt2800_bbp_write(rt2x00dev, 244, 0x00);
++
++	rt2800_bbp_write(rt2x00dev, 158, 0x00);
++	rt2800_bbp_write(rt2x00dev, 159, 0x00);
++	rt2800_bbp_write(rt2x00dev, 158, 0xB0);
++	rt2800_bbp_write(rt2x00dev, 159, 0x00);
++
++	rt2800_bbp_write(rt2x00dev, 30, bbpr30);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 39, rfb0r39);
++	rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, rfb0r42);
++
++	if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags))
++		rt2800_bbp_write(rt2x00dev, 4, bbpr4);
++
++	rt2800_bbp_write(rt2x00dev, 21, 0x01);
++	udelay(1);
++	rt2800_bbp_write(rt2x00dev, 21, 0x00);
++
++	rt2800_rf_configrecover(rt2x00dev, rf_store);
++
++	rt2800_register_write(rt2x00dev, TX_PIN_CFG, macorg1);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00);
++	rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00);
++	rt2800_register_write(rt2x00dev, RF_CONTROL0, macorg2);
++	udelay(1);
++	rt2800_register_write(rt2x00dev, RF_BYPASS0, macorg3);
++	rt2800_register_write(rt2x00dev, RF_CONTROL3, macorg4);
++	rt2800_register_write(rt2x00dev, RF_BYPASS3, macorg5);
++	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
++	rt2800_register_write(rt2x00dev, 0x13b8, mac13b8);
++}
++
+ static void rt2800_bbp_core_soft_reset(struct rt2x00_dev *rt2x00dev,
+ 				       bool set_bw, bool is_ht40)
+ {
+@@ -9653,6 +10554,7 @@ static void rt2800_init_rfcsr_6352(struc
+ 	rt2800_rxdcoc_calibration(rt2x00dev);
+ 	rt2800_bw_filter_calibration(rt2x00dev, true);
+ 	rt2800_bw_filter_calibration(rt2x00dev, false);
++	rt2800_loft_iq_calibration(rt2x00dev);
+ 	rt2800_rxiq_calibration(rt2x00dev);
+ }
+ 
+--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
++++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
+@@ -17,6 +17,16 @@
+ #define WCID_START	33
+ #define WCID_END	222
+ #define STA_IDS_SIZE	(WCID_END - WCID_START + 2)
++#define CHAIN_0		0x0
++#define CHAIN_1		0x1
++#define RF_ALC_NUM	6
++#define CHAIN_NUM	2
++
++struct rf_reg_pair {
++	u8 bank;
++	u8 reg;
++	u8 value;
++};
+ 
+ /* RT2800 driver data structure */
+ struct rt2800_drv_data {