SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/net/wireless/b43/phy_n.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4   IEEE 802.11n PHY support
   5
   6   Copyright (c) 2008 Michael Buesch <mb@bu3sch.de>
   7
   8   This program is free software; you can redistribute it and/or modify
   9   it under the terms of the GNU General Public License as published by
  10   the Free Software Foundation; either version 2 of the License, or
  11   (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; see the file COPYING.  If not, write to
  20   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  21   Boston, MA 02110-1301, USA.
  22
  23 */
  24
  25 #include <linux/delay.h>
  26 #include <linux/types.h>
  27
  28 #include "b43.h"
  29 #include "phy_n.h"
  30 #include "tables_nphy.h"
  31 #include "main.h"
  32
  33 struct nphy_txgains {
  34         u16 txgm[2];
  35         u16 pga[2];
  36         u16 pad[2];
  37         u16 ipa[2];
  38 };
  39
  40 struct nphy_iqcal_params {
  41         u16 txgm;
  42         u16 pga;
  43         u16 pad;
  44         u16 ipa;
  45         u16 cal_gain;
  46         u16 ncorr[5];
  47 };
  48
  49 struct nphy_iq_est {
  50         s32 iq0_prod;
  51         u32 i0_pwr;
  52         u32 q0_pwr;
  53         s32 iq1_prod;
  54         u32 i1_pwr;
  55         u32 q1_pwr;
  56 };
  57
  58 enum b43_nphy_rf_sequence {
  59         B43_RFSEQ_RX2TX,
  60         B43_RFSEQ_TX2RX,
  61         B43_RFSEQ_RESET2RX,
  62         B43_RFSEQ_UPDATE_GAINH,
  63         B43_RFSEQ_UPDATE_GAINL,
  64         B43_RFSEQ_UPDATE_GAINU,
  65 };
  66
  67 static void b43_nphy_force_rf_sequence(struct b43_wldev *dev,
  68                                        enum b43_nphy_rf_sequence seq);
  69
  70 void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
  71 {//TODO
  72 }
  73
  74 static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
  75 {//TODO
  76 }
  77
  78 static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
  79                                                         bool ignore_tssi)
  80 {//TODO
  81         return B43_TXPWR_RES_DONE;
  82 }
  83
  84 static void b43_chantab_radio_upload(struct b43_wldev *dev,
  85                                      const struct b43_nphy_channeltab_entry *e)
  86 {
  87         b43_radio_write16(dev, B2055_PLL_REF, e->radio_pll_ref);
  88         b43_radio_write16(dev, B2055_RF_PLLMOD0, e->radio_rf_pllmod0);
  89         b43_radio_write16(dev, B2055_RF_PLLMOD1, e->radio_rf_pllmod1);
  90         b43_radio_write16(dev, B2055_VCO_CAPTAIL, e->radio_vco_captail);
  91         b43_radio_write16(dev, B2055_VCO_CAL1, e->radio_vco_cal1);
  92         b43_radio_write16(dev, B2055_VCO_CAL2, e->radio_vco_cal2);
  93         b43_radio_write16(dev, B2055_PLL_LFC1, e->radio_pll_lfc1);
  94         b43_radio_write16(dev, B2055_PLL_LFR1, e->radio_pll_lfr1);
  95         b43_radio_write16(dev, B2055_PLL_LFC2, e->radio_pll_lfc2);
  96         b43_radio_write16(dev, B2055_LGBUF_CENBUF, e->radio_lgbuf_cenbuf);
  97         b43_radio_write16(dev, B2055_LGEN_TUNE1, e->radio_lgen_tune1);
  98         b43_radio_write16(dev, B2055_LGEN_TUNE2, e->radio_lgen_tune2);
  99         b43_radio_write16(dev, B2055_C1_LGBUF_ATUNE, e->radio_c1_lgbuf_atune);
 100         b43_radio_write16(dev, B2055_C1_LGBUF_GTUNE, e->radio_c1_lgbuf_gtune);
 101         b43_radio_write16(dev, B2055_C1_RX_RFR1, e->radio_c1_rx_rfr1);
 102         b43_radio_write16(dev, B2055_C1_TX_PGAPADTN, e->radio_c1_tx_pgapadtn);
 103         b43_radio_write16(dev, B2055_C1_TX_MXBGTRIM, e->radio_c1_tx_mxbgtrim);
 104         b43_radio_write16(dev, B2055_C2_LGBUF_ATUNE, e->radio_c2_lgbuf_atune);
 105         b43_radio_write16(dev, B2055_C2_LGBUF_GTUNE, e->radio_c2_lgbuf_gtune);
 106         b43_radio_write16(dev, B2055_C2_RX_RFR1, e->radio_c2_rx_rfr1);
 107         b43_radio_write16(dev, B2055_C2_TX_PGAPADTN, e->radio_c2_tx_pgapadtn);
 108         b43_radio_write16(dev, B2055_C2_TX_MXBGTRIM, e->radio_c2_tx_mxbgtrim);
 109 }
 110
 111 static void b43_chantab_phy_upload(struct b43_wldev *dev,
 112                                    const struct b43_nphy_channeltab_entry *e)
 113 {
 114         b43_phy_write(dev, B43_NPHY_BW1A, e->phy_bw1a);
 115         b43_phy_write(dev, B43_NPHY_BW2, e->phy_bw2);
 116         b43_phy_write(dev, B43_NPHY_BW3, e->phy_bw3);
 117         b43_phy_write(dev, B43_NPHY_BW4, e->phy_bw4);
 118         b43_phy_write(dev, B43_NPHY_BW5, e->phy_bw5);
 119         b43_phy_write(dev, B43_NPHY_BW6, e->phy_bw6);
 120 }
 121
 122 static void b43_nphy_tx_power_fix(struct b43_wldev *dev)
 123 {
 124         //TODO
 125 }
 126
 127 /* Tune the hardware to a new channel. */
 128 static int nphy_channel_switch(struct b43_wldev *dev, unsigned int channel)
 129 {
 130         const struct b43_nphy_channeltab_entry *tabent;
 131
 132         tabent = b43_nphy_get_chantabent(dev, channel);
 133         if (!tabent)
 134                 return -ESRCH;
 135
 136         //FIXME enable/disable band select upper20 in RXCTL
 137         if (0 /*FIXME 5Ghz*/)
 138                 b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, 0x20);
 139         else
 140                 b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, 0x50);
 141         b43_chantab_radio_upload(dev, tabent);
 142         udelay(50);
 143         b43_radio_write16(dev, B2055_VCO_CAL10, 5);
 144         b43_radio_write16(dev, B2055_VCO_CAL10, 45);
 145         b43_radio_write16(dev, B2055_VCO_CAL10, 65);
 146         udelay(300);
 147         if (0 /*FIXME 5Ghz*/)
 148                 b43_phy_set(dev, B43_NPHY_BANDCTL, B43_NPHY_BANDCTL_5GHZ);
 149         else
 150                 b43_phy_mask(dev, B43_NPHY_BANDCTL, ~B43_NPHY_BANDCTL_5GHZ);
 151         b43_chantab_phy_upload(dev, tabent);
 152         b43_nphy_tx_power_fix(dev);
 153
 154         return 0;
 155 }
 156
 157 static void b43_radio_init2055_pre(struct b43_wldev *dev)
 158 {
 159         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 160                      ~B43_NPHY_RFCTL_CMD_PORFORCE);
 161         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 162                     B43_NPHY_RFCTL_CMD_CHIP0PU |
 163                     B43_NPHY_RFCTL_CMD_OEPORFORCE);
 164         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 165                     B43_NPHY_RFCTL_CMD_PORFORCE);
 166 }
 167
 168 static void b43_radio_init2055_post(struct b43_wldev *dev)
 169 {
 170         struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
 171         struct ssb_boardinfo *binfo = &(dev->dev->bus->boardinfo);
 172         int i;
 173         u16 val;
 174
 175         b43_radio_mask(dev, B2055_MASTER1, 0xFFF3);
 176         msleep(1);
 177         if ((sprom->revision != 4) ||
 178            !(sprom->boardflags_hi & B43_BFH_RSSIINV)) {
 179                 if ((binfo->vendor != PCI_VENDOR_ID_BROADCOM) ||
 180                     (binfo->type != 0x46D) ||
 181                     (binfo->rev < 0x41)) {
 182                         b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
 183                         b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
 184                         msleep(1);
 185                 }
 186         }
 187         b43_radio_maskset(dev, B2055_RRCCAL_NOPTSEL, 0x3F, 0x2C);
 188         msleep(1);
 189         b43_radio_write16(dev, B2055_CAL_MISC, 0x3C);
 190         msleep(1);
 191         b43_radio_mask(dev, B2055_CAL_MISC, 0xFFBE);
 192         msleep(1);
 193         b43_radio_set(dev, B2055_CAL_LPOCTL, 0x80);
 194         msleep(1);
 195         b43_radio_set(dev, B2055_CAL_MISC, 0x1);
 196         msleep(1);
 197         b43_radio_set(dev, B2055_CAL_MISC, 0x40);
 198         msleep(1);
 199         for (i = 0; i < 100; i++) {
 200                 val = b43_radio_read16(dev, B2055_CAL_COUT2);
 201                 if (val & 0x80)
 202                         break;
 203                 udelay(10);
 204         }
 205         msleep(1);
 206         b43_radio_mask(dev, B2055_CAL_LPOCTL, 0xFF7F);
 207         msleep(1);
 208         nphy_channel_switch(dev, dev->phy.channel);
 209         b43_radio_write16(dev, B2055_C1_RX_BB_LPF, 0x9);
 210         b43_radio_write16(dev, B2055_C2_RX_BB_LPF, 0x9);
 211         b43_radio_write16(dev, B2055_C1_RX_BB_MIDACHP, 0x83);
 212         b43_radio_write16(dev, B2055_C2_RX_BB_MIDACHP, 0x83);
 213 }
 214
 215 /* Initialize a Broadcom 2055 N-radio */
 216 static void b43_radio_init2055(struct b43_wldev *dev)
 217 {
 218         b43_radio_init2055_pre(dev);
 219         if (b43_status(dev) < B43_STAT_INITIALIZED)
 220                 b2055_upload_inittab(dev, 0, 1);
 221         else
 222                 b2055_upload_inittab(dev, 0/*FIXME on 5ghz band*/, 0);
 223         b43_radio_init2055_post(dev);
 224 }
 225
 226 void b43_nphy_radio_turn_on(struct b43_wldev *dev)
 227 {
 228         b43_radio_init2055(dev);
 229 }
 230
 231 void b43_nphy_radio_turn_off(struct b43_wldev *dev)
 232 {
 233         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 234                      ~B43_NPHY_RFCTL_CMD_EN);
 235 }
 236
 237 /*
 238  * Upload the N-PHY tables.
 239  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/InitTables
 240  */
 241 static void b43_nphy_tables_init(struct b43_wldev *dev)
 242 {
 243         if (dev->phy.rev < 3)
 244                 b43_nphy_rev0_1_2_tables_init(dev);
 245         else
 246                 b43_nphy_rev3plus_tables_init(dev);
 247 }
 248
 249 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PA%20override */
 250 static void b43_nphy_pa_override(struct b43_wldev *dev, bool enable)
 251 {
 252         struct b43_phy_n *nphy = dev->phy.n;
 253         enum ieee80211_band band;
 254         u16 tmp;
 255
 256         if (!enable) {
 257                 nphy->rfctrl_intc1_save = b43_phy_read(dev,
 258                                                        B43_NPHY_RFCTL_INTC1);
 259                 nphy->rfctrl_intc2_save = b43_phy_read(dev,
 260                                                        B43_NPHY_RFCTL_INTC2);
 261                 band = b43_current_band(dev->wl);
 262                 if (dev->phy.rev >= 3) {
 263                         if (band == IEEE80211_BAND_5GHZ)
 264                                 tmp = 0x600;
 265                         else
 266                                 tmp = 0x480;
 267                 } else {
 268                         if (band == IEEE80211_BAND_5GHZ)
 269                                 tmp = 0x180;
 270                         else
 271                                 tmp = 0x120;
 272                 }
 273                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, tmp);
 274                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, tmp);
 275         } else {
 276                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1,
 277                                 nphy->rfctrl_intc1_save);
 278                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2,
 279                                 nphy->rfctrl_intc2_save);
 280         }
 281 }
 282
 283 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxLpFbw */
 284 static void b43_nphy_tx_lp_fbw(struct b43_wldev *dev)
 285 {
 286         struct b43_phy_n *nphy = dev->phy.n;
 287         u16 tmp;
 288         enum ieee80211_band band = b43_current_band(dev->wl);
 289         bool ipa = (nphy->ipa2g_on && band == IEEE80211_BAND_2GHZ) ||
 290                         (nphy->ipa5g_on && band == IEEE80211_BAND_5GHZ);
 291
 292         if (dev->phy.rev >= 3) {
 293                 if (ipa) {
 294                         tmp = 4;
 295                         b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S2,
 296                               (((((tmp << 3) | tmp) << 3) | tmp) << 3) | tmp);
 297                 }
 298
 299                 tmp = 1;
 300                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S2,
 301                               (((((tmp << 3) | tmp) << 3) | tmp) << 3) | tmp);
 302         }
 303 }
 304
 305 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/BmacPhyClkFgc */
 306 static void b43_nphy_bmac_clock_fgc(struct b43_wldev *dev, bool force)
 307 {
 308         u32 tmslow;
 309
 310         if (dev->phy.type != B43_PHYTYPE_N)
 311                 return;
 312
 313         tmslow = ssb_read32(dev->dev, SSB_TMSLOW);
 314         if (force)
 315                 tmslow |= SSB_TMSLOW_FGC;
 316         else
 317                 tmslow &= ~SSB_TMSLOW_FGC;
 318         ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
 319 }
 320
 321 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CCA */
 322 static void b43_nphy_reset_cca(struct b43_wldev *dev)
 323 {
 324         u16 bbcfg;
 325
 326         b43_nphy_bmac_clock_fgc(dev, 1);
 327         bbcfg = b43_phy_read(dev, B43_NPHY_BBCFG);
 328         b43_phy_write(dev, B43_NPHY_BBCFG, bbcfg | B43_NPHY_BBCFG_RSTCCA);
 329         udelay(1);
 330         b43_phy_write(dev, B43_NPHY_BBCFG, bbcfg & ~B43_NPHY_BBCFG_RSTCCA);
 331         b43_nphy_bmac_clock_fgc(dev, 0);
 332         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 333 }
 334
 335 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/MIMOConfig */
 336 static void b43_nphy_update_mimo_config(struct b43_wldev *dev, s32 preamble)
 337 {
 338         u16 mimocfg = b43_phy_read(dev, B43_NPHY_MIMOCFG);
 339
 340         mimocfg |= B43_NPHY_MIMOCFG_AUTO;
 341         if (preamble == 1)
 342                 mimocfg |= B43_NPHY_MIMOCFG_GFMIX;
 343         else
 344                 mimocfg &= ~B43_NPHY_MIMOCFG_GFMIX;
 345
 346         b43_phy_write(dev, B43_NPHY_MIMOCFG, mimocfg);
 347 }
 348
 349 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/Chains */
 350 static void b43_nphy_update_txrx_chain(struct b43_wldev *dev)
 351 {
 352         struct b43_phy_n *nphy = dev->phy.n;
 353
 354         bool override = false;
 355         u16 chain = 0x33;
 356
 357         if (nphy->txrx_chain == 0) {
 358                 chain = 0x11;
 359                 override = true;
 360         } else if (nphy->txrx_chain == 1) {
 361                 chain = 0x22;
 362                 override = true;
 363         }
 364
 365         b43_phy_maskset(dev, B43_NPHY_RFSEQCA,
 366                         ~(B43_NPHY_RFSEQCA_TXEN | B43_NPHY_RFSEQCA_RXEN),
 367                         chain);
 368
 369         if (override)
 370                 b43_phy_set(dev, B43_NPHY_RFSEQMODE,
 371                                 B43_NPHY_RFSEQMODE_CAOVER);
 372         else
 373                 b43_phy_mask(dev, B43_NPHY_RFSEQMODE,
 374                                 ~B43_NPHY_RFSEQMODE_CAOVER);
 375 }
 376
 377 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxIqEst */
 378 static void b43_nphy_rx_iq_est(struct b43_wldev *dev, struct nphy_iq_est *est,
 379                                 u16 samps, u8 time, bool wait)
 380 {
 381         int i;
 382         u16 tmp;
 383
 384         b43_phy_write(dev, B43_NPHY_IQEST_SAMCNT, samps);
 385         b43_phy_maskset(dev, B43_NPHY_IQEST_WT, ~B43_NPHY_IQEST_WT_VAL, time);
 386         if (wait)
 387                 b43_phy_set(dev, B43_NPHY_IQEST_CMD, B43_NPHY_IQEST_CMD_MODE);
 388         else
 389                 b43_phy_mask(dev, B43_NPHY_IQEST_CMD, ~B43_NPHY_IQEST_CMD_MODE);
 390
 391         b43_phy_set(dev, B43_NPHY_IQEST_CMD, B43_NPHY_IQEST_CMD_START);
 392
 393         for (i = 1000; i; i--) {
 394                 tmp = b43_phy_read(dev, B43_NPHY_IQEST_CMD);
 395                 if (!(tmp & B43_NPHY_IQEST_CMD_START)) {
 396                         est->i0_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_IPACC_HI0) << 16) |
 397                                         b43_phy_read(dev, B43_NPHY_IQEST_IPACC_LO0);
 398                         est->q0_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_QPACC_HI0) << 16) |
 399                                         b43_phy_read(dev, B43_NPHY_IQEST_QPACC_LO0);
 400                         est->iq0_prod = (b43_phy_read(dev, B43_NPHY_IQEST_IQACC_HI0) << 16) |
 401                                         b43_phy_read(dev, B43_NPHY_IQEST_IQACC_LO0);
 402
 403                         est->i1_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_IPACC_HI1) << 16) |
 404                                         b43_phy_read(dev, B43_NPHY_IQEST_IPACC_LO1);
 405                         est->q1_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_QPACC_HI1) << 16) |
 406                                         b43_phy_read(dev, B43_NPHY_IQEST_QPACC_LO1);
 407                         est->iq1_prod = (b43_phy_read(dev, B43_NPHY_IQEST_IQACC_HI1) << 16) |
 408                                         b43_phy_read(dev, B43_NPHY_IQEST_IQACC_LO1);
 409                         return;
 410                 }
 411                 udelay(10);
 412         }
 413         memset(est, 0, sizeof(*est));
 414 }
 415
 416 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxIqCoeffs */
 417 static void b43_nphy_rx_iq_coeffs(struct b43_wldev *dev, bool write,
 418                                         struct b43_phy_n_iq_comp *pcomp)
 419 {
 420         if (write) {
 421                 b43_phy_write(dev, B43_NPHY_C1_RXIQ_COMPA0, pcomp->a0);
 422                 b43_phy_write(dev, B43_NPHY_C1_RXIQ_COMPB0, pcomp->b0);
 423                 b43_phy_write(dev, B43_NPHY_C2_RXIQ_COMPA1, pcomp->a1);
 424                 b43_phy_write(dev, B43_NPHY_C2_RXIQ_COMPB1, pcomp->b1);
 425         } else {
 426                 pcomp->a0 = b43_phy_read(dev, B43_NPHY_C1_RXIQ_COMPA0);
 427                 pcomp->b0 = b43_phy_read(dev, B43_NPHY_C1_RXIQ_COMPB0);
 428                 pcomp->a1 = b43_phy_read(dev, B43_NPHY_C2_RXIQ_COMPA1);
 429                 pcomp->b1 = b43_phy_read(dev, B43_NPHY_C2_RXIQ_COMPB1);
 430         }
 431 }
 432
 433 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCalPhyCleanup */
 434 static void b43_nphy_rx_cal_phy_cleanup(struct b43_wldev *dev, u8 core)
 435 {
 436         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
 437
 438         b43_phy_write(dev, B43_NPHY_RFSEQCA, regs[0]);
 439         if (core == 0) {
 440                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, regs[1]);
 441                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, regs[2]);
 442         } else {
 443                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, regs[1]);
 444                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[2]);
 445         }
 446         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[3]);
 447         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[4]);
 448         b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO1, regs[5]);
 449         b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO2, regs[6]);
 450         b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S1, regs[7]);
 451         b43_phy_write(dev, B43_NPHY_RFCTL_OVER, regs[8]);
 452         b43_phy_write(dev, B43_NPHY_PAPD_EN0, regs[9]);
 453         b43_phy_write(dev, B43_NPHY_PAPD_EN1, regs[10]);
 454 }
 455
 456 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCalPhySetup */
 457 static void b43_nphy_rx_cal_phy_setup(struct b43_wldev *dev, u8 core)
 458 {
 459         u8 rxval, txval;
 460         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
 461
 462         regs[0] = b43_phy_read(dev, B43_NPHY_RFSEQCA);
 463         if (core == 0) {
 464                 regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
 465                 regs[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
 466         } else {
 467                 regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
 468                 regs[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
 469         }
 470         regs[3] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
 471         regs[4] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
 472         regs[5] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO1);
 473         regs[6] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO2);
 474         regs[7] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B1S1);
 475         regs[8] = b43_phy_read(dev, B43_NPHY_RFCTL_OVER);
 476         regs[9] = b43_phy_read(dev, B43_NPHY_PAPD_EN0);
 477         regs[10] = b43_phy_read(dev, B43_NPHY_PAPD_EN1);
 478
 479         b43_phy_mask(dev, B43_NPHY_PAPD_EN0, ~0x0001);
 480         b43_phy_mask(dev, B43_NPHY_PAPD_EN1, ~0x0001);
 481
 482         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, (u16)~B43_NPHY_RFSEQCA_RXDIS,
 483                         ((1 - core) << B43_NPHY_RFSEQCA_RXDIS_SHIFT));
 484         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXEN,
 485                         ((1 - core) << B43_NPHY_RFSEQCA_TXEN_SHIFT));
 486         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_RXEN,
 487                         (core << B43_NPHY_RFSEQCA_RXEN_SHIFT));
 488         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXDIS,
 489                         (core << B43_NPHY_RFSEQCA_TXDIS_SHIFT));
 490
 491         if (core == 0) {
 492                 b43_phy_mask(dev, B43_NPHY_AFECTL_C1, ~0x0007);
 493                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER1, 0x0007);
 494         } else {
 495                 b43_phy_mask(dev, B43_NPHY_AFECTL_C2, ~0x0007);
 496                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0007);
 497         }
 498
 499         /* TODO: Call N PHY RF Ctrl Intc Override with 2, 0, 3 as arguments */
 500         /* TODO: Call N PHY RF Intc Override with 8, 0, 3, 0 as arguments */
 501         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
 502
 503         if (core == 0) {
 504                 rxval = 1;
 505                 txval = 8;
 506         } else {
 507                 rxval = 4;
 508                 txval = 2;
 509         }
 510
 511         /* TODO: Call N PHY RF Ctrl Intc Override with 1, rxval, (core + 1) */
 512         /* TODO: Call N PHY RF Ctrl Intc Override with 1, txval, (2 - core) */
 513 }
 514
 515 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalcRxIqComp */
 516 static void b43_nphy_calc_rx_iq_comp(struct b43_wldev *dev, u8 mask)
 517 {
 518         int i;
 519         s32 iq;
 520         u32 ii;
 521         u32 qq;
 522         int iq_nbits, qq_nbits;
 523         int arsh, brsh;
 524         u16 tmp, a, b;
 525
 526         struct nphy_iq_est est;
 527         struct b43_phy_n_iq_comp old;
 528         struct b43_phy_n_iq_comp new = { };
 529         bool error = false;
 530
 531         if (mask == 0)
 532                 return;
 533
 534         b43_nphy_rx_iq_coeffs(dev, false, &old);
 535         b43_nphy_rx_iq_coeffs(dev, true, &new);
 536         b43_nphy_rx_iq_est(dev, &est, 0x4000, 32, false);
 537         new = old;
 538
 539         for (i = 0; i < 2; i++) {
 540                 if (i == 0 && (mask & 1)) {
 541                         iq = est.iq0_prod;
 542                         ii = est.i0_pwr;
 543                         qq = est.q0_pwr;
 544                 } else if (i == 1 && (mask & 2)) {
 545                         iq = est.iq1_prod;
 546                         ii = est.i1_pwr;
 547                         qq = est.q1_pwr;
 548                 } else {
 549                         B43_WARN_ON(1);
 550                         continue;
 551                 }
 552
 553                 if (ii + qq < 2) {
 554                         error = true;
 555                         break;
 556                 }
 557
 558                 iq_nbits = fls(abs(iq));
 559                 qq_nbits = fls(qq);
 560
 561                 arsh = iq_nbits - 20;
 562                 if (arsh >= 0) {
 563                         a = -((iq << (30 - iq_nbits)) + (ii >> (1 + arsh)));
 564                         tmp = ii >> arsh;
 565                 } else {
 566                         a = -((iq << (30 - iq_nbits)) + (ii << (-1 - arsh)));
 567                         tmp = ii << -arsh;
 568                 }
 569                 if (tmp == 0) {
 570                         error = true;
 571                         break;
 572                 }
 573                 a /= tmp;
 574
 575                 brsh = qq_nbits - 11;
 576                 if (brsh >= 0) {
 577                         b = (qq << (31 - qq_nbits));
 578                         tmp = ii >> brsh;
 579                 } else {
 580                         b = (qq << (31 - qq_nbits));
 581                         tmp = ii << -brsh;
 582                 }
 583                 if (tmp == 0) {
 584                         error = true;
 585                         break;
 586                 }
 587                 b = int_sqrt(b / tmp - a * a) - (1 << 10);
 588
 589                 if (i == 0 && (mask & 0x1)) {
 590                         if (dev->phy.rev >= 3) {
 591                                 new.a0 = a & 0x3FF;
 592                                 new.b0 = b & 0x3FF;
 593                         } else {
 594                                 new.a0 = b & 0x3FF;
 595                                 new.b0 = a & 0x3FF;
 596                         }
 597                 } else if (i == 1 && (mask & 0x2)) {
 598                         if (dev->phy.rev >= 3) {
 599                                 new.a1 = a & 0x3FF;
 600                                 new.b1 = b & 0x3FF;
 601                         } else {
 602                                 new.a1 = b & 0x3FF;
 603                                 new.b1 = a & 0x3FF;
 604                         }
 605                 }
 606         }
 607
 608         if (error)
 609                 new = old;
 610
 611         b43_nphy_rx_iq_coeffs(dev, true, &new);
 612 }
 613
 614 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxIqWar */
 615 static void b43_nphy_tx_iq_workaround(struct b43_wldev *dev)
 616 {
 617         u16 array[4];
 618         int i;
 619
 620         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x3C50);
 621         for (i = 0; i < 4; i++)
 622                 array[i] = b43_phy_read(dev, B43_NPHY_TABLE_DATALO);
 623
 624         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW0, array[0]);
 625         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW1, array[1]);
 626         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW2, array[2]);
 627         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW3, array[3]);
 628 }
 629
 630 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/clip-detection */
 631 static void b43_nphy_write_clip_detection(struct b43_wldev *dev, u16 *clip_st)
 632 {
 633         b43_phy_write(dev, B43_NPHY_C1_CLIP1THRES, clip_st[0]);
 634         b43_phy_write(dev, B43_NPHY_C2_CLIP1THRES, clip_st[1]);
 635 }
 636
 637 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/clip-detection */
 638 static void b43_nphy_read_clip_detection(struct b43_wldev *dev, u16 *clip_st)
 639 {
 640         clip_st[0] = b43_phy_read(dev, B43_NPHY_C1_CLIP1THRES);
 641         clip_st[1] = b43_phy_read(dev, B43_NPHY_C2_CLIP1THRES);
 642 }
 643
 644 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/classifier */
 645 static u16 b43_nphy_classifier(struct b43_wldev *dev, u16 mask, u16 val)
 646 {
 647         u16 tmp;
 648
 649         if (dev->dev->id.revision == 16)
 650                 b43_mac_suspend(dev);
 651
 652         tmp = b43_phy_read(dev, B43_NPHY_CLASSCTL);
 653         tmp &= (B43_NPHY_CLASSCTL_CCKEN | B43_NPHY_CLASSCTL_OFDMEN |
 654                 B43_NPHY_CLASSCTL_WAITEDEN);
 655         tmp &= ~mask;
 656         tmp |= (val & mask);
 657         b43_phy_maskset(dev, B43_NPHY_CLASSCTL, 0xFFF8, tmp);
 658
 659         if (dev->dev->id.revision == 16)
 660                 b43_mac_enable(dev);
 661
 662         return tmp;
 663 }
 664
 665 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/carriersearch */
 666 static void b43_nphy_stay_in_carrier_search(struct b43_wldev *dev, bool enable)
 667 {
 668         struct b43_phy *phy = &dev->phy;
 669         struct b43_phy_n *nphy = phy->n;
 670
 671         if (enable) {
 672                 u16 clip[] = { 0xFFFF, 0xFFFF };
 673                 if (nphy->deaf_count++ == 0) {
 674                         nphy->classifier_state = b43_nphy_classifier(dev, 0, 0);
 675                         b43_nphy_classifier(dev, 0x7, 0);
 676                         b43_nphy_read_clip_detection(dev, nphy->clip_state);
 677                         b43_nphy_write_clip_detection(dev, clip);
 678                 }
 679                 b43_nphy_reset_cca(dev);
 680         } else {
 681                 if (--nphy->deaf_count == 0) {
 682                         b43_nphy_classifier(dev, 0x7, nphy->classifier_state);
 683                         b43_nphy_write_clip_detection(dev, nphy->clip_state);
 684                 }
 685         }
 686 }
 687
 688 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/stop-playback */
 689 static void b43_nphy_stop_playback(struct b43_wldev *dev)
 690 {
 691         struct b43_phy_n *nphy = dev->phy.n;
 692         u16 tmp;
 693
 694         if (nphy->hang_avoid)
 695                 b43_nphy_stay_in_carrier_search(dev, 1);
 696
 697         tmp = b43_phy_read(dev, B43_NPHY_SAMP_STAT);
 698         if (tmp & 0x1)
 699                 b43_phy_set(dev, B43_NPHY_SAMP_CMD, B43_NPHY_SAMP_CMD_STOP);
 700         else if (tmp & 0x2)
 701                 b43_phy_mask(dev, B43_NPHY_IQLOCAL_CMDGCTL, (u16)~0x8000);
 702
 703         b43_phy_mask(dev, B43_NPHY_SAMP_CMD, ~0x0004);
 704
 705         if (nphy->bb_mult_save & 0x80000000) {
 706                 tmp = nphy->bb_mult_save & 0xFFFF;
 707                 b43_ntab_write(dev, B43_NTAB16(15, 87), tmp);
 708                 nphy->bb_mult_save = 0;
 709         }
 710
 711         if (nphy->hang_avoid)
 712                 b43_nphy_stay_in_carrier_search(dev, 0);
 713 }
 714
 715 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/WorkaroundsGainCtrl */
 716 static void b43_nphy_gain_crtl_workarounds(struct b43_wldev *dev)
 717 {
 718         struct b43_phy_n *nphy = dev->phy.n;
 719         u8 i, j;
 720         u8 code;
 721
 722         /* TODO: for PHY >= 3
 723         s8 *lna1_gain, *lna2_gain;
 724         u8 *gain_db, *gain_bits;
 725         u16 *rfseq_init;
 726         u8 lpf_gain[6] = { 0x00, 0x06, 0x0C, 0x12, 0x12, 0x12 };
 727         u8 lpf_bits[6] = { 0, 1, 2, 3, 3, 3 };
 728         */
 729
 730         u8 rfseq_events[3] = { 6, 8, 7 };
 731         u8 rfseq_delays[3] = { 10, 30, 1 };
 732
 733         if (dev->phy.rev >= 3) {
 734                 /* TODO */
 735         } else {
 736                 /* Set Clip 2 detect */
 737                 b43_phy_set(dev, B43_NPHY_C1_CGAINI,
 738                                 B43_NPHY_C1_CGAINI_CL2DETECT);
 739                 b43_phy_set(dev, B43_NPHY_C2_CGAINI,
 740                                 B43_NPHY_C2_CGAINI_CL2DETECT);
 741
 742                 /* Set narrowband clip threshold */
 743                 b43_phy_set(dev, B43_NPHY_C1_NBCLIPTHRES, 0x84);
 744                 b43_phy_set(dev, B43_NPHY_C2_NBCLIPTHRES, 0x84);
 745
 746                 if (!dev->phy.is_40mhz) {
 747                         /* Set dwell lengths */
 748                         b43_phy_set(dev, B43_NPHY_CLIP1_NBDWELL_LEN, 0x002B);
 749                         b43_phy_set(dev, B43_NPHY_CLIP2_NBDWELL_LEN, 0x002B);
 750                         b43_phy_set(dev, B43_NPHY_W1CLIP1_DWELL_LEN, 0x0009);
 751                         b43_phy_set(dev, B43_NPHY_W1CLIP2_DWELL_LEN, 0x0009);
 752                 }
 753
 754                 /* Set wideband clip 2 threshold */
 755                 b43_phy_maskset(dev, B43_NPHY_C1_CLIPWBTHRES,
 756                                 ~B43_NPHY_C1_CLIPWBTHRES_CLIP2,
 757                                 21);
 758                 b43_phy_maskset(dev, B43_NPHY_C2_CLIPWBTHRES,
 759                                 ~B43_NPHY_C2_CLIPWBTHRES_CLIP2,
 760                                 21);
 761
 762                 if (!dev->phy.is_40mhz) {
 763                         b43_phy_maskset(dev, B43_NPHY_C1_CGAINI,
 764                                 ~B43_NPHY_C1_CGAINI_GAINBKOFF, 0x1);
 765                         b43_phy_maskset(dev, B43_NPHY_C2_CGAINI,
 766                                 ~B43_NPHY_C2_CGAINI_GAINBKOFF, 0x1);
 767                         b43_phy_maskset(dev, B43_NPHY_C1_CCK_CGAINI,
 768                                 ~B43_NPHY_C1_CCK_CGAINI_GAINBKOFF, 0x1);
 769                         b43_phy_maskset(dev, B43_NPHY_C2_CCK_CGAINI,
 770                                 ~B43_NPHY_C2_CCK_CGAINI_GAINBKOFF, 0x1);
 771                 }
 772
 773                 b43_phy_set(dev, B43_NPHY_CCK_SHIFTB_REF, 0x809C);
 774
 775                 if (nphy->gain_boost) {
 776                         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ &&
 777                             dev->phy.is_40mhz)
 778                                 code = 4;
 779                         else
 780                                 code = 5;
 781                 } else {
 782                         code = dev->phy.is_40mhz ? 6 : 7;
 783                 }
 784
 785                 /* Set HPVGA2 index */
 786                 b43_phy_maskset(dev, B43_NPHY_C1_INITGAIN,
 787                                 ~B43_NPHY_C1_INITGAIN_HPVGA2,
 788                                 code << B43_NPHY_C1_INITGAIN_HPVGA2_SHIFT);
 789                 b43_phy_maskset(dev, B43_NPHY_C2_INITGAIN,
 790                                 ~B43_NPHY_C2_INITGAIN_HPVGA2,
 791                                 code << B43_NPHY_C2_INITGAIN_HPVGA2_SHIFT);
 792
 793                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x1D06);
 794                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
 795                                         (code << 8 | 0x7C));
 796                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
 797                                         (code << 8 | 0x7C));
 798
 799                 /* TODO: b43_nphy_adjust_lna_gain_table(dev); */
 800
 801                 if (nphy->elna_gain_config) {
 802                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x0808);
 803                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0);
 804                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 805                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 806                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 807
 808                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x0C08);
 809                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0);
 810                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 811                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 812                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
 813
 814                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x1D06);
 815                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
 816                                         (code << 8 | 0x74));
 817                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
 818                                         (code << 8 | 0x74));
 819                 }
 820
 821                 if (dev->phy.rev == 2) {
 822                         for (i = 0; i < 4; i++) {
 823                                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
 824                                                 (0x0400 * i) + 0x0020);
 825                                 for (j = 0; j < 21; j++)
 826                                         b43_phy_write(dev,
 827                                                 B43_NPHY_TABLE_DATALO, 3 * j);
 828                         }
 829
 830                         /* TODO: b43_nphy_set_rf_sequence(dev, 5,
 831                                         rfseq_events, rfseq_delays, 3);*/
 832                         b43_phy_maskset(dev, B43_NPHY_OVER_DGAIN1,
 833                                 (u16)~B43_NPHY_OVER_DGAIN_CCKDGECV,
 834                                 0x5A << B43_NPHY_OVER_DGAIN_CCKDGECV_SHIFT);
 835
 836                         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
 837                                 b43_phy_maskset(dev, B43_PHY_N(0xC5D),
 838                                                 0xFF80, 4);
 839                 }
 840         }
 841 }
 842
 843 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/Workarounds */
 844 static void b43_nphy_workarounds(struct b43_wldev *dev)
 845 {
 846         struct ssb_bus *bus = dev->dev->bus;
 847         struct b43_phy *phy = &dev->phy;
 848         struct b43_phy_n *nphy = phy->n;
 849
 850         u8 events1[7] = { 0x0, 0x1, 0x2, 0x8, 0x4, 0x5, 0x3 };
 851         u8 delays1[7] = { 0x8, 0x6, 0x6, 0x2, 0x4, 0x3C, 0x1 };
 852
 853         u8 events2[7] = { 0x0, 0x3, 0x5, 0x4, 0x2, 0x1, 0x8 };
 854         u8 delays2[7] = { 0x8, 0x6, 0x2, 0x4, 0x4, 0x6, 0x1 };
 855
 856         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
 857                 b43_nphy_classifier(dev, 1, 0);
 858         else
 859                 b43_nphy_classifier(dev, 1, 1);
 860
 861         if (nphy->hang_avoid)
 862                 b43_nphy_stay_in_carrier_search(dev, 1);
 863
 864         b43_phy_set(dev, B43_NPHY_IQFLIP,
 865                     B43_NPHY_IQFLIP_ADC1 | B43_NPHY_IQFLIP_ADC2);
 866
 867         if (dev->phy.rev >= 3) {
 868                 /* TODO */
 869         } else {
 870                 if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ &&
 871                     nphy->band5g_pwrgain) {
 872                         b43_radio_mask(dev, B2055_C1_TX_RF_SPARE, ~0x8);
 873                         b43_radio_mask(dev, B2055_C2_TX_RF_SPARE, ~0x8);
 874                 } else {
 875                         b43_radio_set(dev, B2055_C1_TX_RF_SPARE, 0x8);
 876                         b43_radio_set(dev, B2055_C2_TX_RF_SPARE, 0x8);
 877                 }
 878
 879                 /* TODO: convert to b43_ntab_write? */
 880                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2000);
 881                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x000A);
 882                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2010);
 883                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x000A);
 884                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2002);
 885                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0xCDAA);
 886                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2012);
 887                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0xCDAA);
 888
 889                 if (dev->phy.rev < 2) {
 890                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2008);
 891                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0000);
 892                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2018);
 893                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0000);
 894                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2007);
 895                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x7AAB);
 896                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2017);
 897                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x7AAB);
 898                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2006);
 899                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0800);
 900                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x2016);
 901                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0800);
 902                 }
 903
 904                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO1, 0x2D8);
 905                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1, 0x301);
 906                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO2, 0x2D8);
 907                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2, 0x301);
 908
 909                 if (bus->sprom.boardflags2_lo & 0x100 &&
 910                     bus->boardinfo.type == 0x8B) {
 911                         delays1[0] = 0x1;
 912                         delays1[5] = 0x14;
 913                 }
 914                 /*TODO:b43_nphy_set_rf_sequence(dev, 0, events1, delays1, 7);*/
 915                 /*TODO:b43_nphy_set_rf_sequence(dev, 1, events2, delays2, 7);*/
 916
 917                 b43_nphy_gain_crtl_workarounds(dev);
 918
 919                 if (dev->phy.rev < 2) {
 920                         if (b43_phy_read(dev, B43_NPHY_RXCTL) & 0x2)
 921                                 ; /*TODO: b43_mhf(dev, 2, 0x0010, 0x0010, 3);*/
 922                 } else if (dev->phy.rev == 2) {
 923                         b43_phy_write(dev, B43_NPHY_CRSCHECK2, 0);
 924                         b43_phy_write(dev, B43_NPHY_CRSCHECK3, 0);
 925                 }
 926
 927                 if (dev->phy.rev < 2)
 928                         b43_phy_mask(dev, B43_NPHY_SCRAM_SIGCTL,
 929                                         ~B43_NPHY_SCRAM_SIGCTL_SCM);
 930
 931                 /* Set phase track alpha and beta */
 932                 b43_phy_write(dev, B43_NPHY_PHASETR_A0, 0x125);
 933                 b43_phy_write(dev, B43_NPHY_PHASETR_A1, 0x1B3);
 934                 b43_phy_write(dev, B43_NPHY_PHASETR_A2, 0x105);
 935                 b43_phy_write(dev, B43_NPHY_PHASETR_B0, 0x16E);
 936                 b43_phy_write(dev, B43_NPHY_PHASETR_B1, 0xCD);
 937                 b43_phy_write(dev, B43_NPHY_PHASETR_B2, 0x20);
 938
 939                 b43_phy_mask(dev, B43_NPHY_PIL_DW1,
 940                                 (u16)~B43_NPHY_PIL_DW_64QAM);
 941                 b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B1, 0xB5);
 942                 b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B2, 0xA4);
 943                 b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B3, 0x00);
 944
 945                 if (dev->phy.rev == 2)
 946                         b43_phy_set(dev, B43_NPHY_FINERX2_CGC,
 947                                         B43_NPHY_FINERX2_CGC_DECGC);
 948         }
 949
 950         if (nphy->hang_avoid)
 951                 b43_nphy_stay_in_carrier_search(dev, 0);
 952 }
 953
 954 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GenLoadSamples */
 955 static u16 b43_nphy_gen_load_samples(struct b43_wldev *dev, u32 freq, u16 max,
 956                                         bool test)
 957 {
 958         int i;
 959         u16 bw, len, rot, angle;
 960         struct b43_c32 *samples;
 961
 962
 963         bw = (dev->phy.is_40mhz) ? 40 : 20;
 964         len = bw << 3;
 965
 966         if (test) {
 967                 if (b43_phy_read(dev, B43_NPHY_BBCFG) & B43_NPHY_BBCFG_RSTRX)
 968                         bw = 82;
 969                 else
 970                         bw = 80;
 971
 972                 if (dev->phy.is_40mhz)
 973                         bw <<= 1;
 974
 975                 len = bw << 1;
 976         }
 977
 978         samples = kzalloc(len * sizeof(struct b43_c32), GFP_KERNEL);
 979         rot = (((freq * 36) / bw) << 16) / 100;
 980         angle = 0;
 981
 982         for (i = 0; i < len; i++) {
 983                 samples[i] = b43_cordic(angle);
 984                 angle += rot;
 985                 samples[i].q = CORDIC_CONVERT(samples[i].q * max);
 986                 samples[i].i = CORDIC_CONVERT(samples[i].i * max);
 987         }
 988
 989         /* TODO: Call N PHY Load Sample Table with buffer, len as arguments */
 990         kfree(samples);
 991         return len;
 992 }
 993
 994 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RunSamples */
 995 static void b43_nphy_run_samples(struct b43_wldev *dev, u16 samps, u16 loops,
 996                                         u16 wait, bool iqmode, bool dac_test)
 997 {
 998         struct b43_phy_n *nphy = dev->phy.n;
 999         int i;
1000         u16 seq_mode;
1001         u32 tmp;
1002
1003         if (nphy->hang_avoid)
1004                 b43_nphy_stay_in_carrier_search(dev, true);
1005
1006         if ((nphy->bb_mult_save & 0x80000000) == 0) {
1007                 tmp = b43_ntab_read(dev, B43_NTAB16(15, 87));
1008                 nphy->bb_mult_save = (tmp & 0xFFFF) | 0x80000000;
1009         }
1010
1011         if (!dev->phy.is_40mhz)
1012                 tmp = 0x6464;
1013         else
1014                 tmp = 0x4747;
1015         b43_ntab_write(dev, B43_NTAB16(15, 87), tmp);
1016
1017         if (nphy->hang_avoid)
1018                 b43_nphy_stay_in_carrier_search(dev, false);
1019
1020         b43_phy_write(dev, B43_NPHY_SAMP_DEPCNT, (samps - 1));
1021
1022         if (loops != 0xFFFF)
1023                 b43_phy_write(dev, B43_NPHY_SAMP_LOOPCNT, (loops - 1));
1024         else
1025                 b43_phy_write(dev, B43_NPHY_SAMP_LOOPCNT, loops);
1026
1027         b43_phy_write(dev, B43_NPHY_SAMP_WAITCNT, wait);
1028
1029         seq_mode = b43_phy_read(dev, B43_NPHY_RFSEQMODE);
1030
1031         b43_phy_set(dev, B43_NPHY_RFSEQMODE, B43_NPHY_RFSEQMODE_CAOVER);
1032         if (iqmode) {
1033                 b43_phy_mask(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x7FFF);
1034                 b43_phy_set(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x8000);
1035         } else {
1036                 if (dac_test)
1037                         b43_phy_write(dev, B43_NPHY_SAMP_CMD, 5);
1038                 else
1039                         b43_phy_write(dev, B43_NPHY_SAMP_CMD, 1);
1040         }
1041         for (i = 0; i < 100; i++) {
1042                 if (b43_phy_read(dev, B43_NPHY_RFSEQST) & 1) {
1043                         i = 0;
1044                         break;
1045                 }
1046                 udelay(10);
1047         }
1048         if (i)
1049                 b43err(dev->wl, "run samples timeout\n");
1050
1051         b43_phy_write(dev, B43_NPHY_RFSEQMODE, seq_mode);
1052 }
1053
1054 /*
1055  * Transmits a known value for LO calibration
1056  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/TXTone
1057  */
1058 static int b43_nphy_tx_tone(struct b43_wldev *dev, u32 freq, u16 max_val,
1059                                 bool iqmode, bool dac_test)
1060 {
1061         u16 samp = b43_nphy_gen_load_samples(dev, freq, max_val, dac_test);
1062         if (samp == 0)
1063                 return -1;
1064         b43_nphy_run_samples(dev, samp, 0xFFFF, 0, iqmode, dac_test);
1065         return 0;
1066 }
1067
1068 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlCoefSetup */
1069 static void b43_nphy_tx_pwr_ctrl_coef_setup(struct b43_wldev *dev)
1070 {
1071         struct b43_phy_n *nphy = dev->phy.n;
1072         int i, j;
1073         u32 tmp;
1074         u32 cur_real, cur_imag, real_part, imag_part;
1075
1076         u16 buffer[7];
1077
1078         if (nphy->hang_avoid)
1079                 b43_nphy_stay_in_carrier_search(dev, true);
1080
1081         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 7, buffer);
1082
1083         for (i = 0; i < 2; i++) {
1084                 tmp = ((buffer[i * 2] & 0x3FF) << 10) |
1085                         (buffer[i * 2 + 1] & 0x3FF);
1086                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
1087                                 (((i + 26) << 10) | 320));
1088                 for (j = 0; j < 128; j++) {
1089                         b43_phy_write(dev, B43_NPHY_TABLE_DATAHI,
1090                                         ((tmp >> 16) & 0xFFFF));
1091                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
1092                                         (tmp & 0xFFFF));
1093                 }
1094         }
1095
1096         for (i = 0; i < 2; i++) {
1097                 tmp = buffer[5 + i];
1098                 real_part = (tmp >> 8) & 0xFF;
1099                 imag_part = (tmp & 0xFF);
1100                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
1101                                 (((i + 26) << 10) | 448));
1102
1103                 if (dev->phy.rev >= 3) {
1104                         cur_real = real_part;
1105                         cur_imag = imag_part;
1106                         tmp = ((cur_real & 0xFF) << 8) | (cur_imag & 0xFF);
1107                 }
1108
1109                 for (j = 0; j < 128; j++) {
1110                         if (dev->phy.rev < 3) {
1111                                 cur_real = (real_part * loscale[j] + 128) >> 8;
1112                                 cur_imag = (imag_part * loscale[j] + 128) >> 8;
1113                                 tmp = ((cur_real & 0xFF) << 8) |
1114                                         (cur_imag & 0xFF);
1115                         }
1116                         b43_phy_write(dev, B43_NPHY_TABLE_DATAHI,
1117                                         ((tmp >> 16) & 0xFFFF));
1118                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
1119                                         (tmp & 0xFFFF));
1120                 }
1121         }
1122
1123         if (dev->phy.rev >= 3) {
1124                 b43_shm_write16(dev, B43_SHM_SHARED,
1125                                 B43_SHM_SH_NPHY_TXPWR_INDX0, 0xFFFF);
1126                 b43_shm_write16(dev, B43_SHM_SHARED,
1127                                 B43_SHM_SH_NPHY_TXPWR_INDX1, 0xFFFF);
1128         }
1129
1130         if (nphy->hang_avoid)
1131                 b43_nphy_stay_in_carrier_search(dev, false);
1132 }
1133
1134 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ForceRFSeq */
1135 static void b43_nphy_force_rf_sequence(struct b43_wldev *dev,
1136                                        enum b43_nphy_rf_sequence seq)
1137 {
1138         static const u16 trigger[] = {
1139                 [B43_RFSEQ_RX2TX]               = B43_NPHY_RFSEQTR_RX2TX,
1140                 [B43_RFSEQ_TX2RX]               = B43_NPHY_RFSEQTR_TX2RX,
1141                 [B43_RFSEQ_RESET2RX]            = B43_NPHY_RFSEQTR_RST2RX,
1142                 [B43_RFSEQ_UPDATE_GAINH]        = B43_NPHY_RFSEQTR_UPGH,
1143                 [B43_RFSEQ_UPDATE_GAINL]        = B43_NPHY_RFSEQTR_UPGL,
1144                 [B43_RFSEQ_UPDATE_GAINU]        = B43_NPHY_RFSEQTR_UPGU,
1145         };
1146         int i;
1147         u16 seq_mode = b43_phy_read(dev, B43_NPHY_RFSEQMODE);
1148
1149         B43_WARN_ON(seq >= ARRAY_SIZE(trigger));
1150
1151         b43_phy_set(dev, B43_NPHY_RFSEQMODE,
1152                     B43_NPHY_RFSEQMODE_CAOVER | B43_NPHY_RFSEQMODE_TROVER);
1153         b43_phy_set(dev, B43_NPHY_RFSEQTR, trigger[seq]);
1154         for (i = 0; i < 200; i++) {
1155                 if (!(b43_phy_read(dev, B43_NPHY_RFSEQST) & trigger[seq]))
1156                         goto ok;
1157                 msleep(1);
1158         }
1159         b43err(dev->wl, "RF sequence status timeout\n");
1160 ok:
1161         b43_phy_write(dev, B43_NPHY_RFSEQMODE, seq_mode);
1162 }
1163
1164 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlOverride */
1165 static void b43_nphy_rf_control_override(struct b43_wldev *dev, u16 field,
1166                                                 u16 value, u8 core, bool off)
1167 {
1168         int i;
1169         u8 index = fls(field);
1170         u8 addr, en_addr, val_addr;
1171         /* we expect only one bit set */
1172         B43_WARN_ON(field & (~(1 << (index - 1))));
1173
1174         if (dev->phy.rev >= 3) {
1175                 const struct nphy_rf_control_override_rev3 *rf_ctrl;
1176                 for (i = 0; i < 2; i++) {
1177                         if (index == 0 || index == 16) {
1178                                 b43err(dev->wl,
1179                                         "Unsupported RF Ctrl Override call\n");
1180                                 return;
1181                         }
1182
1183                         rf_ctrl = &tbl_rf_control_override_rev3[index - 1];
1184                         en_addr = B43_PHY_N((i == 0) ?
1185                                 rf_ctrl->en_addr0 : rf_ctrl->en_addr1);
1186                         val_addr = B43_PHY_N((i == 0) ?
1187                                 rf_ctrl->val_addr0 : rf_ctrl->val_addr1);
1188
1189                         if (off) {
1190                                 b43_phy_mask(dev, en_addr, ~(field));
1191                                 b43_phy_mask(dev, val_addr,
1192                                                 ~(rf_ctrl->val_mask));
1193                         } else {
1194                                 if (core == 0 || ((1 << core) & i) != 0) {
1195                                         b43_phy_set(dev, en_addr, field);
1196                                         b43_phy_maskset(dev, val_addr,
1197                                                 ~(rf_ctrl->val_mask),
1198                                                 (value << rf_ctrl->val_shift));
1199                                 }
1200                         }
1201                 }
1202         } else {
1203                 const struct nphy_rf_control_override_rev2 *rf_ctrl;
1204                 if (off) {
1205                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, ~(field));
1206                         value = 0;
1207                 } else {
1208                         b43_phy_set(dev, B43_NPHY_RFCTL_OVER, field);
1209                 }
1210
1211                 for (i = 0; i < 2; i++) {
1212                         if (index <= 1 || index == 16) {
1213                                 b43err(dev->wl,
1214                                         "Unsupported RF Ctrl Override call\n");
1215                                 return;
1216                         }
1217
1218                         if (index == 2 || index == 10 ||
1219                             (index >= 13 && index <= 15)) {
1220                                 core = 1;
1221                         }
1222
1223                         rf_ctrl = &tbl_rf_control_override_rev2[index - 2];
1224                         addr = B43_PHY_N((i == 0) ?
1225                                 rf_ctrl->addr0 : rf_ctrl->addr1);
1226
1227                         if ((core & (1 << i)) != 0)
1228                                 b43_phy_maskset(dev, addr, ~(rf_ctrl->bmask),
1229                                                 (value << rf_ctrl->shift));
1230
1231                         b43_phy_set(dev, B43_NPHY_RFCTL_OVER, 0x1);
1232                         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
1233                                         B43_NPHY_RFCTL_CMD_START);
1234                         udelay(1);
1235                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, 0xFFFE);
1236                 }
1237         }
1238 }
1239
1240 static void b43_nphy_bphy_init(struct b43_wldev *dev)
1241 {
1242         unsigned int i;
1243         u16 val;
1244
1245         val = 0x1E1F;
1246         for (i = 0; i < 14; i++) {
1247                 b43_phy_write(dev, B43_PHY_N_BMODE(0x88 + i), val);
1248                 val -= 0x202;
1249         }
1250         val = 0x3E3F;
1251         for (i = 0; i < 16; i++) {
1252                 b43_phy_write(dev, B43_PHY_N_BMODE(0x97 + i), val);
1253                 val -= 0x202;
1254         }
1255         b43_phy_write(dev, B43_PHY_N_BMODE(0x38), 0x668);
1256 }
1257
1258 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ScaleOffsetRssi */
1259 static void b43_nphy_scale_offset_rssi(struct b43_wldev *dev, u16 scale,
1260                                        s8 offset, u8 core, u8 rail, u8 type)
1261 {
1262         u16 tmp;
1263         bool core1or5 = (core == 1) || (core == 5);
1264         bool core2or5 = (core == 2) || (core == 5);
1265
1266         offset = clamp_val(offset, -32, 31);
1267         tmp = ((scale & 0x3F) << 8) | (offset & 0x3F);
1268
1269         if (core1or5 && (rail == 0) && (type == 2))
1270                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, tmp);
1271         if (core1or5 && (rail == 1) && (type == 2))
1272                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, tmp);
1273         if (core2or5 && (rail == 0) && (type == 2))
1274                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, tmp);
1275         if (core2or5 && (rail == 1) && (type == 2))
1276                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, tmp);
1277         if (core1or5 && (rail == 0) && (type == 0))
1278                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, tmp);
1279         if (core1or5 && (rail == 1) && (type == 0))
1280                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, tmp);
1281         if (core2or5 && (rail == 0) && (type == 0))
1282                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, tmp);
1283         if (core2or5 && (rail == 1) && (type == 0))
1284                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, tmp);
1285         if (core1or5 && (rail == 0) && (type == 1))
1286                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, tmp);
1287         if (core1or5 && (rail == 1) && (type == 1))
1288                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, tmp);
1289         if (core2or5 && (rail == 0) && (type == 1))
1290                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, tmp);
1291         if (core2or5 && (rail == 1) && (type == 1))
1292                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, tmp);
1293         if (core1or5 && (rail == 0) && (type == 6))
1294                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TBD, tmp);
1295         if (core1or5 && (rail == 1) && (type == 6))
1296                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TBD, tmp);
1297         if (core2or5 && (rail == 0) && (type == 6))
1298                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TBD, tmp);
1299         if (core2or5 && (rail == 1) && (type == 6))
1300                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TBD, tmp);
1301         if (core1or5 && (rail == 0) && (type == 3))
1302                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_PWRDET, tmp);
1303         if (core1or5 && (rail == 1) && (type == 3))
1304                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_PWRDET, tmp);
1305         if (core2or5 && (rail == 0) && (type == 3))
1306                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_PWRDET, tmp);
1307         if (core2or5 && (rail == 1) && (type == 3))
1308                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_PWRDET, tmp);
1309         if (core1or5 && (type == 4))
1310                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TSSI, tmp);
1311         if (core2or5 && (type == 4))
1312                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TSSI, tmp);
1313         if (core1or5 && (type == 5))
1314                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TSSI, tmp);
1315         if (core2or5 && (type == 5))
1316                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TSSI, tmp);
1317 }
1318
1319 static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1320 {
1321         u16 val;
1322
1323         if (type < 3)
1324                 val = 0;
1325         else if (type == 6)
1326                 val = 1;
1327         else if (type == 3)
1328                 val = 2;
1329         else
1330                 val = 3;
1331
1332         val = (val << 12) | (val << 14);
1333         b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, val);
1334         b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, val);
1335
1336         if (type < 3) {
1337                 b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO1, 0xFFCF,
1338                                 (type + 1) << 4);
1339                 b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO2, 0xFFCF,
1340                                 (type + 1) << 4);
1341         }
1342
1343         /* TODO use some definitions */
1344         if (code == 0) {
1345                 b43_phy_maskset(dev, B43_NPHY_AFECTL_OVER, 0xCFFF, 0);
1346                 if (type < 3) {
1347                         b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD, 0xFEC7, 0);
1348                         b43_phy_maskset(dev, B43_NPHY_RFCTL_OVER, 0xEFDC, 0);
1349                         b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD, 0xFFFE, 0);
1350                         udelay(20);
1351                         b43_phy_maskset(dev, B43_NPHY_RFCTL_OVER, 0xFFFE, 0);
1352                 }
1353         } else {
1354                 b43_phy_maskset(dev, B43_NPHY_AFECTL_OVER, 0xCFFF,
1355                                 0x3000);
1356                 if (type < 3) {
1357                         b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD,
1358                                         0xFEC7, 0x0180);
1359                         b43_phy_maskset(dev, B43_NPHY_RFCTL_OVER,
1360                                         0xEFDC, (code << 1 | 0x1021));
1361                         b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD, 0xFFFE, 0x1);
1362                         udelay(20);
1363                         b43_phy_maskset(dev, B43_NPHY_RFCTL_OVER, 0xFFFE, 0);
1364                 }
1365         }
1366 }
1367
1368 static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1369 {
1370         /* TODO */
1371 }
1372
1373 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSISel */
1374 static void b43_nphy_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1375 {
1376         if (dev->phy.rev >= 3)
1377                 b43_nphy_rev3_rssi_select(dev, code, type);
1378         else
1379                 b43_nphy_rev2_rssi_select(dev, code, type);
1380 }
1381
1382 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetRssi2055Vcm */
1383 static void b43_nphy_set_rssi_2055_vcm(struct b43_wldev *dev, u8 type, u8 *buf)
1384 {
1385         int i;
1386         for (i = 0; i < 2; i++) {
1387                 if (type == 2) {
1388                         if (i == 0) {
1389                                 b43_radio_maskset(dev, B2055_C1_B0NB_RSSIVCM,
1390                                                   0xFC, buf[0]);
1391                                 b43_radio_maskset(dev, B2055_C1_RX_BB_RSSICTL5,
1392                                                   0xFC, buf[1]);
1393                         } else {
1394                                 b43_radio_maskset(dev, B2055_C2_B0NB_RSSIVCM,
1395                                                   0xFC, buf[2 * i]);
1396                                 b43_radio_maskset(dev, B2055_C2_RX_BB_RSSICTL5,
1397                                                   0xFC, buf[2 * i + 1]);
1398                         }
1399                 } else {
1400                         if (i == 0)
1401                                 b43_radio_maskset(dev, B2055_C1_RX_BB_RSSICTL5,
1402                                                   0xF3, buf[0] << 2);
1403                         else
1404                                 b43_radio_maskset(dev, B2055_C2_RX_BB_RSSICTL5,
1405                                                   0xF3, buf[2 * i + 1] << 2);
1406                 }
1407         }
1408 }
1409
1410 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PollRssi */
1411 static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
1412                                 u8 nsamp)
1413 {
1414         int i;
1415         int out;
1416         u16 save_regs_phy[9];
1417         u16 s[2];
1418
1419         if (dev->phy.rev >= 3) {
1420                 save_regs_phy[0] = b43_phy_read(dev,
1421                                                 B43_NPHY_RFCTL_LUT_TRSW_UP1);
1422                 save_regs_phy[1] = b43_phy_read(dev,
1423                                                 B43_NPHY_RFCTL_LUT_TRSW_UP2);
1424                 save_regs_phy[2] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
1425                 save_regs_phy[3] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
1426                 save_regs_phy[4] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
1427                 save_regs_phy[5] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
1428                 save_regs_phy[6] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B1S0);
1429                 save_regs_phy[7] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B32S1);
1430         }
1431
1432         b43_nphy_rssi_select(dev, 5, type);
1433
1434         if (dev->phy.rev < 2) {
1435                 save_regs_phy[8] = b43_phy_read(dev, B43_NPHY_GPIO_SEL);
1436                 b43_phy_write(dev, B43_NPHY_GPIO_SEL, 5);
1437         }
1438
1439         for (i = 0; i < 4; i++)
1440                 buf[i] = 0;
1441
1442         for (i = 0; i < nsamp; i++) {
1443                 if (dev->phy.rev < 2) {
1444                         s[0] = b43_phy_read(dev, B43_NPHY_GPIO_LOOUT);
1445                         s[1] = b43_phy_read(dev, B43_NPHY_GPIO_HIOUT);
1446                 } else {
1447                         s[0] = b43_phy_read(dev, B43_NPHY_RSSI1);
1448                         s[1] = b43_phy_read(dev, B43_NPHY_RSSI2);
1449                 }
1450
1451                 buf[0] += ((s8)((s[0] & 0x3F) << 2)) >> 2;
1452                 buf[1] += ((s8)(((s[0] >> 8) & 0x3F) << 2)) >> 2;
1453                 buf[2] += ((s8)((s[1] & 0x3F) << 2)) >> 2;
1454                 buf[3] += ((s8)(((s[1] >> 8) & 0x3F) << 2)) >> 2;
1455         }
1456         out = (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 |
1457                 (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
1458
1459         if (dev->phy.rev < 2)
1460                 b43_phy_write(dev, B43_NPHY_GPIO_SEL, save_regs_phy[8]);
1461
1462         if (dev->phy.rev >= 3) {
1463                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1,
1464                                 save_regs_phy[0]);
1465                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2,
1466                                 save_regs_phy[1]);
1467                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, save_regs_phy[2]);
1468                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, save_regs_phy[3]);
1469                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, save_regs_phy[4]);
1470                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, save_regs_phy[5]);
1471                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S0, save_regs_phy[6]);
1472                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S1, save_regs_phy[7]);
1473         }
1474
1475         return out;
1476 }
1477
1478 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICal */
1479 static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
1480 {
1481         int i, j;
1482         u8 state[4];
1483         u8 code, val;
1484         u16 class, override;
1485         u8 regs_save_radio[2];
1486         u16 regs_save_phy[2];
1487         s8 offset[4];
1488
1489         u16 clip_state[2];
1490         u16 clip_off[2] = { 0xFFFF, 0xFFFF };
1491         s32 results_min[4] = { };
1492         u8 vcm_final[4] = { };
1493         s32 results[4][4] = { };
1494         s32 miniq[4][2] = { };
1495
1496         if (type == 2) {
1497                 code = 0;
1498                 val = 6;
1499         } else if (type < 2) {
1500                 code = 25;
1501                 val = 4;
1502         } else {
1503                 B43_WARN_ON(1);
1504                 return;
1505         }
1506
1507         class = b43_nphy_classifier(dev, 0, 0);
1508         b43_nphy_classifier(dev, 7, 4);
1509         b43_nphy_read_clip_detection(dev, clip_state);
1510         b43_nphy_write_clip_detection(dev, clip_off);
1511
1512         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
1513                 override = 0x140;
1514         else
1515                 override = 0x110;
1516
1517         regs_save_phy[0] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
1518         regs_save_radio[0] = b43_radio_read16(dev, B2055_C1_PD_RXTX);
1519         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, override);
1520         b43_radio_write16(dev, B2055_C1_PD_RXTX, val);
1521
1522         regs_save_phy[1] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
1523         regs_save_radio[1] = b43_radio_read16(dev, B2055_C2_PD_RXTX);
1524         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, override);
1525         b43_radio_write16(dev, B2055_C2_PD_RXTX, val);
1526
1527         state[0] = b43_radio_read16(dev, B2055_C1_PD_RSSIMISC) & 0x07;
1528         state[1] = b43_radio_read16(dev, B2055_C2_PD_RSSIMISC) & 0x07;
1529         b43_radio_mask(dev, B2055_C1_PD_RSSIMISC, 0xF8);
1530         b43_radio_mask(dev, B2055_C2_PD_RSSIMISC, 0xF8);
1531         state[2] = b43_radio_read16(dev, B2055_C1_SP_RSSI) & 0x07;
1532         state[3] = b43_radio_read16(dev, B2055_C2_SP_RSSI) & 0x07;
1533
1534         b43_nphy_rssi_select(dev, 5, type);
1535         b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 0, type);
1536         b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 1, type);
1537
1538         for (i = 0; i < 4; i++) {
1539                 u8 tmp[4];
1540                 for (j = 0; j < 4; j++)
1541                         tmp[j] = i;
1542                 if (type != 1)
1543                         b43_nphy_set_rssi_2055_vcm(dev, type, tmp);
1544                 b43_nphy_poll_rssi(dev, type, results[i], 8);
1545                 if (type < 2)
1546                         for (j = 0; j < 2; j++)
1547                                 miniq[i][j] = min(results[i][2 * j],
1548                                                 results[i][2 * j + 1]);
1549         }
1550
1551         for (i = 0; i < 4; i++) {
1552                 s32 mind = 40;
1553                 u8 minvcm = 0;
1554                 s32 minpoll = 249;
1555                 s32 curr;
1556                 for (j = 0; j < 4; j++) {
1557                         if (type == 2)
1558                                 curr = abs(results[j][i]);
1559                         else
1560                                 curr = abs(miniq[j][i / 2] - code * 8);
1561
1562                         if (curr < mind) {
1563                                 mind = curr;
1564                                 minvcm = j;
1565                         }
1566
1567                         if (results[j][i] < minpoll)
1568                                 minpoll = results[j][i];
1569                 }
1570                 results_min[i] = minpoll;
1571                 vcm_final[i] = minvcm;
1572         }
1573
1574         if (type != 1)
1575                 b43_nphy_set_rssi_2055_vcm(dev, type, vcm_final);
1576
1577         for (i = 0; i < 4; i++) {
1578                 offset[i] = (code * 8) - results[vcm_final[i]][i];
1579
1580                 if (offset[i] < 0)
1581                         offset[i] = -((abs(offset[i]) + 4) / 8);
1582                 else
1583                         offset[i] = (offset[i] + 4) / 8;
1584
1585                 if (results_min[i] == 248)
1586                         offset[i] = code - 32;
1587
1588                 if (i % 2 == 0)
1589                         b43_nphy_scale_offset_rssi(dev, 0, offset[i], 1, 0,
1590                                                         type);
1591                 else
1592                         b43_nphy_scale_offset_rssi(dev, 0, offset[i], 2, 1,
1593                                                         type);
1594         }
1595
1596         b43_radio_maskset(dev, B2055_C1_PD_RSSIMISC, 0xF8, state[0]);
1597         b43_radio_maskset(dev, B2055_C1_PD_RSSIMISC, 0xF8, state[1]);
1598
1599         switch (state[2]) {
1600         case 1:
1601                 b43_nphy_rssi_select(dev, 1, 2);
1602                 break;
1603         case 4:
1604                 b43_nphy_rssi_select(dev, 1, 0);
1605                 break;
1606         case 2:
1607                 b43_nphy_rssi_select(dev, 1, 1);
1608                 break;
1609         default:
1610                 b43_nphy_rssi_select(dev, 1, 1);
1611                 break;
1612         }
1613
1614         switch (state[3]) {
1615         case 1:
1616                 b43_nphy_rssi_select(dev, 2, 2);
1617                 break;
1618         case 4:
1619                 b43_nphy_rssi_select(dev, 2, 0);
1620                 break;
1621         default:
1622                 b43_nphy_rssi_select(dev, 2, 1);
1623                 break;
1624         }
1625
1626         b43_nphy_rssi_select(dev, 0, type);
1627
1628         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs_save_phy[0]);
1629         b43_radio_write16(dev, B2055_C1_PD_RXTX, regs_save_radio[0]);
1630         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs_save_phy[1]);
1631         b43_radio_write16(dev, B2055_C2_PD_RXTX, regs_save_radio[1]);
1632
1633         b43_nphy_classifier(dev, 7, class);
1634         b43_nphy_write_clip_detection(dev, clip_state);
1635 }
1636
1637 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICalRev3 */
1638 static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
1639 {
1640         /* TODO */
1641 }
1642
1643 /*
1644  * RSSI Calibration
1645  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICal
1646  */
1647 static void b43_nphy_rssi_cal(struct b43_wldev *dev)
1648 {
1649         if (dev->phy.rev >= 3) {
1650                 b43_nphy_rev3_rssi_cal(dev);
1651         } else {
1652                 b43_nphy_rev2_rssi_cal(dev, 2);
1653                 b43_nphy_rev2_rssi_cal(dev, 0);
1654                 b43_nphy_rev2_rssi_cal(dev, 1);
1655         }
1656 }
1657
1658 /*
1659  * Restore RSSI Calibration
1660  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/RestoreRssiCal
1661  */
1662 static void b43_nphy_restore_rssi_cal(struct b43_wldev *dev)
1663 {
1664         struct b43_phy_n *nphy = dev->phy.n;
1665
1666         u16 *rssical_radio_regs = NULL;
1667         u16 *rssical_phy_regs = NULL;
1668
1669         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
1670                 if (!nphy->rssical_chanspec_2G)
1671                         return;
1672                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_2G;
1673                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_2G;
1674         } else {
1675                 if (!nphy->rssical_chanspec_5G)
1676                         return;
1677                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_5G;
1678                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
1679         }
1680
1681         /* TODO use some definitions */
1682         b43_radio_maskset(dev, 0x602B, 0xE3, rssical_radio_regs[0]);
1683         b43_radio_maskset(dev, 0x702B, 0xE3, rssical_radio_regs[1]);
1684
1685         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, rssical_phy_regs[0]);
1686         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, rssical_phy_regs[1]);
1687         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, rssical_phy_regs[2]);
1688         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, rssical_phy_regs[3]);
1689
1690         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, rssical_phy_regs[4]);
1691         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, rssical_phy_regs[5]);
1692         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, rssical_phy_regs[6]);
1693         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, rssical_phy_regs[7]);
1694
1695         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, rssical_phy_regs[8]);
1696         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, rssical_phy_regs[9]);
1697         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, rssical_phy_regs[10]);
1698         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, rssical_phy_regs[11]);
1699 }
1700
1701 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GetIpaGainTbl */
1702 static const u32 *b43_nphy_get_ipa_gain_table(struct b43_wldev *dev)
1703 {
1704         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
1705                 if (dev->phy.rev >= 6) {
1706                         /* TODO If the chip is 47162
1707                                 return txpwrctrl_tx_gain_ipa_rev5 */
1708                         return txpwrctrl_tx_gain_ipa_rev6;
1709                 } else if (dev->phy.rev >= 5) {
1710                         return txpwrctrl_tx_gain_ipa_rev5;
1711                 } else {
1712                         return txpwrctrl_tx_gain_ipa;
1713                 }
1714         } else {
1715                 return txpwrctrl_tx_gain_ipa_5g;
1716         }
1717 }
1718
1719 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalRadioSetup */
1720 static void b43_nphy_tx_cal_radio_setup(struct b43_wldev *dev)
1721 {
1722         struct b43_phy_n *nphy = dev->phy.n;
1723         u16 *save = nphy->tx_rx_cal_radio_saveregs;
1724
1725         if (dev->phy.rev >= 3) {
1726                 /* TODO */
1727         } else {
1728                 save[0] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL1);
1729                 b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL1, 0x29);
1730
1731                 save[1] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL2);
1732                 b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL2, 0x54);
1733
1734                 save[2] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL1);
1735                 b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL1, 0x29);
1736
1737                 save[3] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL2);
1738                 b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL2, 0x54);
1739
1740                 save[3] = b43_radio_read16(dev, B2055_C1_PWRDET_RXTX);
1741                 save[4] = b43_radio_read16(dev, B2055_C2_PWRDET_RXTX);
1742
1743                 if (!(b43_phy_read(dev, B43_NPHY_BANDCTL) &
1744                     B43_NPHY_BANDCTL_5GHZ)) {
1745                         b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x04);
1746                         b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x04);
1747                 } else {
1748                         b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x20);
1749                         b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x20);
1750                 }
1751
1752                 if (dev->phy.rev < 2) {
1753                         b43_radio_set(dev, B2055_C1_TX_BB_MXGM, 0x20);
1754                         b43_radio_set(dev, B2055_C2_TX_BB_MXGM, 0x20);
1755                 } else {
1756                         b43_radio_mask(dev, B2055_C1_TX_BB_MXGM, ~0x20);
1757                         b43_radio_mask(dev, B2055_C2_TX_BB_MXGM, ~0x20);
1758                 }
1759         }
1760 }
1761
1762 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/IqCalGainParams */
1763 static void b43_nphy_iq_cal_gain_params(struct b43_wldev *dev, u16 core,
1764                                         struct nphy_txgains target,
1765                                         struct nphy_iqcal_params *params)
1766 {
1767         int i, j, indx;
1768         u16 gain;
1769
1770         if (dev->phy.rev >= 3) {
1771                 params->txgm = target.txgm[core];
1772                 params->pga = target.pga[core];
1773                 params->pad = target.pad[core];
1774                 params->ipa = target.ipa[core];
1775                 params->cal_gain = (params->txgm << 12) | (params->pga << 8) |
1776                                         (params->pad << 4) | (params->ipa);
1777                 for (j = 0; j < 5; j++)
1778                         params->ncorr[j] = 0x79;
1779         } else {
1780                 gain = (target.pad[core]) | (target.pga[core] << 4) |
1781                         (target.txgm[core] << 8);
1782
1783                 indx = (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ?
1784                         1 : 0;
1785                 for (i = 0; i < 9; i++)
1786                         if (tbl_iqcal_gainparams[indx][i][0] == gain)
1787                                 break;
1788                 i = min(i, 8);
1789
1790                 params->txgm = tbl_iqcal_gainparams[indx][i][1];
1791                 params->pga = tbl_iqcal_gainparams[indx][i][2];
1792                 params->pad = tbl_iqcal_gainparams[indx][i][3];
1793                 params->cal_gain = (params->txgm << 7) | (params->pga << 4) |
1794                                         (params->pad << 2);
1795                 for (j = 0; j < 4; j++)
1796                         params->ncorr[j] = tbl_iqcal_gainparams[indx][i][4 + j];
1797         }
1798 }
1799
1800 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/UpdateTxCalLadder */
1801 static void b43_nphy_update_tx_cal_ladder(struct b43_wldev *dev, u16 core)
1802 {
1803         struct b43_phy_n *nphy = dev->phy.n;
1804         int i;
1805         u16 scale, entry;
1806
1807         u16 tmp = nphy->txcal_bbmult;
1808         if (core == 0)
1809                 tmp >>= 8;
1810         tmp &= 0xff;
1811
1812         for (i = 0; i < 18; i++) {
1813                 scale = (ladder_lo[i].percent * tmp) / 100;
1814                 entry = ((scale & 0xFF) << 8) | ladder_lo[i].g_env;
1815                 b43_ntab_write(dev, B43_NTAB16(15, i), entry);
1816
1817                 scale = (ladder_iq[i].percent * tmp) / 100;
1818                 entry = ((scale & 0xFF) << 8) | ladder_iq[i].g_env;
1819                 b43_ntab_write(dev, B43_NTAB16(15, i + 32), entry);
1820         }
1821 }
1822
1823 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ExtPaSetTxDigiFilts */
1824 static void b43_nphy_ext_pa_set_tx_dig_filters(struct b43_wldev *dev)
1825 {
1826         int i;
1827         for (i = 0; i < 15; i++)
1828                 b43_phy_write(dev, B43_PHY_N(0x2C5 + i),
1829                                 tbl_tx_filter_coef_rev4[2][i]);
1830 }
1831
1832 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/IpaSetTxDigiFilts */
1833 static void b43_nphy_int_pa_set_tx_dig_filters(struct b43_wldev *dev)
1834 {
1835         int i, j;
1836         /* B43_NPHY_TXF_20CO_S0A1, B43_NPHY_TXF_40CO_S0A1, unknown */
1837         u16 offset[] = { 0x186, 0x195, 0x2C5 };
1838
1839         for (i = 0; i < 3; i++)
1840                 for (j = 0; j < 15; j++)
1841                         b43_phy_write(dev, B43_PHY_N(offset[i] + j),
1842                                         tbl_tx_filter_coef_rev4[i][j]);
1843
1844         if (dev->phy.is_40mhz) {
1845                 for (j = 0; j < 15; j++)
1846                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
1847                                         tbl_tx_filter_coef_rev4[3][j]);
1848         } else if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
1849                 for (j = 0; j < 15; j++)
1850                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
1851                                         tbl_tx_filter_coef_rev4[5][j]);
1852         }
1853
1854         if (dev->phy.channel == 14)
1855                 for (j = 0; j < 15; j++)
1856                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
1857                                         tbl_tx_filter_coef_rev4[6][j]);
1858 }
1859
1860 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GetTxGain */
1861 static struct nphy_txgains b43_nphy_get_tx_gains(struct b43_wldev *dev)
1862 {
1863         struct b43_phy_n *nphy = dev->phy.n;
1864
1865         u16 curr_gain[2];
1866         struct nphy_txgains target;
1867         const u32 *table = NULL;
1868
1869         if (nphy->txpwrctrl == 0) {
1870                 int i;
1871
1872                 if (nphy->hang_avoid)
1873                         b43_nphy_stay_in_carrier_search(dev, true);
1874                 b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, curr_gain);
1875                 if (nphy->hang_avoid)
1876                         b43_nphy_stay_in_carrier_search(dev, false);
1877
1878                 for (i = 0; i < 2; ++i) {
1879                         if (dev->phy.rev >= 3) {
1880                                 target.ipa[i] = curr_gain[i] & 0x000F;
1881                                 target.pad[i] = (curr_gain[i] & 0x00F0) >> 4;
1882                                 target.pga[i] = (curr_gain[i] & 0x0F00) >> 8;
1883                                 target.txgm[i] = (curr_gain[i] & 0x7000) >> 12;
1884                         } else {
1885                                 target.ipa[i] = curr_gain[i] & 0x0003;
1886                                 target.pad[i] = (curr_gain[i] & 0x000C) >> 2;
1887                                 target.pga[i] = (curr_gain[i] & 0x0070) >> 4;
1888                                 target.txgm[i] = (curr_gain[i] & 0x0380) >> 7;
1889                         }
1890                 }
1891         } else {
1892                 int i;
1893                 u16 index[2];
1894                 index[0] = (b43_phy_read(dev, B43_NPHY_C1_TXPCTL_STAT) &
1895                         B43_NPHY_TXPCTL_STAT_BIDX) >>
1896                         B43_NPHY_TXPCTL_STAT_BIDX_SHIFT;
1897                 index[1] = (b43_phy_read(dev, B43_NPHY_C2_TXPCTL_STAT) &
1898                         B43_NPHY_TXPCTL_STAT_BIDX) >>
1899                         B43_NPHY_TXPCTL_STAT_BIDX_SHIFT;
1900
1901                 for (i = 0; i < 2; ++i) {
1902                         if (dev->phy.rev >= 3) {
1903                                 enum ieee80211_band band =
1904                                         b43_current_band(dev->wl);
1905
1906                                 if ((nphy->ipa2g_on &&
1907                                      band == IEEE80211_BAND_2GHZ) ||
1908                                     (nphy->ipa5g_on &&
1909                                      band == IEEE80211_BAND_5GHZ)) {
1910                                         table = b43_nphy_get_ipa_gain_table(dev);
1911                                 } else {
1912                                         if (band == IEEE80211_BAND_5GHZ) {
1913                                                 if (dev->phy.rev == 3)
1914                                                         table = b43_ntab_tx_gain_rev3_5ghz;
1915                                                 else if (dev->phy.rev == 4)
1916                                                         table = b43_ntab_tx_gain_rev4_5ghz;
1917                                                 else
1918                                                         table = b43_ntab_tx_gain_rev5plus_5ghz;
1919                                         } else {
1920                                                 table = b43_ntab_tx_gain_rev3plus_2ghz;
1921                                         }
1922                                 }
1923
1924                                 target.ipa[i] = (table[index[i]] >> 16) & 0xF;
1925                                 target.pad[i] = (table[index[i]] >> 20) & 0xF;
1926                                 target.pga[i] = (table[index[i]] >> 24) & 0xF;
1927                                 target.txgm[i] = (table[index[i]] >> 28) & 0xF;
1928                         } else {
1929                                 table = b43_ntab_tx_gain_rev0_1_2;
1930
1931                                 target.ipa[i] = (table[index[i]] >> 16) & 0x3;
1932                                 target.pad[i] = (table[index[i]] >> 18) & 0x3;
1933                                 target.pga[i] = (table[index[i]] >> 20) & 0x7;
1934                                 target.txgm[i] = (table[index[i]] >> 23) & 0x7;
1935                         }
1936                 }
1937         }
1938
1939         return target;
1940 }
1941
1942 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalPhyCleanup */
1943 static void b43_nphy_tx_cal_phy_cleanup(struct b43_wldev *dev)
1944 {
1945         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
1946
1947         if (dev->phy.rev >= 3) {
1948                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, regs[0]);
1949                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, regs[1]);
1950                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, regs[2]);
1951                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[3]);
1952                 b43_phy_write(dev, B43_NPHY_BBCFG, regs[4]);
1953                 b43_ntab_write(dev, B43_NTAB16(8, 3), regs[5]);
1954                 b43_ntab_write(dev, B43_NTAB16(8, 19), regs[6]);
1955                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[7]);
1956                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[8]);
1957                 b43_phy_write(dev, B43_NPHY_PAPD_EN0, regs[9]);
1958                 b43_phy_write(dev, B43_NPHY_PAPD_EN1, regs[10]);
1959                 b43_nphy_reset_cca(dev);
1960         } else {
1961                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, regs[0]);
1962                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, regs[1]);
1963                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[2]);
1964                 b43_ntab_write(dev, B43_NTAB16(8, 2), regs[3]);
1965                 b43_ntab_write(dev, B43_NTAB16(8, 18), regs[4]);
1966                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[5]);
1967                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[6]);
1968         }
1969 }
1970
1971 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalPhySetup */
1972 static void b43_nphy_tx_cal_phy_setup(struct b43_wldev *dev)
1973 {
1974         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
1975         u16 tmp;
1976
1977         regs[0] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
1978         regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
1979         if (dev->phy.rev >= 3) {
1980                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0xF0FF, 0x0A00);
1981                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0xF0FF, 0x0A00);
1982
1983                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
1984                 regs[2] = tmp;
1985                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, tmp | 0x0600);
1986
1987                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
1988                 regs[3] = tmp;
1989                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp | 0x0600);
1990
1991                 regs[4] = b43_phy_read(dev, B43_NPHY_BBCFG);
1992                 b43_phy_mask(dev, B43_NPHY_BBCFG, (u16)~B43_NPHY_BBCFG_RSTRX);
1993
1994                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 3));
1995                 regs[5] = tmp;
1996                 b43_ntab_write(dev, B43_NTAB16(8, 3), 0);
1997
1998                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 19));
1999                 regs[6] = tmp;
2000                 b43_ntab_write(dev, B43_NTAB16(8, 19), 0);
2001                 regs[7] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
2002                 regs[8] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
2003
2004                 /* TODO: Call N PHY RF Ctrl Intc Override with 2, 1, 3 */
2005                 /* TODO: Call N PHY RF Ctrl Intc Override with 1, 2, 1 */
2006                 /* TODO: Call N PHY RF Ctrl Intc Override with 1, 8, 2 */
2007
2008                 regs[9] = b43_phy_read(dev, B43_NPHY_PAPD_EN0);
2009                 regs[10] = b43_phy_read(dev, B43_NPHY_PAPD_EN1);
2010                 b43_phy_mask(dev, B43_NPHY_PAPD_EN0, ~0x0001);
2011                 b43_phy_mask(dev, B43_NPHY_PAPD_EN1, ~0x0001);
2012         } else {
2013                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, 0xA000);
2014                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, 0xA000);
2015                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
2016                 regs[2] = tmp;
2017                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp | 0x3000);
2018                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 2));
2019                 regs[3] = tmp;
2020                 tmp |= 0x2000;
2021                 b43_ntab_write(dev, B43_NTAB16(8, 2), tmp);
2022                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 18));
2023                 regs[4] = tmp;
2024                 tmp |= 0x2000;
2025                 b43_ntab_write(dev, B43_NTAB16(8, 18), tmp);
2026                 regs[5] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
2027                 regs[6] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
2028                 if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
2029                         tmp = 0x0180;
2030                 else
2031                         tmp = 0x0120;
2032                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, tmp);
2033                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, tmp);
2034         }
2035 }
2036
2037 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RestoreCal */
2038 static void b43_nphy_restore_cal(struct b43_wldev *dev)
2039 {
2040         struct b43_phy_n *nphy = dev->phy.n;
2041
2042         u16 coef[4];
2043         u16 *loft = NULL;
2044         u16 *table = NULL;
2045
2046         int i;
2047         u16 *txcal_radio_regs = NULL;
2048         struct b43_phy_n_iq_comp *rxcal_coeffs = NULL;
2049
2050         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2051                 if (nphy->iqcal_chanspec_2G == 0)
2052                         return;
2053                 table = nphy->cal_cache.txcal_coeffs_2G;
2054                 loft = &nphy->cal_cache.txcal_coeffs_2G[5];
2055         } else {
2056                 if (nphy->iqcal_chanspec_5G == 0)
2057                         return;
2058                 table = nphy->cal_cache.txcal_coeffs_5G;
2059                 loft = &nphy->cal_cache.txcal_coeffs_5G[5];
2060         }
2061
2062         b43_ntab_write_bulk(dev, B43_NTAB16(15, 80), 4, table);
2063
2064         for (i = 0; i < 4; i++) {
2065                 if (dev->phy.rev >= 3)
2066                         table[i] = coef[i];
2067                 else
2068                         coef[i] = 0;
2069         }
2070
2071         b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4, coef);
2072         b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2, loft);
2073         b43_ntab_write_bulk(dev, B43_NTAB16(15, 93), 2, loft);
2074
2075         if (dev->phy.rev < 2)
2076                 b43_nphy_tx_iq_workaround(dev);
2077
2078         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2079                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_2G;
2080                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_2G;
2081         } else {
2082                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_5G;
2083                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_5G;
2084         }
2085
2086         /* TODO use some definitions */
2087         if (dev->phy.rev >= 3) {
2088                 b43_radio_write(dev, 0x2021, txcal_radio_regs[0]);
2089                 b43_radio_write(dev, 0x2022, txcal_radio_regs[1]);
2090                 b43_radio_write(dev, 0x3021, txcal_radio_regs[2]);
2091                 b43_radio_write(dev, 0x3022, txcal_radio_regs[3]);
2092                 b43_radio_write(dev, 0x2023, txcal_radio_regs[4]);
2093                 b43_radio_write(dev, 0x2024, txcal_radio_regs[5]);
2094                 b43_radio_write(dev, 0x3023, txcal_radio_regs[6]);
2095                 b43_radio_write(dev, 0x3024, txcal_radio_regs[7]);
2096         } else {
2097                 b43_radio_write(dev, 0x8B, txcal_radio_regs[0]);
2098                 b43_radio_write(dev, 0xBA, txcal_radio_regs[1]);
2099                 b43_radio_write(dev, 0x8D, txcal_radio_regs[2]);
2100                 b43_radio_write(dev, 0xBC, txcal_radio_regs[3]);
2101         }
2102         b43_nphy_rx_iq_coeffs(dev, true, rxcal_coeffs);
2103 }
2104
2105 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalTxIqlo */
2106 static int b43_nphy_cal_tx_iq_lo(struct b43_wldev *dev,
2107                                 struct nphy_txgains target,
2108                                 bool full, bool mphase)
2109 {
2110         struct b43_phy_n *nphy = dev->phy.n;
2111         int i;
2112         int error = 0;
2113         int freq;
2114         bool avoid = false;
2115         u8 length;
2116         u16 tmp, core, type, count, max, numb, last, cmd;
2117         const u16 *table;
2118         bool phy6or5x;
2119
2120         u16 buffer[11];
2121         u16 diq_start = 0;
2122         u16 save[2];
2123         u16 gain[2];
2124         struct nphy_iqcal_params params[2];
2125         bool updated[2] = { };
2126
2127         b43_nphy_stay_in_carrier_search(dev, true);
2128
2129         if (dev->phy.rev >= 4) {
2130                 avoid = nphy->hang_avoid;
2131                 nphy->hang_avoid = 0;
2132         }
2133
2134         b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, save);
2135
2136         for (i = 0; i < 2; i++) {
2137                 b43_nphy_iq_cal_gain_params(dev, i, target, &params[i]);
2138                 gain[i] = params[i].cal_gain;
2139         }
2140
2141         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, gain);
2142
2143         b43_nphy_tx_cal_radio_setup(dev);
2144         b43_nphy_tx_cal_phy_setup(dev);
2145
2146         phy6or5x = dev->phy.rev >= 6 ||
2147                 (dev->phy.rev == 5 && nphy->ipa2g_on &&
2148                 b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ);
2149         if (phy6or5x) {
2150                 /* TODO */
2151         }
2152
2153         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x8AA9);
2154
2155         if (!dev->phy.is_40mhz)
2156                 freq = 2500;
2157         else
2158                 freq = 5000;
2159
2160         if (nphy->mphase_cal_phase_id > 2)
2161                 b43_nphy_run_samples(dev, (dev->phy.is_40mhz ? 40 : 20) * 8,
2162                                         0xFFFF, 0, true, false);
2163         else
2164                 error = b43_nphy_tx_tone(dev, freq, 250, true, false);
2165
2166         if (error == 0) {
2167                 if (nphy->mphase_cal_phase_id > 2) {
2168                         table = nphy->mphase_txcal_bestcoeffs;
2169                         length = 11;
2170                         if (dev->phy.rev < 3)
2171                                 length -= 2;
2172                 } else {
2173                         if (!full && nphy->txiqlocal_coeffsvalid) {
2174                                 table = nphy->txiqlocal_bestc;
2175                                 length = 11;
2176                                 if (dev->phy.rev < 3)
2177                                         length -= 2;
2178                         } else {
2179                                 full = true;
2180                                 if (dev->phy.rev >= 3) {
2181                                         table = tbl_tx_iqlo_cal_startcoefs_nphyrev3;
2182                                         length = B43_NTAB_TX_IQLO_CAL_STARTCOEFS_REV3;
2183                                 } else {
2184                                         table = tbl_tx_iqlo_cal_startcoefs;
2185                                         length = B43_NTAB_TX_IQLO_CAL_STARTCOEFS;
2186                                 }
2187                         }
2188                 }
2189
2190                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 64), length, table);
2191
2192                 if (full) {
2193                         if (dev->phy.rev >= 3)
2194                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_FULLCAL_REV3;
2195                         else
2196                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_FULLCAL;
2197                 } else {
2198                         if (dev->phy.rev >= 3)
2199                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_RECAL_REV3;
2200                         else
2201                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_RECAL;
2202                 }
2203
2204                 if (mphase) {
2205                         count = nphy->mphase_txcal_cmdidx;
2206                         numb = min(max,
2207                                 (u16)(count + nphy->mphase_txcal_numcmds));
2208                 } else {
2209                         count = 0;
2210                         numb = max;
2211                 }
2212
2213                 for (; count < numb; count++) {
2214                         if (full) {
2215                                 if (dev->phy.rev >= 3)
2216                                         cmd = tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3[count];
2217                                 else
2218                                         cmd = tbl_tx_iqlo_cal_cmds_fullcal[count];
2219                         } else {
2220                                 if (dev->phy.rev >= 3)
2221                                         cmd = tbl_tx_iqlo_cal_cmds_recal_nphyrev3[count];
2222                                 else
2223                                         cmd = tbl_tx_iqlo_cal_cmds_recal[count];
2224                         }
2225
2226                         core = (cmd & 0x3000) >> 12;
2227                         type = (cmd & 0x0F00) >> 8;
2228
2229                         if (phy6or5x && updated[core] == 0) {
2230                                 b43_nphy_update_tx_cal_ladder(dev, core);
2231                                 updated[core] = 1;
2232                         }
2233
2234                         tmp = (params[core].ncorr[type] << 8) | 0x66;
2235                         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDNNUM, tmp);
2236
2237                         if (type == 1 || type == 3 || type == 4) {
2238                                 buffer[0] = b43_ntab_read(dev,
2239                                                 B43_NTAB16(15, 69 + core));
2240                                 diq_start = buffer[0];
2241                                 buffer[0] = 0;
2242                                 b43_ntab_write(dev, B43_NTAB16(15, 69 + core),
2243                                                 0);
2244                         }
2245
2246                         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMD, cmd);
2247                         for (i = 0; i < 2000; i++) {
2248                                 tmp = b43_phy_read(dev, B43_NPHY_IQLOCAL_CMD);
2249                                 if (tmp & 0xC000)
2250                                         break;
2251                                 udelay(10);
2252                         }
2253
2254                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
2255                                                 buffer);
2256                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 64), length,
2257                                                 buffer);
2258
2259                         if (type == 1 || type == 3 || type == 4)
2260                                 buffer[0] = diq_start;
2261                 }
2262
2263                 if (mphase)
2264                         nphy->mphase_txcal_cmdidx = (numb >= max) ? 0 : numb;
2265
2266                 last = (dev->phy.rev < 3) ? 6 : 7;
2267
2268                 if (!mphase || nphy->mphase_cal_phase_id == last) {
2269                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 96), 4, buffer);
2270                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 4, buffer);
2271                         if (dev->phy.rev < 3) {
2272                                 buffer[0] = 0;
2273                                 buffer[1] = 0;
2274                                 buffer[2] = 0;
2275                                 buffer[3] = 0;
2276                         }
2277                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4,
2278                                                 buffer);
2279                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 101), 2,
2280                                                 buffer);
2281                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2,
2282                                                 buffer);
2283                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 93), 2,
2284                                                 buffer);
2285                         length = 11;
2286                         if (dev->phy.rev < 3)
2287                                 length -= 2;
2288                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
2289                                                 nphy->txiqlocal_bestc);
2290                         nphy->txiqlocal_coeffsvalid = true;
2291                         /* TODO: Set nphy->txiqlocal_chanspec to
2292                                 the current channel */
2293                 } else {
2294                         length = 11;
2295                         if (dev->phy.rev < 3)
2296                                 length -= 2;
2297                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
2298                                                 nphy->mphase_txcal_bestcoeffs);
2299                 }
2300
2301                 b43_nphy_stop_playback(dev);
2302                 b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0);
2303         }
2304
2305         b43_nphy_tx_cal_phy_cleanup(dev);
2306         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, save);
2307
2308         if (dev->phy.rev < 2 && (!mphase || nphy->mphase_cal_phase_id == last))
2309                 b43_nphy_tx_iq_workaround(dev);
2310
2311         if (dev->phy.rev >= 4)
2312                 nphy->hang_avoid = avoid;
2313
2314         b43_nphy_stay_in_carrier_search(dev, false);
2315
2316         return error;
2317 }
2318
2319 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalRxIqRev2 */
2320 static int b43_nphy_rev2_cal_rx_iq(struct b43_wldev *dev,
2321                         struct nphy_txgains target, u8 type, bool debug)
2322 {
2323         struct b43_phy_n *nphy = dev->phy.n;
2324         int i, j, index;
2325         u8 rfctl[2];
2326         u8 afectl_core;
2327         u16 tmp[6];
2328         u16 cur_hpf1, cur_hpf2, cur_lna;
2329         u32 real, imag;
2330         enum ieee80211_band band;
2331
2332         u8 use;
2333         u16 cur_hpf;
2334         u16 lna[3] = { 3, 3, 1 };
2335         u16 hpf1[3] = { 7, 2, 0 };
2336         u16 hpf2[3] = { 2, 0, 0 };
2337         u32 power[3] = { };
2338         u16 gain_save[2];
2339         u16 cal_gain[2];
2340         struct nphy_iqcal_params cal_params[2];
2341         struct nphy_iq_est est;
2342         int ret = 0;
2343         bool playtone = true;
2344         int desired = 13;
2345
2346         b43_nphy_stay_in_carrier_search(dev, 1);
2347
2348         if (dev->phy.rev < 2)
2349                 ;/* TODO: Call N PHY Reapply TX Cal Coeffs */
2350         b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, gain_save);
2351         for (i = 0; i < 2; i++) {
2352                 b43_nphy_iq_cal_gain_params(dev, i, target, &cal_params[i]);
2353                 cal_gain[i] = cal_params[i].cal_gain;
2354         }
2355         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, cal_gain);
2356
2357         for (i = 0; i < 2; i++) {
2358                 if (i == 0) {
2359                         rfctl[0] = B43_NPHY_RFCTL_INTC1;
2360                         rfctl[1] = B43_NPHY_RFCTL_INTC2;
2361                         afectl_core = B43_NPHY_AFECTL_C1;
2362                 } else {
2363                         rfctl[0] = B43_NPHY_RFCTL_INTC2;
2364                         rfctl[1] = B43_NPHY_RFCTL_INTC1;
2365                         afectl_core = B43_NPHY_AFECTL_C2;
2366                 }
2367
2368                 tmp[1] = b43_phy_read(dev, B43_NPHY_RFSEQCA);
2369                 tmp[2] = b43_phy_read(dev, afectl_core);
2370                 tmp[3] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
2371                 tmp[4] = b43_phy_read(dev, rfctl[0]);
2372                 tmp[5] = b43_phy_read(dev, rfctl[1]);
2373
2374                 b43_phy_maskset(dev, B43_NPHY_RFSEQCA,
2375                                 (u16)~B43_NPHY_RFSEQCA_RXDIS,
2376                                 ((1 - i) << B43_NPHY_RFSEQCA_RXDIS_SHIFT));
2377                 b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXEN,
2378                                 (1 - i));
2379                 b43_phy_set(dev, afectl_core, 0x0006);
2380                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0006);
2381
2382                 band = b43_current_band(dev->wl);
2383
2384                 if (nphy->rxcalparams & 0xFF000000) {
2385                         if (band == IEEE80211_BAND_5GHZ)
2386                                 b43_phy_write(dev, rfctl[0], 0x140);
2387                         else
2388                                 b43_phy_write(dev, rfctl[0], 0x110);
2389                 } else {
2390                         if (band == IEEE80211_BAND_5GHZ)
2391                                 b43_phy_write(dev, rfctl[0], 0x180);
2392                         else
2393                                 b43_phy_write(dev, rfctl[0], 0x120);
2394                 }
2395
2396                 if (band == IEEE80211_BAND_5GHZ)
2397                         b43_phy_write(dev, rfctl[1], 0x148);
2398                 else
2399                         b43_phy_write(dev, rfctl[1], 0x114);
2400
2401                 if (nphy->rxcalparams & 0x10000) {
2402                         b43_radio_maskset(dev, B2055_C1_GENSPARE2, 0xFC,
2403                                         (i + 1));
2404                         b43_radio_maskset(dev, B2055_C2_GENSPARE2, 0xFC,
2405                                         (2 - i));
2406                 }
2407
2408                 for (j = 0; i < 4; j++) {
2409                         if (j < 3) {
2410                                 cur_lna = lna[j];
2411                                 cur_hpf1 = hpf1[j];
2412                                 cur_hpf2 = hpf2[j];
2413                         } else {
2414                                 if (power[1] > 10000) {
2415                                         use = 1;
2416                                         cur_hpf = cur_hpf1;
2417                                         index = 2;
2418                                 } else {
2419                                         if (power[0] > 10000) {
2420                                                 use = 1;
2421                                                 cur_hpf = cur_hpf1;
2422                                                 index = 1;
2423                                         } else {
2424                                                 index = 0;
2425                                                 use = 2;
2426                                                 cur_hpf = cur_hpf2;
2427                                         }
2428                                 }
2429                                 cur_lna = lna[index];
2430                                 cur_hpf1 = hpf1[index];
2431                                 cur_hpf2 = hpf2[index];
2432                                 cur_hpf += desired - hweight32(power[index]);
2433                                 cur_hpf = clamp_val(cur_hpf, 0, 10);
2434                                 if (use == 1)
2435                                         cur_hpf1 = cur_hpf;
2436                                 else
2437                                         cur_hpf2 = cur_hpf;
2438                         }
2439
2440                         tmp[0] = ((cur_hpf2 << 8) | (cur_hpf1 << 4) |
2441                                         (cur_lna << 2));
2442                         b43_nphy_rf_control_override(dev, 0x400, tmp[0], 3,
2443                                                                         false);
2444                         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
2445                         b43_nphy_stop_playback(dev);
2446
2447                         if (playtone) {
2448                                 ret = b43_nphy_tx_tone(dev, 4000,
2449                                                 (nphy->rxcalparams & 0xFFFF),
2450                                                 false, false);
2451                                 playtone = false;
2452                         } else {
2453                                 b43_nphy_run_samples(dev, 160, 0xFFFF, 0,
2454                                                         false, false);
2455                         }
2456
2457                         if (ret == 0) {
2458                                 if (j < 3) {
2459                                         b43_nphy_rx_iq_est(dev, &est, 1024, 32,
2460                                                                         false);
2461                                         if (i == 0) {
2462                                                 real = est.i0_pwr;
2463                                                 imag = est.q0_pwr;
2464                                         } else {
2465                                                 real = est.i1_pwr;
2466                                                 imag = est.q1_pwr;
2467                                         }
2468                                         power[i] = ((real + imag) / 1024) + 1;
2469                                 } else {
2470                                         b43_nphy_calc_rx_iq_comp(dev, 1 << i);
2471                                 }
2472                                 b43_nphy_stop_playback(dev);
2473                         }
2474
2475                         if (ret != 0)
2476                                 break;
2477                 }
2478
2479                 b43_radio_mask(dev, B2055_C1_GENSPARE2, 0xFC);
2480                 b43_radio_mask(dev, B2055_C2_GENSPARE2, 0xFC);
2481                 b43_phy_write(dev, rfctl[1], tmp[5]);
2482                 b43_phy_write(dev, rfctl[0], tmp[4]);
2483                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp[3]);
2484                 b43_phy_write(dev, afectl_core, tmp[2]);
2485                 b43_phy_write(dev, B43_NPHY_RFSEQCA, tmp[1]);
2486
2487                 if (ret != 0)
2488                         break;
2489         }
2490
2491         b43_nphy_rf_control_override(dev, 0x400, 0, 3, true);
2492         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
2493         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, gain_save);
2494
2495         b43_nphy_stay_in_carrier_search(dev, 0);
2496
2497         return ret;
2498 }
2499
2500 static int b43_nphy_rev3_cal_rx_iq(struct b43_wldev *dev,
2501                         struct nphy_txgains target, u8 type, bool debug)
2502 {
2503         return -1;
2504 }
2505
2506 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalRxIq */
2507 static int b43_nphy_cal_rx_iq(struct b43_wldev *dev,
2508                         struct nphy_txgains target, u8 type, bool debug)
2509 {
2510         if (dev->phy.rev >= 3)
2511                 return b43_nphy_rev3_cal_rx_iq(dev, target, type, debug);
2512         else
2513                 return b43_nphy_rev2_cal_rx_iq(dev, target, type, debug);
2514 }
2515
2516 /*
2517  * Init N-PHY
2518  * http://bcm-v4.sipsolutions.net/802.11/PHY/Init/N
2519  */
2520 int b43_phy_initn(struct b43_wldev *dev)
2521 {
2522         struct ssb_bus *bus = dev->dev->bus;
2523         struct b43_phy *phy = &dev->phy;
2524         struct b43_phy_n *nphy = phy->n;
2525         u8 tx_pwr_state;
2526         struct nphy_txgains target;
2527         u16 tmp;
2528         enum ieee80211_band tmp2;
2529         bool do_rssi_cal;
2530
2531         u16 clip[2];
2532         bool do_cal = false;
2533
2534         if ((dev->phy.rev >= 3) &&
2535            (bus->sprom.boardflags_lo & B43_BFL_EXTLNA) &&
2536            (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)) {
2537                 chipco_set32(&dev->dev->bus->chipco, SSB_CHIPCO_CHIPCTL, 0x40);
2538         }
2539         nphy->deaf_count = 0;
2540         b43_nphy_tables_init(dev);
2541         nphy->crsminpwr_adjusted = false;
2542         nphy->noisevars_adjusted = false;
2543
2544         /* Clear all overrides */
2545         if (dev->phy.rev >= 3) {
2546                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S1, 0);
2547                 b43_phy_write(dev, B43_NPHY_RFCTL_OVER, 0);
2548                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S0, 0);
2549                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S1, 0);
2550         } else {
2551                 b43_phy_write(dev, B43_NPHY_RFCTL_OVER, 0);
2552         }
2553         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, 0);
2554         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, 0);
2555         if (dev->phy.rev < 6) {
2556                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC3, 0);
2557                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC4, 0);
2558         }
2559         b43_phy_mask(dev, B43_NPHY_RFSEQMODE,
2560                      ~(B43_NPHY_RFSEQMODE_CAOVER |
2561                        B43_NPHY_RFSEQMODE_TROVER));
2562         if (dev->phy.rev >= 3)
2563                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, 0);
2564         b43_phy_write(dev, B43_NPHY_AFECTL_OVER, 0);
2565
2566         if (dev->phy.rev <= 2) {
2567                 tmp = (dev->phy.rev == 2) ? 0x3B : 0x40;
2568                 b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3,
2569                                 ~B43_NPHY_BPHY_CTL3_SCALE,
2570                                 tmp << B43_NPHY_BPHY_CTL3_SCALE_SHIFT);
2571         }
2572         b43_phy_write(dev, B43_NPHY_AFESEQ_TX2RX_PUD_20M, 0x20);
2573         b43_phy_write(dev, B43_NPHY_AFESEQ_TX2RX_PUD_40M, 0x20);
2574
2575         if (bus->sprom.boardflags2_lo & 0x100 ||
2576             (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
2577              bus->boardinfo.type == 0x8B))
2578                 b43_phy_write(dev, B43_NPHY_TXREALFD, 0xA0);
2579         else
2580                 b43_phy_write(dev, B43_NPHY_TXREALFD, 0xB8);
2581         b43_phy_write(dev, B43_NPHY_MIMO_CRSTXEXT, 0xC8);
2582         b43_phy_write(dev, B43_NPHY_PLOAD_CSENSE_EXTLEN, 0x50);
2583         b43_phy_write(dev, B43_NPHY_TXRIFS_FRDEL, 0x30);
2584
2585         b43_nphy_update_mimo_config(dev, nphy->preamble_override);
2586         b43_nphy_update_txrx_chain(dev);
2587
2588         if (phy->rev < 2) {
2589                 b43_phy_write(dev, B43_NPHY_DUP40_GFBL, 0xAA8);
2590                 b43_phy_write(dev, B43_NPHY_DUP40_BL, 0x9A4);
2591         }
2592
2593         tmp2 = b43_current_band(dev->wl);
2594         if ((nphy->ipa2g_on && tmp2 == IEEE80211_BAND_2GHZ) ||
2595             (nphy->ipa5g_on && tmp2 == IEEE80211_BAND_5GHZ)) {
2596                 b43_phy_set(dev, B43_NPHY_PAPD_EN0, 0x1);
2597                 b43_phy_maskset(dev, B43_NPHY_EPS_TABLE_ADJ0, 0x007F,
2598                                 nphy->papd_epsilon_offset[0] << 7);
2599                 b43_phy_set(dev, B43_NPHY_PAPD_EN1, 0x1);
2600                 b43_phy_maskset(dev, B43_NPHY_EPS_TABLE_ADJ1, 0x007F,
2601                                 nphy->papd_epsilon_offset[1] << 7);
2602                 b43_nphy_int_pa_set_tx_dig_filters(dev);
2603         } else if (phy->rev >= 5) {
2604                 b43_nphy_ext_pa_set_tx_dig_filters(dev);
2605         }
2606
2607         b43_nphy_workarounds(dev);
2608
2609         /* Reset CCA, in init code it differs a little from standard way */
2610         b43_nphy_bmac_clock_fgc(dev, 1);
2611         tmp = b43_phy_read(dev, B43_NPHY_BBCFG);
2612         b43_phy_write(dev, B43_NPHY_BBCFG, tmp | B43_NPHY_BBCFG_RSTCCA);
2613         b43_phy_write(dev, B43_NPHY_BBCFG, tmp & ~B43_NPHY_BBCFG_RSTCCA);
2614         b43_nphy_bmac_clock_fgc(dev, 0);
2615
2616         /* TODO N PHY MAC PHY Clock Set with argument 1 */
2617
2618         b43_nphy_pa_override(dev, false);
2619         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
2620         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
2621         b43_nphy_pa_override(dev, true);
2622
2623         b43_nphy_classifier(dev, 0, 0);
2624         b43_nphy_read_clip_detection(dev, clip);
2625         tx_pwr_state = nphy->txpwrctrl;
2626         /* TODO N PHY TX power control with argument 0
2627                 (turning off power control) */
2628         /* TODO Fix the TX Power Settings */
2629         /* TODO N PHY TX Power Control Idle TSSI */
2630         /* TODO N PHY TX Power Control Setup */
2631
2632         if (phy->rev >= 3) {
2633                 /* TODO */
2634         } else {
2635                 b43_ntab_write_bulk(dev, B43_NTAB32(26, 192), 128,
2636                                         b43_ntab_tx_gain_rev0_1_2);
2637                 b43_ntab_write_bulk(dev, B43_NTAB32(27, 192), 128,
2638                                         b43_ntab_tx_gain_rev0_1_2);
2639         }
2640
2641         if (nphy->phyrxchain != 3)
2642                 ;/* TODO N PHY RX Core Set State with phyrxchain as argument */
2643         if (nphy->mphase_cal_phase_id > 0)
2644                 ;/* TODO PHY Periodic Calibration Multi-Phase Restart */
2645
2646         do_rssi_cal = false;
2647         if (phy->rev >= 3) {
2648                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
2649                         do_rssi_cal = (nphy->rssical_chanspec_2G == 0);
2650                 else
2651                         do_rssi_cal = (nphy->rssical_chanspec_5G == 0);
2652
2653                 if (do_rssi_cal)
2654                         b43_nphy_rssi_cal(dev);
2655                 else
2656                         b43_nphy_restore_rssi_cal(dev);
2657         } else {
2658                 b43_nphy_rssi_cal(dev);
2659         }
2660
2661         if (!((nphy->measure_hold & 0x6) != 0)) {
2662                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
2663                         do_cal = (nphy->iqcal_chanspec_2G == 0);
2664                 else
2665                         do_cal = (nphy->iqcal_chanspec_5G == 0);
2666
2667                 if (nphy->mute)
2668                         do_cal = false;
2669
2670                 if (do_cal) {
2671                         target = b43_nphy_get_tx_gains(dev);
2672
2673                         if (nphy->antsel_type == 2)
2674                                 ;/*TODO NPHY Superswitch Init with argument 1*/
2675                         if (nphy->perical != 2) {
2676                                 b43_nphy_rssi_cal(dev);
2677                                 if (phy->rev >= 3) {
2678                                         nphy->cal_orig_pwr_idx[0] =
2679                                             nphy->txpwrindex[0].index_internal;
2680                                         nphy->cal_orig_pwr_idx[1] =
2681                                             nphy->txpwrindex[1].index_internal;
2682                                         /* TODO N PHY Pre Calibrate TX Gain */
2683                                         target = b43_nphy_get_tx_gains(dev);
2684                                 }
2685                         }
2686                 }
2687         }
2688
2689         if (!b43_nphy_cal_tx_iq_lo(dev, target, true, false)) {
2690                 if (b43_nphy_cal_rx_iq(dev, target, 2, 0) == 0)
2691                         ;/* Call N PHY Save Cal */
2692                 else if (nphy->mphase_cal_phase_id == 0)
2693                         ;/* N PHY Periodic Calibration with argument 3 */
2694         } else {
2695                 b43_nphy_restore_cal(dev);
2696         }
2697
2698         b43_nphy_tx_pwr_ctrl_coef_setup(dev);
2699         /* TODO N PHY TX Power Control Enable with argument tx_pwr_state */
2700         b43_phy_write(dev, B43_NPHY_TXMACIF_HOLDOFF, 0x0015);
2701         b43_phy_write(dev, B43_NPHY_TXMACDELAY, 0x0320);
2702         if (phy->rev >= 3 && phy->rev <= 6)
2703                 b43_phy_write(dev, B43_NPHY_PLOAD_CSENSE_EXTLEN, 0x0014);
2704         b43_nphy_tx_lp_fbw(dev);
2705         /* TODO N PHY Spur Workaround */
2706
2707         b43err(dev->wl, "IEEE 802.11n devices are not supported, yet.\n");
2708         return 0;
2709 }
2710
2711 static int b43_nphy_op_allocate(struct b43_wldev *dev)
2712 {
2713         struct b43_phy_n *nphy;
2714
2715         nphy = kzalloc(sizeof(*nphy), GFP_KERNEL);
2716         if (!nphy)
2717                 return -ENOMEM;
2718         dev->phy.n = nphy;
2719
2720         return 0;
2721 }
2722
2723 static void b43_nphy_op_prepare_structs(struct b43_wldev *dev)
2724 {
2725         struct b43_phy *phy = &dev->phy;
2726         struct b43_phy_n *nphy = phy->n;
2727
2728         memset(nphy, 0, sizeof(*nphy));
2729
2730         //TODO init struct b43_phy_n
2731 }
2732
2733 static void b43_nphy_op_free(struct b43_wldev *dev)
2734 {
2735         struct b43_phy *phy = &dev->phy;
2736         struct b43_phy_n *nphy = phy->n;
2737
2738         kfree(nphy);
2739         phy->n = NULL;
2740 }
2741
2742 static int b43_nphy_op_init(struct b43_wldev *dev)
2743 {
2744         return b43_phy_initn(dev);
2745 }
2746
2747 static inline void check_phyreg(struct b43_wldev *dev, u16 offset)
2748 {
2749 #if B43_DEBUG
2750         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
2751                 /* OFDM registers are onnly available on A/G-PHYs */
2752                 b43err(dev->wl, "Invalid OFDM PHY access at "
2753                        "0x%04X on N-PHY\n", offset);
2754                 dump_stack();
2755         }
2756         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
2757                 /* Ext-G registers are only available on G-PHYs */
2758                 b43err(dev->wl, "Invalid EXT-G PHY access at "
2759                        "0x%04X on N-PHY\n", offset);
2760                 dump_stack();
2761         }
2762 #endif /* B43_DEBUG */
2763 }
2764
2765 static u16 b43_nphy_op_read(struct b43_wldev *dev, u16 reg)
2766 {
2767         check_phyreg(dev, reg);
2768         b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
2769         return b43_read16(dev, B43_MMIO_PHY_DATA);
2770 }
2771
2772 static void b43_nphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
2773 {
2774         check_phyreg(dev, reg);
2775         b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
2776         b43_write16(dev, B43_MMIO_PHY_DATA, value);
2777 }
2778
2779 static u16 b43_nphy_op_radio_read(struct b43_wldev *dev, u16 reg)
2780 {
2781         /* Register 1 is a 32-bit register. */
2782         B43_WARN_ON(reg == 1);
2783         /* N-PHY needs 0x100 for read access */
2784         reg |= 0x100;
2785
2786         b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
2787         return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
2788 }
2789
2790 static void b43_nphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
2791 {
2792         /* Register 1 is a 32-bit register. */
2793         B43_WARN_ON(reg == 1);
2794
2795         b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
2796         b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
2797 }
2798
2799 static void b43_nphy_op_software_rfkill(struct b43_wldev *dev,
2800                                         bool blocked)
2801 {//TODO
2802 }
2803
2804 static void b43_nphy_op_switch_analog(struct b43_wldev *dev, bool on)
2805 {
2806         b43_phy_write(dev, B43_NPHY_AFECTL_OVER,
2807                       on ? 0 : 0x7FFF);
2808 }
2809
2810 static int b43_nphy_op_switch_channel(struct b43_wldev *dev,
2811                                       unsigned int new_channel)
2812 {
2813         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2814                 if ((new_channel < 1) || (new_channel > 14))
2815                         return -EINVAL;
2816         } else {
2817                 if (new_channel > 200)
2818                         return -EINVAL;
2819         }
2820
2821         return nphy_channel_switch(dev, new_channel);
2822 }
2823
2824 static unsigned int b43_nphy_op_get_default_chan(struct b43_wldev *dev)
2825 {
2826         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
2827                 return 1;
2828         return 36;
2829 }
2830
2831 const struct b43_phy_operations b43_phyops_n = {
2832         .allocate               = b43_nphy_op_allocate,
2833         .free                   = b43_nphy_op_free,
2834         .prepare_structs        = b43_nphy_op_prepare_structs,
2835         .init                   = b43_nphy_op_init,
2836         .phy_read               = b43_nphy_op_read,
2837         .phy_write              = b43_nphy_op_write,
2838         .radio_read             = b43_nphy_op_radio_read,
2839         .radio_write            = b43_nphy_op_radio_write,
2840         .software_rfkill        = b43_nphy_op_software_rfkill,
2841         .switch_analog          = b43_nphy_op_switch_analog,
2842         .switch_channel         = b43_nphy_op_switch_channel,
2843         .get_default_chan       = b43_nphy_op_get_default_chan,
2844         .recalc_txpower         = b43_nphy_op_recalc_txpower,
2845         .adjust_txpower         = b43_nphy_op_adjust_txpower,
2846 };