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