package/kernel/mac80211/patches/rt2x00/987-rt2x00-add-TX-LOFT-calibration.patch

   1 From e02adea15f762d2add77b2b7714706f5c3c2f9c9 Mon Sep 17 00:00:00 2001
   2 From: Daniel Golle <daniel@makrotopia.org>
   3 Date: Thu, 11 Jan 2018 19:53:49 +0100
   4 Subject: [PATCH 09/16] rt2x00: add TX LOFT calibration for MT7620
   5 MIME-Version: 1.0
   6 Content-Type: text/plain; charset=UTF-8
   7 Content-Transfer-Encoding: 8bit
   8 To: linux-wireless@vger.kernel.org,
   9     Stanislaw Gruszka <stf_xl@wp.pl>,
  10     Helmut Schaa <helmut.schaa@googlemail.com>
  11 Cc: Kalle Valo <kvalo@kernel.org>,
  12     David S. Miller <davem@davemloft.net>,
  13     Eric Dumazet <edumazet@google.com>,
  14     Jakub Kicinski <kuba@kernel.org>,
  15     Paolo Abeni <pabeni@redhat.com>,
  16     Johannes Berg <johannes.berg@intel.com>
  17
  18 From: Tomislav Požega <pozega.tomislav@gmail.com>
  19
  20 Add TX LOFT calibration from mtk driver.
  21
  22 Signed-off-by: Tomislav Požega <pozega.tomislav@gmail.com>
  23 ---
  24  .../net/wireless/ralink/rt2x00/rt2800lib.c    | 922 ++++++++++++++++++
  25  .../net/wireless/ralink/rt2x00/rt2800lib.h    |  10 +
  26  2 files changed, 932 insertions(+)
  27
  28 --- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
  29 +++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
  30 @@ -9080,6 +9080,927 @@ restore_value:
  31         rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
  32  }
  33
  34 +static void rt2800_rf_configstore(struct rt2x00_dev *rt2x00dev,
  35 +                                 struct rf_reg_pair rf_reg_record[][13], u8 chain)
  36 +{
  37 +       u8 rfvalue = 0;
  38 +
  39 +       if (chain == CHAIN_0) {
  40 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 1);
  41 +               rf_reg_record[CHAIN_0][0].bank = 0;
  42 +               rf_reg_record[CHAIN_0][0].reg = 1;
  43 +               rf_reg_record[CHAIN_0][0].value = rfvalue;
  44 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 2);
  45 +               rf_reg_record[CHAIN_0][1].bank = 0;
  46 +               rf_reg_record[CHAIN_0][1].reg = 2;
  47 +               rf_reg_record[CHAIN_0][1].value = rfvalue;
  48 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 35);
  49 +               rf_reg_record[CHAIN_0][2].bank = 0;
  50 +               rf_reg_record[CHAIN_0][2].reg = 35;
  51 +               rf_reg_record[CHAIN_0][2].value = rfvalue;
  52 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
  53 +               rf_reg_record[CHAIN_0][3].bank = 0;
  54 +               rf_reg_record[CHAIN_0][3].reg = 42;
  55 +               rf_reg_record[CHAIN_0][3].value = rfvalue;
  56 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 0);
  57 +               rf_reg_record[CHAIN_0][4].bank = 4;
  58 +               rf_reg_record[CHAIN_0][4].reg = 0;
  59 +               rf_reg_record[CHAIN_0][4].value = rfvalue;
  60 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 2);
  61 +               rf_reg_record[CHAIN_0][5].bank = 4;
  62 +               rf_reg_record[CHAIN_0][5].reg = 2;
  63 +               rf_reg_record[CHAIN_0][5].value = rfvalue;
  64 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 4, 34);
  65 +               rf_reg_record[CHAIN_0][6].bank = 4;
  66 +               rf_reg_record[CHAIN_0][6].reg = 34;
  67 +               rf_reg_record[CHAIN_0][6].value = rfvalue;
  68 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 3);
  69 +               rf_reg_record[CHAIN_0][7].bank = 5;
  70 +               rf_reg_record[CHAIN_0][7].reg = 3;
  71 +               rf_reg_record[CHAIN_0][7].value = rfvalue;
  72 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
  73 +               rf_reg_record[CHAIN_0][8].bank = 5;
  74 +               rf_reg_record[CHAIN_0][8].reg = 4;
  75 +               rf_reg_record[CHAIN_0][8].value = rfvalue;
  76 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 17);
  77 +               rf_reg_record[CHAIN_0][9].bank = 5;
  78 +               rf_reg_record[CHAIN_0][9].reg = 17;
  79 +               rf_reg_record[CHAIN_0][9].value = rfvalue;
  80 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 18);
  81 +               rf_reg_record[CHAIN_0][10].bank = 5;
  82 +               rf_reg_record[CHAIN_0][10].reg = 18;
  83 +               rf_reg_record[CHAIN_0][10].value = rfvalue;
  84 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 19);
  85 +               rf_reg_record[CHAIN_0][11].bank = 5;
  86 +               rf_reg_record[CHAIN_0][11].reg = 19;
  87 +               rf_reg_record[CHAIN_0][11].value = rfvalue;
  88 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 5, 20);
  89 +               rf_reg_record[CHAIN_0][12].bank = 5;
  90 +               rf_reg_record[CHAIN_0][12].reg = 20;
  91 +               rf_reg_record[CHAIN_0][12].value = rfvalue;
  92 +       } else if (chain == CHAIN_1) {
  93 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 1);
  94 +               rf_reg_record[CHAIN_1][0].bank = 0;
  95 +               rf_reg_record[CHAIN_1][0].reg = 1;
  96 +               rf_reg_record[CHAIN_1][0].value = rfvalue;
  97 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 2);
  98 +               rf_reg_record[CHAIN_1][1].bank = 0;
  99 +               rf_reg_record[CHAIN_1][1].reg = 2;
 100 +               rf_reg_record[CHAIN_1][1].value = rfvalue;
 101 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 35);
 102 +               rf_reg_record[CHAIN_1][2].bank = 0;
 103 +               rf_reg_record[CHAIN_1][2].reg = 35;
 104 +               rf_reg_record[CHAIN_1][2].value = rfvalue;
 105 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
 106 +               rf_reg_record[CHAIN_1][3].bank = 0;
 107 +               rf_reg_record[CHAIN_1][3].reg = 42;
 108 +               rf_reg_record[CHAIN_1][3].value = rfvalue;
 109 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 0);
 110 +               rf_reg_record[CHAIN_1][4].bank = 6;
 111 +               rf_reg_record[CHAIN_1][4].reg = 0;
 112 +               rf_reg_record[CHAIN_1][4].value = rfvalue;
 113 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 2);
 114 +               rf_reg_record[CHAIN_1][5].bank = 6;
 115 +               rf_reg_record[CHAIN_1][5].reg = 2;
 116 +               rf_reg_record[CHAIN_1][5].value = rfvalue;
 117 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 6, 34);
 118 +               rf_reg_record[CHAIN_1][6].bank = 6;
 119 +               rf_reg_record[CHAIN_1][6].reg = 34;
 120 +               rf_reg_record[CHAIN_1][6].value = rfvalue;
 121 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 3);
 122 +               rf_reg_record[CHAIN_1][7].bank = 7;
 123 +               rf_reg_record[CHAIN_1][7].reg = 3;
 124 +               rf_reg_record[CHAIN_1][7].value = rfvalue;
 125 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 4);
 126 +               rf_reg_record[CHAIN_1][8].bank = 7;
 127 +               rf_reg_record[CHAIN_1][8].reg = 4;
 128 +               rf_reg_record[CHAIN_1][8].value = rfvalue;
 129 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 17);
 130 +               rf_reg_record[CHAIN_1][9].bank = 7;
 131 +               rf_reg_record[CHAIN_1][9].reg = 17;
 132 +               rf_reg_record[CHAIN_1][9].value = rfvalue;
 133 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 18);
 134 +               rf_reg_record[CHAIN_1][10].bank = 7;
 135 +               rf_reg_record[CHAIN_1][10].reg = 18;
 136 +               rf_reg_record[CHAIN_1][10].value = rfvalue;
 137 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 19);
 138 +               rf_reg_record[CHAIN_1][11].bank = 7;
 139 +               rf_reg_record[CHAIN_1][11].reg = 19;
 140 +               rf_reg_record[CHAIN_1][11].value = rfvalue;
 141 +               rfvalue = rt2800_rfcsr_read_bank(rt2x00dev, 7, 20);
 142 +               rf_reg_record[CHAIN_1][12].bank = 7;
 143 +               rf_reg_record[CHAIN_1][12].reg = 20;
 144 +               rf_reg_record[CHAIN_1][12].value = rfvalue;
 145 +       } else {
 146 +               rt2x00_warn(rt2x00dev, "Unknown chain = %u\n", chain);
 147 +       }
 148 +}
 149 +
 150 +static void rt2800_rf_configrecover(struct rt2x00_dev *rt2x00dev,
 151 +                                   struct rf_reg_pair rf_record[][13])
 152 +{
 153 +       u8 chain_index = 0, record_index = 0;
 154 +       u8 bank = 0, rf_register = 0, value = 0;
 155 +
 156 +       for (chain_index = 0; chain_index < 2; chain_index++) {
 157 +               for (record_index = 0; record_index < 13; record_index++) {
 158 +                       bank = rf_record[chain_index][record_index].bank;
 159 +                       rf_register = rf_record[chain_index][record_index].reg;
 160 +                       value = rf_record[chain_index][record_index].value;
 161 +                       rt2800_rfcsr_write_bank(rt2x00dev, bank, rf_register, value);
 162 +                       rt2x00_dbg(rt2x00dev, "bank: %d, rf_register: %d, value: %x\n",
 163 +                                  bank, rf_register, value);
 164 +               }
 165 +       }
 166 +}
 167 +
 168 +static void rt2800_setbbptonegenerator(struct rt2x00_dev *rt2x00dev)
 169 +{
 170 +       rt2800_bbp_write(rt2x00dev, 158, 0xAA);
 171 +       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 172 +
 173 +       rt2800_bbp_write(rt2x00dev, 158, 0xAB);
 174 +       rt2800_bbp_write(rt2x00dev, 159, 0x0A);
 175 +
 176 +       rt2800_bbp_write(rt2x00dev, 158, 0xAC);
 177 +       rt2800_bbp_write(rt2x00dev, 159, 0x3F);
 178 +
 179 +       rt2800_bbp_write(rt2x00dev, 158, 0xAD);
 180 +       rt2800_bbp_write(rt2x00dev, 159, 0x3F);
 181 +
 182 +       rt2800_bbp_write(rt2x00dev, 244, 0x40);
 183 +}
 184 +
 185 +static u32 rt2800_do_fft_accumulation(struct rt2x00_dev *rt2x00dev, u8 tidx, u8 read_neg)
 186 +{
 187 +       u32 macvalue = 0;
 188 +       int fftout_i = 0, fftout_q = 0;
 189 +       u32 ptmp = 0, pint = 0;
 190 +       u8 bbp = 0;
 191 +       u8 tidxi;
 192 +
 193 +       rt2800_bbp_write(rt2x00dev, 158, 0x00);
 194 +       rt2800_bbp_write(rt2x00dev, 159, 0x9b);
 195 +
 196 +       bbp = 0x9b;
 197 +
 198 +       while (bbp == 0x9b) {
 199 +               usleep_range(10, 50);
 200 +               bbp = rt2800_bbp_read(rt2x00dev, 159);
 201 +               bbp = bbp & 0xff;
 202 +       }
 203 +
 204 +       rt2800_bbp_write(rt2x00dev, 158, 0xba);
 205 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 206 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 207 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 208 +
 209 +       macvalue = rt2800_register_read(rt2x00dev, 0x057C);
 210 +
 211 +       fftout_i = (macvalue >> 16);
 212 +       fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
 213 +       fftout_q = (macvalue & 0xffff);
 214 +       fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
 215 +       ptmp = (fftout_i * fftout_i);
 216 +       ptmp = ptmp + (fftout_q * fftout_q);
 217 +       pint = ptmp;
 218 +       rt2x00_dbg(rt2x00dev, "I = %d,  Q = %d, power = %x\n", fftout_i, fftout_q, pint);
 219 +       if (read_neg) {
 220 +               pint = pint >> 1;
 221 +               tidxi = 0x40 - tidx;
 222 +               tidxi = tidxi & 0x3f;
 223 +
 224 +               rt2800_bbp_write(rt2x00dev, 158, 0xba);
 225 +               rt2800_bbp_write(rt2x00dev, 159, tidxi);
 226 +               rt2800_bbp_write(rt2x00dev, 159, tidxi);
 227 +               rt2800_bbp_write(rt2x00dev, 159, tidxi);
 228 +
 229 +               macvalue = rt2800_register_read(rt2x00dev, 0x057C);
 230 +
 231 +               fftout_i = (macvalue >> 16);
 232 +               fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
 233 +               fftout_q = (macvalue & 0xffff);
 234 +               fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
 235 +               ptmp = (fftout_i * fftout_i);
 236 +               ptmp = ptmp + (fftout_q * fftout_q);
 237 +               ptmp = ptmp >> 1;
 238 +               pint = pint + ptmp;
 239 +       }
 240 +
 241 +       return pint;
 242 +}
 243 +
 244 +static u32 rt2800_read_fft_accumulation(struct rt2x00_dev *rt2x00dev, u8 tidx)
 245 +{
 246 +       u32 macvalue = 0;
 247 +       int fftout_i = 0, fftout_q = 0;
 248 +       u32 ptmp = 0, pint = 0;
 249 +
 250 +       rt2800_bbp_write(rt2x00dev, 158, 0xBA);
 251 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 252 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 253 +       rt2800_bbp_write(rt2x00dev, 159, tidx);
 254 +
 255 +       macvalue = rt2800_register_read(rt2x00dev, 0x057C);
 256 +
 257 +       fftout_i = (macvalue >> 16);
 258 +       fftout_i = (fftout_i & 0x8000) ? (fftout_i - 0x10000) : fftout_i;
 259 +       fftout_q = (macvalue & 0xffff);
 260 +       fftout_q = (fftout_q & 0x8000) ? (fftout_q - 0x10000) : fftout_q;
 261 +       ptmp = (fftout_i * fftout_i);
 262 +       ptmp = ptmp + (fftout_q * fftout_q);
 263 +       pint = ptmp;
 264 +
 265 +       return pint;
 266 +}
 267 +
 268 +static void rt2800_write_dc(struct rt2x00_dev *rt2x00dev, u8 ch_idx, u8 alc, u8 iorq, u8 dc)
 269 +{
 270 +       u8 bbp = 0;
 271 +
 272 +       rt2800_bbp_write(rt2x00dev, 158, 0xb0);
 273 +       bbp = alc | 0x80;
 274 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 275 +
 276 +       if (ch_idx == 0)
 277 +               bbp = (iorq == 0) ? 0xb1 : 0xb2;
 278 +       else
 279 +               bbp = (iorq == 0) ? 0xb8 : 0xb9;
 280 +
 281 +       rt2800_bbp_write(rt2x00dev, 158, bbp);
 282 +       bbp = dc;
 283 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 284 +}
 285 +
 286 +static void rt2800_loft_search(struct rt2x00_dev *rt2x00dev, u8 ch_idx,
 287 +                              u8 alc_idx, u8 dc_result[][RF_ALC_NUM][2])
 288 +{
 289 +       u32 p0 = 0, p1 = 0, pf = 0;
 290 +       char idx0 = 0, idx1 = 0;
 291 +       u8 idxf[] = {0x00, 0x00};
 292 +       u8 ibit = 0x20;
 293 +       u8 iorq;
 294 +       char bidx;
 295 +
 296 +       rt2800_bbp_write(rt2x00dev, 158, 0xb0);
 297 +       rt2800_bbp_write(rt2x00dev, 159, 0x80);
 298 +
 299 +       for (bidx = 5; bidx >= 0; bidx--) {
 300 +               for (iorq = 0; iorq <= 1; iorq++) {
 301 +                       rt2x00_dbg(rt2x00dev, "\n========================================================\n");
 302 +
 303 +                       if (idxf[iorq] == 0x20) {
 304 +                               idx0 = 0x20;
 305 +                               p0 = pf;
 306 +                       } else {
 307 +                               idx0 = idxf[iorq] - ibit;
 308 +                               idx0 = idx0 & 0x3F;
 309 +                               rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idx0);
 310 +                               p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
 311 +                       }
 312 +
 313 +                       idx1 = idxf[iorq] + (bidx == 5 ? 0 : ibit);
 314 +                       idx1 = idx1 & 0x3F;
 315 +                       rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idx1);
 316 +                       p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
 317 +
 318 +                       rt2x00_dbg(rt2x00dev, "alc=%u, IorQ=%u, idx_final=%2x\n",
 319 +                                  alc_idx, iorq, idxf[iorq]);
 320 +                       rt2x00_dbg(rt2x00dev, "p0=%x, p1=%x, pf=%x, idx_0=%x, idx_1=%x, ibit=%x\n",
 321 +                                  p0, p1, pf, idx0, idx1, ibit);
 322 +
 323 +                       if (bidx != 5 && pf <= p0 && pf < p1) {
 324 +                               idxf[iorq] = idxf[iorq];
 325 +                       } else if (p0 < p1) {
 326 +                               pf = p0;
 327 +                               idxf[iorq] = idx0 & 0x3F;
 328 +                       } else {
 329 +                               pf = p1;
 330 +                               idxf[iorq] = idx1 & 0x3F;
 331 +                       }
 332 +                       rt2x00_dbg(rt2x00dev, "IorQ=%u, idx_final[%u]:%x, pf:%8x\n",
 333 +                                  iorq, iorq, idxf[iorq], pf);
 334 +
 335 +                       rt2800_write_dc(rt2x00dev, ch_idx, 0, iorq, idxf[iorq]);
 336 +               }
 337 +               ibit = ibit >> 1;
 338 +       }
 339 +       dc_result[ch_idx][alc_idx][0] = idxf[0];
 340 +       dc_result[ch_idx][alc_idx][1] = idxf[1];
 341 +}
 342 +
 343 +static void rt2800_iq_search(struct rt2x00_dev *rt2x00dev, u8 ch_idx, u8 *ges, u8 *pes)
 344 +{
 345 +       u32 p0 = 0, p1 = 0, pf = 0;
 346 +       char perr = 0, gerr = 0, iq_err = 0;
 347 +       char pef = 0, gef = 0;
 348 +       char psta, pend;
 349 +       char gsta, gend;
 350 +
 351 +       u8 ibit = 0x20;
 352 +       u8 first_search = 0x00, touch_neg_max = 0x00;
 353 +       char idx0 = 0, idx1 = 0;
 354 +       u8 gop;
 355 +       u8 bbp = 0;
 356 +       char bidx;
 357 +
 358 +       for (bidx = 5; bidx >= 1; bidx--) {
 359 +               for (gop = 0; gop < 2; gop++) {
 360 +                       rt2x00_dbg(rt2x00dev, "\n==============================================\n");
 361 +
 362 +                       if (gop == 1 || bidx < 4) {
 363 +                               if (gop == 0)
 364 +                                       iq_err = gerr;
 365 +                               else
 366 +                                       iq_err = perr;
 367 +
 368 +                               first_search = (gop == 0) ? (bidx == 3) : (bidx == 5);
 369 +                               touch_neg_max = (gop) ? ((iq_err & 0x0F) == 0x08) :
 370 +                                                       ((iq_err & 0x3F) == 0x20);
 371 +
 372 +                               if (touch_neg_max) {
 373 +                                       p0 = pf;
 374 +                                       idx0 = iq_err;
 375 +                               } else {
 376 +                                       idx0 = iq_err - ibit;
 377 +                                       bbp = (ch_idx == 0) ? ((gop == 0) ? 0x28 : 0x29) :
 378 +                                                             ((gop == 0) ? 0x46 : 0x47);
 379 +
 380 +                                       rt2800_bbp_write(rt2x00dev, 158, bbp);
 381 +                                       rt2800_bbp_write(rt2x00dev, 159, idx0);
 382 +
 383 +                                       p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
 384 +                               }
 385 +
 386 +                               idx1 = iq_err + (first_search ? 0 : ibit);
 387 +                               idx1 = (gop == 0) ? (idx1 & 0x0F) : (idx1 & 0x3F);
 388 +
 389 +                               bbp = (ch_idx == 0) ? (gop == 0) ? 0x28 : 0x29 :
 390 +                                     (gop == 0) ? 0x46 : 0x47;
 391 +
 392 +                               rt2800_bbp_write(rt2x00dev, 158, bbp);
 393 +                               rt2800_bbp_write(rt2x00dev, 159, idx1);
 394 +
 395 +                               p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
 396 +
 397 +                               rt2x00_dbg(rt2x00dev,
 398 +                                          "p0=%x, p1=%x, pwer_final=%x, idx0=%x, idx1=%x, iq_err=%x, gop=%d, ibit=%x\n",
 399 +                                          p0, p1, pf, idx0, idx1, iq_err, gop, ibit);
 400 +
 401 +                               if (!(!first_search && pf <= p0 && pf < p1)) {
 402 +                                       if (p0 < p1) {
 403 +                                               pf = p0;
 404 +                                               iq_err = idx0;
 405 +                                       } else {
 406 +                                               pf = p1;
 407 +                                               iq_err = idx1;
 408 +                                       }
 409 +                               }
 410 +
 411 +                               bbp = (ch_idx == 0) ? (gop == 0) ? 0x28 : 0x29 :
 412 +                                                     (gop == 0) ? 0x46 : 0x47;
 413 +
 414 +                               rt2800_bbp_write(rt2x00dev, 158, bbp);
 415 +                               rt2800_bbp_write(rt2x00dev, 159, iq_err);
 416 +
 417 +                               if (gop == 0)
 418 +                                       gerr = iq_err;
 419 +                               else
 420 +                                       perr = iq_err;
 421 +
 422 +                               rt2x00_dbg(rt2x00dev, "IQCalibration pf=%8x (%2x, %2x) !\n",
 423 +                                          pf, gerr & 0x0F, perr & 0x3F);
 424 +                       }
 425 +               }
 426 +
 427 +               if (bidx > 0)
 428 +                       ibit = (ibit >> 1);
 429 +       }
 430 +       gerr = (gerr & 0x08) ? (gerr & 0x0F) - 0x10 : (gerr & 0x0F);
 431 +       perr = (perr & 0x20) ? (perr & 0x3F) - 0x40 : (perr & 0x3F);
 432 +
 433 +       gerr = (gerr < -0x07) ? -0x07 : (gerr > 0x05) ? 0x05 : gerr;
 434 +       gsta = gerr - 1;
 435 +       gend = gerr + 2;
 436 +
 437 +       perr = (perr < -0x1f) ? -0x1f : (perr > 0x1d) ? 0x1d : perr;
 438 +       psta = perr - 1;
 439 +       pend = perr + 2;
 440 +
 441 +       for (gef = gsta; gef <= gend; gef = gef + 1)
 442 +               for (pef = psta; pef <= pend; pef = pef + 1) {
 443 +                       bbp = (ch_idx == 0) ? 0x28 : 0x46;
 444 +                       rt2800_bbp_write(rt2x00dev, 158, bbp);
 445 +                       rt2800_bbp_write(rt2x00dev, 159, gef & 0x0F);
 446 +
 447 +                       bbp = (ch_idx == 0) ? 0x29 : 0x47;
 448 +                       rt2800_bbp_write(rt2x00dev, 158, bbp);
 449 +                       rt2800_bbp_write(rt2x00dev, 159, pef & 0x3F);
 450 +
 451 +                       p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 1);
 452 +                       if (gef == gsta && pef == psta) {
 453 +                               pf = p1;
 454 +                               gerr = gef;
 455 +                               perr = pef;
 456 +                       } else if (pf > p1) {
 457 +                               pf = p1;
 458 +                               gerr = gef;
 459 +                               perr = pef;
 460 +                       }
 461 +                       rt2x00_dbg(rt2x00dev, "Fine IQCalibration p1=%8x pf=%8x (%2x, %2x) !\n",
 462 +                                  p1, pf, gef & 0x0F, pef & 0x3F);
 463 +               }
 464 +
 465 +       ges[ch_idx] = gerr & 0x0F;
 466 +       pes[ch_idx] = perr & 0x3F;
 467 +}
 468 +
 469 +static void rt2800_rf_aux_tx0_loopback(struct rt2x00_dev *rt2x00dev)
 470 +{
 471 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, 0x21);
 472 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 2, 0x10);
 473 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x00);
 474 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x1b);
 475 +       rt2800_rfcsr_write_bank(rt2x00dev, 4, 0, 0x81);
 476 +       rt2800_rfcsr_write_bank(rt2x00dev, 4, 2, 0x81);
 477 +       rt2800_rfcsr_write_bank(rt2x00dev, 4, 34, 0xee);
 478 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 3, 0x2d);
 479 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, 0x2d);
 480 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, 0x80);
 481 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, 0xd7);
 482 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, 0xa2);
 483 +       rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, 0x20);
 484 +}
 485 +
 486 +static void rt2800_rf_aux_tx1_loopback(struct rt2x00_dev *rt2x00dev)
 487 +{
 488 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, 0x22);
 489 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 2, 0x20);
 490 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x00);
 491 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x4b);
 492 +       rt2800_rfcsr_write_bank(rt2x00dev, 6, 0, 0x81);
 493 +       rt2800_rfcsr_write_bank(rt2x00dev, 6, 2, 0x81);
 494 +       rt2800_rfcsr_write_bank(rt2x00dev, 6, 34, 0xee);
 495 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 3, 0x2d);
 496 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 4, 0x2d);
 497 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 17, 0x80);
 498 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 18, 0xd7);
 499 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 19, 0xa2);
 500 +       rt2800_rfcsr_write_bank(rt2x00dev, 7, 20, 0x20);
 501 +}
 502 +
 503 +void rt2800_loft_iq_calibration(struct rt2x00_dev *rt2x00dev)
 504 +{
 505 +       struct rf_reg_pair rf_store[CHAIN_NUM][13];
 506 +       u32 macorg1 = 0;
 507 +       u32 macorg2 = 0;
 508 +       u32 macorg3 = 0;
 509 +       u32 macorg4 = 0;
 510 +       u32 macorg5 = 0;
 511 +       u32 orig528 = 0;
 512 +       u32 orig52c = 0;
 513 +
 514 +       u32 savemacsysctrl = 0, mtxcycle = 0;
 515 +       u32 macvalue = 0;
 516 +       u32 mac13b8 = 0;
 517 +       u32 p0 = 0, p1 = 0;
 518 +       u32 p0_idx10 = 0, p1_idx10 = 0;
 519 +
 520 +       u8 rfvalue;
 521 +       u8 loft_dc_search_result[CHAIN_NUM][RF_ALC_NUM][2];
 522 +       u8 ger[CHAIN_NUM], per[CHAIN_NUM];
 523 +       u8 rf_gain[] = {0x00, 0x01, 0x02, 0x04, 0x08, 0x0c};
 524 +       u8 rfvga_gain_table[] = {0x24, 0x25, 0x26, 0x27, 0x28, 0x2c, 0x2d, 0x2e, 0x2f, 0x30,
 525 +                                0x31, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3F};
 526 +       u8 vga_gain[] = {14, 14};
 527 +       u8 bbp_2324gain[] = {0x16, 0x14, 0x12, 0x10, 0x0c, 0x08};
 528 +       u8 bbp = 0, ch_idx = 0, rf_alc_idx = 0, idx = 0;
 529 +       u8 bbpr30, rfb0r39, rfb0r42;
 530 +       u8 bbpr1;
 531 +       u8 bbpr4;
 532 +       u8 bbpr241, bbpr242;
 533 +       u8 count_step;
 534 +
 535 +       savemacsysctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 536 +       macorg1 = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
 537 +       macorg2 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
 538 +       macorg3 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
 539 +       macorg4 = rt2800_register_read(rt2x00dev, RF_CONTROL3);
 540 +       macorg5 = rt2800_register_read(rt2x00dev, RF_BYPASS3);
 541 +       mac13b8 = rt2800_register_read(rt2x00dev, 0x13b8);
 542 +       orig528 = rt2800_register_read(rt2x00dev, RF_CONTROL2);
 543 +       orig52c = rt2800_register_read(rt2x00dev, RF_BYPASS2);
 544 +
 545 +       macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 546 +       macvalue &= (~0x04);
 547 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
 548 +
 549 +       for (mtxcycle = 0; mtxcycle < 10000; mtxcycle++) {
 550 +               macvalue = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 551 +               if (macvalue & 0x01)
 552 +                       usleep_range(50, 100);
 553 +               else
 554 +                       break;
 555 +       }
 556 +
 557 +       macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 558 +       macvalue &= (~0x08);
 559 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
 560 +
 561 +       for (mtxcycle = 0; mtxcycle < 10000; mtxcycle++) {
 562 +               macvalue = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 563 +               if (macvalue & 0x02)
 564 +                       usleep_range(50, 100);
 565 +               else
 566 +                       break;
 567 +       }
 568 +
 569 +       for (ch_idx = 0; ch_idx < 2; ch_idx++)
 570 +               rt2800_rf_configstore(rt2x00dev, rf_store, ch_idx);
 571 +
 572 +       bbpr30 = rt2800_bbp_read(rt2x00dev, 30);
 573 +       rfb0r39 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 39);
 574 +       rfb0r42 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 42);
 575 +
 576 +       rt2800_bbp_write(rt2x00dev, 30, 0x1F);
 577 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 39, 0x80);
 578 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, 0x5B);
 579 +
 580 +       rt2800_bbp_write(rt2x00dev, 23, 0x00);
 581 +       rt2800_bbp_write(rt2x00dev, 24, 0x00);
 582 +
 583 +       rt2800_setbbptonegenerator(rt2x00dev);
 584 +
 585 +       for (ch_idx = 0; ch_idx < 2; ch_idx++) {
 586 +               rt2800_bbp_write(rt2x00dev, 23, 0x00);
 587 +               rt2800_bbp_write(rt2x00dev, 24, 0x00);
 588 +               rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00);
 589 +               rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0x0000000F);
 590 +               rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00000004);
 591 +               rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00003306);
 592 +               rt2800_register_write(rt2x00dev, 0x13b8, 0x10);
 593 +               udelay(1);
 594 +
 595 +               if (ch_idx == 0)
 596 +                       rt2800_rf_aux_tx0_loopback(rt2x00dev);
 597 +               else
 598 +                       rt2800_rf_aux_tx1_loopback(rt2x00dev);
 599 +
 600 +               udelay(1);
 601 +
 602 +               if (ch_idx == 0)
 603 +                       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00001004);
 604 +               else
 605 +                       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00002004);
 606 +
 607 +               rt2800_bbp_write(rt2x00dev, 158, 0x05);
 608 +               rt2800_bbp_write(rt2x00dev, 159, 0x00);
 609 +
 610 +               rt2800_bbp_write(rt2x00dev, 158, 0x01);
 611 +               if (ch_idx == 0)
 612 +                       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 613 +               else
 614 +                       rt2800_bbp_write(rt2x00dev, 159, 0x01);
 615 +
 616 +               vga_gain[ch_idx] = 18;
 617 +               for (rf_alc_idx = 0; rf_alc_idx < 3; rf_alc_idx++) {
 618 +                       rt2800_bbp_write(rt2x00dev, 23, bbp_2324gain[rf_alc_idx]);
 619 +                       rt2800_bbp_write(rt2x00dev, 24, bbp_2324gain[rf_alc_idx]);
 620 +
 621 +                       macvalue = rt2800_register_read(rt2x00dev, RF_CONTROL3);
 622 +                       macvalue &= (~0x0000F1F1);
 623 +                       macvalue |= (rf_gain[rf_alc_idx] << 4);
 624 +                       macvalue |= (rf_gain[rf_alc_idx] << 12);
 625 +                       rt2800_register_write(rt2x00dev, RF_CONTROL3, macvalue);
 626 +                       macvalue = (0x0000F1F1);
 627 +                       rt2800_register_write(rt2x00dev, RF_BYPASS3, macvalue);
 628 +
 629 +                       if (rf_alc_idx == 0) {
 630 +                               rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x21);
 631 +                               for (; vga_gain[ch_idx] > 0;
 632 +                                    vga_gain[ch_idx] = vga_gain[ch_idx] - 2) {
 633 +                                       rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
 634 +                                       rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
 635 +                                       rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
 636 +                                       rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x00);
 637 +                                       rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x00);
 638 +                                       p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
 639 +                                       rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x21);
 640 +                                       p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x0A, 0);
 641 +                                       rt2x00_dbg(rt2x00dev, "LOFT AGC %d %d\n", p0, p1);
 642 +                                       if ((p0 < 7000 * 7000) && (p1 < (7000 * 7000)))
 643 +                                               break;
 644 +                               }
 645 +
 646 +                               rt2800_write_dc(rt2x00dev, ch_idx, 0, 0, 0x00);
 647 +                               rt2800_write_dc(rt2x00dev, ch_idx, 0, 1, 0x00);
 648 +
 649 +                               rt2x00_dbg(rt2x00dev, "Used VGA %d %x\n", vga_gain[ch_idx],
 650 +                                          rfvga_gain_table[vga_gain[ch_idx]]);
 651 +
 652 +                               if (vga_gain[ch_idx] < 0)
 653 +                                       vga_gain[ch_idx] = 0;
 654 +                       }
 655 +
 656 +                       rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
 657 +
 658 +                       rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
 659 +                       rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
 660 +
 661 +                       rt2800_loft_search(rt2x00dev, ch_idx, rf_alc_idx, loft_dc_search_result);
 662 +               }
 663 +       }
 664 +
 665 +       for (rf_alc_idx = 0; rf_alc_idx < 3; rf_alc_idx++) {
 666 +               for (idx = 0; idx < 4; idx++) {
 667 +                       rt2800_bbp_write(rt2x00dev, 158, 0xB0);
 668 +                       bbp = (idx << 2) + rf_alc_idx;
 669 +                       rt2800_bbp_write(rt2x00dev, 159, bbp);
 670 +                       rt2x00_dbg(rt2x00dev, " ALC %2x,", bbp);
 671 +
 672 +                       rt2800_bbp_write(rt2x00dev, 158, 0xb1);
 673 +                       bbp = loft_dc_search_result[CHAIN_0][rf_alc_idx][0x00];
 674 +                       bbp = bbp & 0x3F;
 675 +                       rt2800_bbp_write(rt2x00dev, 159, bbp);
 676 +                       rt2x00_dbg(rt2x00dev, " I0 %2x,", bbp);
 677 +
 678 +                       rt2800_bbp_write(rt2x00dev, 158, 0xb2);
 679 +                       bbp = loft_dc_search_result[CHAIN_0][rf_alc_idx][0x01];
 680 +                       bbp = bbp & 0x3F;
 681 +                       rt2800_bbp_write(rt2x00dev, 159, bbp);
 682 +                       rt2x00_dbg(rt2x00dev, " Q0 %2x,", bbp);
 683 +
 684 +                       rt2800_bbp_write(rt2x00dev, 158, 0xb8);
 685 +                       bbp = loft_dc_search_result[CHAIN_1][rf_alc_idx][0x00];
 686 +                       bbp = bbp & 0x3F;
 687 +                       rt2800_bbp_write(rt2x00dev, 159, bbp);
 688 +                       rt2x00_dbg(rt2x00dev, " I1 %2x,", bbp);
 689 +
 690 +                       rt2800_bbp_write(rt2x00dev, 158, 0xb9);
 691 +                       bbp = loft_dc_search_result[CHAIN_1][rf_alc_idx][0x01];
 692 +                       bbp = bbp & 0x3F;
 693 +                       rt2800_bbp_write(rt2x00dev, 159, bbp);
 694 +                       rt2x00_dbg(rt2x00dev, " Q1 %2x\n", bbp);
 695 +               }
 696 +       }
 697 +
 698 +       rt2800_bbp_write(rt2x00dev, 23, 0x00);
 699 +       rt2800_bbp_write(rt2x00dev, 24, 0x00);
 700 +
 701 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
 702 +
 703 +       rt2800_bbp_write(rt2x00dev, 158, 0x00);
 704 +       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 705 +
 706 +       bbp = 0x00;
 707 +       rt2800_bbp_write(rt2x00dev, 244, 0x00);
 708 +
 709 +       rt2800_bbp_write(rt2x00dev, 21, 0x01);
 710 +       udelay(1);
 711 +       rt2800_bbp_write(rt2x00dev, 21, 0x00);
 712 +
 713 +       rt2800_rf_configrecover(rt2x00dev, rf_store);
 714 +
 715 +       rt2800_register_write(rt2x00dev, TX_PIN_CFG, macorg1);
 716 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
 717 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00);
 718 +       rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00);
 719 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, macorg2);
 720 +       udelay(1);
 721 +       rt2800_register_write(rt2x00dev, RF_BYPASS0, macorg3);
 722 +       rt2800_register_write(rt2x00dev, RF_CONTROL3, macorg4);
 723 +       rt2800_register_write(rt2x00dev, RF_BYPASS3, macorg5);
 724 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
 725 +       rt2800_register_write(rt2x00dev, RF_CONTROL2, orig528);
 726 +       rt2800_register_write(rt2x00dev, RF_BYPASS2, orig52c);
 727 +       rt2800_register_write(rt2x00dev, 0x13b8, mac13b8);
 728 +
 729 +       savemacsysctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 730 +       macorg1 = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
 731 +       macorg2 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
 732 +       macorg3 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
 733 +       macorg4 = rt2800_register_read(rt2x00dev, RF_CONTROL3);
 734 +       macorg5 = rt2800_register_read(rt2x00dev, RF_BYPASS3);
 735 +
 736 +       bbpr1 = rt2800_bbp_read(rt2x00dev, 1);
 737 +       bbpr4 = rt2800_bbp_read(rt2x00dev, 4);
 738 +       bbpr241 = rt2800_bbp_read(rt2x00dev, 241);
 739 +       bbpr242 = rt2800_bbp_read(rt2x00dev, 242);
 740 +       mac13b8 = rt2800_register_read(rt2x00dev, 0x13b8);
 741 +
 742 +       macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 743 +       macvalue &= (~0x04);
 744 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
 745 +       for (mtxcycle = 0; mtxcycle < 10000; mtxcycle++) {
 746 +               macvalue = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 747 +               if (macvalue & 0x01)
 748 +                       usleep_range(50, 100);
 749 +               else
 750 +                       break;
 751 +       }
 752 +
 753 +       macvalue = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 754 +       macvalue &= (~0x08);
 755 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, macvalue);
 756 +       for (mtxcycle = 0; mtxcycle < 10000; mtxcycle++) {
 757 +               macvalue = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 758 +               if (macvalue & 0x02)
 759 +                       usleep_range(50, 100);
 760 +               else
 761 +                       break;
 762 +       }
 763 +
 764 +       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 765 +               rt2800_register_write(rt2x00dev, RF_CONTROL3, 0x00000101);
 766 +               rt2800_register_write(rt2x00dev, RF_BYPASS3, 0x0000F1F1);
 767 +       }
 768 +
 769 +       rt2800_bbp_write(rt2x00dev, 23, 0x00);
 770 +       rt2800_bbp_write(rt2x00dev, 24, 0x00);
 771 +
 772 +       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 773 +               rt2800_bbp_write(rt2x00dev, 4, bbpr4 & (~0x18));
 774 +               rt2800_bbp_write(rt2x00dev, 21, 0x01);
 775 +               udelay(1);
 776 +               rt2800_bbp_write(rt2x00dev, 21, 0x00);
 777 +
 778 +               rt2800_bbp_write(rt2x00dev, 241, 0x14);
 779 +               rt2800_bbp_write(rt2x00dev, 242, 0x80);
 780 +               rt2800_bbp_write(rt2x00dev, 244, 0x31);
 781 +       } else {
 782 +               rt2800_setbbptonegenerator(rt2x00dev);
 783 +       }
 784 +
 785 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00000004);
 786 +       rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00003306);
 787 +       udelay(1);
 788 +
 789 +       rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0x0000000F);
 790 +
 791 +       if (!test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 792 +               rt2800_register_write(rt2x00dev, RF_CONTROL3, 0x00000000);
 793 +               rt2800_register_write(rt2x00dev, RF_BYPASS3, 0x0000F1F1);
 794 +       }
 795 +
 796 +       rt2800_register_write(rt2x00dev, 0x13b8, 0x00000010);
 797 +
 798 +       for (ch_idx = 0; ch_idx < 2; ch_idx++)
 799 +               rt2800_rf_configstore(rt2x00dev, rf_store, ch_idx);
 800 +
 801 +       rt2800_rfcsr_write_dccal(rt2x00dev, 3, 0x3B);
 802 +       rt2800_rfcsr_write_dccal(rt2x00dev, 4, 0x3B);
 803 +
 804 +       rt2800_bbp_write(rt2x00dev, 158, 0x03);
 805 +       rt2800_bbp_write(rt2x00dev, 159, 0x60);
 806 +       rt2800_bbp_write(rt2x00dev, 158, 0xB0);
 807 +       rt2800_bbp_write(rt2x00dev, 159, 0x80);
 808 +
 809 +       for (ch_idx = 0; ch_idx < 2; ch_idx++) {
 810 +               rt2800_bbp_write(rt2x00dev, 23, 0x00);
 811 +               rt2800_bbp_write(rt2x00dev, 24, 0x00);
 812 +
 813 +               if (ch_idx == 0) {
 814 +                       rt2800_bbp_write(rt2x00dev, 158, 0x01);
 815 +                       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 816 +                       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 817 +                               bbp = bbpr1 & (~0x18);
 818 +                               bbp = bbp | 0x00;
 819 +                               rt2800_bbp_write(rt2x00dev, 1, bbp);
 820 +                       }
 821 +                       rt2800_rf_aux_tx0_loopback(rt2x00dev);
 822 +                       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00001004);
 823 +               } else {
 824 +                       rt2800_bbp_write(rt2x00dev, 158, 0x01);
 825 +                       rt2800_bbp_write(rt2x00dev, 159, 0x01);
 826 +                       if (test_bit(CAPABILITY_EXTERNAL_PA_TX1, &rt2x00dev->cap_flags)) {
 827 +                               bbp = bbpr1 & (~0x18);
 828 +                               bbp = bbp | 0x08;
 829 +                               rt2800_bbp_write(rt2x00dev, 1, bbp);
 830 +                       }
 831 +                       rt2800_rf_aux_tx1_loopback(rt2x00dev);
 832 +                       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00002004);
 833 +               }
 834 +
 835 +               rt2800_bbp_write(rt2x00dev, 158, 0x05);
 836 +               rt2800_bbp_write(rt2x00dev, 159, 0x04);
 837 +
 838 +               bbp = (ch_idx == 0) ? 0x28 : 0x46;
 839 +               rt2800_bbp_write(rt2x00dev, 158, bbp);
 840 +               rt2800_bbp_write(rt2x00dev, 159, 0x00);
 841 +
 842 +               if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 843 +                       rt2800_bbp_write(rt2x00dev, 23, 0x06);
 844 +                       rt2800_bbp_write(rt2x00dev, 24, 0x06);
 845 +                       count_step = 1;
 846 +               } else {
 847 +                       rt2800_bbp_write(rt2x00dev, 23, 0x1F);
 848 +                       rt2800_bbp_write(rt2x00dev, 24, 0x1F);
 849 +                       count_step = 2;
 850 +               }
 851 +
 852 +               for (; vga_gain[ch_idx] < 19; vga_gain[ch_idx] = (vga_gain[ch_idx] + count_step)) {
 853 +                       rfvalue = rfvga_gain_table[vga_gain[ch_idx]];
 854 +                       rt2800_rfcsr_write_dccal(rt2x00dev, 3, rfvalue);
 855 +                       rt2800_rfcsr_write_dccal(rt2x00dev, 4, rfvalue);
 856 +
 857 +                       bbp = (ch_idx == 0) ? 0x29 : 0x47;
 858 +                       rt2800_bbp_write(rt2x00dev, 158, bbp);
 859 +                       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 860 +                       p0 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 0);
 861 +                       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags))
 862 +                               p0_idx10 = rt2800_read_fft_accumulation(rt2x00dev, 0x0A);
 863 +
 864 +                       bbp = (ch_idx == 0) ? 0x29 : 0x47;
 865 +                       rt2800_bbp_write(rt2x00dev, 158, bbp);
 866 +                       rt2800_bbp_write(rt2x00dev, 159, 0x21);
 867 +                       p1 = rt2800_do_fft_accumulation(rt2x00dev, 0x14, 0);
 868 +                       if (test_bit(CAPABILITY_EXTERNAL_PA_TX1, &rt2x00dev->cap_flags))
 869 +                               p1_idx10 = rt2800_read_fft_accumulation(rt2x00dev, 0x0A);
 870 +
 871 +                       rt2x00_dbg(rt2x00dev, "IQ AGC %d %d\n", p0, p1);
 872 +
 873 +                       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 874 +                               rt2x00_dbg(rt2x00dev, "IQ AGC IDX 10 %d %d\n", p0_idx10, p1_idx10);
 875 +                               if ((p0_idx10 > 7000 * 7000) || (p1_idx10 > 7000 * 7000)) {
 876 +                                       if (vga_gain[ch_idx] != 0)
 877 +                                               vga_gain[ch_idx] = vga_gain[ch_idx] - 1;
 878 +                                       break;
 879 +                               }
 880 +                       }
 881 +
 882 +                       if ((p0 > 2500 * 2500) || (p1 > 2500 * 2500))
 883 +                               break;
 884 +               }
 885 +
 886 +               if (vga_gain[ch_idx] > 18)
 887 +                       vga_gain[ch_idx] = 18;
 888 +               rt2x00_dbg(rt2x00dev, "Used VGA %d %x\n", vga_gain[ch_idx],
 889 +                          rfvga_gain_table[vga_gain[ch_idx]]);
 890 +
 891 +               bbp = (ch_idx == 0) ? 0x29 : 0x47;
 892 +               rt2800_bbp_write(rt2x00dev, 158, bbp);
 893 +               rt2800_bbp_write(rt2x00dev, 159, 0x00);
 894 +
 895 +               rt2800_iq_search(rt2x00dev, ch_idx, ger, per);
 896 +       }
 897 +
 898 +       rt2800_bbp_write(rt2x00dev, 23, 0x00);
 899 +       rt2800_bbp_write(rt2x00dev, 24, 0x00);
 900 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x04);
 901 +
 902 +       rt2800_bbp_write(rt2x00dev, 158, 0x28);
 903 +       bbp = ger[CHAIN_0] & 0x0F;
 904 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 905 +
 906 +       rt2800_bbp_write(rt2x00dev, 158, 0x29);
 907 +       bbp = per[CHAIN_0] & 0x3F;
 908 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 909 +
 910 +       rt2800_bbp_write(rt2x00dev, 158, 0x46);
 911 +       bbp = ger[CHAIN_1] & 0x0F;
 912 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 913 +
 914 +       rt2800_bbp_write(rt2x00dev, 158, 0x47);
 915 +       bbp = per[CHAIN_1] & 0x3F;
 916 +       rt2800_bbp_write(rt2x00dev, 159, bbp);
 917 +
 918 +       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags)) {
 919 +               rt2800_bbp_write(rt2x00dev, 1, bbpr1);
 920 +               rt2800_bbp_write(rt2x00dev, 241, bbpr241);
 921 +               rt2800_bbp_write(rt2x00dev, 242, bbpr242);
 922 +       }
 923 +       rt2800_bbp_write(rt2x00dev, 244, 0x00);
 924 +
 925 +       rt2800_bbp_write(rt2x00dev, 158, 0x00);
 926 +       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 927 +       rt2800_bbp_write(rt2x00dev, 158, 0xB0);
 928 +       rt2800_bbp_write(rt2x00dev, 159, 0x00);
 929 +
 930 +       rt2800_bbp_write(rt2x00dev, 30, bbpr30);
 931 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 39, rfb0r39);
 932 +       rt2800_rfcsr_write_bank(rt2x00dev, 0, 42, rfb0r42);
 933 +
 934 +       if (test_bit(CAPABILITY_EXTERNAL_PA_TX0, &rt2x00dev->cap_flags))
 935 +               rt2800_bbp_write(rt2x00dev, 4, bbpr4);
 936 +
 937 +       rt2800_bbp_write(rt2x00dev, 21, 0x01);
 938 +       udelay(1);
 939 +       rt2800_bbp_write(rt2x00dev, 21, 0x00);
 940 +
 941 +       rt2800_rf_configrecover(rt2x00dev, rf_store);
 942 +
 943 +       rt2800_register_write(rt2x00dev, TX_PIN_CFG, macorg1);
 944 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, 0x00);
 945 +       rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x00);
 946 +       rt2800_register_write(rt2x00dev, RF_CONTROL0, macorg2);
 947 +       udelay(1);
 948 +       rt2800_register_write(rt2x00dev, RF_BYPASS0, macorg3);
 949 +       rt2800_register_write(rt2x00dev, RF_CONTROL3, macorg4);
 950 +       rt2800_register_write(rt2x00dev, RF_BYPASS3, macorg5);
 951 +       rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
 952 +       rt2800_register_write(rt2x00dev, 0x13b8, mac13b8);
 953 +}
 954 +
 955  static void rt2800_bbp_core_soft_reset(struct rt2x00_dev *rt2x00dev,
 956                                        bool set_bw, bool is_ht40)
 957  {
 958 @@ -9692,6 +10613,7 @@ static void rt2800_init_rfcsr_6352(struc
 959         rt2800_rxdcoc_calibration(rt2x00dev);
 960         rt2800_bw_filter_calibration(rt2x00dev, true);
 961         rt2800_bw_filter_calibration(rt2x00dev, false);
 962 +       rt2800_loft_iq_calibration(rt2x00dev);
 963         rt2800_rxiq_calibration(rt2x00dev);
 964  }
 965
 966 --- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
 967 +++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
 968 @@ -17,6 +17,16 @@
 969  #define WCID_START     33
 970  #define WCID_END       222
 971  #define STA_IDS_SIZE   (WCID_END - WCID_START + 2)
 972 +#define CHAIN_0                0x0
 973 +#define CHAIN_1                0x1
 974 +#define RF_ALC_NUM     6
 975 +#define CHAIN_NUM      2
 976 +
 977 +struct rf_reg_pair {
 978 +       u8 bank;
 979 +       u8 reg;
 980 +       u8 value;
 981 +};
 982
 983  /* RT2800 driver data structure */
 984  struct rt2800_drv_data {