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

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