Pileus Git - ~andy/linux/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 <m@bues.ch>
   7   Copyright (c) 2010-2011 Rafał Miłecki <zajec5@gmail.com>
   8
   9   This program is free software; you can redistribute it and/or modify
  10   it under the terms of the GNU General Public License as published by
  11   the Free Software Foundation; either version 2 of the License, or
  12   (at your option) any later version.
  13
  14   This program is distributed in the hope that it will be useful,
  15   but WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17   GNU General Public License for more details.
  18
  19   You should have received a copy of the GNU General Public License
  20   along with this program; see the file COPYING.  If not, write to
  21   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  22   Boston, MA 02110-1301, USA.
  23
  24 */
  25
  26 #include <linux/delay.h>
  27 #include <linux/slab.h>
  28 #include <linux/types.h>
  29
  30 #include "b43.h"
  31 #include "phy_n.h"
  32 #include "tables_nphy.h"
  33 #include "radio_2055.h"
  34 #include "radio_2056.h"
  35 #include "main.h"
  36
  37 struct nphy_txgains {
  38         u16 txgm[2];
  39         u16 pga[2];
  40         u16 pad[2];
  41         u16 ipa[2];
  42 };
  43
  44 struct nphy_iqcal_params {
  45         u16 txgm;
  46         u16 pga;
  47         u16 pad;
  48         u16 ipa;
  49         u16 cal_gain;
  50         u16 ncorr[5];
  51 };
  52
  53 struct nphy_iq_est {
  54         s32 iq0_prod;
  55         u32 i0_pwr;
  56         u32 q0_pwr;
  57         s32 iq1_prod;
  58         u32 i1_pwr;
  59         u32 q1_pwr;
  60 };
  61
  62 enum b43_nphy_rf_sequence {
  63         B43_RFSEQ_RX2TX,
  64         B43_RFSEQ_TX2RX,
  65         B43_RFSEQ_RESET2RX,
  66         B43_RFSEQ_UPDATE_GAINH,
  67         B43_RFSEQ_UPDATE_GAINL,
  68         B43_RFSEQ_UPDATE_GAINU,
  69 };
  70
  71 enum b43_nphy_rssi_type {
  72         B43_NPHY_RSSI_X = 0,
  73         B43_NPHY_RSSI_Y,
  74         B43_NPHY_RSSI_Z,
  75         B43_NPHY_RSSI_PWRDET,
  76         B43_NPHY_RSSI_TSSI_I,
  77         B43_NPHY_RSSI_TSSI_Q,
  78         B43_NPHY_RSSI_TBD,
  79 };
  80
  81 static inline bool b43_nphy_ipa(struct b43_wldev *dev)
  82 {
  83         enum ieee80211_band band = b43_current_band(dev->wl);
  84         return ((dev->phy.n->ipa2g_on && band == IEEE80211_BAND_2GHZ) ||
  85                 (dev->phy.n->ipa5g_on && band == IEEE80211_BAND_5GHZ));
  86 }
  87
  88 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCoreGetState */
  89 static u8 b43_nphy_get_rx_core_state(struct b43_wldev *dev)
  90 {
  91         return (b43_phy_read(dev, B43_NPHY_RFSEQCA) & B43_NPHY_RFSEQCA_RXEN) >>
  92                 B43_NPHY_RFSEQCA_RXEN_SHIFT;
  93 }
  94
  95 /**************************************************
  96  * RF (just without b43_nphy_rf_control_intc_override)
  97  **************************************************/
  98
  99 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ForceRFSeq */
 100 static void b43_nphy_force_rf_sequence(struct b43_wldev *dev,
 101                                        enum b43_nphy_rf_sequence seq)
 102 {
 103         static const u16 trigger[] = {
 104                 [B43_RFSEQ_RX2TX]               = B43_NPHY_RFSEQTR_RX2TX,
 105                 [B43_RFSEQ_TX2RX]               = B43_NPHY_RFSEQTR_TX2RX,
 106                 [B43_RFSEQ_RESET2RX]            = B43_NPHY_RFSEQTR_RST2RX,
 107                 [B43_RFSEQ_UPDATE_GAINH]        = B43_NPHY_RFSEQTR_UPGH,
 108                 [B43_RFSEQ_UPDATE_GAINL]        = B43_NPHY_RFSEQTR_UPGL,
 109                 [B43_RFSEQ_UPDATE_GAINU]        = B43_NPHY_RFSEQTR_UPGU,
 110         };
 111         int i;
 112         u16 seq_mode = b43_phy_read(dev, B43_NPHY_RFSEQMODE);
 113
 114         B43_WARN_ON(seq >= ARRAY_SIZE(trigger));
 115
 116         b43_phy_set(dev, B43_NPHY_RFSEQMODE,
 117                     B43_NPHY_RFSEQMODE_CAOVER | B43_NPHY_RFSEQMODE_TROVER);
 118         b43_phy_set(dev, B43_NPHY_RFSEQTR, trigger[seq]);
 119         for (i = 0; i < 200; i++) {
 120                 if (!(b43_phy_read(dev, B43_NPHY_RFSEQST) & trigger[seq]))
 121                         goto ok;
 122                 msleep(1);
 123         }
 124         b43err(dev->wl, "RF sequence status timeout\n");
 125 ok:
 126         b43_phy_write(dev, B43_NPHY_RFSEQMODE, seq_mode);
 127 }
 128
 129 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlOverride */
 130 static void b43_nphy_rf_control_override(struct b43_wldev *dev, u16 field,
 131                                                 u16 value, u8 core, bool off)
 132 {
 133         int i;
 134         u8 index = fls(field);
 135         u8 addr, en_addr, val_addr;
 136         /* we expect only one bit set */
 137         B43_WARN_ON(field & (~(1 << (index - 1))));
 138
 139         if (dev->phy.rev >= 3) {
 140                 const struct nphy_rf_control_override_rev3 *rf_ctrl;
 141                 for (i = 0; i < 2; i++) {
 142                         if (index == 0 || index == 16) {
 143                                 b43err(dev->wl,
 144                                         "Unsupported RF Ctrl Override call\n");
 145                                 return;
 146                         }
 147
 148                         rf_ctrl = &tbl_rf_control_override_rev3[index - 1];
 149                         en_addr = B43_PHY_N((i == 0) ?
 150                                 rf_ctrl->en_addr0 : rf_ctrl->en_addr1);
 151                         val_addr = B43_PHY_N((i == 0) ?
 152                                 rf_ctrl->val_addr0 : rf_ctrl->val_addr1);
 153
 154                         if (off) {
 155                                 b43_phy_mask(dev, en_addr, ~(field));
 156                                 b43_phy_mask(dev, val_addr,
 157                                                 ~(rf_ctrl->val_mask));
 158                         } else {
 159                                 if (core == 0 || ((1 << i) & core)) {
 160                                         b43_phy_set(dev, en_addr, field);
 161                                         b43_phy_maskset(dev, val_addr,
 162                                                 ~(rf_ctrl->val_mask),
 163                                                 (value << rf_ctrl->val_shift));
 164                                 }
 165                         }
 166                 }
 167         } else {
 168                 const struct nphy_rf_control_override_rev2 *rf_ctrl;
 169                 if (off) {
 170                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, ~(field));
 171                         value = 0;
 172                 } else {
 173                         b43_phy_set(dev, B43_NPHY_RFCTL_OVER, field);
 174                 }
 175
 176                 for (i = 0; i < 2; i++) {
 177                         if (index <= 1 || index == 16) {
 178                                 b43err(dev->wl,
 179                                         "Unsupported RF Ctrl Override call\n");
 180                                 return;
 181                         }
 182
 183                         if (index == 2 || index == 10 ||
 184                             (index >= 13 && index <= 15)) {
 185                                 core = 1;
 186                         }
 187
 188                         rf_ctrl = &tbl_rf_control_override_rev2[index - 2];
 189                         addr = B43_PHY_N((i == 0) ?
 190                                 rf_ctrl->addr0 : rf_ctrl->addr1);
 191
 192                         if ((1 << i) & core)
 193                                 b43_phy_maskset(dev, addr, ~(rf_ctrl->bmask),
 194                                                 (value << rf_ctrl->shift));
 195
 196                         b43_phy_set(dev, B43_NPHY_RFCTL_OVER, 0x1);
 197                         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 198                                         B43_NPHY_RFCTL_CMD_START);
 199                         udelay(1);
 200                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, 0xFFFE);
 201                 }
 202         }
 203 }
 204
 205 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlIntcOverride */
 206 static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
 207                                                 u16 value, u8 core)
 208 {
 209         u8 i, j;
 210         u16 reg, tmp, val;
 211
 212         B43_WARN_ON(dev->phy.rev < 3);
 213         B43_WARN_ON(field > 4);
 214
 215         for (i = 0; i < 2; i++) {
 216                 if ((core == 1 && i == 1) || (core == 2 && !i))
 217                         continue;
 218
 219                 reg = (i == 0) ?
 220                         B43_NPHY_RFCTL_INTC1 : B43_NPHY_RFCTL_INTC2;
 221                 b43_phy_set(dev, reg, 0x400);
 222
 223                 switch (field) {
 224                 case 0:
 225                         b43_phy_write(dev, reg, 0);
 226                         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 227                         break;
 228                 case 1:
 229                         if (!i) {
 230                                 b43_phy_maskset(dev, B43_NPHY_RFCTL_INTC1,
 231                                                 0xFC3F, (value << 6));
 232                                 b43_phy_maskset(dev, B43_NPHY_TXF_40CO_B1S1,
 233                                                 0xFFFE, 1);
 234                                 b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 235                                                 B43_NPHY_RFCTL_CMD_START);
 236                                 for (j = 0; j < 100; j++) {
 237                                         if (!(b43_phy_read(dev, B43_NPHY_RFCTL_CMD) & B43_NPHY_RFCTL_CMD_START)) {
 238                                                 j = 0;
 239                                                 break;
 240                                         }
 241                                         udelay(10);
 242                                 }
 243                                 if (j)
 244                                         b43err(dev->wl,
 245                                                 "intc override timeout\n");
 246                                 b43_phy_mask(dev, B43_NPHY_TXF_40CO_B1S1,
 247                                                 0xFFFE);
 248                         } else {
 249                                 b43_phy_maskset(dev, B43_NPHY_RFCTL_INTC2,
 250                                                 0xFC3F, (value << 6));
 251                                 b43_phy_maskset(dev, B43_NPHY_RFCTL_OVER,
 252                                                 0xFFFE, 1);
 253                                 b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 254                                                 B43_NPHY_RFCTL_CMD_RXTX);
 255                                 for (j = 0; j < 100; j++) {
 256                                         if (!(b43_phy_read(dev, B43_NPHY_RFCTL_CMD) & B43_NPHY_RFCTL_CMD_RXTX)) {
 257                                                 j = 0;
 258                                                 break;
 259                                         }
 260                                         udelay(10);
 261                                 }
 262                                 if (j)
 263                                         b43err(dev->wl,
 264                                                 "intc override timeout\n");
 265                                 b43_phy_mask(dev, B43_NPHY_RFCTL_OVER,
 266                                                 0xFFFE);
 267                         }
 268                         break;
 269                 case 2:
 270                         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 271                                 tmp = 0x0020;
 272                                 val = value << 5;
 273                         } else {
 274                                 tmp = 0x0010;
 275                                 val = value << 4;
 276                         }
 277                         b43_phy_maskset(dev, reg, ~tmp, val);
 278                         break;
 279                 case 3:
 280                         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 281                                 tmp = 0x0001;
 282                                 val = value;
 283                         } else {
 284                                 tmp = 0x0004;
 285                                 val = value << 2;
 286                         }
 287                         b43_phy_maskset(dev, reg, ~tmp, val);
 288                         break;
 289                 case 4:
 290                         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 291                                 tmp = 0x0002;
 292                                 val = value << 1;
 293                         } else {
 294                                 tmp = 0x0008;
 295                                 val = value << 3;
 296                         }
 297                         b43_phy_maskset(dev, reg, ~tmp, val);
 298                         break;
 299                 }
 300         }
 301 }
 302
 303 /**************************************************
 304  * Various PHY ops
 305  **************************************************/
 306
 307 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/clip-detection */
 308 static void b43_nphy_write_clip_detection(struct b43_wldev *dev,
 309                                           const u16 *clip_st)
 310 {
 311         b43_phy_write(dev, B43_NPHY_C1_CLIP1THRES, clip_st[0]);
 312         b43_phy_write(dev, B43_NPHY_C2_CLIP1THRES, clip_st[1]);
 313 }
 314
 315 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/clip-detection */
 316 static void b43_nphy_read_clip_detection(struct b43_wldev *dev, u16 *clip_st)
 317 {
 318         clip_st[0] = b43_phy_read(dev, B43_NPHY_C1_CLIP1THRES);
 319         clip_st[1] = b43_phy_read(dev, B43_NPHY_C2_CLIP1THRES);
 320 }
 321
 322 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/classifier */
 323 static u16 b43_nphy_classifier(struct b43_wldev *dev, u16 mask, u16 val)
 324 {
 325         u16 tmp;
 326
 327         if (dev->dev->core_rev == 16)
 328                 b43_mac_suspend(dev);
 329
 330         tmp = b43_phy_read(dev, B43_NPHY_CLASSCTL);
 331         tmp &= (B43_NPHY_CLASSCTL_CCKEN | B43_NPHY_CLASSCTL_OFDMEN |
 332                 B43_NPHY_CLASSCTL_WAITEDEN);
 333         tmp &= ~mask;
 334         tmp |= (val & mask);
 335         b43_phy_maskset(dev, B43_NPHY_CLASSCTL, 0xFFF8, tmp);
 336
 337         if (dev->dev->core_rev == 16)
 338                 b43_mac_enable(dev);
 339
 340         return tmp;
 341 }
 342
 343 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CCA */
 344 static void b43_nphy_reset_cca(struct b43_wldev *dev)
 345 {
 346         u16 bbcfg;
 347
 348         b43_phy_force_clock(dev, 1);
 349         bbcfg = b43_phy_read(dev, B43_NPHY_BBCFG);
 350         b43_phy_write(dev, B43_NPHY_BBCFG, bbcfg | B43_NPHY_BBCFG_RSTCCA);
 351         udelay(1);
 352         b43_phy_write(dev, B43_NPHY_BBCFG, bbcfg & ~B43_NPHY_BBCFG_RSTCCA);
 353         b43_phy_force_clock(dev, 0);
 354         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 355 }
 356
 357 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/carriersearch */
 358 static void b43_nphy_stay_in_carrier_search(struct b43_wldev *dev, bool enable)
 359 {
 360         struct b43_phy *phy = &dev->phy;
 361         struct b43_phy_n *nphy = phy->n;
 362
 363         if (enable) {
 364                 static const u16 clip[] = { 0xFFFF, 0xFFFF };
 365                 if (nphy->deaf_count++ == 0) {
 366                         nphy->classifier_state = b43_nphy_classifier(dev, 0, 0);
 367                         b43_nphy_classifier(dev, 0x7, 0);
 368                         b43_nphy_read_clip_detection(dev, nphy->clip_state);
 369                         b43_nphy_write_clip_detection(dev, clip);
 370                 }
 371                 b43_nphy_reset_cca(dev);
 372         } else {
 373                 if (--nphy->deaf_count == 0) {
 374                         b43_nphy_classifier(dev, 0x7, nphy->classifier_state);
 375                         b43_nphy_write_clip_detection(dev, nphy->clip_state);
 376                 }
 377         }
 378 }
 379
 380 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/AdjustLnaGainTbl */
 381 static void b43_nphy_adjust_lna_gain_table(struct b43_wldev *dev)
 382 {
 383         struct b43_phy_n *nphy = dev->phy.n;
 384
 385         u8 i;
 386         s16 tmp;
 387         u16 data[4];
 388         s16 gain[2];
 389         u16 minmax[2];
 390         static const u16 lna_gain[4] = { -2, 10, 19, 25 };
 391
 392         if (nphy->hang_avoid)
 393                 b43_nphy_stay_in_carrier_search(dev, 1);
 394
 395         if (nphy->gain_boost) {
 396                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
 397                         gain[0] = 6;
 398                         gain[1] = 6;
 399                 } else {
 400                         tmp = 40370 - 315 * dev->phy.channel;
 401                         gain[0] = ((tmp >> 13) + ((tmp >> 12) & 1));
 402                         tmp = 23242 - 224 * dev->phy.channel;
 403                         gain[1] = ((tmp >> 13) + ((tmp >> 12) & 1));
 404                 }
 405         } else {
 406                 gain[0] = 0;
 407                 gain[1] = 0;
 408         }
 409
 410         for (i = 0; i < 2; i++) {
 411                 if (nphy->elna_gain_config) {
 412                         data[0] = 19 + gain[i];
 413                         data[1] = 25 + gain[i];
 414                         data[2] = 25 + gain[i];
 415                         data[3] = 25 + gain[i];
 416                 } else {
 417                         data[0] = lna_gain[0] + gain[i];
 418                         data[1] = lna_gain[1] + gain[i];
 419                         data[2] = lna_gain[2] + gain[i];
 420                         data[3] = lna_gain[3] + gain[i];
 421                 }
 422                 b43_ntab_write_bulk(dev, B43_NTAB16(i, 8), 4, data);
 423
 424                 minmax[i] = 23 + gain[i];
 425         }
 426
 427         b43_phy_maskset(dev, B43_NPHY_C1_MINMAX_GAIN, ~B43_NPHY_C1_MINGAIN,
 428                                 minmax[0] << B43_NPHY_C1_MINGAIN_SHIFT);
 429         b43_phy_maskset(dev, B43_NPHY_C2_MINMAX_GAIN, ~B43_NPHY_C2_MINGAIN,
 430                                 minmax[1] << B43_NPHY_C2_MINGAIN_SHIFT);
 431
 432         if (nphy->hang_avoid)
 433                 b43_nphy_stay_in_carrier_search(dev, 0);
 434 }
 435
 436 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetRfSeq */
 437 static void b43_nphy_set_rf_sequence(struct b43_wldev *dev, u8 cmd,
 438                                         u8 *events, u8 *delays, u8 length)
 439 {
 440         struct b43_phy_n *nphy = dev->phy.n;
 441         u8 i;
 442         u8 end = (dev->phy.rev >= 3) ? 0x1F : 0x0F;
 443         u16 offset1 = cmd << 4;
 444         u16 offset2 = offset1 + 0x80;
 445
 446         if (nphy->hang_avoid)
 447                 b43_nphy_stay_in_carrier_search(dev, true);
 448
 449         b43_ntab_write_bulk(dev, B43_NTAB8(7, offset1), length, events);
 450         b43_ntab_write_bulk(dev, B43_NTAB8(7, offset2), length, delays);
 451
 452         for (i = length; i < 16; i++) {
 453                 b43_ntab_write(dev, B43_NTAB8(7, offset1 + i), end);
 454                 b43_ntab_write(dev, B43_NTAB8(7, offset2 + i), 1);
 455         }
 456
 457         if (nphy->hang_avoid)
 458                 b43_nphy_stay_in_carrier_search(dev, false);
 459 }
 460
 461 /**************************************************
 462  * Radio 0x2056
 463  **************************************************/
 464
 465 static void b43_chantab_radio_2056_upload(struct b43_wldev *dev,
 466                                 const struct b43_nphy_channeltab_entry_rev3 *e)
 467 {
 468         b43_radio_write(dev, B2056_SYN_PLL_VCOCAL1, e->radio_syn_pll_vcocal1);
 469         b43_radio_write(dev, B2056_SYN_PLL_VCOCAL2, e->radio_syn_pll_vcocal2);
 470         b43_radio_write(dev, B2056_SYN_PLL_REFDIV, e->radio_syn_pll_refdiv);
 471         b43_radio_write(dev, B2056_SYN_PLL_MMD2, e->radio_syn_pll_mmd2);
 472         b43_radio_write(dev, B2056_SYN_PLL_MMD1, e->radio_syn_pll_mmd1);
 473         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER1,
 474                                         e->radio_syn_pll_loopfilter1);
 475         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER2,
 476                                         e->radio_syn_pll_loopfilter2);
 477         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER3,
 478                                         e->radio_syn_pll_loopfilter3);
 479         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER4,
 480                                         e->radio_syn_pll_loopfilter4);
 481         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER5,
 482                                         e->radio_syn_pll_loopfilter5);
 483         b43_radio_write(dev, B2056_SYN_RESERVED_ADDR27,
 484                                         e->radio_syn_reserved_addr27);
 485         b43_radio_write(dev, B2056_SYN_RESERVED_ADDR28,
 486                                         e->radio_syn_reserved_addr28);
 487         b43_radio_write(dev, B2056_SYN_RESERVED_ADDR29,
 488                                         e->radio_syn_reserved_addr29);
 489         b43_radio_write(dev, B2056_SYN_LOGEN_VCOBUF1,
 490                                         e->radio_syn_logen_vcobuf1);
 491         b43_radio_write(dev, B2056_SYN_LOGEN_MIXER2, e->radio_syn_logen_mixer2);
 492         b43_radio_write(dev, B2056_SYN_LOGEN_BUF3, e->radio_syn_logen_buf3);
 493         b43_radio_write(dev, B2056_SYN_LOGEN_BUF4, e->radio_syn_logen_buf4);
 494
 495         b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAA_TUNE,
 496                                         e->radio_rx0_lnaa_tune);
 497         b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAG_TUNE,
 498                                         e->radio_rx0_lnag_tune);
 499
 500         b43_radio_write(dev, B2056_TX0 | B2056_TX_INTPAA_BOOST_TUNE,
 501                                         e->radio_tx0_intpaa_boost_tune);
 502         b43_radio_write(dev, B2056_TX0 | B2056_TX_INTPAG_BOOST_TUNE,
 503                                         e->radio_tx0_intpag_boost_tune);
 504         b43_radio_write(dev, B2056_TX0 | B2056_TX_PADA_BOOST_TUNE,
 505                                         e->radio_tx0_pada_boost_tune);
 506         b43_radio_write(dev, B2056_TX0 | B2056_TX_PADG_BOOST_TUNE,
 507                                         e->radio_tx0_padg_boost_tune);
 508         b43_radio_write(dev, B2056_TX0 | B2056_TX_PGAA_BOOST_TUNE,
 509                                         e->radio_tx0_pgaa_boost_tune);
 510         b43_radio_write(dev, B2056_TX0 | B2056_TX_PGAG_BOOST_TUNE,
 511                                         e->radio_tx0_pgag_boost_tune);
 512         b43_radio_write(dev, B2056_TX0 | B2056_TX_MIXA_BOOST_TUNE,
 513                                         e->radio_tx0_mixa_boost_tune);
 514         b43_radio_write(dev, B2056_TX0 | B2056_TX_MIXG_BOOST_TUNE,
 515                                         e->radio_tx0_mixg_boost_tune);
 516
 517         b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAA_TUNE,
 518                                         e->radio_rx1_lnaa_tune);
 519         b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAG_TUNE,
 520                                         e->radio_rx1_lnag_tune);
 521
 522         b43_radio_write(dev, B2056_TX1 | B2056_TX_INTPAA_BOOST_TUNE,
 523                                         e->radio_tx1_intpaa_boost_tune);
 524         b43_radio_write(dev, B2056_TX1 | B2056_TX_INTPAG_BOOST_TUNE,
 525                                         e->radio_tx1_intpag_boost_tune);
 526         b43_radio_write(dev, B2056_TX1 | B2056_TX_PADA_BOOST_TUNE,
 527                                         e->radio_tx1_pada_boost_tune);
 528         b43_radio_write(dev, B2056_TX1 | B2056_TX_PADG_BOOST_TUNE,
 529                                         e->radio_tx1_padg_boost_tune);
 530         b43_radio_write(dev, B2056_TX1 | B2056_TX_PGAA_BOOST_TUNE,
 531                                         e->radio_tx1_pgaa_boost_tune);
 532         b43_radio_write(dev, B2056_TX1 | B2056_TX_PGAG_BOOST_TUNE,
 533                                         e->radio_tx1_pgag_boost_tune);
 534         b43_radio_write(dev, B2056_TX1 | B2056_TX_MIXA_BOOST_TUNE,
 535                                         e->radio_tx1_mixa_boost_tune);
 536         b43_radio_write(dev, B2056_TX1 | B2056_TX_MIXG_BOOST_TUNE,
 537                                         e->radio_tx1_mixg_boost_tune);
 538 }
 539
 540 /* http://bcm-v4.sipsolutions.net/802.11/PHY/Radio/2056Setup */
 541 static void b43_radio_2056_setup(struct b43_wldev *dev,
 542                                 const struct b43_nphy_channeltab_entry_rev3 *e)
 543 {
 544         struct ssb_sprom *sprom = dev->dev->bus_sprom;
 545         enum ieee80211_band band = b43_current_band(dev->wl);
 546         u16 offset;
 547         u8 i;
 548         u16 bias, cbias, pag_boost, pgag_boost, mixg_boost, padg_boost;
 549
 550         B43_WARN_ON(dev->phy.rev < 3);
 551
 552         b43_chantab_radio_2056_upload(dev, e);
 553         b2056_upload_syn_pll_cp2(dev, band == IEEE80211_BAND_5GHZ);
 554
 555         if (sprom->boardflags2_lo & B43_BFL2_GPLL_WAR &&
 556             b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
 557                 b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER1, 0x1F);
 558                 b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER2, 0x1F);
 559                 if (dev->dev->chip_id == 0x4716) {
 560                         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER4, 0x14);
 561                         b43_radio_write(dev, B2056_SYN_PLL_CP2, 0);
 562                 } else {
 563                         b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER4, 0x0B);
 564                         b43_radio_write(dev, B2056_SYN_PLL_CP2, 0x14);
 565                 }
 566         }
 567         if (sprom->boardflags2_lo & B43_BFL2_APLL_WAR &&
 568             b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 569                 b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER1, 0x1F);
 570                 b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER2, 0x1F);
 571                 b43_radio_write(dev, B2056_SYN_PLL_LOOPFILTER4, 0x05);
 572                 b43_radio_write(dev, B2056_SYN_PLL_CP2, 0x0C);
 573         }
 574
 575         if (dev->phy.n->ipa2g_on && band == IEEE80211_BAND_2GHZ) {
 576                 for (i = 0; i < 2; i++) {
 577                         offset = i ? B2056_TX1 : B2056_TX0;
 578                         if (dev->phy.rev >= 5) {
 579                                 b43_radio_write(dev,
 580                                         offset | B2056_TX_PADG_IDAC, 0xcc);
 581
 582                                 if (dev->dev->chip_id == 0x4716) {
 583                                         bias = 0x40;
 584                                         cbias = 0x45;
 585                                         pag_boost = 0x5;
 586                                         pgag_boost = 0x33;
 587                                         mixg_boost = 0x55;
 588                                 } else {
 589                                         bias = 0x25;
 590                                         cbias = 0x20;
 591                                         pag_boost = 0x4;
 592                                         pgag_boost = 0x03;
 593                                         mixg_boost = 0x65;
 594                                 }
 595                                 padg_boost = 0x77;
 596
 597                                 b43_radio_write(dev,
 598                                         offset | B2056_TX_INTPAG_IMAIN_STAT,
 599                                         bias);
 600                                 b43_radio_write(dev,
 601                                         offset | B2056_TX_INTPAG_IAUX_STAT,
 602                                         bias);
 603                                 b43_radio_write(dev,
 604                                         offset | B2056_TX_INTPAG_CASCBIAS,
 605                                         cbias);
 606                                 b43_radio_write(dev,
 607                                         offset | B2056_TX_INTPAG_BOOST_TUNE,
 608                                         pag_boost);
 609                                 b43_radio_write(dev,
 610                                         offset | B2056_TX_PGAG_BOOST_TUNE,
 611                                         pgag_boost);
 612                                 b43_radio_write(dev,
 613                                         offset | B2056_TX_PADG_BOOST_TUNE,
 614                                         padg_boost);
 615                                 b43_radio_write(dev,
 616                                         offset | B2056_TX_MIXG_BOOST_TUNE,
 617                                         mixg_boost);
 618                         } else {
 619                                 bias = dev->phy.is_40mhz ? 0x40 : 0x20;
 620                                 b43_radio_write(dev,
 621                                         offset | B2056_TX_INTPAG_IMAIN_STAT,
 622                                         bias);
 623                                 b43_radio_write(dev,
 624                                         offset | B2056_TX_INTPAG_IAUX_STAT,
 625                                         bias);
 626                                 b43_radio_write(dev,
 627                                         offset | B2056_TX_INTPAG_CASCBIAS,
 628                                         0x30);
 629                         }
 630                         b43_radio_write(dev, offset | B2056_TX_PA_SPARE1, 0xee);
 631                 }
 632         } else if (dev->phy.n->ipa5g_on && band == IEEE80211_BAND_5GHZ) {
 633                 /* TODO */
 634         }
 635
 636         udelay(50);
 637         /* VCO calibration */
 638         b43_radio_write(dev, B2056_SYN_PLL_VCOCAL12, 0x00);
 639         b43_radio_write(dev, B2056_TX_INTPAA_PA_MISC, 0x38);
 640         b43_radio_write(dev, B2056_TX_INTPAA_PA_MISC, 0x18);
 641         b43_radio_write(dev, B2056_TX_INTPAA_PA_MISC, 0x38);
 642         b43_radio_write(dev, B2056_TX_INTPAA_PA_MISC, 0x39);
 643         udelay(300);
 644 }
 645
 646 static void b43_radio_init2056_pre(struct b43_wldev *dev)
 647 {
 648         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 649                      ~B43_NPHY_RFCTL_CMD_CHIP0PU);
 650         /* Maybe wl meant to reset and set (order?) RFCTL_CMD_OEPORFORCE? */
 651         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 652                      B43_NPHY_RFCTL_CMD_OEPORFORCE);
 653         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 654                     ~B43_NPHY_RFCTL_CMD_OEPORFORCE);
 655         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 656                     B43_NPHY_RFCTL_CMD_CHIP0PU);
 657 }
 658
 659 static void b43_radio_init2056_post(struct b43_wldev *dev)
 660 {
 661         b43_radio_set(dev, B2056_SYN_COM_CTRL, 0xB);
 662         b43_radio_set(dev, B2056_SYN_COM_PU, 0x2);
 663         b43_radio_set(dev, B2056_SYN_COM_RESET, 0x2);
 664         msleep(1);
 665         b43_radio_mask(dev, B2056_SYN_COM_RESET, ~0x2);
 666         b43_radio_mask(dev, B2056_SYN_PLL_MAST2, ~0xFC);
 667         b43_radio_mask(dev, B2056_SYN_RCCAL_CTRL0, ~0x1);
 668         /*
 669         if (nphy->init_por)
 670                 Call Radio 2056 Recalibrate
 671         */
 672 }
 673
 674 /*
 675  * Initialize a Broadcom 2056 N-radio
 676  * http://bcm-v4.sipsolutions.net/802.11/Radio/2056/Init
 677  */
 678 static void b43_radio_init2056(struct b43_wldev *dev)
 679 {
 680         b43_radio_init2056_pre(dev);
 681         b2056_upload_inittabs(dev, 0, 0);
 682         b43_radio_init2056_post(dev);
 683 }
 684
 685 /**************************************************
 686  * Radio 0x2055
 687  **************************************************/
 688
 689 static void b43_chantab_radio_upload(struct b43_wldev *dev,
 690                                 const struct b43_nphy_channeltab_entry_rev2 *e)
 691 {
 692         b43_radio_write(dev, B2055_PLL_REF, e->radio_pll_ref);
 693         b43_radio_write(dev, B2055_RF_PLLMOD0, e->radio_rf_pllmod0);
 694         b43_radio_write(dev, B2055_RF_PLLMOD1, e->radio_rf_pllmod1);
 695         b43_radio_write(dev, B2055_VCO_CAPTAIL, e->radio_vco_captail);
 696         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 697
 698         b43_radio_write(dev, B2055_VCO_CAL1, e->radio_vco_cal1);
 699         b43_radio_write(dev, B2055_VCO_CAL2, e->radio_vco_cal2);
 700         b43_radio_write(dev, B2055_PLL_LFC1, e->radio_pll_lfc1);
 701         b43_radio_write(dev, B2055_PLL_LFR1, e->radio_pll_lfr1);
 702         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 703
 704         b43_radio_write(dev, B2055_PLL_LFC2, e->radio_pll_lfc2);
 705         b43_radio_write(dev, B2055_LGBUF_CENBUF, e->radio_lgbuf_cenbuf);
 706         b43_radio_write(dev, B2055_LGEN_TUNE1, e->radio_lgen_tune1);
 707         b43_radio_write(dev, B2055_LGEN_TUNE2, e->radio_lgen_tune2);
 708         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 709
 710         b43_radio_write(dev, B2055_C1_LGBUF_ATUNE, e->radio_c1_lgbuf_atune);
 711         b43_radio_write(dev, B2055_C1_LGBUF_GTUNE, e->radio_c1_lgbuf_gtune);
 712         b43_radio_write(dev, B2055_C1_RX_RFR1, e->radio_c1_rx_rfr1);
 713         b43_radio_write(dev, B2055_C1_TX_PGAPADTN, e->radio_c1_tx_pgapadtn);
 714         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 715
 716         b43_radio_write(dev, B2055_C1_TX_MXBGTRIM, e->radio_c1_tx_mxbgtrim);
 717         b43_radio_write(dev, B2055_C2_LGBUF_ATUNE, e->radio_c2_lgbuf_atune);
 718         b43_radio_write(dev, B2055_C2_LGBUF_GTUNE, e->radio_c2_lgbuf_gtune);
 719         b43_radio_write(dev, B2055_C2_RX_RFR1, e->radio_c2_rx_rfr1);
 720         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 721
 722         b43_radio_write(dev, B2055_C2_TX_PGAPADTN, e->radio_c2_tx_pgapadtn);
 723         b43_radio_write(dev, B2055_C2_TX_MXBGTRIM, e->radio_c2_tx_mxbgtrim);
 724 }
 725
 726 /* http://bcm-v4.sipsolutions.net/802.11/PHY/Radio/2055Setup */
 727 static void b43_radio_2055_setup(struct b43_wldev *dev,
 728                                 const struct b43_nphy_channeltab_entry_rev2 *e)
 729 {
 730         B43_WARN_ON(dev->phy.rev >= 3);
 731
 732         b43_chantab_radio_upload(dev, e);
 733         udelay(50);
 734         b43_radio_write(dev, B2055_VCO_CAL10, 0x05);
 735         b43_radio_write(dev, B2055_VCO_CAL10, 0x45);
 736         b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
 737         b43_radio_write(dev, B2055_VCO_CAL10, 0x65);
 738         udelay(300);
 739 }
 740
 741 static void b43_radio_init2055_pre(struct b43_wldev *dev)
 742 {
 743         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 744                      ~B43_NPHY_RFCTL_CMD_PORFORCE);
 745         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 746                     B43_NPHY_RFCTL_CMD_CHIP0PU |
 747                     B43_NPHY_RFCTL_CMD_OEPORFORCE);
 748         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
 749                     B43_NPHY_RFCTL_CMD_PORFORCE);
 750 }
 751
 752 static void b43_radio_init2055_post(struct b43_wldev *dev)
 753 {
 754         struct b43_phy_n *nphy = dev->phy.n;
 755         struct ssb_sprom *sprom = dev->dev->bus_sprom;
 756         int i;
 757         u16 val;
 758         bool workaround = false;
 759
 760         if (sprom->revision < 4)
 761                 workaround = (dev->dev->board_vendor != PCI_VENDOR_ID_BROADCOM
 762                               && dev->dev->board_type == 0x46D
 763                               && dev->dev->board_rev >= 0x41);
 764         else
 765                 workaround =
 766                         !(sprom->boardflags2_lo & B43_BFL2_RXBB_INT_REG_DIS);
 767
 768         b43_radio_mask(dev, B2055_MASTER1, 0xFFF3);
 769         if (workaround) {
 770                 b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
 771                 b43_radio_mask(dev, B2055_C2_RX_BB_REG, 0x7F);
 772         }
 773         b43_radio_maskset(dev, B2055_RRCCAL_NOPTSEL, 0xFFC0, 0x2C);
 774         b43_radio_write(dev, B2055_CAL_MISC, 0x3C);
 775         b43_radio_mask(dev, B2055_CAL_MISC, 0xFFBE);
 776         b43_radio_set(dev, B2055_CAL_LPOCTL, 0x80);
 777         b43_radio_set(dev, B2055_CAL_MISC, 0x1);
 778         msleep(1);
 779         b43_radio_set(dev, B2055_CAL_MISC, 0x40);
 780         for (i = 0; i < 200; i++) {
 781                 val = b43_radio_read(dev, B2055_CAL_COUT2);
 782                 if (val & 0x80) {
 783                         i = 0;
 784                         break;
 785                 }
 786                 udelay(10);
 787         }
 788         if (i)
 789                 b43err(dev->wl, "radio post init timeout\n");
 790         b43_radio_mask(dev, B2055_CAL_LPOCTL, 0xFF7F);
 791         b43_switch_channel(dev, dev->phy.channel);
 792         b43_radio_write(dev, B2055_C1_RX_BB_LPF, 0x9);
 793         b43_radio_write(dev, B2055_C2_RX_BB_LPF, 0x9);
 794         b43_radio_write(dev, B2055_C1_RX_BB_MIDACHP, 0x83);
 795         b43_radio_write(dev, B2055_C2_RX_BB_MIDACHP, 0x83);
 796         b43_radio_maskset(dev, B2055_C1_LNA_GAINBST, 0xFFF8, 0x6);
 797         b43_radio_maskset(dev, B2055_C2_LNA_GAINBST, 0xFFF8, 0x6);
 798         if (!nphy->gain_boost) {
 799                 b43_radio_set(dev, B2055_C1_RX_RFSPC1, 0x2);
 800                 b43_radio_set(dev, B2055_C2_RX_RFSPC1, 0x2);
 801         } else {
 802                 b43_radio_mask(dev, B2055_C1_RX_RFSPC1, 0xFFFD);
 803                 b43_radio_mask(dev, B2055_C2_RX_RFSPC1, 0xFFFD);
 804         }
 805         udelay(2);
 806 }
 807
 808 /*
 809  * Initialize a Broadcom 2055 N-radio
 810  * http://bcm-v4.sipsolutions.net/802.11/Radio/2055/Init
 811  */
 812 static void b43_radio_init2055(struct b43_wldev *dev)
 813 {
 814         b43_radio_init2055_pre(dev);
 815         if (b43_status(dev) < B43_STAT_INITIALIZED) {
 816                 /* Follow wl, not specs. Do not force uploading all regs */
 817                 b2055_upload_inittab(dev, 0, 0);
 818         } else {
 819                 bool ghz5 = b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ;
 820                 b2055_upload_inittab(dev, ghz5, 0);
 821         }
 822         b43_radio_init2055_post(dev);
 823 }
 824
 825 /**************************************************
 826  * Samples
 827  **************************************************/
 828
 829 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/LoadSampleTable */
 830 static int b43_nphy_load_samples(struct b43_wldev *dev,
 831                                         struct b43_c32 *samples, u16 len) {
 832         struct b43_phy_n *nphy = dev->phy.n;
 833         u16 i;
 834         u32 *data;
 835
 836         data = kzalloc(len * sizeof(u32), GFP_KERNEL);
 837         if (!data) {
 838                 b43err(dev->wl, "allocation for samples loading failed\n");
 839                 return -ENOMEM;
 840         }
 841         if (nphy->hang_avoid)
 842                 b43_nphy_stay_in_carrier_search(dev, 1);
 843
 844         for (i = 0; i < len; i++) {
 845                 data[i] = (samples[i].i & 0x3FF << 10);
 846                 data[i] |= samples[i].q & 0x3FF;
 847         }
 848         b43_ntab_write_bulk(dev, B43_NTAB32(17, 0), len, data);
 849
 850         kfree(data);
 851         if (nphy->hang_avoid)
 852                 b43_nphy_stay_in_carrier_search(dev, 0);
 853         return 0;
 854 }
 855
 856 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GenLoadSamples */
 857 static u16 b43_nphy_gen_load_samples(struct b43_wldev *dev, u32 freq, u16 max,
 858                                         bool test)
 859 {
 860         int i;
 861         u16 bw, len, rot, angle;
 862         struct b43_c32 *samples;
 863
 864
 865         bw = (dev->phy.is_40mhz) ? 40 : 20;
 866         len = bw << 3;
 867
 868         if (test) {
 869                 if (b43_phy_read(dev, B43_NPHY_BBCFG) & B43_NPHY_BBCFG_RSTRX)
 870                         bw = 82;
 871                 else
 872                         bw = 80;
 873
 874                 if (dev->phy.is_40mhz)
 875                         bw <<= 1;
 876
 877                 len = bw << 1;
 878         }
 879
 880         samples = kcalloc(len, sizeof(struct b43_c32), GFP_KERNEL);
 881         if (!samples) {
 882                 b43err(dev->wl, "allocation for samples generation failed\n");
 883                 return 0;
 884         }
 885         rot = (((freq * 36) / bw) << 16) / 100;
 886         angle = 0;
 887
 888         for (i = 0; i < len; i++) {
 889                 samples[i] = b43_cordic(angle);
 890                 angle += rot;
 891                 samples[i].q = CORDIC_CONVERT(samples[i].q * max);
 892                 samples[i].i = CORDIC_CONVERT(samples[i].i * max);
 893         }
 894
 895         i = b43_nphy_load_samples(dev, samples, len);
 896         kfree(samples);
 897         return (i < 0) ? 0 : len;
 898 }
 899
 900 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RunSamples */
 901 static void b43_nphy_run_samples(struct b43_wldev *dev, u16 samps, u16 loops,
 902                                         u16 wait, bool iqmode, bool dac_test)
 903 {
 904         struct b43_phy_n *nphy = dev->phy.n;
 905         int i;
 906         u16 seq_mode;
 907         u32 tmp;
 908
 909         if (nphy->hang_avoid)
 910                 b43_nphy_stay_in_carrier_search(dev, true);
 911
 912         if ((nphy->bb_mult_save & 0x80000000) == 0) {
 913                 tmp = b43_ntab_read(dev, B43_NTAB16(15, 87));
 914                 nphy->bb_mult_save = (tmp & 0xFFFF) | 0x80000000;
 915         }
 916
 917         if (!dev->phy.is_40mhz)
 918                 tmp = 0x6464;
 919         else
 920                 tmp = 0x4747;
 921         b43_ntab_write(dev, B43_NTAB16(15, 87), tmp);
 922
 923         if (nphy->hang_avoid)
 924                 b43_nphy_stay_in_carrier_search(dev, false);
 925
 926         b43_phy_write(dev, B43_NPHY_SAMP_DEPCNT, (samps - 1));
 927
 928         if (loops != 0xFFFF)
 929                 b43_phy_write(dev, B43_NPHY_SAMP_LOOPCNT, (loops - 1));
 930         else
 931                 b43_phy_write(dev, B43_NPHY_SAMP_LOOPCNT, loops);
 932
 933         b43_phy_write(dev, B43_NPHY_SAMP_WAITCNT, wait);
 934
 935         seq_mode = b43_phy_read(dev, B43_NPHY_RFSEQMODE);
 936
 937         b43_phy_set(dev, B43_NPHY_RFSEQMODE, B43_NPHY_RFSEQMODE_CAOVER);
 938         if (iqmode) {
 939                 b43_phy_mask(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x7FFF);
 940                 b43_phy_set(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x8000);
 941         } else {
 942                 if (dac_test)
 943                         b43_phy_write(dev, B43_NPHY_SAMP_CMD, 5);
 944                 else
 945                         b43_phy_write(dev, B43_NPHY_SAMP_CMD, 1);
 946         }
 947         for (i = 0; i < 100; i++) {
 948                 if (!(b43_phy_read(dev, B43_NPHY_RFSEQST) & 1)) {
 949                         i = 0;
 950                         break;
 951                 }
 952                 udelay(10);
 953         }
 954         if (i)
 955                 b43err(dev->wl, "run samples timeout\n");
 956
 957         b43_phy_write(dev, B43_NPHY_RFSEQMODE, seq_mode);
 958 }
 959
 960 /**************************************************
 961  * RSSI
 962  **************************************************/
 963
 964 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ScaleOffsetRssi */
 965 static void b43_nphy_scale_offset_rssi(struct b43_wldev *dev, u16 scale,
 966                                         s8 offset, u8 core, u8 rail,
 967                                         enum b43_nphy_rssi_type type)
 968 {
 969         u16 tmp;
 970         bool core1or5 = (core == 1) || (core == 5);
 971         bool core2or5 = (core == 2) || (core == 5);
 972
 973         offset = clamp_val(offset, -32, 31);
 974         tmp = ((scale & 0x3F) << 8) | (offset & 0x3F);
 975
 976         if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_Z))
 977                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, tmp);
 978         if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_Z))
 979                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, tmp);
 980         if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_Z))
 981                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, tmp);
 982         if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_Z))
 983                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, tmp);
 984
 985         if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_X))
 986                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, tmp);
 987         if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_X))
 988                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, tmp);
 989         if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_X))
 990                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, tmp);
 991         if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_X))
 992                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, tmp);
 993
 994         if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_Y))
 995                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, tmp);
 996         if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_Y))
 997                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, tmp);
 998         if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_Y))
 999                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, tmp);
1000         if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_Y))
1001                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, tmp);
1002
1003         if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_TBD))
1004                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TBD, tmp);
1005         if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_TBD))
1006                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TBD, tmp);
1007         if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_TBD))
1008                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TBD, tmp);
1009         if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_TBD))
1010                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TBD, tmp);
1011
1012         if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_PWRDET))
1013                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_PWRDET, tmp);
1014         if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_PWRDET))
1015                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_PWRDET, tmp);
1016         if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_PWRDET))
1017                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_PWRDET, tmp);
1018         if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_PWRDET))
1019                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_PWRDET, tmp);
1020
1021         if (core1or5 && (type == B43_NPHY_RSSI_TSSI_I))
1022                 b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TSSI, tmp);
1023         if (core2or5 && (type == B43_NPHY_RSSI_TSSI_I))
1024                 b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TSSI, tmp);
1025
1026         if (core1or5 && (type == B43_NPHY_RSSI_TSSI_Q))
1027                 b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TSSI, tmp);
1028         if (core2or5 && (type == B43_NPHY_RSSI_TSSI_Q))
1029                 b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TSSI, tmp);
1030 }
1031
1032 static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1033 {
1034         u8 i;
1035         u16 reg, val;
1036
1037         if (code == 0) {
1038                 b43_phy_mask(dev, B43_NPHY_AFECTL_OVER1, 0xFDFF);
1039                 b43_phy_mask(dev, B43_NPHY_AFECTL_OVER, 0xFDFF);
1040                 b43_phy_mask(dev, B43_NPHY_AFECTL_C1, 0xFCFF);
1041                 b43_phy_mask(dev, B43_NPHY_AFECTL_C2, 0xFCFF);
1042                 b43_phy_mask(dev, B43_NPHY_TXF_40CO_B1S0, 0xFFDF);
1043                 b43_phy_mask(dev, B43_NPHY_TXF_40CO_B32S1, 0xFFDF);
1044                 b43_phy_mask(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1, 0xFFC3);
1045                 b43_phy_mask(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2, 0xFFC3);
1046         } else {
1047                 for (i = 0; i < 2; i++) {
1048                         if ((code == 1 && i == 1) || (code == 2 && !i))
1049                                 continue;
1050
1051                         reg = (i == 0) ?
1052                                 B43_NPHY_AFECTL_OVER1 : B43_NPHY_AFECTL_OVER;
1053                         b43_phy_maskset(dev, reg, 0xFDFF, 0x0200);
1054
1055                         if (type < 3) {
1056                                 reg = (i == 0) ?
1057                                         B43_NPHY_AFECTL_C1 :
1058                                         B43_NPHY_AFECTL_C2;
1059                                 b43_phy_maskset(dev, reg, 0xFCFF, 0);
1060
1061                                 reg = (i == 0) ?
1062                                         B43_NPHY_RFCTL_LUT_TRSW_UP1 :
1063                                         B43_NPHY_RFCTL_LUT_TRSW_UP2;
1064                                 b43_phy_maskset(dev, reg, 0xFFC3, 0);
1065
1066                                 if (type == 0)
1067                                         val = (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ? 4 : 8;
1068                                 else if (type == 1)
1069                                         val = 16;
1070                                 else
1071                                         val = 32;
1072                                 b43_phy_set(dev, reg, val);
1073
1074                                 reg = (i == 0) ?
1075                                         B43_NPHY_TXF_40CO_B1S0 :
1076                                         B43_NPHY_TXF_40CO_B32S1;
1077                                 b43_phy_set(dev, reg, 0x0020);
1078                         } else {
1079                                 if (type == 6)
1080                                         val = 0x0100;
1081                                 else if (type == 3)
1082                                         val = 0x0200;
1083                                 else
1084                                         val = 0x0300;
1085
1086                                 reg = (i == 0) ?
1087                                         B43_NPHY_AFECTL_C1 :
1088                                         B43_NPHY_AFECTL_C2;
1089
1090                                 b43_phy_maskset(dev, reg, 0xFCFF, val);
1091                                 b43_phy_maskset(dev, reg, 0xF3FF, val << 2);
1092
1093                                 if (type != 3 && type != 6) {
1094                                         enum ieee80211_band band =
1095                                                 b43_current_band(dev->wl);
1096
1097                                         if (b43_nphy_ipa(dev))
1098                                                 val = (band == IEEE80211_BAND_5GHZ) ? 0xC : 0xE;
1099                                         else
1100                                                 val = 0x11;
1101                                         reg = (i == 0) ? 0x2000 : 0x3000;
1102                                         reg |= B2055_PADDRV;
1103                                         b43_radio_write16(dev, reg, val);
1104
1105                                         reg = (i == 0) ?
1106                                                 B43_NPHY_AFECTL_OVER1 :
1107                                                 B43_NPHY_AFECTL_OVER;
1108                                         b43_phy_set(dev, reg, 0x0200);
1109                                 }
1110                         }
1111                 }
1112         }
1113 }
1114
1115 static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1116 {
1117         u16 val;
1118
1119         if (type < 3)
1120                 val = 0;
1121         else if (type == 6)
1122                 val = 1;
1123         else if (type == 3)
1124                 val = 2;
1125         else
1126                 val = 3;
1127
1128         val = (val << 12) | (val << 14);
1129         b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, val);
1130         b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, val);
1131
1132         if (type < 3) {
1133                 b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO1, 0xFFCF,
1134                                 (type + 1) << 4);
1135                 b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO2, 0xFFCF,
1136                                 (type + 1) << 4);
1137         }
1138
1139         if (code == 0) {
1140                 b43_phy_mask(dev, B43_NPHY_AFECTL_OVER, ~0x3000);
1141                 if (type < 3) {
1142                         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
1143                                 ~(B43_NPHY_RFCTL_CMD_RXEN |
1144                                   B43_NPHY_RFCTL_CMD_CORESEL));
1145                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER,
1146                                 ~(0x1 << 12 |
1147                                   0x1 << 5 |
1148                                   0x1 << 1 |
1149                                   0x1));
1150                         b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
1151                                 ~B43_NPHY_RFCTL_CMD_START);
1152                         udelay(20);
1153                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, ~0x1);
1154                 }
1155         } else {
1156                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x3000);
1157                 if (type < 3) {
1158                         b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD,
1159                                 ~(B43_NPHY_RFCTL_CMD_RXEN |
1160                                   B43_NPHY_RFCTL_CMD_CORESEL),
1161                                 (B43_NPHY_RFCTL_CMD_RXEN |
1162                                  code << B43_NPHY_RFCTL_CMD_CORESEL_SHIFT));
1163                         b43_phy_set(dev, B43_NPHY_RFCTL_OVER,
1164                                 (0x1 << 12 |
1165                                   0x1 << 5 |
1166                                   0x1 << 1 |
1167                                   0x1));
1168                         b43_phy_set(dev, B43_NPHY_RFCTL_CMD,
1169                                 B43_NPHY_RFCTL_CMD_START);
1170                         udelay(20);
1171                         b43_phy_mask(dev, B43_NPHY_RFCTL_OVER, ~0x1);
1172                 }
1173         }
1174 }
1175
1176 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSISel */
1177 static void b43_nphy_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
1178 {
1179         if (dev->phy.rev >= 3)
1180                 b43_nphy_rev3_rssi_select(dev, code, type);
1181         else
1182                 b43_nphy_rev2_rssi_select(dev, code, type);
1183 }
1184
1185 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetRssi2055Vcm */
1186 static void b43_nphy_set_rssi_2055_vcm(struct b43_wldev *dev, u8 type, u8 *buf)
1187 {
1188         int i;
1189         for (i = 0; i < 2; i++) {
1190                 if (type == 2) {
1191                         if (i == 0) {
1192                                 b43_radio_maskset(dev, B2055_C1_B0NB_RSSIVCM,
1193                                                   0xFC, buf[0]);
1194                                 b43_radio_maskset(dev, B2055_C1_RX_BB_RSSICTL5,
1195                                                   0xFC, buf[1]);
1196                         } else {
1197                                 b43_radio_maskset(dev, B2055_C2_B0NB_RSSIVCM,
1198                                                   0xFC, buf[2 * i]);
1199                                 b43_radio_maskset(dev, B2055_C2_RX_BB_RSSICTL5,
1200                                                   0xFC, buf[2 * i + 1]);
1201                         }
1202                 } else {
1203                         if (i == 0)
1204                                 b43_radio_maskset(dev, B2055_C1_RX_BB_RSSICTL5,
1205                                                   0xF3, buf[0] << 2);
1206                         else
1207                                 b43_radio_maskset(dev, B2055_C2_RX_BB_RSSICTL5,
1208                                                   0xF3, buf[2 * i + 1] << 2);
1209                 }
1210         }
1211 }
1212
1213 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PollRssi */
1214 static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
1215                                 u8 nsamp)
1216 {
1217         int i;
1218         int out;
1219         u16 save_regs_phy[9];
1220         u16 s[2];
1221
1222         if (dev->phy.rev >= 3) {
1223                 save_regs_phy[0] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
1224                 save_regs_phy[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
1225                 save_regs_phy[2] = b43_phy_read(dev,
1226                                                 B43_NPHY_RFCTL_LUT_TRSW_UP1);
1227                 save_regs_phy[3] = b43_phy_read(dev,
1228                                                 B43_NPHY_RFCTL_LUT_TRSW_UP2);
1229                 save_regs_phy[4] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
1230                 save_regs_phy[5] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
1231                 save_regs_phy[6] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B1S0);
1232                 save_regs_phy[7] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B32S1);
1233                 save_regs_phy[8] = 0;
1234         } else {
1235                 save_regs_phy[0] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
1236                 save_regs_phy[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
1237                 save_regs_phy[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
1238                 save_regs_phy[3] = b43_phy_read(dev, B43_NPHY_RFCTL_CMD);
1239                 save_regs_phy[4] = b43_phy_read(dev, B43_NPHY_RFCTL_OVER);
1240                 save_regs_phy[5] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO1);
1241                 save_regs_phy[6] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO2);
1242                 save_regs_phy[7] = 0;
1243                 save_regs_phy[8] = 0;
1244         }
1245
1246         b43_nphy_rssi_select(dev, 5, type);
1247
1248         if (dev->phy.rev < 2) {
1249                 save_regs_phy[8] = b43_phy_read(dev, B43_NPHY_GPIO_SEL);
1250                 b43_phy_write(dev, B43_NPHY_GPIO_SEL, 5);
1251         }
1252
1253         for (i = 0; i < 4; i++)
1254                 buf[i] = 0;
1255
1256         for (i = 0; i < nsamp; i++) {
1257                 if (dev->phy.rev < 2) {
1258                         s[0] = b43_phy_read(dev, B43_NPHY_GPIO_LOOUT);
1259                         s[1] = b43_phy_read(dev, B43_NPHY_GPIO_HIOUT);
1260                 } else {
1261                         s[0] = b43_phy_read(dev, B43_NPHY_RSSI1);
1262                         s[1] = b43_phy_read(dev, B43_NPHY_RSSI2);
1263                 }
1264
1265                 buf[0] += ((s8)((s[0] & 0x3F) << 2)) >> 2;
1266                 buf[1] += ((s8)(((s[0] >> 8) & 0x3F) << 2)) >> 2;
1267                 buf[2] += ((s8)((s[1] & 0x3F) << 2)) >> 2;
1268                 buf[3] += ((s8)(((s[1] >> 8) & 0x3F) << 2)) >> 2;
1269         }
1270         out = (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 |
1271                 (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
1272
1273         if (dev->phy.rev < 2)
1274                 b43_phy_write(dev, B43_NPHY_GPIO_SEL, save_regs_phy[8]);
1275
1276         if (dev->phy.rev >= 3) {
1277                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, save_regs_phy[0]);
1278                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, save_regs_phy[1]);
1279                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1,
1280                                 save_regs_phy[2]);
1281                 b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2,
1282                                 save_regs_phy[3]);
1283                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, save_regs_phy[4]);
1284                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, save_regs_phy[5]);
1285                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S0, save_regs_phy[6]);
1286                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S1, save_regs_phy[7]);
1287         } else {
1288                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, save_regs_phy[0]);
1289                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, save_regs_phy[1]);
1290                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, save_regs_phy[2]);
1291                 b43_phy_write(dev, B43_NPHY_RFCTL_CMD, save_regs_phy[3]);
1292                 b43_phy_write(dev, B43_NPHY_RFCTL_OVER, save_regs_phy[4]);
1293                 b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO1, save_regs_phy[5]);
1294                 b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO2, save_regs_phy[6]);
1295         }
1296
1297         return out;
1298 }
1299
1300 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICalRev3 */
1301 static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
1302 {
1303         struct b43_phy_n *nphy = dev->phy.n;
1304
1305         u16 saved_regs_phy_rfctl[2];
1306         u16 saved_regs_phy[13];
1307         u16 regs_to_store[] = {
1308                 B43_NPHY_AFECTL_OVER1, B43_NPHY_AFECTL_OVER,
1309                 B43_NPHY_AFECTL_C1, B43_NPHY_AFECTL_C2,
1310                 B43_NPHY_TXF_40CO_B1S1, B43_NPHY_RFCTL_OVER,
1311                 B43_NPHY_TXF_40CO_B1S0, B43_NPHY_TXF_40CO_B32S1,
1312                 B43_NPHY_RFCTL_CMD,
1313                 B43_NPHY_RFCTL_LUT_TRSW_UP1, B43_NPHY_RFCTL_LUT_TRSW_UP2,
1314                 B43_NPHY_RFCTL_RSSIO1, B43_NPHY_RFCTL_RSSIO2
1315         };
1316
1317         u16 class;
1318
1319         u16 clip_state[2];
1320         u16 clip_off[2] = { 0xFFFF, 0xFFFF };
1321
1322         u8 vcm_final = 0;
1323         s8 offset[4];
1324         s32 results[8][4] = { };
1325         s32 results_min[4] = { };
1326         s32 poll_results[4] = { };
1327
1328         u16 *rssical_radio_regs = NULL;
1329         u16 *rssical_phy_regs = NULL;
1330
1331         u16 r; /* routing */
1332         u8 rx_core_state;
1333         u8 core, i, j;
1334
1335         class = b43_nphy_classifier(dev, 0, 0);
1336         b43_nphy_classifier(dev, 7, 4);
1337         b43_nphy_read_clip_detection(dev, clip_state);
1338         b43_nphy_write_clip_detection(dev, clip_off);
1339
1340         saved_regs_phy_rfctl[0] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
1341         saved_regs_phy_rfctl[1] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
1342         for (i = 0; i < ARRAY_SIZE(regs_to_store); i++)
1343                 saved_regs_phy[i] = b43_phy_read(dev, regs_to_store[i]);
1344
1345         b43_nphy_rf_control_intc_override(dev, 0, 0, 7);
1346         b43_nphy_rf_control_intc_override(dev, 1, 1, 7);
1347         b43_nphy_rf_control_override(dev, 0x1, 0, 0, false);
1348         b43_nphy_rf_control_override(dev, 0x2, 1, 0, false);
1349         b43_nphy_rf_control_override(dev, 0x80, 1, 0, false);
1350         b43_nphy_rf_control_override(dev, 0x40, 1, 0, false);
1351
1352         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
1353                 b43_nphy_rf_control_override(dev, 0x20, 0, 0, false);
1354                 b43_nphy_rf_control_override(dev, 0x10, 1, 0, false);
1355         } else {
1356                 b43_nphy_rf_control_override(dev, 0x10, 0, 0, false);
1357                 b43_nphy_rf_control_override(dev, 0x20, 1, 0, false);
1358         }
1359
1360         rx_core_state = b43_nphy_get_rx_core_state(dev);
1361         for (core = 0; core < 2; core++) {
1362                 if (!(rx_core_state & (1 << core)))
1363                         continue;
1364                 r = core ? B2056_RX1 : B2056_RX0;
1365                 b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 0, 2);
1366                 b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 1, 2);
1367                 for (i = 0; i < 8; i++) {
1368                         b43_radio_maskset(dev, r | B2056_RX_RSSI_MISC, 0xE3,
1369                                         i << 2);
1370                         b43_nphy_poll_rssi(dev, 2, results[i], 8);
1371                 }
1372                 for (i = 0; i < 4; i++) {
1373                         s32 curr;
1374                         s32 mind = 40;
1375                         s32 minpoll = 249;
1376                         u8 minvcm = 0;
1377                         if (2 * core != i)
1378                                 continue;
1379                         for (j = 0; j < 8; j++) {
1380                                 curr = results[j][i] * results[j][i] +
1381                                         results[j][i + 1] * results[j][i];
1382                                 if (curr < mind) {
1383                                         mind = curr;
1384                                         minvcm = j;
1385                                 }
1386                                 if (results[j][i] < minpoll)
1387                                         minpoll = results[j][i];
1388                         }
1389                         vcm_final = minvcm;
1390                         results_min[i] = minpoll;
1391                 }
1392                 b43_radio_maskset(dev, r | B2056_RX_RSSI_MISC, 0xE3,
1393                                   vcm_final << 2);
1394                 for (i = 0; i < 4; i++) {
1395                         if (core != i / 2)
1396                                 continue;
1397                         offset[i] = -results[vcm_final][i];
1398                         if (offset[i] < 0)
1399                                 offset[i] = -((abs(offset[i]) + 4) / 8);
1400                         else
1401                                 offset[i] = (offset[i] + 4) / 8;
1402                         if (results_min[i] == 248)
1403                                 offset[i] = -32;
1404                         b43_nphy_scale_offset_rssi(dev, 0, offset[i],
1405                                                    (i / 2 == 0) ? 1 : 2,
1406                                                    (i % 2 == 0) ? 0 : 1,
1407                                                    2);
1408                 }
1409         }
1410         for (core = 0; core < 2; core++) {
1411                 if (!(rx_core_state & (1 << core)))
1412                         continue;
1413                 for (i = 0; i < 2; i++) {
1414                         b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 0, i);
1415                         b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 1, i);
1416                         b43_nphy_poll_rssi(dev, i, poll_results, 8);
1417                         for (j = 0; j < 4; j++) {
1418                                 if (j / 2 == core)
1419                                         offset[j] = 232 - poll_results[j];
1420                                 if (offset[j] < 0)
1421                                         offset[j] = -(abs(offset[j] + 4) / 8);
1422                                 else
1423                                         offset[j] = (offset[j] + 4) / 8;
1424                                 b43_nphy_scale_offset_rssi(dev, 0,
1425                                         offset[2 * core], core + 1, j % 2, i);
1426                         }
1427                 }
1428         }
1429
1430         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, saved_regs_phy_rfctl[0]);
1431         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, saved_regs_phy_rfctl[1]);
1432
1433         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
1434
1435         b43_phy_set(dev, B43_NPHY_TXF_40CO_B1S1, 0x1);
1436         b43_phy_set(dev, B43_NPHY_RFCTL_CMD, B43_NPHY_RFCTL_CMD_START);
1437         b43_phy_mask(dev, B43_NPHY_TXF_40CO_B1S1, ~0x1);
1438
1439         b43_phy_set(dev, B43_NPHY_RFCTL_OVER, 0x1);
1440         b43_phy_set(dev, B43_NPHY_RFCTL_CMD, B43_NPHY_RFCTL_CMD_RXTX);
1441         b43_phy_mask(dev, B43_NPHY_TXF_40CO_B1S1, ~0x1);
1442
1443         for (i = 0; i < ARRAY_SIZE(regs_to_store); i++)
1444                 b43_phy_write(dev, regs_to_store[i], saved_regs_phy[i]);
1445
1446         /* Store for future configuration */
1447         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
1448                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_2G;
1449                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_2G;
1450         } else {
1451                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_5G;
1452                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
1453         }
1454         rssical_radio_regs[0] = b43_radio_read(dev, 0x602B);
1455         rssical_radio_regs[0] = b43_radio_read(dev, 0x702B);
1456         rssical_phy_regs[0] = b43_phy_read(dev, B43_NPHY_RSSIMC_0I_RSSI_Z);
1457         rssical_phy_regs[1] = b43_phy_read(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z);
1458         rssical_phy_regs[2] = b43_phy_read(dev, B43_NPHY_RSSIMC_1I_RSSI_Z);
1459         rssical_phy_regs[3] = b43_phy_read(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z);
1460         rssical_phy_regs[4] = b43_phy_read(dev, B43_NPHY_RSSIMC_0I_RSSI_X);
1461         rssical_phy_regs[5] = b43_phy_read(dev, B43_NPHY_RSSIMC_0Q_RSSI_X);
1462         rssical_phy_regs[6] = b43_phy_read(dev, B43_NPHY_RSSIMC_1I_RSSI_X);
1463         rssical_phy_regs[7] = b43_phy_read(dev, B43_NPHY_RSSIMC_1Q_RSSI_X);
1464         rssical_phy_regs[8] = b43_phy_read(dev, B43_NPHY_RSSIMC_0I_RSSI_Y);
1465         rssical_phy_regs[9] = b43_phy_read(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y);
1466         rssical_phy_regs[10] = b43_phy_read(dev, B43_NPHY_RSSIMC_1I_RSSI_Y);
1467         rssical_phy_regs[11] = b43_phy_read(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y);
1468
1469         /* Remember for which channel we store configuration */
1470         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
1471                 nphy->rssical_chanspec_2G.center_freq = dev->phy.channel_freq;
1472         else
1473                 nphy->rssical_chanspec_5G.center_freq = dev->phy.channel_freq;
1474
1475         /* End of calibration, restore configuration */
1476         b43_nphy_classifier(dev, 7, class);
1477         b43_nphy_write_clip_detection(dev, clip_state);
1478 }
1479
1480 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICal */
1481 static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
1482 {
1483         int i, j;
1484         u8 state[4];
1485         u8 code, val;
1486         u16 class, override;
1487         u8 regs_save_radio[2];
1488         u16 regs_save_phy[2];
1489
1490         s8 offset[4];
1491         u8 core;
1492         u8 rail;
1493
1494         u16 clip_state[2];
1495         u16 clip_off[2] = { 0xFFFF, 0xFFFF };
1496         s32 results_min[4] = { };
1497         u8 vcm_final[4] = { };
1498         s32 results[4][4] = { };
1499         s32 miniq[4][2] = { };
1500
1501         if (type == 2) {
1502                 code = 0;
1503                 val = 6;
1504         } else if (type < 2) {
1505                 code = 25;
1506                 val = 4;
1507         } else {
1508                 B43_WARN_ON(1);
1509                 return;
1510         }
1511
1512         class = b43_nphy_classifier(dev, 0, 0);
1513         b43_nphy_classifier(dev, 7, 4);
1514         b43_nphy_read_clip_detection(dev, clip_state);
1515         b43_nphy_write_clip_detection(dev, clip_off);
1516
1517         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
1518                 override = 0x140;
1519         else
1520                 override = 0x110;
1521
1522         regs_save_phy[0] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
1523         regs_save_radio[0] = b43_radio_read16(dev, B2055_C1_PD_RXTX);
1524         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, override);
1525         b43_radio_write16(dev, B2055_C1_PD_RXTX, val);
1526
1527         regs_save_phy[1] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
1528         regs_save_radio[1] = b43_radio_read16(dev, B2055_C2_PD_RXTX);
1529         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, override);
1530         b43_radio_write16(dev, B2055_C2_PD_RXTX, val);
1531
1532         state[0] = b43_radio_read16(dev, B2055_C1_PD_RSSIMISC) & 0x07;
1533         state[1] = b43_radio_read16(dev, B2055_C2_PD_RSSIMISC) & 0x07;
1534         b43_radio_mask(dev, B2055_C1_PD_RSSIMISC, 0xF8);
1535         b43_radio_mask(dev, B2055_C2_PD_RSSIMISC, 0xF8);
1536         state[2] = b43_radio_read16(dev, B2055_C1_SP_RSSI) & 0x07;
1537         state[3] = b43_radio_read16(dev, B2055_C2_SP_RSSI) & 0x07;
1538
1539         b43_nphy_rssi_select(dev, 5, type);
1540         b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 0, type);
1541         b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 1, type);
1542
1543         for (i = 0; i < 4; i++) {
1544                 u8 tmp[4];
1545                 for (j = 0; j < 4; j++)
1546                         tmp[j] = i;
1547                 if (type != 1)
1548                         b43_nphy_set_rssi_2055_vcm(dev, type, tmp);
1549                 b43_nphy_poll_rssi(dev, type, results[i], 8);
1550                 if (type < 2)
1551                         for (j = 0; j < 2; j++)
1552                                 miniq[i][j] = min(results[i][2 * j],
1553                                                 results[i][2 * j + 1]);
1554         }
1555
1556         for (i = 0; i < 4; i++) {
1557                 s32 mind = 40;
1558                 u8 minvcm = 0;
1559                 s32 minpoll = 249;
1560                 s32 curr;
1561                 for (j = 0; j < 4; j++) {
1562                         if (type == 2)
1563                                 curr = abs(results[j][i]);
1564                         else
1565                                 curr = abs(miniq[j][i / 2] - code * 8);
1566
1567                         if (curr < mind) {
1568                                 mind = curr;
1569                                 minvcm = j;
1570                         }
1571
1572                         if (results[j][i] < minpoll)
1573                                 minpoll = results[j][i];
1574                 }
1575                 results_min[i] = minpoll;
1576                 vcm_final[i] = minvcm;
1577         }
1578
1579         if (type != 1)
1580                 b43_nphy_set_rssi_2055_vcm(dev, type, vcm_final);
1581
1582         for (i = 0; i < 4; i++) {
1583                 offset[i] = (code * 8) - results[vcm_final[i]][i];
1584
1585                 if (offset[i] < 0)
1586                         offset[i] = -((abs(offset[i]) + 4) / 8);
1587                 else
1588                         offset[i] = (offset[i] + 4) / 8;
1589
1590                 if (results_min[i] == 248)
1591                         offset[i] = code - 32;
1592
1593                 core = (i / 2) ? 2 : 1;
1594                 rail = (i % 2) ? 1 : 0;
1595
1596                 b43_nphy_scale_offset_rssi(dev, 0, offset[i], core, rail,
1597                                                 type);
1598         }
1599
1600         b43_radio_maskset(dev, B2055_C1_PD_RSSIMISC, 0xF8, state[0]);
1601         b43_radio_maskset(dev, B2055_C2_PD_RSSIMISC, 0xF8, state[1]);
1602
1603         switch (state[2]) {
1604         case 1:
1605                 b43_nphy_rssi_select(dev, 1, 2);
1606                 break;
1607         case 4:
1608                 b43_nphy_rssi_select(dev, 1, 0);
1609                 break;
1610         case 2:
1611                 b43_nphy_rssi_select(dev, 1, 1);
1612                 break;
1613         default:
1614                 b43_nphy_rssi_select(dev, 1, 1);
1615                 break;
1616         }
1617
1618         switch (state[3]) {
1619         case 1:
1620                 b43_nphy_rssi_select(dev, 2, 2);
1621                 break;
1622         case 4:
1623                 b43_nphy_rssi_select(dev, 2, 0);
1624                 break;
1625         default:
1626                 b43_nphy_rssi_select(dev, 2, 1);
1627                 break;
1628         }
1629
1630         b43_nphy_rssi_select(dev, 0, type);
1631
1632         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs_save_phy[0]);
1633         b43_radio_write16(dev, B2055_C1_PD_RXTX, regs_save_radio[0]);
1634         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs_save_phy[1]);
1635         b43_radio_write16(dev, B2055_C2_PD_RXTX, regs_save_radio[1]);
1636
1637         b43_nphy_classifier(dev, 7, class);
1638         b43_nphy_write_clip_detection(dev, clip_state);
1639         /* Specs don't say about reset here, but it makes wl and b43 dumps
1640            identical, it really seems wl performs this */
1641         b43_nphy_reset_cca(dev);
1642 }
1643
1644 /*
1645  * RSSI Calibration
1646  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICal
1647  */
1648 static void b43_nphy_rssi_cal(struct b43_wldev *dev)
1649 {
1650         if (dev->phy.rev >= 3) {
1651                 b43_nphy_rev3_rssi_cal(dev);
1652         } else {
1653                 b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_Z);
1654                 b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_X);
1655                 b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_Y);
1656         }
1657 }
1658
1659 /**************************************************
1660  * Workarounds
1661  **************************************************/
1662
1663 static void b43_nphy_gain_ctl_workarounds_rev3plus(struct b43_wldev *dev)
1664 {
1665         struct ssb_sprom *sprom = dev->dev->bus_sprom;
1666
1667         bool ghz5;
1668         bool ext_lna;
1669         u16 rssi_gain;
1670         struct nphy_gain_ctl_workaround_entry *e;
1671         u8 lpf_gain[6] = { 0x00, 0x06, 0x0C, 0x12, 0x12, 0x12 };
1672         u8 lpf_bits[6] = { 0, 1, 2, 3, 3, 3 };
1673
1674         /* Prepare values */
1675         ghz5 = b43_phy_read(dev, B43_NPHY_BANDCTL)
1676                 & B43_NPHY_BANDCTL_5GHZ;
1677         ext_lna = ghz5 ? sprom->boardflags_hi & B43_BFH_EXTLNA_5GHZ :
1678                 sprom->boardflags_lo & B43_BFL_EXTLNA;
1679         e = b43_nphy_get_gain_ctl_workaround_ent(dev, ghz5, ext_lna);
1680         if (ghz5 && dev->phy.rev >= 5)
1681                 rssi_gain = 0x90;
1682         else
1683                 rssi_gain = 0x50;
1684
1685         b43_phy_set(dev, B43_NPHY_RXCTL, 0x0040);
1686
1687         /* Set Clip 2 detect */
1688         b43_phy_set(dev, B43_NPHY_C1_CGAINI,
1689                         B43_NPHY_C1_CGAINI_CL2DETECT);
1690         b43_phy_set(dev, B43_NPHY_C2_CGAINI,
1691                         B43_NPHY_C2_CGAINI_CL2DETECT);
1692
1693         b43_radio_write(dev, B2056_RX0 | B2056_RX_BIASPOLE_LNAG1_IDAC,
1694                         0x17);
1695         b43_radio_write(dev, B2056_RX1 | B2056_RX_BIASPOLE_LNAG1_IDAC,
1696                         0x17);
1697         b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAG2_IDAC, 0xF0);
1698         b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAG2_IDAC, 0xF0);
1699         b43_radio_write(dev, B2056_RX0 | B2056_RX_RSSI_POLE, 0x00);
1700         b43_radio_write(dev, B2056_RX1 | B2056_RX_RSSI_POLE, 0x00);
1701         b43_radio_write(dev, B2056_RX0 | B2056_RX_RSSI_GAIN,
1702                         rssi_gain);
1703         b43_radio_write(dev, B2056_RX1 | B2056_RX_RSSI_GAIN,
1704                         rssi_gain);
1705         b43_radio_write(dev, B2056_RX0 | B2056_RX_BIASPOLE_LNAA1_IDAC,
1706                         0x17);
1707         b43_radio_write(dev, B2056_RX1 | B2056_RX_BIASPOLE_LNAA1_IDAC,
1708                         0x17);
1709         b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAA2_IDAC, 0xFF);
1710         b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAA2_IDAC, 0xFF);
1711
1712         b43_ntab_write_bulk(dev, B43_NTAB8(0, 8), 4, e->lna1_gain);
1713         b43_ntab_write_bulk(dev, B43_NTAB8(1, 8), 4, e->lna1_gain);
1714         b43_ntab_write_bulk(dev, B43_NTAB8(0, 16), 4, e->lna2_gain);
1715         b43_ntab_write_bulk(dev, B43_NTAB8(1, 16), 4, e->lna2_gain);
1716         b43_ntab_write_bulk(dev, B43_NTAB8(0, 32), 10, e->gain_db);
1717         b43_ntab_write_bulk(dev, B43_NTAB8(1, 32), 10, e->gain_db);
1718         b43_ntab_write_bulk(dev, B43_NTAB8(2, 32), 10, e->gain_bits);
1719         b43_ntab_write_bulk(dev, B43_NTAB8(3, 32), 10, e->gain_bits);
1720         b43_ntab_write_bulk(dev, B43_NTAB8(0, 0x40), 6, lpf_gain);
1721         b43_ntab_write_bulk(dev, B43_NTAB8(1, 0x40), 6, lpf_gain);
1722         b43_ntab_write_bulk(dev, B43_NTAB8(2, 0x40), 6, lpf_bits);
1723         b43_ntab_write_bulk(dev, B43_NTAB8(3, 0x40), 6, lpf_bits);
1724
1725         b43_phy_write(dev, B43_NPHY_C1_INITGAIN, e->init_gain);
1726         b43_phy_write(dev, 0x2A7, e->init_gain);
1727         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x106), 2,
1728                                 e->rfseq_init);
1729
1730         /* TODO: check defines. Do not match variables names */
1731         b43_phy_write(dev, B43_NPHY_C1_CLIP1_MEDGAIN, e->cliphi_gain);
1732         b43_phy_write(dev, 0x2A9, e->cliphi_gain);
1733         b43_phy_write(dev, B43_NPHY_C1_CLIP2_GAIN, e->clipmd_gain);
1734         b43_phy_write(dev, 0x2AB, e->clipmd_gain);
1735         b43_phy_write(dev, B43_NPHY_C2_CLIP1_HIGAIN, e->cliplo_gain);
1736         b43_phy_write(dev, 0x2AD, e->cliplo_gain);
1737
1738         b43_phy_maskset(dev, 0x27D, 0xFF00, e->crsmin);
1739         b43_phy_maskset(dev, 0x280, 0xFF00, e->crsminl);
1740         b43_phy_maskset(dev, 0x283, 0xFF00, e->crsminu);
1741         b43_phy_write(dev, B43_NPHY_C1_NBCLIPTHRES, e->nbclip);
1742         b43_phy_write(dev, B43_NPHY_C2_NBCLIPTHRES, e->nbclip);
1743         b43_phy_maskset(dev, B43_NPHY_C1_CLIPWBTHRES,
1744                         ~B43_NPHY_C1_CLIPWBTHRES_CLIP2, e->wlclip);
1745         b43_phy_maskset(dev, B43_NPHY_C2_CLIPWBTHRES,
1746                         ~B43_NPHY_C2_CLIPWBTHRES_CLIP2, e->wlclip);
1747         b43_phy_write(dev, B43_NPHY_CCK_SHIFTB_REF, 0x809C);
1748 }
1749
1750 static void b43_nphy_gain_ctl_workarounds_rev1_2(struct b43_wldev *dev)
1751 {
1752         struct b43_phy_n *nphy = dev->phy.n;
1753
1754         u8 i, j;
1755         u8 code;
1756         u16 tmp;
1757         u8 rfseq_events[3] = { 6, 8, 7 };
1758         u8 rfseq_delays[3] = { 10, 30, 1 };
1759
1760         /* Set Clip 2 detect */
1761         b43_phy_set(dev, B43_NPHY_C1_CGAINI, B43_NPHY_C1_CGAINI_CL2DETECT);
1762         b43_phy_set(dev, B43_NPHY_C2_CGAINI, B43_NPHY_C2_CGAINI_CL2DETECT);
1763
1764         /* Set narrowband clip threshold */
1765         b43_phy_write(dev, B43_NPHY_C1_NBCLIPTHRES, 0x84);
1766         b43_phy_write(dev, B43_NPHY_C2_NBCLIPTHRES, 0x84);
1767
1768         if (!dev->phy.is_40mhz) {
1769                 /* Set dwell lengths */
1770                 b43_phy_write(dev, B43_NPHY_CLIP1_NBDWELL_LEN, 0x002B);
1771                 b43_phy_write(dev, B43_NPHY_CLIP2_NBDWELL_LEN, 0x002B);
1772                 b43_phy_write(dev, B43_NPHY_W1CLIP1_DWELL_LEN, 0x0009);
1773                 b43_phy_write(dev, B43_NPHY_W1CLIP2_DWELL_LEN, 0x0009);
1774         }
1775
1776         /* Set wideband clip 2 threshold */
1777         b43_phy_maskset(dev, B43_NPHY_C1_CLIPWBTHRES,
1778                         ~B43_NPHY_C1_CLIPWBTHRES_CLIP2, 21);
1779         b43_phy_maskset(dev, B43_NPHY_C2_CLIPWBTHRES,
1780                         ~B43_NPHY_C2_CLIPWBTHRES_CLIP2, 21);
1781
1782         if (!dev->phy.is_40mhz) {
1783                 b43_phy_maskset(dev, B43_NPHY_C1_CGAINI,
1784                         ~B43_NPHY_C1_CGAINI_GAINBKOFF, 0x1);
1785                 b43_phy_maskset(dev, B43_NPHY_C2_CGAINI,
1786                         ~B43_NPHY_C2_CGAINI_GAINBKOFF, 0x1);
1787                 b43_phy_maskset(dev, B43_NPHY_C1_CCK_CGAINI,
1788                         ~B43_NPHY_C1_CCK_CGAINI_GAINBKOFF, 0x1);
1789                 b43_phy_maskset(dev, B43_NPHY_C2_CCK_CGAINI,
1790                         ~B43_NPHY_C2_CCK_CGAINI_GAINBKOFF, 0x1);
1791         }
1792
1793         b43_phy_write(dev, B43_NPHY_CCK_SHIFTB_REF, 0x809C);
1794
1795         if (nphy->gain_boost) {
1796                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ &&
1797                         dev->phy.is_40mhz)
1798                         code = 4;
1799                 else
1800                         code = 5;
1801         } else {
1802                 code = dev->phy.is_40mhz ? 6 : 7;
1803         }
1804
1805         /* Set HPVGA2 index */
1806         b43_phy_maskset(dev, B43_NPHY_C1_INITGAIN, ~B43_NPHY_C1_INITGAIN_HPVGA2,
1807                         code << B43_NPHY_C1_INITGAIN_HPVGA2_SHIFT);
1808         b43_phy_maskset(dev, B43_NPHY_C2_INITGAIN, ~B43_NPHY_C2_INITGAIN_HPVGA2,
1809                         code << B43_NPHY_C2_INITGAIN_HPVGA2_SHIFT);
1810
1811         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x1D06);
1812         /* specs say about 2 loops, but wl does 4 */
1813         for (i = 0; i < 4; i++)
1814                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, (code << 8 | 0x7C));
1815
1816         b43_nphy_adjust_lna_gain_table(dev);
1817
1818         if (nphy->elna_gain_config) {
1819                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x0808);
1820                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0);
1821                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1822                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1823                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1824
1825                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x0C08);
1826                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x0);
1827                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1828                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1829                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0x1);
1830
1831                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x1D06);
1832                 /* specs say about 2 loops, but wl does 4 */
1833                 for (i = 0; i < 4; i++)
1834                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
1835                                                 (code << 8 | 0x74));
1836         }
1837
1838         if (dev->phy.rev == 2) {
1839                 for (i = 0; i < 4; i++) {
1840                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
1841                                         (0x0400 * i) + 0x0020);
1842                         for (j = 0; j < 21; j++) {
1843                                 tmp = j * (i < 2 ? 3 : 1);
1844                                 b43_phy_write(dev,
1845                                         B43_NPHY_TABLE_DATALO, tmp);
1846                         }
1847                 }
1848         }
1849
1850         b43_nphy_set_rf_sequence(dev, 5, rfseq_events, rfseq_delays, 3);
1851         b43_phy_maskset(dev, B43_NPHY_OVER_DGAIN1,
1852                 ~B43_NPHY_OVER_DGAIN_CCKDGECV & 0xFFFF,
1853                 0x5A << B43_NPHY_OVER_DGAIN_CCKDGECV_SHIFT);
1854
1855         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
1856                 b43_phy_maskset(dev, B43_PHY_N(0xC5D), 0xFF80, 4);
1857 }
1858
1859 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/WorkaroundsGainCtrl */
1860 static void b43_nphy_gain_ctl_workarounds(struct b43_wldev *dev)
1861 {
1862         if (dev->phy.rev >= 3)
1863                 b43_nphy_gain_ctl_workarounds_rev3plus(dev);
1864         else
1865                 b43_nphy_gain_ctl_workarounds_rev1_2(dev);
1866 }
1867
1868 static void b43_nphy_workarounds_rev3plus(struct b43_wldev *dev)
1869 {
1870         struct b43_phy_n *nphy = dev->phy.n;
1871         struct ssb_sprom *sprom = dev->dev->bus_sprom;
1872
1873         /* TX to RX */
1874         u8 tx2rx_events[8] = { 0x4, 0x3, 0x6, 0x5, 0x2, 0x1, 0x8, 0x1F };
1875         u8 tx2rx_delays[8] = { 8, 4, 2, 2, 4, 4, 6, 1 };
1876         /* RX to TX */
1877         u8 rx2tx_events_ipa[9] = { 0x0, 0x1, 0x2, 0x8, 0x5, 0x6, 0xF, 0x3,
1878                                         0x1F };
1879         u8 rx2tx_delays_ipa[9] = { 8, 6, 6, 4, 4, 16, 43, 1, 1 };
1880         u8 rx2tx_events[9] = { 0x0, 0x1, 0x2, 0x8, 0x5, 0x6, 0x3, 0x4, 0x1F };
1881         u8 rx2tx_delays[9] = { 8, 6, 6, 4, 4, 18, 42, 1, 1 };
1882
1883         u16 tmp16;
1884         u32 tmp32;
1885
1886         b43_phy_write(dev, 0x23f, 0x1f8);
1887         b43_phy_write(dev, 0x240, 0x1f8);
1888
1889         tmp32 = b43_ntab_read(dev, B43_NTAB32(30, 0));
1890         tmp32 &= 0xffffff;
1891         b43_ntab_write(dev, B43_NTAB32(30, 0), tmp32);
1892
1893         b43_phy_write(dev, B43_NPHY_PHASETR_A0, 0x0125);
1894         b43_phy_write(dev, B43_NPHY_PHASETR_A1, 0x01B3);
1895         b43_phy_write(dev, B43_NPHY_PHASETR_A2, 0x0105);
1896         b43_phy_write(dev, B43_NPHY_PHASETR_B0, 0x016E);
1897         b43_phy_write(dev, B43_NPHY_PHASETR_B1, 0x00CD);
1898         b43_phy_write(dev, B43_NPHY_PHASETR_B2, 0x0020);
1899
1900         b43_phy_write(dev, B43_NPHY_C2_CLIP1_MEDGAIN, 0x000C);
1901         b43_phy_write(dev, 0x2AE, 0x000C);
1902
1903         /* TX to RX */
1904         b43_nphy_set_rf_sequence(dev, 1, tx2rx_events, tx2rx_delays,
1905                                  ARRAY_SIZE(tx2rx_events));
1906
1907         /* RX to TX */
1908         if (b43_nphy_ipa(dev))
1909                 b43_nphy_set_rf_sequence(dev, 0, rx2tx_events_ipa,
1910                                 rx2tx_delays_ipa, ARRAY_SIZE(rx2tx_events_ipa));
1911         if (nphy->hw_phyrxchain != 3 &&
1912             nphy->hw_phyrxchain != nphy->hw_phytxchain) {
1913                 if (b43_nphy_ipa(dev)) {
1914                         rx2tx_delays[5] = 59;
1915                         rx2tx_delays[6] = 1;
1916                         rx2tx_events[7] = 0x1F;
1917                 }
1918                 b43_nphy_set_rf_sequence(dev, 1, rx2tx_events, rx2tx_delays,
1919                                          ARRAY_SIZE(rx2tx_events));
1920         }
1921
1922         tmp16 = (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) ?
1923                 0x2 : 0x9C40;
1924         b43_phy_write(dev, B43_NPHY_ENDROP_TLEN, tmp16);
1925
1926         b43_phy_maskset(dev, 0x294, 0xF0FF, 0x0700);
1927
1928         b43_ntab_write(dev, B43_NTAB32(16, 3), 0x18D);
1929         b43_ntab_write(dev, B43_NTAB32(16, 127), 0x18D);
1930
1931         b43_nphy_gain_ctl_workarounds(dev);
1932
1933         b43_ntab_write(dev, B43_NTAB16(8, 0), 2);
1934         b43_ntab_write(dev, B43_NTAB16(8, 16), 2);
1935
1936         /* TODO */
1937
1938         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_MAST_BIAS, 0x00);
1939         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_MAST_BIAS, 0x00);
1940         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_BIAS_MAIN, 0x06);
1941         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_BIAS_MAIN, 0x06);
1942         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_BIAS_AUX, 0x07);
1943         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_BIAS_AUX, 0x07);
1944         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_LOB_BIAS, 0x88);
1945         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_LOB_BIAS, 0x88);
1946         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_CMFB_IDAC, 0x00);
1947         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_CMFB_IDAC, 0x00);
1948         b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXG_CMFB_IDAC, 0x00);
1949         b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXG_CMFB_IDAC, 0x00);
1950
1951         /* N PHY WAR TX Chain Update with hw_phytxchain as argument */
1952
1953         if ((sprom->boardflags2_lo & B43_BFL2_APLL_WAR &&
1954              b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ||
1955             (sprom->boardflags2_lo & B43_BFL2_GPLL_WAR &&
1956              b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ))
1957                 tmp32 = 0x00088888;
1958         else
1959                 tmp32 = 0x88888888;
1960         b43_ntab_write(dev, B43_NTAB32(30, 1), tmp32);
1961         b43_ntab_write(dev, B43_NTAB32(30, 2), tmp32);
1962         b43_ntab_write(dev, B43_NTAB32(30, 3), tmp32);
1963
1964         if (dev->phy.rev == 4 &&
1965                 b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
1966                 b43_radio_write(dev, B2056_TX0 | B2056_TX_GMBB_IDAC,
1967                                 0x70);
1968                 b43_radio_write(dev, B2056_TX1 | B2056_TX_GMBB_IDAC,
1969                                 0x70);
1970         }
1971
1972         b43_phy_write(dev, 0x224, 0x03eb);
1973         b43_phy_write(dev, 0x225, 0x03eb);
1974         b43_phy_write(dev, 0x226, 0x0341);
1975         b43_phy_write(dev, 0x227, 0x0341);
1976         b43_phy_write(dev, 0x228, 0x042b);
1977         b43_phy_write(dev, 0x229, 0x042b);
1978         b43_phy_write(dev, 0x22a, 0x0381);
1979         b43_phy_write(dev, 0x22b, 0x0381);
1980         b43_phy_write(dev, 0x22c, 0x042b);
1981         b43_phy_write(dev, 0x22d, 0x042b);
1982         b43_phy_write(dev, 0x22e, 0x0381);
1983         b43_phy_write(dev, 0x22f, 0x0381);
1984 }
1985
1986 static void b43_nphy_workarounds_rev1_2(struct b43_wldev *dev)
1987 {
1988         struct ssb_sprom *sprom = dev->dev->bus_sprom;
1989         struct b43_phy *phy = &dev->phy;
1990         struct b43_phy_n *nphy = phy->n;
1991
1992         u8 events1[7] = { 0x0, 0x1, 0x2, 0x8, 0x4, 0x5, 0x3 };
1993         u8 delays1[7] = { 0x8, 0x6, 0x6, 0x2, 0x4, 0x3C, 0x1 };
1994
1995         u8 events2[7] = { 0x0, 0x3, 0x5, 0x4, 0x2, 0x1, 0x8 };
1996         u8 delays2[7] = { 0x8, 0x6, 0x2, 0x4, 0x4, 0x6, 0x1 };
1997
1998         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ &&
1999             nphy->band5g_pwrgain) {
2000                 b43_radio_mask(dev, B2055_C1_TX_RF_SPARE, ~0x8);
2001                 b43_radio_mask(dev, B2055_C2_TX_RF_SPARE, ~0x8);
2002         } else {
2003                 b43_radio_set(dev, B2055_C1_TX_RF_SPARE, 0x8);
2004                 b43_radio_set(dev, B2055_C2_TX_RF_SPARE, 0x8);
2005         }
2006
2007         b43_ntab_write(dev, B43_NTAB16(8, 0x00), 0x000A);
2008         b43_ntab_write(dev, B43_NTAB16(8, 0x10), 0x000A);
2009         b43_ntab_write(dev, B43_NTAB16(8, 0x02), 0xCDAA);
2010         b43_ntab_write(dev, B43_NTAB16(8, 0x12), 0xCDAA);
2011
2012         if (dev->phy.rev < 2) {
2013                 b43_ntab_write(dev, B43_NTAB16(8, 0x08), 0x0000);
2014                 b43_ntab_write(dev, B43_NTAB16(8, 0x18), 0x0000);
2015                 b43_ntab_write(dev, B43_NTAB16(8, 0x07), 0x7AAB);
2016                 b43_ntab_write(dev, B43_NTAB16(8, 0x17), 0x7AAB);
2017                 b43_ntab_write(dev, B43_NTAB16(8, 0x06), 0x0800);
2018                 b43_ntab_write(dev, B43_NTAB16(8, 0x16), 0x0800);
2019         }
2020
2021         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO1, 0x2D8);
2022         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1, 0x301);
2023         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO2, 0x2D8);
2024         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2, 0x301);
2025
2026         if (sprom->boardflags2_lo & B43_BFL2_SKWRKFEM_BRD &&
2027             dev->dev->board_type == 0x8B) {
2028                 delays1[0] = 0x1;
2029                 delays1[5] = 0x14;
2030         }
2031         b43_nphy_set_rf_sequence(dev, 0, events1, delays1, 7);
2032         b43_nphy_set_rf_sequence(dev, 1, events2, delays2, 7);
2033
2034         b43_nphy_gain_ctl_workarounds(dev);
2035
2036         if (dev->phy.rev < 2) {
2037                 if (b43_phy_read(dev, B43_NPHY_RXCTL) & 0x2)
2038                         b43_hf_write(dev, b43_hf_read(dev) |
2039                                         B43_HF_MLADVW);
2040         } else if (dev->phy.rev == 2) {
2041                 b43_phy_write(dev, B43_NPHY_CRSCHECK2, 0);
2042                 b43_phy_write(dev, B43_NPHY_CRSCHECK3, 0);
2043         }
2044
2045         if (dev->phy.rev < 2)
2046                 b43_phy_mask(dev, B43_NPHY_SCRAM_SIGCTL,
2047                                 ~B43_NPHY_SCRAM_SIGCTL_SCM);
2048
2049         /* Set phase track alpha and beta */
2050         b43_phy_write(dev, B43_NPHY_PHASETR_A0, 0x125);
2051         b43_phy_write(dev, B43_NPHY_PHASETR_A1, 0x1B3);
2052         b43_phy_write(dev, B43_NPHY_PHASETR_A2, 0x105);
2053         b43_phy_write(dev, B43_NPHY_PHASETR_B0, 0x16E);
2054         b43_phy_write(dev, B43_NPHY_PHASETR_B1, 0xCD);
2055         b43_phy_write(dev, B43_NPHY_PHASETR_B2, 0x20);
2056
2057         b43_phy_mask(dev, B43_NPHY_PIL_DW1,
2058                         ~B43_NPHY_PIL_DW_64QAM & 0xFFFF);
2059         b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B1, 0xB5);
2060         b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B2, 0xA4);
2061         b43_phy_write(dev, B43_NPHY_TXF_20CO_S2B3, 0x00);
2062
2063         if (dev->phy.rev == 2)
2064                 b43_phy_set(dev, B43_NPHY_FINERX2_CGC,
2065                                 B43_NPHY_FINERX2_CGC_DECGC);
2066 }
2067
2068 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/Workarounds */
2069 static void b43_nphy_workarounds(struct b43_wldev *dev)
2070 {
2071         struct b43_phy *phy = &dev->phy;
2072         struct b43_phy_n *nphy = phy->n;
2073
2074         if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
2075                 b43_nphy_classifier(dev, 1, 0);
2076         else
2077                 b43_nphy_classifier(dev, 1, 1);
2078
2079         if (nphy->hang_avoid)
2080                 b43_nphy_stay_in_carrier_search(dev, 1);
2081
2082         b43_phy_set(dev, B43_NPHY_IQFLIP,
2083                     B43_NPHY_IQFLIP_ADC1 | B43_NPHY_IQFLIP_ADC2);
2084
2085         if (dev->phy.rev >= 3)
2086                 b43_nphy_workarounds_rev3plus(dev);
2087         else
2088                 b43_nphy_workarounds_rev1_2(dev);
2089
2090         if (nphy->hang_avoid)
2091                 b43_nphy_stay_in_carrier_search(dev, 0);
2092 }
2093
2094 /**************************************************
2095  * Tx/Rx common
2096  **************************************************/
2097
2098 /*
2099  * Transmits a known value for LO calibration
2100  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/TXTone
2101  */
2102 static int b43_nphy_tx_tone(struct b43_wldev *dev, u32 freq, u16 max_val,
2103                                 bool iqmode, bool dac_test)
2104 {
2105         u16 samp = b43_nphy_gen_load_samples(dev, freq, max_val, dac_test);
2106         if (samp == 0)
2107                 return -1;
2108         b43_nphy_run_samples(dev, samp, 0xFFFF, 0, iqmode, dac_test);
2109         return 0;
2110 }
2111
2112 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/Chains */
2113 static void b43_nphy_update_txrx_chain(struct b43_wldev *dev)
2114 {
2115         struct b43_phy_n *nphy = dev->phy.n;
2116
2117         bool override = false;
2118         u16 chain = 0x33;
2119
2120         if (nphy->txrx_chain == 0) {
2121                 chain = 0x11;
2122                 override = true;
2123         } else if (nphy->txrx_chain == 1) {
2124                 chain = 0x22;
2125                 override = true;
2126         }
2127
2128         b43_phy_maskset(dev, B43_NPHY_RFSEQCA,
2129                         ~(B43_NPHY_RFSEQCA_TXEN | B43_NPHY_RFSEQCA_RXEN),
2130                         chain);
2131
2132         if (override)
2133                 b43_phy_set(dev, B43_NPHY_RFSEQMODE,
2134                                 B43_NPHY_RFSEQMODE_CAOVER);
2135         else
2136                 b43_phy_mask(dev, B43_NPHY_RFSEQMODE,
2137                                 ~B43_NPHY_RFSEQMODE_CAOVER);
2138 }
2139
2140 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/stop-playback */
2141 static void b43_nphy_stop_playback(struct b43_wldev *dev)
2142 {
2143         struct b43_phy_n *nphy = dev->phy.n;
2144         u16 tmp;
2145
2146         if (nphy->hang_avoid)
2147                 b43_nphy_stay_in_carrier_search(dev, 1);
2148
2149         tmp = b43_phy_read(dev, B43_NPHY_SAMP_STAT);
2150         if (tmp & 0x1)
2151                 b43_phy_set(dev, B43_NPHY_SAMP_CMD, B43_NPHY_SAMP_CMD_STOP);
2152         else if (tmp & 0x2)
2153                 b43_phy_mask(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x7FFF);
2154
2155         b43_phy_mask(dev, B43_NPHY_SAMP_CMD, ~0x0004);
2156
2157         if (nphy->bb_mult_save & 0x80000000) {
2158                 tmp = nphy->bb_mult_save & 0xFFFF;
2159                 b43_ntab_write(dev, B43_NTAB16(15, 87), tmp);
2160                 nphy->bb_mult_save = 0;
2161         }
2162
2163         if (nphy->hang_avoid)
2164                 b43_nphy_stay_in_carrier_search(dev, 0);
2165 }
2166
2167 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/IqCalGainParams */
2168 static void b43_nphy_iq_cal_gain_params(struct b43_wldev *dev, u16 core,
2169                                         struct nphy_txgains target,
2170                                         struct nphy_iqcal_params *params)
2171 {
2172         int i, j, indx;
2173         u16 gain;
2174
2175         if (dev->phy.rev >= 3) {
2176                 params->txgm = target.txgm[core];
2177                 params->pga = target.pga[core];
2178                 params->pad = target.pad[core];
2179                 params->ipa = target.ipa[core];
2180                 params->cal_gain = (params->txgm << 12) | (params->pga << 8) |
2181                                         (params->pad << 4) | (params->ipa);
2182                 for (j = 0; j < 5; j++)
2183                         params->ncorr[j] = 0x79;
2184         } else {
2185                 gain = (target.pad[core]) | (target.pga[core] << 4) |
2186                         (target.txgm[core] << 8);
2187
2188                 indx = (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ?
2189                         1 : 0;
2190                 for (i = 0; i < 9; i++)
2191                         if (tbl_iqcal_gainparams[indx][i][0] == gain)
2192                                 break;
2193                 i = min(i, 8);
2194
2195                 params->txgm = tbl_iqcal_gainparams[indx][i][1];
2196                 params->pga = tbl_iqcal_gainparams[indx][i][2];
2197                 params->pad = tbl_iqcal_gainparams[indx][i][3];
2198                 params->cal_gain = (params->txgm << 7) | (params->pga << 4) |
2199                                         (params->pad << 2);
2200                 for (j = 0; j < 4; j++)
2201                         params->ncorr[j] = tbl_iqcal_gainparams[indx][i][4 + j];
2202         }
2203 }
2204
2205 /**************************************************
2206  * Tx and Rx
2207  **************************************************/
2208
2209 void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
2210 {//TODO
2211 }
2212
2213 static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
2214 {//TODO
2215 }
2216
2217 static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
2218                                                         bool ignore_tssi)
2219 {//TODO
2220         return B43_TXPWR_RES_DONE;
2221 }
2222
2223 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlEnable */
2224 static void b43_nphy_tx_power_ctrl(struct b43_wldev *dev, bool enable)
2225 {
2226         struct b43_phy_n *nphy = dev->phy.n;
2227         u8 i;
2228         u16 bmask, val, tmp;
2229         enum ieee80211_band band = b43_current_band(dev->wl);
2230
2231         if (nphy->hang_avoid)
2232                 b43_nphy_stay_in_carrier_search(dev, 1);
2233
2234         nphy->txpwrctrl = enable;
2235         if (!enable) {
2236                 if (dev->phy.rev >= 3 &&
2237                     (b43_phy_read(dev, B43_NPHY_TXPCTL_CMD) &
2238                      (B43_NPHY_TXPCTL_CMD_COEFF |
2239                       B43_NPHY_TXPCTL_CMD_HWPCTLEN |
2240                       B43_NPHY_TXPCTL_CMD_PCTLEN))) {
2241                         /* We disable enabled TX pwr ctl, save it's state */
2242                         nphy->tx_pwr_idx[0] = b43_phy_read(dev,
2243                                                 B43_NPHY_C1_TXPCTL_STAT) & 0x7f;
2244                         nphy->tx_pwr_idx[1] = b43_phy_read(dev,
2245                                                 B43_NPHY_C2_TXPCTL_STAT) & 0x7f;
2246                 }
2247
2248                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x6840);
2249                 for (i = 0; i < 84; i++)
2250                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0);
2251
2252                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x6C40);
2253                 for (i = 0; i < 84; i++)
2254                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO, 0);
2255
2256                 tmp = B43_NPHY_TXPCTL_CMD_COEFF | B43_NPHY_TXPCTL_CMD_HWPCTLEN;
2257                 if (dev->phy.rev >= 3)
2258                         tmp |= B43_NPHY_TXPCTL_CMD_PCTLEN;
2259                 b43_phy_mask(dev, B43_NPHY_TXPCTL_CMD, ~tmp);
2260
2261                 if (dev->phy.rev >= 3) {
2262                         b43_phy_set(dev, B43_NPHY_AFECTL_OVER1, 0x0100);
2263                         b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0100);
2264                 } else {
2265                         b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x4000);
2266                 }
2267
2268                 if (dev->phy.rev == 2)
2269                         b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3,
2270                                 ~B43_NPHY_BPHY_CTL3_SCALE, 0x53);
2271                 else if (dev->phy.rev < 2)
2272                         b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3,
2273                                 ~B43_NPHY_BPHY_CTL3_SCALE, 0x5A);
2274
2275                 if (dev->phy.rev < 2 && dev->phy.is_40mhz)
2276                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_TSSIRPSMW);
2277         } else {
2278                 b43_ntab_write_bulk(dev, B43_NTAB16(26, 64), 84,
2279                                     nphy->adj_pwr_tbl);
2280                 b43_ntab_write_bulk(dev, B43_NTAB16(27, 64), 84,
2281                                     nphy->adj_pwr_tbl);
2282
2283                 bmask = B43_NPHY_TXPCTL_CMD_COEFF |
2284                         B43_NPHY_TXPCTL_CMD_HWPCTLEN;
2285                 /* wl does useless check for "enable" param here */
2286                 val = B43_NPHY_TXPCTL_CMD_COEFF | B43_NPHY_TXPCTL_CMD_HWPCTLEN;
2287                 if (dev->phy.rev >= 3) {
2288                         bmask |= B43_NPHY_TXPCTL_CMD_PCTLEN;
2289                         if (val)
2290                                 val |= B43_NPHY_TXPCTL_CMD_PCTLEN;
2291                 }
2292                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_CMD, ~(bmask), val);
2293
2294                 if (band == IEEE80211_BAND_5GHZ) {
2295                         b43_phy_maskset(dev, B43_NPHY_TXPCTL_CMD,
2296                                         ~B43_NPHY_TXPCTL_CMD_INIT, 0x64);
2297                         if (dev->phy.rev > 1)
2298                                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_INIT,
2299                                                 ~B43_NPHY_TXPCTL_INIT_PIDXI1,
2300                                                 0x64);
2301                 }
2302
2303                 if (dev->phy.rev >= 3) {
2304                         if (nphy->tx_pwr_idx[0] != 128 &&
2305                             nphy->tx_pwr_idx[1] != 128) {
2306                                 /* Recover TX pwr ctl state */
2307                                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_CMD,
2308                                                 ~B43_NPHY_TXPCTL_CMD_INIT,
2309                                                 nphy->tx_pwr_idx[0]);
2310                                 if (dev->phy.rev > 1)
2311                                         b43_phy_maskset(dev,
2312                                                 B43_NPHY_TXPCTL_INIT,
2313                                                 ~0xff, nphy->tx_pwr_idx[1]);
2314                         }
2315                 }
2316
2317                 if (dev->phy.rev >= 3) {
2318                         b43_phy_mask(dev, B43_NPHY_AFECTL_OVER1, ~0x100);
2319                         b43_phy_mask(dev, B43_NPHY_AFECTL_OVER, ~0x100);
2320                 } else {
2321                         b43_phy_mask(dev, B43_NPHY_AFECTL_OVER, ~0x4000);
2322                 }
2323
2324                 if (dev->phy.rev == 2)
2325                         b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3, ~0xFF, 0x3b);
2326                 else if (dev->phy.rev < 2)
2327                         b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3, ~0xFF, 0x40);
2328
2329                 if (dev->phy.rev < 2 && dev->phy.is_40mhz)
2330                         b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_TSSIRPSMW);
2331
2332                 if (b43_nphy_ipa(dev)) {
2333                         b43_phy_mask(dev, B43_NPHY_PAPD_EN0, ~0x4);
2334                         b43_phy_mask(dev, B43_NPHY_PAPD_EN1, ~0x4);
2335                 }
2336         }
2337
2338         if (nphy->hang_avoid)
2339                 b43_nphy_stay_in_carrier_search(dev, 0);
2340 }
2341
2342 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrFix */
2343 static void b43_nphy_tx_power_fix(struct b43_wldev *dev)
2344 {
2345         struct b43_phy_n *nphy = dev->phy.n;
2346         struct ssb_sprom *sprom = dev->dev->bus_sprom;
2347
2348         u8 txpi[2], bbmult, i;
2349         u16 tmp, radio_gain, dac_gain;
2350         u16 freq = dev->phy.channel_freq;
2351         u32 txgain;
2352         /* u32 gaintbl; rev3+ */
2353
2354         if (nphy->hang_avoid)
2355                 b43_nphy_stay_in_carrier_search(dev, 1);
2356
2357         if (dev->phy.rev >= 7) {
2358                 txpi[0] = txpi[1] = 30;
2359         } else if (dev->phy.rev >= 3) {
2360                 txpi[0] = 40;
2361                 txpi[1] = 40;
2362         } else if (sprom->revision < 4) {
2363                 txpi[0] = 72;
2364                 txpi[1] = 72;
2365         } else {
2366                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2367                         txpi[0] = sprom->txpid2g[0];
2368                         txpi[1] = sprom->txpid2g[1];
2369                 } else if (freq >= 4900 && freq < 5100) {
2370                         txpi[0] = sprom->txpid5gl[0];
2371                         txpi[1] = sprom->txpid5gl[1];
2372                 } else if (freq >= 5100 && freq < 5500) {
2373                         txpi[0] = sprom->txpid5g[0];
2374                         txpi[1] = sprom->txpid5g[1];
2375                 } else if (freq >= 5500) {
2376                         txpi[0] = sprom->txpid5gh[0];
2377                         txpi[1] = sprom->txpid5gh[1];
2378                 } else {
2379                         txpi[0] = 91;
2380                         txpi[1] = 91;
2381                 }
2382         }
2383         if (dev->phy.rev < 7 &&
2384             (txpi[0] < 40 || txpi[0] > 100 || txpi[1] < 40 || txpi[1] > 100))
2385                 txpi[0] = txpi[1] = 91;
2386
2387         /*
2388         for (i = 0; i < 2; i++) {
2389                 nphy->txpwrindex[i].index_internal = txpi[i];
2390                 nphy->txpwrindex[i].index_internal_save = txpi[i];
2391         }
2392         */
2393
2394         for (i = 0; i < 2; i++) {
2395                 txgain = *(b43_nphy_get_tx_gain_table(dev) + txpi[i]);
2396
2397                 if (dev->phy.rev >= 3)
2398                         radio_gain = (txgain >> 16) & 0x1FFFF;
2399                 else
2400                         radio_gain = (txgain >> 16) & 0x1FFF;
2401
2402                 if (dev->phy.rev >= 7)
2403                         dac_gain = (txgain >> 8) & 0x7;
2404                 else
2405                         dac_gain = (txgain >> 8) & 0x3F;
2406                 bbmult = txgain & 0xFF;
2407
2408                 if (dev->phy.rev >= 3) {
2409                         if (i == 0)
2410                                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER1, 0x0100);
2411                         else
2412                                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0100);
2413                 } else {
2414                         b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x4000);
2415                 }
2416
2417                 if (i == 0)
2418                         b43_phy_write(dev, B43_NPHY_AFECTL_DACGAIN1, dac_gain);
2419                 else
2420                         b43_phy_write(dev, B43_NPHY_AFECTL_DACGAIN2, dac_gain);
2421
2422                 b43_ntab_write(dev, B43_NTAB16(0x7, 0x110 + i), radio_gain);
2423
2424                 tmp = b43_ntab_read(dev, B43_NTAB16(0xF, 0x57));
2425                 if (i == 0)
2426                         tmp = (tmp & 0x00FF) | (bbmult << 8);
2427                 else
2428                         tmp = (tmp & 0xFF00) | bbmult;
2429                 b43_ntab_write(dev, B43_NTAB16(0xF, 0x57), tmp);
2430
2431                 if (b43_nphy_ipa(dev)) {
2432                         u32 tmp32;
2433                         u16 reg = (i == 0) ?
2434                                 B43_NPHY_PAPD_EN0 : B43_NPHY_PAPD_EN1;
2435                         tmp32 = b43_ntab_read(dev, B43_NTAB32(26 + i,
2436                                                               576 + txpi[i]));
2437                         b43_phy_maskset(dev, reg, 0xE00F, (u32) tmp32 << 4);
2438                         b43_phy_set(dev, reg, 0x4);
2439                 }
2440         }
2441
2442         b43_phy_mask(dev, B43_NPHY_BPHY_CTL2, ~B43_NPHY_BPHY_CTL2_LUT);
2443
2444         if (nphy->hang_avoid)
2445                 b43_nphy_stay_in_carrier_search(dev, 0);
2446 }
2447
2448 static void b43_nphy_ipa_internal_tssi_setup(struct b43_wldev *dev)
2449 {
2450         struct b43_phy *phy = &dev->phy;
2451
2452         u8 core;
2453         u16 r; /* routing */
2454
2455         if (phy->rev >= 7) {
2456                 for (core = 0; core < 2; core++) {
2457                         r = core ? 0x190 : 0x170;
2458                         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2459                                 b43_radio_write(dev, r + 0x5, 0x5);
2460                                 b43_radio_write(dev, r + 0x9, 0xE);
2461                                 if (phy->rev != 5)
2462                                         b43_radio_write(dev, r + 0xA, 0);
2463                                 if (phy->rev != 7)
2464                                         b43_radio_write(dev, r + 0xB, 1);
2465                                 else
2466                                         b43_radio_write(dev, r + 0xB, 0x31);
2467                         } else {
2468                                 b43_radio_write(dev, r + 0x5, 0x9);
2469                                 b43_radio_write(dev, r + 0x9, 0xC);
2470                                 b43_radio_write(dev, r + 0xB, 0x0);
2471                                 if (phy->rev != 5)
2472                                         b43_radio_write(dev, r + 0xA, 1);
2473                                 else
2474                                         b43_radio_write(dev, r + 0xA, 0x31);
2475                         }
2476                         b43_radio_write(dev, r + 0x6, 0);
2477                         b43_radio_write(dev, r + 0x7, 0);
2478                         b43_radio_write(dev, r + 0x8, 3);
2479                         b43_radio_write(dev, r + 0xC, 0);
2480                 }
2481         } else {
2482                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
2483                         b43_radio_write(dev, B2056_SYN_RESERVED_ADDR31, 0x128);
2484                 else
2485                         b43_radio_write(dev, B2056_SYN_RESERVED_ADDR31, 0x80);
2486                 b43_radio_write(dev, B2056_SYN_RESERVED_ADDR30, 0);
2487                 b43_radio_write(dev, B2056_SYN_GPIO_MASTER1, 0x29);
2488
2489                 for (core = 0; core < 2; core++) {
2490                         r = core ? B2056_TX1 : B2056_TX0;
2491
2492                         b43_radio_write(dev, r | B2056_TX_IQCAL_VCM_HG, 0);
2493                         b43_radio_write(dev, r | B2056_TX_IQCAL_IDAC, 0);
2494                         b43_radio_write(dev, r | B2056_TX_TSSI_VCM, 3);
2495                         b43_radio_write(dev, r | B2056_TX_TX_AMP_DET, 0);
2496                         b43_radio_write(dev, r | B2056_TX_TSSI_MISC1, 8);
2497                         b43_radio_write(dev, r | B2056_TX_TSSI_MISC2, 0);
2498                         b43_radio_write(dev, r | B2056_TX_TSSI_MISC3, 0);
2499                         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2500                                 b43_radio_write(dev, r | B2056_TX_TX_SSI_MASTER,
2501                                                 0x5);
2502                                 if (phy->rev != 5)
2503                                         b43_radio_write(dev, r | B2056_TX_TSSIA,
2504                                                         0x00);
2505                                 if (phy->rev >= 5)
2506                                         b43_radio_write(dev, r | B2056_TX_TSSIG,
2507                                                         0x31);
2508                                 else
2509                                         b43_radio_write(dev, r | B2056_TX_TSSIG,
2510                                                         0x11);
2511                                 b43_radio_write(dev, r | B2056_TX_TX_SSI_MUX,
2512                                                 0xE);
2513                         } else {
2514                                 b43_radio_write(dev, r | B2056_TX_TX_SSI_MASTER,
2515                                                 0x9);
2516                                 b43_radio_write(dev, r | B2056_TX_TSSIA, 0x31);
2517                                 b43_radio_write(dev, r | B2056_TX_TSSIG, 0x0);
2518                                 b43_radio_write(dev, r | B2056_TX_TX_SSI_MUX,
2519                                                 0xC);
2520                         }
2521                 }
2522         }
2523 }
2524
2525 /*
2526  * Stop radio and transmit known signal. Then check received signal strength to
2527  * get TSSI (Transmit Signal Strength Indicator).
2528  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlIdleTssi
2529  */
2530 static void b43_nphy_tx_power_ctl_idle_tssi(struct b43_wldev *dev)
2531 {
2532         struct b43_phy *phy = &dev->phy;
2533         struct b43_phy_n *nphy = dev->phy.n;
2534
2535         u32 tmp;
2536         s32 rssi[4] = { };
2537
2538         /* TODO: check if we can transmit */
2539
2540         if (b43_nphy_ipa(dev))
2541                 b43_nphy_ipa_internal_tssi_setup(dev);
2542
2543         if (phy->rev >= 7)
2544                 ; /* TODO: Override Rev7 with 0x2000, 0, 3, 0, 0 as arguments */
2545         else if (phy->rev >= 3)
2546                 b43_nphy_rf_control_override(dev, 0x2000, 0, 3, false);
2547
2548         b43_nphy_stop_playback(dev);
2549         b43_nphy_tx_tone(dev, 0xFA0, 0, false, false);
2550         udelay(20);
2551         tmp = b43_nphy_poll_rssi(dev, 4, rssi, 1);
2552         b43_nphy_stop_playback(dev);
2553         b43_nphy_rssi_select(dev, 0, 0);
2554
2555         if (phy->rev >= 7)
2556                 ; /* TODO: Override Rev7 with 0x2000, 0, 3, 1, 0 as arguments */
2557         else if (phy->rev >= 3)
2558                 b43_nphy_rf_control_override(dev, 0x2000, 0, 3, true);
2559
2560         if (phy->rev >= 3) {
2561                 nphy->pwr_ctl_info[0].idle_tssi_5g = (tmp >> 24) & 0xFF;
2562                 nphy->pwr_ctl_info[1].idle_tssi_5g = (tmp >> 8) & 0xFF;
2563         } else {
2564                 nphy->pwr_ctl_info[0].idle_tssi_5g = (tmp >> 16) & 0xFF;
2565                 nphy->pwr_ctl_info[1].idle_tssi_5g = tmp & 0xFF;
2566         }
2567         nphy->pwr_ctl_info[0].idle_tssi_2g = (tmp >> 24) & 0xFF;
2568         nphy->pwr_ctl_info[1].idle_tssi_2g = (tmp >> 8) & 0xFF;
2569 }
2570
2571 /* http://bcm-v4.sipsolutions.net/PHY/N/TxPwrLimitToTbl */
2572 static void b43_nphy_tx_prepare_adjusted_power_table(struct b43_wldev *dev)
2573 {
2574         struct b43_phy_n *nphy = dev->phy.n;
2575
2576         u8 idx, delta;
2577         u8 i, stf_mode;
2578
2579         for (i = 0; i < 4; i++)
2580                 nphy->adj_pwr_tbl[i] = nphy->tx_power_offset[i];
2581
2582         for (stf_mode = 0; stf_mode < 4; stf_mode++) {
2583                 delta = 0;
2584                 switch (stf_mode) {
2585                 case 0:
2586                         if (dev->phy.is_40mhz && dev->phy.rev >= 5) {
2587                                 idx = 68;
2588                         } else {
2589                                 delta = 1;
2590                                 idx = dev->phy.is_40mhz ? 52 : 4;
2591                         }
2592                         break;
2593                 case 1:
2594                         idx = dev->phy.is_40mhz ? 76 : 28;
2595                         break;
2596                 case 2:
2597                         idx = dev->phy.is_40mhz ? 84 : 36;
2598                         break;
2599                 case 3:
2600                         idx = dev->phy.is_40mhz ? 92 : 44;
2601                         break;
2602                 }
2603
2604                 for (i = 0; i < 20; i++) {
2605                         nphy->adj_pwr_tbl[4 + 4 * i + stf_mode] =
2606                                 nphy->tx_power_offset[idx];
2607                         if (i == 0)
2608                                 idx += delta;
2609                         if (i == 14)
2610                                 idx += 1 - delta;
2611                         if (i == 3 || i == 4 || i == 7 || i == 8 || i == 11 ||
2612                             i == 13)
2613                                 idx += 1;
2614                 }
2615         }
2616 }
2617
2618 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlSetup */
2619 static void b43_nphy_tx_power_ctl_setup(struct b43_wldev *dev)
2620 {
2621         struct b43_phy_n *nphy = dev->phy.n;
2622         struct ssb_sprom *sprom = dev->dev->bus_sprom;
2623
2624         s16 a1[2], b0[2], b1[2];
2625         u8 idle[2];
2626         s8 target[2];
2627         s32 num, den, pwr;
2628         u32 regval[64];
2629
2630         u16 freq = dev->phy.channel_freq;
2631         u16 tmp;
2632         u16 r; /* routing */
2633         u8 i, c;
2634
2635         if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12) {
2636                 b43_maskset32(dev, B43_MMIO_MACCTL, ~0, 0x200000);
2637                 b43_read32(dev, B43_MMIO_MACCTL);
2638                 udelay(1);
2639         }
2640
2641         if (nphy->hang_avoid)
2642                 b43_nphy_stay_in_carrier_search(dev, true);
2643
2644         b43_phy_set(dev, B43_NPHY_TSSIMODE, B43_NPHY_TSSIMODE_EN);
2645         if (dev->phy.rev >= 3)
2646                 b43_phy_mask(dev, B43_NPHY_TXPCTL_CMD,
2647                              ~B43_NPHY_TXPCTL_CMD_PCTLEN & 0xFFFF);
2648         else
2649                 b43_phy_set(dev, B43_NPHY_TXPCTL_CMD,
2650                             B43_NPHY_TXPCTL_CMD_PCTLEN);
2651
2652         if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12)
2653                 b43_maskset32(dev, B43_MMIO_MACCTL, ~0x200000, 0);
2654
2655         if (sprom->revision < 4) {
2656                 idle[0] = nphy->pwr_ctl_info[0].idle_tssi_2g;
2657                 idle[1] = nphy->pwr_ctl_info[1].idle_tssi_2g;
2658                 target[0] = target[1] = 52;
2659                 a1[0] = a1[1] = -424;
2660                 b0[0] = b0[1] = 5612;
2661                 b1[0] = b1[1] = -1393;
2662         } else {
2663                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
2664                         for (c = 0; c < 2; c++) {
2665                                 idle[c] = nphy->pwr_ctl_info[c].idle_tssi_2g;
2666                                 target[c] = sprom->core_pwr_info[c].maxpwr_2g;
2667                                 a1[c] = sprom->core_pwr_info[c].pa_2g[0];
2668                                 b0[c] = sprom->core_pwr_info[c].pa_2g[1];
2669                                 b1[c] = sprom->core_pwr_info[c].pa_2g[2];
2670                         }
2671                 } else if (freq >= 4900 && freq < 5100) {
2672                         for (c = 0; c < 2; c++) {
2673                                 idle[c] = nphy->pwr_ctl_info[c].idle_tssi_5g;
2674                                 target[c] = sprom->core_pwr_info[c].maxpwr_5gl;
2675                                 a1[c] = sprom->core_pwr_info[c].pa_5gl[0];
2676                                 b0[c] = sprom->core_pwr_info[c].pa_5gl[1];
2677                                 b1[c] = sprom->core_pwr_info[c].pa_5gl[2];
2678                         }
2679                 } else if (freq >= 5100 && freq < 5500) {
2680                         for (c = 0; c < 2; c++) {
2681                                 idle[c] = nphy->pwr_ctl_info[c].idle_tssi_5g;
2682                                 target[c] = sprom->core_pwr_info[c].maxpwr_5g;
2683                                 a1[c] = sprom->core_pwr_info[c].pa_5g[0];
2684                                 b0[c] = sprom->core_pwr_info[c].pa_5g[1];
2685                                 b1[c] = sprom->core_pwr_info[c].pa_5g[2];
2686                         }
2687                 } else if (freq >= 5500) {
2688                         for (c = 0; c < 2; c++) {
2689                                 idle[c] = nphy->pwr_ctl_info[c].idle_tssi_5g;
2690                                 target[c] = sprom->core_pwr_info[c].maxpwr_5gh;
2691                                 a1[c] = sprom->core_pwr_info[c].pa_5gh[0];
2692                                 b0[c] = sprom->core_pwr_info[c].pa_5gh[1];
2693                                 b1[c] = sprom->core_pwr_info[c].pa_5gh[2];
2694                         }
2695                 } else {
2696                         idle[0] = nphy->pwr_ctl_info[0].idle_tssi_5g;
2697                         idle[1] = nphy->pwr_ctl_info[1].idle_tssi_5g;
2698                         target[0] = target[1] = 52;
2699                         a1[0] = a1[1] = -424;
2700                         b0[0] = b0[1] = 5612;
2701                         b1[0] = b1[1] = -1393;
2702                 }
2703         }
2704         /* target[0] = target[1] = nphy->tx_power_max; */
2705
2706         if (dev->phy.rev >= 3) {
2707                 if (sprom->fem.ghz2.tssipos)
2708                         b43_phy_set(dev, B43_NPHY_TXPCTL_ITSSI, 0x4000);
2709                 if (dev->phy.rev >= 7) {
2710                         for (c = 0; c < 2; c++) {
2711                                 r = c ? 0x190 : 0x170;
2712                                 if (b43_nphy_ipa(dev))
2713                                         b43_radio_write(dev, r + 0x9, (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) ? 0xE : 0xC);
2714                         }
2715                 } else {
2716                         if (b43_nphy_ipa(dev)) {
2717                                 tmp = (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ? 0xC : 0xE;
2718                                 b43_radio_write(dev,
2719                                         B2056_TX0 | B2056_TX_TX_SSI_MUX, tmp);
2720                                 b43_radio_write(dev,
2721                                         B2056_TX1 | B2056_TX_TX_SSI_MUX, tmp);
2722                         } else {
2723                                 b43_radio_write(dev,
2724                                         B2056_TX0 | B2056_TX_TX_SSI_MUX, 0x11);
2725                                 b43_radio_write(dev,
2726                                         B2056_TX1 | B2056_TX_TX_SSI_MUX, 0x11);
2727                         }
2728                 }
2729         }
2730
2731         if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12) {
2732                 b43_maskset32(dev, B43_MMIO_MACCTL, ~0, 0x200000);
2733                 b43_read32(dev, B43_MMIO_MACCTL);
2734                 udelay(1);
2735         }
2736
2737         if (dev->phy.rev >= 7) {
2738                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_CMD,
2739                                 ~B43_NPHY_TXPCTL_CMD_INIT, 0x19);
2740                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_INIT,
2741                                 ~B43_NPHY_TXPCTL_INIT_PIDXI1, 0x19);
2742         } else {
2743                 b43_phy_maskset(dev, B43_NPHY_TXPCTL_CMD,
2744                                 ~B43_NPHY_TXPCTL_CMD_INIT, 0x40);
2745                 if (dev->phy.rev > 1)
2746                         b43_phy_maskset(dev, B43_NPHY_TXPCTL_INIT,
2747                                 ~B43_NPHY_TXPCTL_INIT_PIDXI1, 0x40);
2748         }
2749
2750         if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12)
2751                 b43_maskset32(dev, B43_MMIO_MACCTL, ~0x200000, 0);
2752
2753         b43_phy_write(dev, B43_NPHY_TXPCTL_N,
2754                       0xF0 << B43_NPHY_TXPCTL_N_TSSID_SHIFT |
2755                       3 << B43_NPHY_TXPCTL_N_NPTIL2_SHIFT);
2756         b43_phy_write(dev, B43_NPHY_TXPCTL_ITSSI,
2757                       idle[0] << B43_NPHY_TXPCTL_ITSSI_0_SHIFT |
2758                       idle[1] << B43_NPHY_TXPCTL_ITSSI_1_SHIFT |
2759                       B43_NPHY_TXPCTL_ITSSI_BINF);
2760         b43_phy_write(dev, B43_NPHY_TXPCTL_TPWR,
2761                       target[0] << B43_NPHY_TXPCTL_TPWR_0_SHIFT |
2762                       target[1] << B43_NPHY_TXPCTL_TPWR_1_SHIFT);
2763
2764         for (c = 0; c < 2; c++) {
2765                 for (i = 0; i < 64; i++) {
2766                         num = 8 * (16 * b0[c] + b1[c] * i);
2767                         den = 32768 + a1[c] * i;
2768                         pwr = max((4 * num + den / 2) / den, -8);
2769                         if (dev->phy.rev < 3 && (i <= (31 - idle[c] + 1)))
2770                                 pwr = max(pwr, target[c] + 1);
2771                         regval[i] = pwr;
2772                 }
2773                 b43_ntab_write_bulk(dev, B43_NTAB32(26 + c, 0), 64, regval);
2774         }
2775
2776         b43_nphy_tx_prepare_adjusted_power_table(dev);
2777         /*
2778         b43_ntab_write_bulk(dev, B43_NTAB16(26, 64), 84, nphy->adj_pwr_tbl);
2779         b43_ntab_write_bulk(dev, B43_NTAB16(27, 64), 84, nphy->adj_pwr_tbl);
2780         */
2781
2782         if (nphy->hang_avoid)
2783                 b43_nphy_stay_in_carrier_search(dev, false);
2784 }
2785
2786 static void b43_nphy_tx_gain_table_upload(struct b43_wldev *dev)
2787 {
2788         struct b43_phy *phy = &dev->phy;
2789
2790         const u32 *table = NULL;
2791         u32 rfpwr_offset;
2792         u8 pga_gain;
2793         int i;
2794
2795         table = b43_nphy_get_tx_gain_table(dev);
2796         b43_ntab_write_bulk(dev, B43_NTAB32(26, 192), 128, table);
2797         b43_ntab_write_bulk(dev, B43_NTAB32(27, 192), 128, table);
2798
2799         if (phy->rev >= 3) {
2800 #if 0
2801                 nphy->gmval = (table[0] >> 16) & 0x7000;
2802 #endif
2803
2804                 for (i = 0; i < 128; i++) {
2805                         pga_gain = (table[i] >> 24) & 0xF;
2806                         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
2807                                 rfpwr_offset =
2808                                  b43_ntab_papd_pga_gain_delta_ipa_2g[pga_gain];
2809                         else
2810                                 rfpwr_offset =
2811                                  0; /* FIXME */
2812                         b43_ntab_write(dev, B43_NTAB32(26, 576 + i),
2813                                        rfpwr_offset);
2814                         b43_ntab_write(dev, B43_NTAB32(27, 576 + i),
2815                                        rfpwr_offset);
2816                 }
2817         }
2818 }
2819
2820 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PA%20override */
2821 static void b43_nphy_pa_override(struct b43_wldev *dev, bool enable)
2822 {
2823         struct b43_phy_n *nphy = dev->phy.n;
2824         enum ieee80211_band band;
2825         u16 tmp;
2826
2827         if (!enable) {
2828                 nphy->rfctrl_intc1_save = b43_phy_read(dev,
2829                                                        B43_NPHY_RFCTL_INTC1);
2830                 nphy->rfctrl_intc2_save = b43_phy_read(dev,
2831                                                        B43_NPHY_RFCTL_INTC2);
2832                 band = b43_current_band(dev->wl);
2833                 if (dev->phy.rev >= 3) {
2834                         if (band == IEEE80211_BAND_5GHZ)
2835                                 tmp = 0x600;
2836                         else
2837                                 tmp = 0x480;
2838                 } else {
2839                         if (band == IEEE80211_BAND_5GHZ)
2840                                 tmp = 0x180;
2841                         else
2842                                 tmp = 0x120;
2843                 }
2844                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, tmp);
2845                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, tmp);
2846         } else {
2847                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1,
2848                                 nphy->rfctrl_intc1_save);
2849                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2,
2850                                 nphy->rfctrl_intc2_save);
2851         }
2852 }
2853
2854 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxLpFbw */
2855 static void b43_nphy_tx_lp_fbw(struct b43_wldev *dev)
2856 {
2857         u16 tmp;
2858
2859         if (dev->phy.rev >= 3) {
2860                 if (b43_nphy_ipa(dev)) {
2861                         tmp = 4;
2862                         b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S2,
2863                               (((((tmp << 3) | tmp) << 3) | tmp) << 3) | tmp);
2864                 }
2865
2866                 tmp = 1;
2867                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S2,
2868                               (((((tmp << 3) | tmp) << 3) | tmp) << 3) | tmp);
2869         }
2870 }
2871
2872 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxIqEst */
2873 static void b43_nphy_rx_iq_est(struct b43_wldev *dev, struct nphy_iq_est *est,
2874                                 u16 samps, u8 time, bool wait)
2875 {
2876         int i;
2877         u16 tmp;
2878
2879         b43_phy_write(dev, B43_NPHY_IQEST_SAMCNT, samps);
2880         b43_phy_maskset(dev, B43_NPHY_IQEST_WT, ~B43_NPHY_IQEST_WT_VAL, time);
2881         if (wait)
2882                 b43_phy_set(dev, B43_NPHY_IQEST_CMD, B43_NPHY_IQEST_CMD_MODE);
2883         else
2884                 b43_phy_mask(dev, B43_NPHY_IQEST_CMD, ~B43_NPHY_IQEST_CMD_MODE);
2885
2886         b43_phy_set(dev, B43_NPHY_IQEST_CMD, B43_NPHY_IQEST_CMD_START);
2887
2888         for (i = 1000; i; i--) {
2889                 tmp = b43_phy_read(dev, B43_NPHY_IQEST_CMD);
2890                 if (!(tmp & B43_NPHY_IQEST_CMD_START)) {
2891                         est->i0_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_IPACC_HI0) << 16) |
2892                                         b43_phy_read(dev, B43_NPHY_IQEST_IPACC_LO0);
2893                         est->q0_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_QPACC_HI0) << 16) |
2894                                         b43_phy_read(dev, B43_NPHY_IQEST_QPACC_LO0);
2895                         est->iq0_prod = (b43_phy_read(dev, B43_NPHY_IQEST_IQACC_HI0) << 16) |
2896                                         b43_phy_read(dev, B43_NPHY_IQEST_IQACC_LO0);
2897
2898                         est->i1_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_IPACC_HI1) << 16) |
2899                                         b43_phy_read(dev, B43_NPHY_IQEST_IPACC_LO1);
2900                         est->q1_pwr = (b43_phy_read(dev, B43_NPHY_IQEST_QPACC_HI1) << 16) |
2901                                         b43_phy_read(dev, B43_NPHY_IQEST_QPACC_LO1);
2902                         est->iq1_prod = (b43_phy_read(dev, B43_NPHY_IQEST_IQACC_HI1) << 16) |
2903                                         b43_phy_read(dev, B43_NPHY_IQEST_IQACC_LO1);
2904                         return;
2905                 }
2906                 udelay(10);
2907         }
2908         memset(est, 0, sizeof(*est));
2909 }
2910
2911 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxIqCoeffs */
2912 static void b43_nphy_rx_iq_coeffs(struct b43_wldev *dev, bool write,
2913                                         struct b43_phy_n_iq_comp *pcomp)
2914 {
2915         if (write) {
2916                 b43_phy_write(dev, B43_NPHY_C1_RXIQ_COMPA0, pcomp->a0);
2917                 b43_phy_write(dev, B43_NPHY_C1_RXIQ_COMPB0, pcomp->b0);
2918                 b43_phy_write(dev, B43_NPHY_C2_RXIQ_COMPA1, pcomp->a1);
2919                 b43_phy_write(dev, B43_NPHY_C2_RXIQ_COMPB1, pcomp->b1);
2920         } else {
2921                 pcomp->a0 = b43_phy_read(dev, B43_NPHY_C1_RXIQ_COMPA0);
2922                 pcomp->b0 = b43_phy_read(dev, B43_NPHY_C1_RXIQ_COMPB0);
2923                 pcomp->a1 = b43_phy_read(dev, B43_NPHY_C2_RXIQ_COMPA1);
2924                 pcomp->b1 = b43_phy_read(dev, B43_NPHY_C2_RXIQ_COMPB1);
2925         }
2926 }
2927
2928 #if 0
2929 /* Ready but not used anywhere */
2930 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCalPhyCleanup */
2931 static void b43_nphy_rx_cal_phy_cleanup(struct b43_wldev *dev, u8 core)
2932 {
2933         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
2934
2935         b43_phy_write(dev, B43_NPHY_RFSEQCA, regs[0]);
2936         if (core == 0) {
2937                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, regs[1]);
2938                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, regs[2]);
2939         } else {
2940                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, regs[1]);
2941                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[2]);
2942         }
2943         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[3]);
2944         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[4]);
2945         b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO1, regs[5]);
2946         b43_phy_write(dev, B43_NPHY_RFCTL_RSSIO2, regs[6]);
2947         b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S1, regs[7]);
2948         b43_phy_write(dev, B43_NPHY_RFCTL_OVER, regs[8]);
2949         b43_phy_write(dev, B43_NPHY_PAPD_EN0, regs[9]);
2950         b43_phy_write(dev, B43_NPHY_PAPD_EN1, regs[10]);
2951 }
2952
2953 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCalPhySetup */
2954 static void b43_nphy_rx_cal_phy_setup(struct b43_wldev *dev, u8 core)
2955 {
2956         u8 rxval, txval;
2957         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
2958
2959         regs[0] = b43_phy_read(dev, B43_NPHY_RFSEQCA);
2960         if (core == 0) {
2961                 regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
2962                 regs[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
2963         } else {
2964                 regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
2965                 regs[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
2966         }
2967         regs[3] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
2968         regs[4] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
2969         regs[5] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO1);
2970         regs[6] = b43_phy_read(dev, B43_NPHY_RFCTL_RSSIO2);
2971         regs[7] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B1S1);
2972         regs[8] = b43_phy_read(dev, B43_NPHY_RFCTL_OVER);
2973         regs[9] = b43_phy_read(dev, B43_NPHY_PAPD_EN0);
2974         regs[10] = b43_phy_read(dev, B43_NPHY_PAPD_EN1);
2975
2976         b43_phy_mask(dev, B43_NPHY_PAPD_EN0, ~0x0001);
2977         b43_phy_mask(dev, B43_NPHY_PAPD_EN1, ~0x0001);
2978
2979         b43_phy_maskset(dev, B43_NPHY_RFSEQCA,
2980                         ~B43_NPHY_RFSEQCA_RXDIS & 0xFFFF,
2981                         ((1 - core) << B43_NPHY_RFSEQCA_RXDIS_SHIFT));
2982         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXEN,
2983                         ((1 - core) << B43_NPHY_RFSEQCA_TXEN_SHIFT));
2984         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_RXEN,
2985                         (core << B43_NPHY_RFSEQCA_RXEN_SHIFT));
2986         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXDIS,
2987                         (core << B43_NPHY_RFSEQCA_TXDIS_SHIFT));
2988
2989         if (core == 0) {
2990                 b43_phy_mask(dev, B43_NPHY_AFECTL_C1, ~0x0007);
2991                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER1, 0x0007);
2992         } else {
2993                 b43_phy_mask(dev, B43_NPHY_AFECTL_C2, ~0x0007);
2994                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0007);
2995         }
2996
2997         b43_nphy_rf_control_intc_override(dev, 2, 0, 3);
2998         b43_nphy_rf_control_override(dev, 8, 0, 3, false);
2999         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
3000
3001         if (core == 0) {
3002                 rxval = 1;
3003                 txval = 8;
3004         } else {
3005                 rxval = 4;
3006                 txval = 2;
3007         }
3008         b43_nphy_rf_control_intc_override(dev, 1, rxval, (core + 1));
3009         b43_nphy_rf_control_intc_override(dev, 1, txval, (2 - core));
3010 }
3011 #endif
3012
3013 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalcRxIqComp */
3014 static void b43_nphy_calc_rx_iq_comp(struct b43_wldev *dev, u8 mask)
3015 {
3016         int i;
3017         s32 iq;
3018         u32 ii;
3019         u32 qq;
3020         int iq_nbits, qq_nbits;
3021         int arsh, brsh;
3022         u16 tmp, a, b;
3023
3024         struct nphy_iq_est est;
3025         struct b43_phy_n_iq_comp old;
3026         struct b43_phy_n_iq_comp new = { };
3027         bool error = false;
3028
3029         if (mask == 0)
3030                 return;
3031
3032         b43_nphy_rx_iq_coeffs(dev, false, &old);
3033         b43_nphy_rx_iq_coeffs(dev, true, &new);
3034         b43_nphy_rx_iq_est(dev, &est, 0x4000, 32, false);
3035         new = old;
3036
3037         for (i = 0; i < 2; i++) {
3038                 if (i == 0 && (mask & 1)) {
3039                         iq = est.iq0_prod;
3040                         ii = est.i0_pwr;
3041                         qq = est.q0_pwr;
3042                 } else if (i == 1 && (mask & 2)) {
3043                         iq = est.iq1_prod;
3044                         ii = est.i1_pwr;
3045                         qq = est.q1_pwr;
3046                 } else {
3047                         continue;
3048                 }
3049
3050                 if (ii + qq < 2) {
3051                         error = true;
3052                         break;
3053                 }
3054
3055                 iq_nbits = fls(abs(iq));
3056                 qq_nbits = fls(qq);
3057
3058                 arsh = iq_nbits - 20;
3059                 if (arsh >= 0) {
3060                         a = -((iq << (30 - iq_nbits)) + (ii >> (1 + arsh)));
3061                         tmp = ii >> arsh;
3062                 } else {
3063                         a = -((iq << (30 - iq_nbits)) + (ii << (-1 - arsh)));
3064                         tmp = ii << -arsh;
3065                 }
3066                 if (tmp == 0) {
3067                         error = true;
3068                         break;
3069                 }
3070                 a /= tmp;
3071
3072                 brsh = qq_nbits - 11;
3073                 if (brsh >= 0) {
3074                         b = (qq << (31 - qq_nbits));
3075                         tmp = ii >> brsh;
3076                 } else {
3077                         b = (qq << (31 - qq_nbits));
3078                         tmp = ii << -brsh;
3079                 }
3080                 if (tmp == 0) {
3081                         error = true;
3082                         break;
3083                 }
3084                 b = int_sqrt(b / tmp - a * a) - (1 << 10);
3085
3086                 if (i == 0 && (mask & 0x1)) {
3087                         if (dev->phy.rev >= 3) {
3088                                 new.a0 = a & 0x3FF;
3089                                 new.b0 = b & 0x3FF;
3090                         } else {
3091                                 new.a0 = b & 0x3FF;
3092                                 new.b0 = a & 0x3FF;
3093                         }
3094                 } else if (i == 1 && (mask & 0x2)) {
3095                         if (dev->phy.rev >= 3) {
3096                                 new.a1 = a & 0x3FF;
3097                                 new.b1 = b & 0x3FF;
3098                         } else {
3099                                 new.a1 = b & 0x3FF;
3100                                 new.b1 = a & 0x3FF;
3101                         }
3102                 }
3103         }
3104
3105         if (error)
3106                 new = old;
3107
3108         b43_nphy_rx_iq_coeffs(dev, true, &new);
3109 }
3110
3111 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxIqWar */
3112 static void b43_nphy_tx_iq_workaround(struct b43_wldev *dev)
3113 {
3114         u16 array[4];
3115         b43_ntab_read_bulk(dev, B43_NTAB16(0xF, 0x50), 4, array);
3116
3117         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW0, array[0]);
3118         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW1, array[1]);
3119         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW2, array[2]);
3120         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_NPHY_TXIQW3, array[3]);
3121 }
3122
3123 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SpurWar */
3124 static void b43_nphy_spur_workaround(struct b43_wldev *dev)
3125 {
3126         struct b43_phy_n *nphy = dev->phy.n;
3127
3128         u8 channel = dev->phy.channel;
3129         int tone[2] = { 57, 58 };
3130         u32 noise[2] = { 0x3FF, 0x3FF };
3131
3132         B43_WARN_ON(dev->phy.rev < 3);
3133
3134         if (nphy->hang_avoid)
3135                 b43_nphy_stay_in_carrier_search(dev, 1);
3136
3137         if (nphy->gband_spurwar_en) {
3138                 /* TODO: N PHY Adjust Analog Pfbw (7) */
3139                 if (channel == 11 && dev->phy.is_40mhz)
3140                         ; /* TODO: N PHY Adjust Min Noise Var(2, tone, noise)*/
3141                 else
3142                         ; /* TODO: N PHY Adjust Min Noise Var(0, NULL, NULL)*/
3143                 /* TODO: N PHY Adjust CRS Min Power (0x1E) */
3144         }
3145
3146         if (nphy->aband_spurwar_en) {
3147                 if (channel == 54) {
3148                         tone[0] = 0x20;
3149                         noise[0] = 0x25F;
3150                 } else if (channel == 38 || channel == 102 || channel == 118) {
3151                         if (0 /* FIXME */) {
3152                                 tone[0] = 0x20;
3153                                 noise[0] = 0x21F;
3154                         } else {
3155                                 tone[0] = 0;
3156                                 noise[0] = 0;
3157                         }
3158                 } else if (channel == 134) {
3159                         tone[0] = 0x20;
3160                         noise[0] = 0x21F;
3161                 } else if (channel == 151) {
3162                         tone[0] = 0x10;
3163                         noise[0] = 0x23F;
3164                 } else if (channel == 153 || channel == 161) {
3165                         tone[0] = 0x30;
3166                         noise[0] = 0x23F;
3167                 } else {
3168                         tone[0] = 0;
3169                         noise[0] = 0;
3170                 }
3171
3172                 if (!tone[0] && !noise[0])
3173                         ; /* TODO: N PHY Adjust Min Noise Var(1, tone, noise)*/
3174                 else
3175                         ; /* TODO: N PHY Adjust Min Noise Var(0, NULL, NULL)*/
3176         }
3177
3178         if (nphy->hang_avoid)
3179                 b43_nphy_stay_in_carrier_search(dev, 0);
3180 }
3181
3182 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlCoefSetup */
3183 static void b43_nphy_tx_pwr_ctrl_coef_setup(struct b43_wldev *dev)
3184 {
3185         struct b43_phy_n *nphy = dev->phy.n;
3186         int i, j;
3187         u32 tmp;
3188         u32 cur_real, cur_imag, real_part, imag_part;
3189
3190         u16 buffer[7];
3191
3192         if (nphy->hang_avoid)
3193                 b43_nphy_stay_in_carrier_search(dev, true);
3194
3195         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 7, buffer);
3196
3197         for (i = 0; i < 2; i++) {
3198                 tmp = ((buffer[i * 2] & 0x3FF) << 10) |
3199                         (buffer[i * 2 + 1] & 0x3FF);
3200                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
3201                                 (((i + 26) << 10) | 320));
3202                 for (j = 0; j < 128; j++) {
3203                         b43_phy_write(dev, B43_NPHY_TABLE_DATAHI,
3204                                         ((tmp >> 16) & 0xFFFF));
3205                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
3206                                         (tmp & 0xFFFF));
3207                 }
3208         }
3209
3210         for (i = 0; i < 2; i++) {
3211                 tmp = buffer[5 + i];
3212                 real_part = (tmp >> 8) & 0xFF;
3213                 imag_part = (tmp & 0xFF);
3214                 b43_phy_write(dev, B43_NPHY_TABLE_ADDR,
3215                                 (((i + 26) << 10) | 448));
3216
3217                 if (dev->phy.rev >= 3) {
3218                         cur_real = real_part;
3219                         cur_imag = imag_part;
3220                         tmp = ((cur_real & 0xFF) << 8) | (cur_imag & 0xFF);
3221                 }
3222
3223                 for (j = 0; j < 128; j++) {
3224                         if (dev->phy.rev < 3) {
3225                                 cur_real = (real_part * loscale[j] + 128) >> 8;
3226                                 cur_imag = (imag_part * loscale[j] + 128) >> 8;
3227                                 tmp = ((cur_real & 0xFF) << 8) |
3228                                         (cur_imag & 0xFF);
3229                         }
3230                         b43_phy_write(dev, B43_NPHY_TABLE_DATAHI,
3231                                         ((tmp >> 16) & 0xFFFF));
3232                         b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
3233                                         (tmp & 0xFFFF));
3234                 }
3235         }
3236
3237         if (dev->phy.rev >= 3) {
3238                 b43_shm_write16(dev, B43_SHM_SHARED,
3239                                 B43_SHM_SH_NPHY_TXPWR_INDX0, 0xFFFF);
3240                 b43_shm_write16(dev, B43_SHM_SHARED,
3241                                 B43_SHM_SH_NPHY_TXPWR_INDX1, 0xFFFF);
3242         }
3243
3244         if (nphy->hang_avoid)
3245                 b43_nphy_stay_in_carrier_search(dev, false);
3246 }
3247
3248 /*
3249  * Restore RSSI Calibration
3250  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/RestoreRssiCal
3251  */
3252 static void b43_nphy_restore_rssi_cal(struct b43_wldev *dev)
3253 {
3254         struct b43_phy_n *nphy = dev->phy.n;
3255
3256         u16 *rssical_radio_regs = NULL;
3257         u16 *rssical_phy_regs = NULL;
3258
3259         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
3260                 if (!nphy->rssical_chanspec_2G.center_freq)
3261                         return;
3262                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_2G;
3263                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_2G;
3264         } else {
3265                 if (!nphy->rssical_chanspec_5G.center_freq)
3266                         return;
3267                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_5G;
3268                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
3269         }
3270
3271         /* TODO use some definitions */
3272         b43_radio_maskset(dev, 0x602B, 0xE3, rssical_radio_regs[0]);
3273         b43_radio_maskset(dev, 0x702B, 0xE3, rssical_radio_regs[1]);
3274
3275         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, rssical_phy_regs[0]);
3276         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, rssical_phy_regs[1]);
3277         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, rssical_phy_regs[2]);
3278         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, rssical_phy_regs[3]);
3279
3280         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, rssical_phy_regs[4]);
3281         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, rssical_phy_regs[5]);
3282         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, rssical_phy_regs[6]);
3283         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, rssical_phy_regs[7]);
3284
3285         b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, rssical_phy_regs[8]);
3286         b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, rssical_phy_regs[9]);
3287         b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, rssical_phy_regs[10]);
3288         b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, rssical_phy_regs[11]);
3289 }
3290
3291 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalRadioSetup */
3292 static void b43_nphy_tx_cal_radio_setup(struct b43_wldev *dev)
3293 {
3294         struct b43_phy_n *nphy = dev->phy.n;
3295         u16 *save = nphy->tx_rx_cal_radio_saveregs;
3296         u16 tmp;
3297         u8 offset, i;
3298
3299         if (dev->phy.rev >= 3) {
3300             for (i = 0; i < 2; i++) {
3301                 tmp = (i == 0) ? 0x2000 : 0x3000;
3302                 offset = i * 11;
3303
3304                 save[offset + 0] = b43_radio_read16(dev, B2055_CAL_RVARCTL);
3305                 save[offset + 1] = b43_radio_read16(dev, B2055_CAL_LPOCTL);
3306                 save[offset + 2] = b43_radio_read16(dev, B2055_CAL_TS);
3307                 save[offset + 3] = b43_radio_read16(dev, B2055_CAL_RCCALRTS);
3308                 save[offset + 4] = b43_radio_read16(dev, B2055_CAL_RCALRTS);
3309                 save[offset + 5] = b43_radio_read16(dev, B2055_PADDRV);
3310                 save[offset + 6] = b43_radio_read16(dev, B2055_XOCTL1);
3311                 save[offset + 7] = b43_radio_read16(dev, B2055_XOCTL2);
3312                 save[offset + 8] = b43_radio_read16(dev, B2055_XOREGUL);
3313                 save[offset + 9] = b43_radio_read16(dev, B2055_XOMISC);
3314                 save[offset + 10] = b43_radio_read16(dev, B2055_PLL_LFC1);
3315
3316                 if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
3317                         b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x0A);
3318                         b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
3319                         b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
3320                         b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
3321                         b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
3322                         if (nphy->ipa5g_on) {
3323                                 b43_radio_write16(dev, tmp | B2055_PADDRV, 4);
3324                                 b43_radio_write16(dev, tmp | B2055_XOCTL1, 1);
3325                         } else {
3326                                 b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
3327                                 b43_radio_write16(dev, tmp | B2055_XOCTL1, 0x2F);
3328                         }
3329                         b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
3330                 } else {
3331                         b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x06);
3332                         b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
3333                         b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
3334                         b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
3335                         b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
3336                         b43_radio_write16(dev, tmp | B2055_XOCTL1, 0);
3337                         if (nphy->ipa2g_on) {
3338                                 b43_radio_write16(dev, tmp | B2055_PADDRV, 6);
3339                                 b43_radio_write16(dev, tmp | B2055_XOCTL2,
3340                                         (dev->phy.rev < 5) ? 0x11 : 0x01);
3341                         } else {
3342                                 b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
3343                                 b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
3344                         }
3345                 }
3346                 b43_radio_write16(dev, tmp | B2055_XOREGUL, 0);
3347                 b43_radio_write16(dev, tmp | B2055_XOMISC, 0);
3348                 b43_radio_write16(dev, tmp | B2055_PLL_LFC1, 0);
3349             }
3350         } else {
3351                 save[0] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL1);
3352                 b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL1, 0x29);
3353
3354                 save[1] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL2);
3355                 b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL2, 0x54);
3356
3357                 save[2] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL1);
3358                 b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL1, 0x29);
3359
3360                 save[3] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL2);
3361                 b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL2, 0x54);
3362
3363                 save[3] = b43_radio_read16(dev, B2055_C1_PWRDET_RXTX);
3364                 save[4] = b43_radio_read16(dev, B2055_C2_PWRDET_RXTX);
3365
3366                 if (!(b43_phy_read(dev, B43_NPHY_BANDCTL) &
3367                     B43_NPHY_BANDCTL_5GHZ)) {
3368                         b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x04);
3369                         b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x04);
3370                 } else {
3371                         b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x20);
3372                         b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x20);
3373                 }
3374
3375                 if (dev->phy.rev < 2) {
3376                         b43_radio_set(dev, B2055_C1_TX_BB_MXGM, 0x20);
3377                         b43_radio_set(dev, B2055_C2_TX_BB_MXGM, 0x20);
3378                 } else {
3379                         b43_radio_mask(dev, B2055_C1_TX_BB_MXGM, ~0x20);
3380                         b43_radio_mask(dev, B2055_C2_TX_BB_MXGM, ~0x20);
3381                 }
3382         }
3383 }
3384
3385 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/UpdateTxCalLadder */
3386 static void b43_nphy_update_tx_cal_ladder(struct b43_wldev *dev, u16 core)
3387 {
3388         struct b43_phy_n *nphy = dev->phy.n;
3389         int i;
3390         u16 scale, entry;
3391
3392         u16 tmp = nphy->txcal_bbmult;
3393         if (core == 0)
3394                 tmp >>= 8;
3395         tmp &= 0xff;
3396
3397         for (i = 0; i < 18; i++) {
3398                 scale = (ladder_lo[i].percent * tmp) / 100;
3399                 entry = ((scale & 0xFF) << 8) | ladder_lo[i].g_env;
3400                 b43_ntab_write(dev, B43_NTAB16(15, i), entry);
3401
3402                 scale = (ladder_iq[i].percent * tmp) / 100;
3403                 entry = ((scale & 0xFF) << 8) | ladder_iq[i].g_env;
3404                 b43_ntab_write(dev, B43_NTAB16(15, i + 32), entry);
3405         }
3406 }
3407
3408 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ExtPaSetTxDigiFilts */
3409 static void b43_nphy_ext_pa_set_tx_dig_filters(struct b43_wldev *dev)
3410 {
3411         int i;
3412         for (i = 0; i < 15; i++)
3413                 b43_phy_write(dev, B43_PHY_N(0x2C5 + i),
3414                                 tbl_tx_filter_coef_rev4[2][i]);
3415 }
3416
3417 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/IpaSetTxDigiFilts */
3418 static void b43_nphy_int_pa_set_tx_dig_filters(struct b43_wldev *dev)
3419 {
3420         int i, j;
3421         /* B43_NPHY_TXF_20CO_S0A1, B43_NPHY_TXF_40CO_S0A1, unknown */
3422         static const u16 offset[] = { 0x186, 0x195, 0x2C5 };
3423
3424         for (i = 0; i < 3; i++)
3425                 for (j = 0; j < 15; j++)
3426                         b43_phy_write(dev, B43_PHY_N(offset[i] + j),
3427                                         tbl_tx_filter_coef_rev4[i][j]);
3428
3429         if (dev->phy.is_40mhz) {
3430                 for (j = 0; j < 15; j++)
3431                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
3432                                         tbl_tx_filter_coef_rev4[3][j]);
3433         } else if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
3434                 for (j = 0; j < 15; j++)
3435                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
3436                                         tbl_tx_filter_coef_rev4[5][j]);
3437         }
3438
3439         if (dev->phy.channel == 14)
3440                 for (j = 0; j < 15; j++)
3441                         b43_phy_write(dev, B43_PHY_N(offset[0] + j),
3442                                         tbl_tx_filter_coef_rev4[6][j]);
3443 }
3444
3445 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GetTxGain */
3446 static struct nphy_txgains b43_nphy_get_tx_gains(struct b43_wldev *dev)
3447 {
3448         struct b43_phy_n *nphy = dev->phy.n;
3449
3450         u16 curr_gain[2];
3451         struct nphy_txgains target;
3452         const u32 *table = NULL;
3453
3454         if (!nphy->txpwrctrl) {
3455                 int i;
3456
3457                 if (nphy->hang_avoid)
3458                         b43_nphy_stay_in_carrier_search(dev, true);
3459                 b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, curr_gain);
3460                 if (nphy->hang_avoid)
3461                         b43_nphy_stay_in_carrier_search(dev, false);
3462
3463                 for (i = 0; i < 2; ++i) {
3464                         if (dev->phy.rev >= 3) {
3465                                 target.ipa[i] = curr_gain[i] & 0x000F;
3466                                 target.pad[i] = (curr_gain[i] & 0x00F0) >> 4;
3467                                 target.pga[i] = (curr_gain[i] & 0x0F00) >> 8;
3468                                 target.txgm[i] = (curr_gain[i] & 0x7000) >> 12;
3469                         } else {
3470                                 target.ipa[i] = curr_gain[i] & 0x0003;
3471                                 target.pad[i] = (curr_gain[i] & 0x000C) >> 2;
3472                                 target.pga[i] = (curr_gain[i] & 0x0070) >> 4;
3473                                 target.txgm[i] = (curr_gain[i] & 0x0380) >> 7;
3474                         }
3475                 }
3476         } else {
3477                 int i;
3478                 u16 index[2];
3479                 index[0] = (b43_phy_read(dev, B43_NPHY_C1_TXPCTL_STAT) &
3480                         B43_NPHY_TXPCTL_STAT_BIDX) >>
3481                         B43_NPHY_TXPCTL_STAT_BIDX_SHIFT;
3482                 index[1] = (b43_phy_read(dev, B43_NPHY_C2_TXPCTL_STAT) &
3483                         B43_NPHY_TXPCTL_STAT_BIDX) >>
3484                         B43_NPHY_TXPCTL_STAT_BIDX_SHIFT;
3485
3486                 for (i = 0; i < 2; ++i) {
3487                         table = b43_nphy_get_tx_gain_table(dev);
3488                         if (dev->phy.rev >= 3) {
3489                                 target.ipa[i] = (table[index[i]] >> 16) & 0xF;
3490                                 target.pad[i] = (table[index[i]] >> 20) & 0xF;
3491                                 target.pga[i] = (table[index[i]] >> 24) & 0xF;
3492                                 target.txgm[i] = (table[index[i]] >> 28) & 0xF;
3493                         } else {
3494                                 target.ipa[i] = (table[index[i]] >> 16) & 0x3;
3495                                 target.pad[i] = (table[index[i]] >> 18) & 0x3;
3496                                 target.pga[i] = (table[index[i]] >> 20) & 0x7;
3497                                 target.txgm[i] = (table[index[i]] >> 23) & 0x7;
3498                         }
3499                 }
3500         }
3501
3502         return target;
3503 }
3504
3505 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalPhyCleanup */
3506 static void b43_nphy_tx_cal_phy_cleanup(struct b43_wldev *dev)
3507 {
3508         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
3509
3510         if (dev->phy.rev >= 3) {
3511                 b43_phy_write(dev, B43_NPHY_AFECTL_C1, regs[0]);
3512                 b43_phy_write(dev, B43_NPHY_AFECTL_C2, regs[1]);
3513                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, regs[2]);
3514                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[3]);
3515                 b43_phy_write(dev, B43_NPHY_BBCFG, regs[4]);
3516                 b43_ntab_write(dev, B43_NTAB16(8, 3), regs[5]);
3517                 b43_ntab_write(dev, B43_NTAB16(8, 19), regs[6]);
3518                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[7]);
3519                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[8]);
3520                 b43_phy_write(dev, B43_NPHY_PAPD_EN0, regs[9]);
3521                 b43_phy_write(dev, B43_NPHY_PAPD_EN1, regs[10]);
3522                 b43_nphy_reset_cca(dev);
3523         } else {
3524                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, regs[0]);
3525                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, regs[1]);
3526                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, regs[2]);
3527                 b43_ntab_write(dev, B43_NTAB16(8, 2), regs[3]);
3528                 b43_ntab_write(dev, B43_NTAB16(8, 18), regs[4]);
3529                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs[5]);
3530                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs[6]);
3531         }
3532 }
3533
3534 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxCalPhySetup */
3535 static void b43_nphy_tx_cal_phy_setup(struct b43_wldev *dev)
3536 {
3537         u16 *regs = dev->phy.n->tx_rx_cal_phy_saveregs;
3538         u16 tmp;
3539
3540         regs[0] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
3541         regs[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
3542         if (dev->phy.rev >= 3) {
3543                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0xF0FF, 0x0A00);
3544                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0xF0FF, 0x0A00);
3545
3546                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER1);
3547                 regs[2] = tmp;
3548                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, tmp | 0x0600);
3549
3550                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
3551                 regs[3] = tmp;
3552                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp | 0x0600);
3553
3554                 regs[4] = b43_phy_read(dev, B43_NPHY_BBCFG);
3555                 b43_phy_mask(dev, B43_NPHY_BBCFG,
3556                              ~B43_NPHY_BBCFG_RSTRX & 0xFFFF);
3557
3558                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 3));
3559                 regs[5] = tmp;
3560                 b43_ntab_write(dev, B43_NTAB16(8, 3), 0);
3561
3562                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 19));
3563                 regs[6] = tmp;
3564                 b43_ntab_write(dev, B43_NTAB16(8, 19), 0);
3565                 regs[7] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
3566                 regs[8] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
3567
3568                 b43_nphy_rf_control_intc_override(dev, 2, 1, 3);
3569                 b43_nphy_rf_control_intc_override(dev, 1, 2, 1);
3570                 b43_nphy_rf_control_intc_override(dev, 1, 8, 2);
3571
3572                 regs[9] = b43_phy_read(dev, B43_NPHY_PAPD_EN0);
3573                 regs[10] = b43_phy_read(dev, B43_NPHY_PAPD_EN1);
3574                 b43_phy_mask(dev, B43_NPHY_PAPD_EN0, ~0x0001);
3575                 b43_phy_mask(dev, B43_NPHY_PAPD_EN1, ~0x0001);
3576         } else {
3577                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, 0xA000);
3578                 b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, 0xA000);
3579                 tmp = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
3580                 regs[2] = tmp;
3581                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp | 0x3000);
3582                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 2));
3583                 regs[3] = tmp;
3584                 tmp |= 0x2000;
3585                 b43_ntab_write(dev, B43_NTAB16(8, 2), tmp);
3586                 tmp = b43_ntab_read(dev, B43_NTAB16(8, 18));
3587                 regs[4] = tmp;
3588                 tmp |= 0x2000;
3589                 b43_ntab_write(dev, B43_NTAB16(8, 18), tmp);
3590                 regs[5] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
3591                 regs[6] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
3592                 if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
3593                         tmp = 0x0180;
3594                 else
3595                         tmp = 0x0120;
3596                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, tmp);
3597                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, tmp);
3598         }
3599 }
3600
3601 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SaveCal */
3602 static void b43_nphy_save_cal(struct b43_wldev *dev)
3603 {
3604         struct b43_phy_n *nphy = dev->phy.n;
3605
3606         struct b43_phy_n_iq_comp *rxcal_coeffs = NULL;
3607         u16 *txcal_radio_regs = NULL;
3608         struct b43_chanspec *iqcal_chanspec;
3609         u16 *table = NULL;
3610
3611         if (nphy->hang_avoid)
3612                 b43_nphy_stay_in_carrier_search(dev, 1);
3613
3614         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
3615                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_2G;
3616                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_2G;
3617                 iqcal_chanspec = &nphy->iqcal_chanspec_2G;
3618                 table = nphy->cal_cache.txcal_coeffs_2G;
3619         } else {
3620                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_5G;
3621                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_5G;
3622                 iqcal_chanspec = &nphy->iqcal_chanspec_5G;
3623                 table = nphy->cal_cache.txcal_coeffs_5G;
3624         }
3625
3626         b43_nphy_rx_iq_coeffs(dev, false, rxcal_coeffs);
3627         /* TODO use some definitions */
3628         if (dev->phy.rev >= 3) {
3629                 txcal_radio_regs[0] = b43_radio_read(dev, 0x2021);
3630                 txcal_radio_regs[1] = b43_radio_read(dev, 0x2022);
3631                 txcal_radio_regs[2] = b43_radio_read(dev, 0x3021);
3632                 txcal_radio_regs[3] = b43_radio_read(dev, 0x3022);
3633                 txcal_radio_regs[4] = b43_radio_read(dev, 0x2023);
3634                 txcal_radio_regs[5] = b43_radio_read(dev, 0x2024);
3635                 txcal_radio_regs[6] = b43_radio_read(dev, 0x3023);
3636                 txcal_radio_regs[7] = b43_radio_read(dev, 0x3024);
3637         } else {
3638                 txcal_radio_regs[0] = b43_radio_read(dev, 0x8B);
3639                 txcal_radio_regs[1] = b43_radio_read(dev, 0xBA);
3640                 txcal_radio_regs[2] = b43_radio_read(dev, 0x8D);
3641                 txcal_radio_regs[3] = b43_radio_read(dev, 0xBC);
3642         }
3643         iqcal_chanspec->center_freq = dev->phy.channel_freq;
3644         iqcal_chanspec->channel_type = dev->phy.channel_type;
3645         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 8, table);
3646
3647         if (nphy->hang_avoid)
3648                 b43_nphy_stay_in_carrier_search(dev, 0);
3649 }
3650
3651 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RestoreCal */
3652 static void b43_nphy_restore_cal(struct b43_wldev *dev)
3653 {
3654         struct b43_phy_n *nphy = dev->phy.n;
3655
3656         u16 coef[4];
3657         u16 *loft = NULL;
3658         u16 *table = NULL;
3659
3660         int i;
3661         u16 *txcal_radio_regs = NULL;
3662         struct b43_phy_n_iq_comp *rxcal_coeffs = NULL;
3663
3664         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
3665                 if (!nphy->iqcal_chanspec_2G.center_freq)
3666                         return;
3667                 table = nphy->cal_cache.txcal_coeffs_2G;
3668                 loft = &nphy->cal_cache.txcal_coeffs_2G[5];
3669         } else {
3670                 if (!nphy->iqcal_chanspec_5G.center_freq)
3671                         return;
3672                 table = nphy->cal_cache.txcal_coeffs_5G;
3673                 loft = &nphy->cal_cache.txcal_coeffs_5G[5];
3674         }
3675
3676         b43_ntab_write_bulk(dev, B43_NTAB16(15, 80), 4, table);
3677
3678         for (i = 0; i < 4; i++) {
3679                 if (dev->phy.rev >= 3)
3680                         table[i] = coef[i];
3681                 else
3682                         coef[i] = 0;
3683         }
3684
3685         b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4, coef);
3686         b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2, loft);
3687         b43_ntab_write_bulk(dev, B43_NTAB16(15, 93), 2, loft);
3688
3689         if (dev->phy.rev < 2)
3690                 b43_nphy_tx_iq_workaround(dev);
3691
3692         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
3693                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_2G;
3694                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_2G;
3695         } else {
3696                 txcal_radio_regs = nphy->cal_cache.txcal_radio_regs_5G;
3697                 rxcal_coeffs = &nphy->cal_cache.rxcal_coeffs_5G;
3698         }
3699
3700         /* TODO use some definitions */
3701         if (dev->phy.rev >= 3) {
3702                 b43_radio_write(dev, 0x2021, txcal_radio_regs[0]);
3703                 b43_radio_write(dev, 0x2022, txcal_radio_regs[1]);
3704                 b43_radio_write(dev, 0x3021, txcal_radio_regs[2]);
3705                 b43_radio_write(dev, 0x3022, txcal_radio_regs[3]);
3706                 b43_radio_write(dev, 0x2023, txcal_radio_regs[4]);
3707                 b43_radio_write(dev, 0x2024, txcal_radio_regs[5]);
3708                 b43_radio_write(dev, 0x3023, txcal_radio_regs[6]);
3709                 b43_radio_write(dev, 0x3024, txcal_radio_regs[7]);
3710         } else {
3711                 b43_radio_write(dev, 0x8B, txcal_radio_regs[0]);
3712                 b43_radio_write(dev, 0xBA, txcal_radio_regs[1]);
3713                 b43_radio_write(dev, 0x8D, txcal_radio_regs[2]);
3714                 b43_radio_write(dev, 0xBC, txcal_radio_regs[3]);
3715         }
3716         b43_nphy_rx_iq_coeffs(dev, true, rxcal_coeffs);
3717 }
3718
3719 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalTxIqlo */
3720 static int b43_nphy_cal_tx_iq_lo(struct b43_wldev *dev,
3721                                 struct nphy_txgains target,
3722                                 bool full, bool mphase)
3723 {
3724         struct b43_phy_n *nphy = dev->phy.n;
3725         int i;
3726         int error = 0;
3727         int freq;
3728         bool avoid = false;
3729         u8 length;
3730         u16 tmp, core, type, count, max, numb, last = 0, cmd;
3731         const u16 *table;
3732         bool phy6or5x;
3733
3734         u16 buffer[11];
3735         u16 diq_start = 0;
3736         u16 save[2];
3737         u16 gain[2];
3738         struct nphy_iqcal_params params[2];
3739         bool updated[2] = { };
3740
3741         b43_nphy_stay_in_carrier_search(dev, true);
3742
3743         if (dev->phy.rev >= 4) {
3744                 avoid = nphy->hang_avoid;
3745                 nphy->hang_avoid = false;
3746         }
3747
3748         b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, save);
3749
3750         for (i = 0; i < 2; i++) {
3751                 b43_nphy_iq_cal_gain_params(dev, i, target, &params[i]);
3752                 gain[i] = params[i].cal_gain;
3753         }
3754
3755         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, gain);
3756
3757         b43_nphy_tx_cal_radio_setup(dev);
3758         b43_nphy_tx_cal_phy_setup(dev);
3759
3760         phy6or5x = dev->phy.rev >= 6 ||
3761                 (dev->phy.rev == 5 && nphy->ipa2g_on &&
3762                 b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ);
3763         if (phy6or5x) {
3764                 if (dev->phy.is_40mhz) {
3765                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 0), 18,
3766                                         tbl_tx_iqlo_cal_loft_ladder_40);
3767                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 32), 18,
3768                                         tbl_tx_iqlo_cal_iqimb_ladder_40);
3769                 } else {
3770                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 0), 18,
3771                                         tbl_tx_iqlo_cal_loft_ladder_20);
3772                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 32), 18,
3773                                         tbl_tx_iqlo_cal_iqimb_ladder_20);
3774                 }
3775         }
3776
3777         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0x8AA9);
3778
3779         if (!dev->phy.is_40mhz)
3780                 freq = 2500;
3781         else
3782                 freq = 5000;
3783
3784         if (nphy->mphase_cal_phase_id > 2)
3785                 b43_nphy_run_samples(dev, (dev->phy.is_40mhz ? 40 : 20) * 8,
3786                                         0xFFFF, 0, true, false);
3787         else
3788                 error = b43_nphy_tx_tone(dev, freq, 250, true, false);
3789
3790         if (error == 0) {
3791                 if (nphy->mphase_cal_phase_id > 2) {
3792                         table = nphy->mphase_txcal_bestcoeffs;
3793                         length = 11;
3794                         if (dev->phy.rev < 3)
3795                                 length -= 2;
3796                 } else {
3797                         if (!full && nphy->txiqlocal_coeffsvalid) {
3798                                 table = nphy->txiqlocal_bestc;
3799                                 length = 11;
3800                                 if (dev->phy.rev < 3)
3801                                         length -= 2;
3802                         } else {
3803                                 full = true;
3804                                 if (dev->phy.rev >= 3) {
3805                                         table = tbl_tx_iqlo_cal_startcoefs_nphyrev3;
3806                                         length = B43_NTAB_TX_IQLO_CAL_STARTCOEFS_REV3;
3807                                 } else {
3808                                         table = tbl_tx_iqlo_cal_startcoefs;
3809                                         length = B43_NTAB_TX_IQLO_CAL_STARTCOEFS;
3810                                 }
3811                         }
3812                 }
3813
3814                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 64), length, table);
3815
3816                 if (full) {
3817                         if (dev->phy.rev >= 3)
3818                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_FULLCAL_REV3;
3819                         else
3820                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_FULLCAL;
3821                 } else {
3822                         if (dev->phy.rev >= 3)
3823                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_RECAL_REV3;
3824                         else
3825                                 max = B43_NTAB_TX_IQLO_CAL_CMDS_RECAL;
3826                 }
3827
3828                 if (mphase) {
3829                         count = nphy->mphase_txcal_cmdidx;
3830                         numb = min(max,
3831                                 (u16)(count + nphy->mphase_txcal_numcmds));
3832                 } else {
3833                         count = 0;
3834                         numb = max;
3835                 }
3836
3837                 for (; count < numb; count++) {
3838                         if (full) {
3839                                 if (dev->phy.rev >= 3)
3840                                         cmd = tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3[count];
3841                                 else
3842                                         cmd = tbl_tx_iqlo_cal_cmds_fullcal[count];
3843                         } else {
3844                                 if (dev->phy.rev >= 3)
3845                                         cmd = tbl_tx_iqlo_cal_cmds_recal_nphyrev3[count];
3846                                 else
3847                                         cmd = tbl_tx_iqlo_cal_cmds_recal[count];
3848                         }
3849
3850                         core = (cmd & 0x3000) >> 12;
3851                         type = (cmd & 0x0F00) >> 8;
3852
3853                         if (phy6or5x && updated[core] == 0) {
3854                                 b43_nphy_update_tx_cal_ladder(dev, core);
3855                                 updated[core] = true;
3856                         }
3857
3858                         tmp = (params[core].ncorr[type] << 8) | 0x66;
3859                         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDNNUM, tmp);
3860
3861                         if (type == 1 || type == 3 || type == 4) {
3862                                 buffer[0] = b43_ntab_read(dev,
3863                                                 B43_NTAB16(15, 69 + core));
3864                                 diq_start = buffer[0];
3865                                 buffer[0] = 0;
3866                                 b43_ntab_write(dev, B43_NTAB16(15, 69 + core),
3867                                                 0);
3868                         }
3869
3870                         b43_phy_write(dev, B43_NPHY_IQLOCAL_CMD, cmd);
3871                         for (i = 0; i < 2000; i++) {
3872                                 tmp = b43_phy_read(dev, B43_NPHY_IQLOCAL_CMD);
3873                                 if (tmp & 0xC000)
3874                                         break;
3875                                 udelay(10);
3876                         }
3877
3878                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
3879                                                 buffer);
3880                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 64), length,
3881                                                 buffer);
3882
3883                         if (type == 1 || type == 3 || type == 4)
3884                                 buffer[0] = diq_start;
3885                 }
3886
3887                 if (mphase)
3888                         nphy->mphase_txcal_cmdidx = (numb >= max) ? 0 : numb;
3889
3890                 last = (dev->phy.rev < 3) ? 6 : 7;
3891
3892                 if (!mphase || nphy->mphase_cal_phase_id == last) {
3893                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 96), 4, buffer);
3894                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 4, buffer);
3895                         if (dev->phy.rev < 3) {
3896                                 buffer[0] = 0;
3897                                 buffer[1] = 0;
3898                                 buffer[2] = 0;
3899                                 buffer[3] = 0;
3900                         }
3901                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4,
3902                                                 buffer);
3903                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 101), 2,
3904                                                 buffer);
3905                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2,
3906                                                 buffer);
3907                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 93), 2,
3908                                                 buffer);
3909                         length = 11;
3910                         if (dev->phy.rev < 3)
3911                                 length -= 2;
3912                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
3913                                                 nphy->txiqlocal_bestc);
3914                         nphy->txiqlocal_coeffsvalid = true;
3915                         nphy->txiqlocal_chanspec.center_freq =
3916                                                         dev->phy.channel_freq;
3917                         nphy->txiqlocal_chanspec.channel_type =
3918                                                         dev->phy.channel_type;
3919                 } else {
3920                         length = 11;
3921                         if (dev->phy.rev < 3)
3922                                 length -= 2;
3923                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
3924                                                 nphy->mphase_txcal_bestcoeffs);
3925                 }
3926
3927                 b43_nphy_stop_playback(dev);
3928                 b43_phy_write(dev, B43_NPHY_IQLOCAL_CMDGCTL, 0);
3929         }
3930
3931         b43_nphy_tx_cal_phy_cleanup(dev);
3932         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, save);
3933
3934         if (dev->phy.rev < 2 && (!mphase || nphy->mphase_cal_phase_id == last))
3935                 b43_nphy_tx_iq_workaround(dev);
3936
3937         if (dev->phy.rev >= 4)
3938                 nphy->hang_avoid = avoid;
3939
3940         b43_nphy_stay_in_carrier_search(dev, false);
3941
3942         return error;
3943 }
3944
3945 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ReapplyTxCalCoeffs */
3946 static void b43_nphy_reapply_tx_cal_coeffs(struct b43_wldev *dev)
3947 {
3948         struct b43_phy_n *nphy = dev->phy.n;
3949         u8 i;
3950         u16 buffer[7];
3951         bool equal = true;
3952
3953         if (!nphy->txiqlocal_coeffsvalid ||
3954             nphy->txiqlocal_chanspec.center_freq != dev->phy.channel_freq ||
3955             nphy->txiqlocal_chanspec.channel_type != dev->phy.channel_type)
3956                 return;
3957
3958         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 7, buffer);
3959         for (i = 0; i < 4; i++) {
3960                 if (buffer[i] != nphy->txiqlocal_bestc[i]) {
3961                         equal = false;
3962                         break;
3963                 }
3964         }
3965
3966         if (!equal) {
3967                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 80), 4,
3968                                         nphy->txiqlocal_bestc);
3969                 for (i = 0; i < 4; i++)
3970                         buffer[i] = 0;
3971                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4,
3972                                         buffer);
3973                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2,
3974                                         &nphy->txiqlocal_bestc[5]);
3975                 b43_ntab_write_bulk(dev, B43_NTAB16(15, 93), 2,
3976                                         &nphy->txiqlocal_bestc[5]);
3977         }
3978 }
3979
3980 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalRxIqRev2 */
3981 static int b43_nphy_rev2_cal_rx_iq(struct b43_wldev *dev,
3982                         struct nphy_txgains target, u8 type, bool debug)
3983 {
3984         struct b43_phy_n *nphy = dev->phy.n;
3985         int i, j, index;
3986         u8 rfctl[2];
3987         u8 afectl_core;
3988         u16 tmp[6];
3989         u16 uninitialized_var(cur_hpf1), uninitialized_var(cur_hpf2), cur_lna;
3990         u32 real, imag;
3991         enum ieee80211_band band;
3992
3993         u8 use;
3994         u16 cur_hpf;
3995         u16 lna[3] = { 3, 3, 1 };
3996         u16 hpf1[3] = { 7, 2, 0 };
3997         u16 hpf2[3] = { 2, 0, 0 };
3998         u32 power[3] = { };
3999         u16 gain_save[2];
4000         u16 cal_gain[2];
4001         struct nphy_iqcal_params cal_params[2];
4002         struct nphy_iq_est est;
4003         int ret = 0;
4004         bool playtone = true;
4005         int desired = 13;
4006
4007         b43_nphy_stay_in_carrier_search(dev, 1);
4008
4009         if (dev->phy.rev < 2)
4010                 b43_nphy_reapply_tx_cal_coeffs(dev);
4011         b43_ntab_read_bulk(dev, B43_NTAB16(7, 0x110), 2, gain_save);
4012         for (i = 0; i < 2; i++) {
4013                 b43_nphy_iq_cal_gain_params(dev, i, target, &cal_params[i]);
4014                 cal_gain[i] = cal_params[i].cal_gain;
4015         }
4016         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, cal_gain);
4017
4018         for (i = 0; i < 2; i++) {
4019                 if (i == 0) {
4020                         rfctl[0] = B43_NPHY_RFCTL_INTC1;
4021                         rfctl[1] = B43_NPHY_RFCTL_INTC2;
4022                         afectl_core = B43_NPHY_AFECTL_C1;
4023                 } else {
4024                         rfctl[0] = B43_NPHY_RFCTL_INTC2;
4025                         rfctl[1] = B43_NPHY_RFCTL_INTC1;
4026                         afectl_core = B43_NPHY_AFECTL_C2;
4027                 }
4028
4029                 tmp[1] = b43_phy_read(dev, B43_NPHY_RFSEQCA);
4030                 tmp[2] = b43_phy_read(dev, afectl_core);
4031                 tmp[3] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
4032                 tmp[4] = b43_phy_read(dev, rfctl[0]);
4033                 tmp[5] = b43_phy_read(dev, rfctl[1]);
4034
4035                 b43_phy_maskset(dev, B43_NPHY_RFSEQCA,
4036                                 ~B43_NPHY_RFSEQCA_RXDIS & 0xFFFF,
4037                                 ((1 - i) << B43_NPHY_RFSEQCA_RXDIS_SHIFT));
4038                 b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_TXEN,
4039                                 (1 - i));
4040                 b43_phy_set(dev, afectl_core, 0x0006);
4041                 b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0006);
4042
4043                 band = b43_current_band(dev->wl);
4044
4045                 if (nphy->rxcalparams & 0xFF000000) {
4046                         if (band == IEEE80211_BAND_5GHZ)
4047                                 b43_phy_write(dev, rfctl[0], 0x140);
4048                         else
4049                                 b43_phy_write(dev, rfctl[0], 0x110);
4050                 } else {
4051                         if (band == IEEE80211_BAND_5GHZ)
4052                                 b43_phy_write(dev, rfctl[0], 0x180);
4053                         else
4054                                 b43_phy_write(dev, rfctl[0], 0x120);
4055                 }
4056
4057                 if (band == IEEE80211_BAND_5GHZ)
4058                         b43_phy_write(dev, rfctl[1], 0x148);
4059                 else
4060                         b43_phy_write(dev, rfctl[1], 0x114);
4061
4062                 if (nphy->rxcalparams & 0x10000) {
4063                         b43_radio_maskset(dev, B2055_C1_GENSPARE2, 0xFC,
4064                                         (i + 1));
4065                         b43_radio_maskset(dev, B2055_C2_GENSPARE2, 0xFC,
4066                                         (2 - i));
4067                 }
4068
4069                 for (j = 0; j < 4; j++) {
4070                         if (j < 3) {
4071                                 cur_lna = lna[j];
4072                                 cur_hpf1 = hpf1[j];
4073                                 cur_hpf2 = hpf2[j];
4074                         } else {
4075                                 if (power[1] > 10000) {
4076                                         use = 1;
4077                                         cur_hpf = cur_hpf1;
4078                                         index = 2;
4079                                 } else {
4080                                         if (power[0] > 10000) {
4081                                                 use = 1;
4082                                                 cur_hpf = cur_hpf1;
4083                                                 index = 1;
4084                                         } else {
4085                                                 index = 0;
4086                                                 use = 2;
4087                                                 cur_hpf = cur_hpf2;
4088                                         }
4089                                 }
4090                                 cur_lna = lna[index];
4091                                 cur_hpf1 = hpf1[index];
4092                                 cur_hpf2 = hpf2[index];
4093                                 cur_hpf += desired - hweight32(power[index]);
4094                                 cur_hpf = clamp_val(cur_hpf, 0, 10);
4095                                 if (use == 1)
4096                                         cur_hpf1 = cur_hpf;
4097                                 else
4098                                         cur_hpf2 = cur_hpf;
4099                         }
4100
4101                         tmp[0] = ((cur_hpf2 << 8) | (cur_hpf1 << 4) |
4102                                         (cur_lna << 2));
4103                         b43_nphy_rf_control_override(dev, 0x400, tmp[0], 3,
4104                                                                         false);
4105                         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
4106                         b43_nphy_stop_playback(dev);
4107
4108                         if (playtone) {
4109                                 ret = b43_nphy_tx_tone(dev, 4000,
4110                                                 (nphy->rxcalparams & 0xFFFF),
4111                                                 false, false);
4112                                 playtone = false;
4113                         } else {
4114                                 b43_nphy_run_samples(dev, 160, 0xFFFF, 0,
4115                                                         false, false);
4116                         }
4117
4118                         if (ret == 0) {
4119                                 if (j < 3) {
4120                                         b43_nphy_rx_iq_est(dev, &est, 1024, 32,
4121                                                                         false);
4122                                         if (i == 0) {
4123                                                 real = est.i0_pwr;
4124                                                 imag = est.q0_pwr;
4125                                         } else {
4126                                                 real = est.i1_pwr;
4127                                                 imag = est.q1_pwr;
4128                                         }
4129                                         power[i] = ((real + imag) / 1024) + 1;
4130                                 } else {
4131                                         b43_nphy_calc_rx_iq_comp(dev, 1 << i);
4132                                 }
4133                                 b43_nphy_stop_playback(dev);
4134                         }
4135
4136                         if (ret != 0)
4137                                 break;
4138                 }
4139
4140                 b43_radio_mask(dev, B2055_C1_GENSPARE2, 0xFC);
4141                 b43_radio_mask(dev, B2055_C2_GENSPARE2, 0xFC);
4142                 b43_phy_write(dev, rfctl[1], tmp[5]);
4143                 b43_phy_write(dev, rfctl[0], tmp[4]);
4144                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, tmp[3]);
4145                 b43_phy_write(dev, afectl_core, tmp[2]);
4146                 b43_phy_write(dev, B43_NPHY_RFSEQCA, tmp[1]);
4147
4148                 if (ret != 0)
4149                         break;
4150         }
4151
4152         b43_nphy_rf_control_override(dev, 0x400, 0, 3, true);
4153         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
4154         b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, gain_save);
4155
4156         b43_nphy_stay_in_carrier_search(dev, 0);
4157
4158         return ret;
4159 }
4160
4161 static int b43_nphy_rev3_cal_rx_iq(struct b43_wldev *dev,
4162                         struct nphy_txgains target, u8 type, bool debug)
4163 {
4164         return -1;
4165 }
4166
4167 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CalRxIq */
4168 static int b43_nphy_cal_rx_iq(struct b43_wldev *dev,
4169                         struct nphy_txgains target, u8 type, bool debug)
4170 {
4171         if (dev->phy.rev >= 3)
4172                 return b43_nphy_rev3_cal_rx_iq(dev, target, type, debug);
4173         else
4174                 return b43_nphy_rev2_cal_rx_iq(dev, target, type, debug);
4175 }
4176
4177 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RxCoreSetState */
4178 static void b43_nphy_set_rx_core_state(struct b43_wldev *dev, u8 mask)
4179 {
4180         struct b43_phy *phy = &dev->phy;
4181         struct b43_phy_n *nphy = phy->n;
4182         /* u16 buf[16]; it's rev3+ */
4183
4184         nphy->phyrxchain = mask;
4185
4186         if (0 /* FIXME clk */)
4187                 return;
4188
4189         b43_mac_suspend(dev);
4190
4191         if (nphy->hang_avoid)
4192                 b43_nphy_stay_in_carrier_search(dev, true);
4193
4194         b43_phy_maskset(dev, B43_NPHY_RFSEQCA, ~B43_NPHY_RFSEQCA_RXEN,
4195                         (mask & 0x3) << B43_NPHY_RFSEQCA_RXEN_SHIFT);
4196
4197         if ((mask & 0x3) != 0x3) {
4198                 b43_phy_write(dev, B43_NPHY_HPANT_SWTHRES, 1);
4199                 if (dev->phy.rev >= 3) {
4200                         /* TODO */
4201                 }
4202         } else {
4203                 b43_phy_write(dev, B43_NPHY_HPANT_SWTHRES, 0x1E);
4204                 if (dev->phy.rev >= 3) {
4205                         /* TODO */
4206                 }
4207         }
4208
4209         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
4210
4211         if (nphy->hang_avoid)
4212                 b43_nphy_stay_in_carrier_search(dev, false);
4213
4214         b43_mac_enable(dev);
4215 }
4216
4217 /**************************************************
4218  * N-PHY init
4219  **************************************************/
4220
4221 /*
4222  * Upload the N-PHY tables.
4223  * http://bcm-v4.sipsolutions.net/802.11/PHY/N/InitTables
4224  */
4225 static void b43_nphy_tables_init(struct b43_wldev *dev)
4226 {
4227         if (dev->phy.rev < 3)
4228                 b43_nphy_rev0_1_2_tables_init(dev);
4229         else
4230                 b43_nphy_rev3plus_tables_init(dev);
4231 }
4232
4233 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/MIMOConfig */
4234 static void b43_nphy_update_mimo_config(struct b43_wldev *dev, s32 preamble)
4235 {
4236         u16 mimocfg = b43_phy_read(dev, B43_NPHY_MIMOCFG);
4237
4238         mimocfg |= B43_NPHY_MIMOCFG_AUTO;
4239         if (preamble == 1)
4240                 mimocfg |= B43_NPHY_MIMOCFG_GFMIX;
4241         else
4242                 mimocfg &= ~B43_NPHY_MIMOCFG_GFMIX;
4243
4244         b43_phy_write(dev, B43_NPHY_MIMOCFG, mimocfg);
4245 }
4246
4247 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/BPHYInit */
4248 static void b43_nphy_bphy_init(struct b43_wldev *dev)
4249 {
4250         unsigned int i;
4251         u16 val;
4252
4253         val = 0x1E1F;
4254         for (i = 0; i < 16; i++) {
4255                 b43_phy_write(dev, B43_PHY_N_BMODE(0x88 + i), val);
4256                 val -= 0x202;
4257         }
4258         val = 0x3E3F;
4259         for (i = 0; i < 16; i++) {
4260                 b43_phy_write(dev, B43_PHY_N_BMODE(0x98 + i), val);
4261                 val -= 0x202;
4262         }
4263         b43_phy_write(dev, B43_PHY_N_BMODE(0x38), 0x668);
4264 }
4265
4266 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SuperSwitchInit */
4267 static void b43_nphy_superswitch_init(struct b43_wldev *dev, bool init)
4268 {
4269         if (dev->phy.rev >= 3) {
4270                 if (!init)
4271                         return;
4272                 if (0 /* FIXME */) {
4273                         b43_ntab_write(dev, B43_NTAB16(9, 2), 0x211);
4274                         b43_ntab_write(dev, B43_NTAB16(9, 3), 0x222);
4275                         b43_ntab_write(dev, B43_NTAB16(9, 8), 0x144);
4276                         b43_ntab_write(dev, B43_NTAB16(9, 12), 0x188);
4277                 }
4278         } else {
4279                 b43_phy_write(dev, B43_NPHY_GPIO_LOOEN, 0);
4280                 b43_phy_write(dev, B43_NPHY_GPIO_HIOEN, 0);
4281
4282                 switch (dev->dev->bus_type) {
4283 #ifdef CONFIG_B43_BCMA
4284                 case B43_BUS_BCMA:
4285                         bcma_chipco_gpio_control(&dev->dev->bdev->bus->drv_cc,
4286                                                  0xFC00, 0xFC00);
4287                         break;
4288 #endif
4289 #ifdef CONFIG_B43_SSB
4290                 case B43_BUS_SSB:
4291                         ssb_chipco_gpio_control(&dev->dev->sdev->bus->chipco,
4292                                                 0xFC00, 0xFC00);
4293                         break;
4294 #endif
4295                 }
4296
4297                 b43_maskset32(dev, B43_MMIO_MACCTL, ~B43_MACCTL_GPOUTSMSK, 0);
4298                 b43_maskset16(dev, B43_MMIO_GPIO_MASK, ~0, 0xFC00);
4299                 b43_maskset16(dev, B43_MMIO_GPIO_CONTROL, (~0xFC00 & 0xFFFF),
4300                               0);
4301
4302                 if (init) {
4303                         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO1, 0x2D8);
4304                         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1, 0x301);
4305                         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO2, 0x2D8);
4306                         b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2, 0x301);
4307                 }
4308         }
4309 }
4310
4311 /* http://bcm-v4.sipsolutions.net/802.11/PHY/Init/N */
4312 int b43_phy_initn(struct b43_wldev *dev)
4313 {
4314         struct ssb_sprom *sprom = dev->dev->bus_sprom;
4315         struct b43_phy *phy = &dev->phy;
4316         struct b43_phy_n *nphy = phy->n;
4317         u8 tx_pwr_state;
4318         struct nphy_txgains target;
4319         u16 tmp;
4320         enum ieee80211_band tmp2;
4321         bool do_rssi_cal;
4322
4323         u16 clip[2];
4324         bool do_cal = false;
4325
4326         if ((dev->phy.rev >= 3) &&
4327            (sprom->boardflags_lo & B43_BFL_EXTLNA) &&
4328            (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)) {
4329                 switch (dev->dev->bus_type) {
4330 #ifdef CONFIG_B43_BCMA
4331                 case B43_BUS_BCMA:
4332                         bcma_cc_set32(&dev->dev->bdev->bus->drv_cc,
4333                                       BCMA_CC_CHIPCTL, 0x40);
4334                         break;
4335 #endif
4336 #ifdef CONFIG_B43_SSB
4337                 case B43_BUS_SSB:
4338                         chipco_set32(&dev->dev->sdev->bus->chipco,
4339                                      SSB_CHIPCO_CHIPCTL, 0x40);
4340                         break;
4341 #endif
4342                 }
4343         }
4344         nphy->deaf_count = 0;
4345         b43_nphy_tables_init(dev);
4346         nphy->crsminpwr_adjusted = false;
4347         nphy->noisevars_adjusted = false;
4348
4349         /* Clear all overrides */
4350         if (dev->phy.rev >= 3) {
4351                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S1, 0);
4352                 b43_phy_write(dev, B43_NPHY_RFCTL_OVER, 0);
4353                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S0, 0);
4354                 b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S1, 0);
4355         } else {
4356                 b43_phy_write(dev, B43_NPHY_RFCTL_OVER, 0);
4357         }
4358         b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, 0);
4359         b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, 0);
4360         if (dev->phy.rev < 6) {
4361                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC3, 0);
4362                 b43_phy_write(dev, B43_NPHY_RFCTL_INTC4, 0);
4363         }
4364         b43_phy_mask(dev, B43_NPHY_RFSEQMODE,
4365                      ~(B43_NPHY_RFSEQMODE_CAOVER |
4366                        B43_NPHY_RFSEQMODE_TROVER));
4367         if (dev->phy.rev >= 3)
4368                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, 0);
4369         b43_phy_write(dev, B43_NPHY_AFECTL_OVER, 0);
4370
4371         if (dev->phy.rev <= 2) {
4372                 tmp = (dev->phy.rev == 2) ? 0x3B : 0x40;
4373                 b43_phy_maskset(dev, B43_NPHY_BPHY_CTL3,
4374                                 ~B43_NPHY_BPHY_CTL3_SCALE,
4375                                 tmp << B43_NPHY_BPHY_CTL3_SCALE_SHIFT);
4376         }
4377         b43_phy_write(dev, B43_NPHY_AFESEQ_TX2RX_PUD_20M, 0x20);
4378         b43_phy_write(dev, B43_NPHY_AFESEQ_TX2RX_PUD_40M, 0x20);
4379
4380         if (sprom->boardflags2_lo & B43_BFL2_SKWRKFEM_BRD ||
4381             (dev->dev->board_vendor == PCI_VENDOR_ID_APPLE &&
4382              dev->dev->board_type == 0x8B))
4383                 b43_phy_write(dev, B43_NPHY_TXREALFD, 0xA0);
4384         else
4385                 b43_phy_write(dev, B43_NPHY_TXREALFD, 0xB8);
4386         b43_phy_write(dev, B43_NPHY_MIMO_CRSTXEXT, 0xC8);
4387         b43_phy_write(dev, B43_NPHY_PLOAD_CSENSE_EXTLEN, 0x50);
4388         b43_phy_write(dev, B43_NPHY_TXRIFS_FRDEL, 0x30);
4389
4390         b43_nphy_update_mimo_config(dev, nphy->preamble_override);
4391         b43_nphy_update_txrx_chain(dev);
4392
4393         if (phy->rev < 2) {
4394                 b43_phy_write(dev, B43_NPHY_DUP40_GFBL, 0xAA8);
4395                 b43_phy_write(dev, B43_NPHY_DUP40_BL, 0x9A4);
4396         }
4397
4398         tmp2 = b43_current_band(dev->wl);
4399         if (b43_nphy_ipa(dev)) {
4400                 b43_phy_set(dev, B43_NPHY_PAPD_EN0, 0x1);
4401                 b43_phy_maskset(dev, B43_NPHY_EPS_TABLE_ADJ0, 0x007F,
4402                                 nphy->papd_epsilon_offset[0] << 7);
4403                 b43_phy_set(dev, B43_NPHY_PAPD_EN1, 0x1);
4404                 b43_phy_maskset(dev, B43_NPHY_EPS_TABLE_ADJ1, 0x007F,
4405                                 nphy->papd_epsilon_offset[1] << 7);
4406                 b43_nphy_int_pa_set_tx_dig_filters(dev);
4407         } else if (phy->rev >= 5) {
4408                 b43_nphy_ext_pa_set_tx_dig_filters(dev);
4409         }
4410
4411         b43_nphy_workarounds(dev);
4412
4413         /* Reset CCA, in init code it differs a little from standard way */
4414         b43_phy_force_clock(dev, 1);
4415         tmp = b43_phy_read(dev, B43_NPHY_BBCFG);
4416         b43_phy_write(dev, B43_NPHY_BBCFG, tmp | B43_NPHY_BBCFG_RSTCCA);
4417         b43_phy_write(dev, B43_NPHY_BBCFG, tmp & ~B43_NPHY_BBCFG_RSTCCA);
4418         b43_phy_force_clock(dev, 0);
4419
4420         b43_mac_phy_clock_set(dev, true);
4421
4422         b43_nphy_pa_override(dev, false);
4423         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
4424         b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
4425         b43_nphy_pa_override(dev, true);
4426
4427         b43_nphy_classifier(dev, 0, 0);
4428         b43_nphy_read_clip_detection(dev, clip);
4429         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
4430                 b43_nphy_bphy_init(dev);
4431
4432         tx_pwr_state = nphy->txpwrctrl;
4433         b43_nphy_tx_power_ctrl(dev, false);
4434         b43_nphy_tx_power_fix(dev);
4435         b43_nphy_tx_power_ctl_idle_tssi(dev);
4436         b43_nphy_tx_power_ctl_setup(dev);
4437         b43_nphy_tx_gain_table_upload(dev);
4438
4439         if (nphy->phyrxchain != 3)
4440                 b43_nphy_set_rx_core_state(dev, nphy->phyrxchain);
4441         if (nphy->mphase_cal_phase_id > 0)
4442                 ;/* TODO PHY Periodic Calibration Multi-Phase Restart */
4443
4444         do_rssi_cal = false;
4445         if (phy->rev >= 3) {
4446                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
4447                         do_rssi_cal = !nphy->rssical_chanspec_2G.center_freq;
4448                 else
4449                         do_rssi_cal = !nphy->rssical_chanspec_5G.center_freq;
4450
4451                 if (do_rssi_cal)
4452                         b43_nphy_rssi_cal(dev);
4453                 else
4454                         b43_nphy_restore_rssi_cal(dev);
4455         } else {
4456                 b43_nphy_rssi_cal(dev);
4457         }
4458
4459         if (!((nphy->measure_hold & 0x6) != 0)) {
4460                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
4461                         do_cal = !nphy->iqcal_chanspec_2G.center_freq;
4462                 else
4463                         do_cal = !nphy->iqcal_chanspec_5G.center_freq;
4464
4465                 if (nphy->mute)
4466                         do_cal = false;
4467
4468                 if (do_cal) {
4469                         target = b43_nphy_get_tx_gains(dev);
4470
4471                         if (nphy->antsel_type == 2)
4472                                 b43_nphy_superswitch_init(dev, true);
4473                         if (nphy->perical != 2) {
4474                                 b43_nphy_rssi_cal(dev);
4475                                 if (phy->rev >= 3) {
4476                                         nphy->cal_orig_pwr_idx[0] =
4477                                             nphy->txpwrindex[0].index_internal;
4478                                         nphy->cal_orig_pwr_idx[1] =
4479                                             nphy->txpwrindex[1].index_internal;
4480                                         /* TODO N PHY Pre Calibrate TX Gain */
4481                                         target = b43_nphy_get_tx_gains(dev);
4482                                 }
4483                                 if (!b43_nphy_cal_tx_iq_lo(dev, target, true, false))
4484                                         if (b43_nphy_cal_rx_iq(dev, target, 2, 0) == 0)
4485                                                 b43_nphy_save_cal(dev);
4486                         } else if (nphy->mphase_cal_phase_id == 0)
4487                                 ;/* N PHY Periodic Calibration with arg 3 */
4488                 } else {
4489                         b43_nphy_restore_cal(dev);
4490                 }
4491         }
4492
4493         b43_nphy_tx_pwr_ctrl_coef_setup(dev);
4494         b43_nphy_tx_power_ctrl(dev, tx_pwr_state);
4495         b43_phy_write(dev, B43_NPHY_TXMACIF_HOLDOFF, 0x0015);
4496         b43_phy_write(dev, B43_NPHY_TXMACDELAY, 0x0320);
4497         if (phy->rev >= 3 && phy->rev <= 6)
4498                 b43_phy_write(dev, B43_NPHY_PLOAD_CSENSE_EXTLEN, 0x0014);
4499         b43_nphy_tx_lp_fbw(dev);
4500         if (phy->rev >= 3)
4501                 b43_nphy_spur_workaround(dev);
4502
4503         return 0;
4504 }
4505
4506 /**************************************************
4507  * Channel switching ops.
4508  **************************************************/
4509
4510 static void b43_chantab_phy_upload(struct b43_wldev *dev,
4511                                    const struct b43_phy_n_sfo_cfg *e)
4512 {
4513         b43_phy_write(dev, B43_NPHY_BW1A, e->phy_bw1a);
4514         b43_phy_write(dev, B43_NPHY_BW2, e->phy_bw2);
4515         b43_phy_write(dev, B43_NPHY_BW3, e->phy_bw3);
4516         b43_phy_write(dev, B43_NPHY_BW4, e->phy_bw4);
4517         b43_phy_write(dev, B43_NPHY_BW5, e->phy_bw5);
4518         b43_phy_write(dev, B43_NPHY_BW6, e->phy_bw6);
4519 }
4520
4521 /* http://bcm-v4.sipsolutions.net/802.11/PmuSpurAvoid */
4522 static void b43_nphy_pmu_spur_avoid(struct b43_wldev *dev, bool avoid)
4523 {
4524         struct bcma_drv_cc __maybe_unused *cc;
4525         u32 __maybe_unused pmu_ctl;
4526
4527         switch (dev->dev->bus_type) {
4528 #ifdef CONFIG_B43_BCMA
4529         case B43_BUS_BCMA:
4530                 cc = &dev->dev->bdev->bus->drv_cc;
4531                 if (dev->dev->chip_id == 43224 || dev->dev->chip_id == 43225) {
4532                         if (avoid) {
4533                                 bcma_chipco_pll_write(cc, 0x0, 0x11500010);
4534                                 bcma_chipco_pll_write(cc, 0x1, 0x000C0C06);
4535                                 bcma_chipco_pll_write(cc, 0x2, 0x0F600a08);
4536                                 bcma_chipco_pll_write(cc, 0x3, 0x00000000);
4537                                 bcma_chipco_pll_write(cc, 0x4, 0x2001E920);
4538                                 bcma_chipco_pll_write(cc, 0x5, 0x88888815);
4539                         } else {
4540                                 bcma_chipco_pll_write(cc, 0x0, 0x11100010);
4541                                 bcma_chipco_pll_write(cc, 0x1, 0x000c0c06);
4542                                 bcma_chipco_pll_write(cc, 0x2, 0x03000a08);
4543                                 bcma_chipco_pll_write(cc, 0x3, 0x00000000);
4544                                 bcma_chipco_pll_write(cc, 0x4, 0x200005c0);
4545                                 bcma_chipco_pll_write(cc, 0x5, 0x88888815);
4546                         }
4547                         pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD;
4548                 } else if (dev->dev->chip_id == 0x4716) {
4549                         if (avoid) {
4550                                 bcma_chipco_pll_write(cc, 0x0, 0x11500060);
4551                                 bcma_chipco_pll_write(cc, 0x1, 0x080C0C06);
4552                                 bcma_chipco_pll_write(cc, 0x2, 0x0F600000);
4553                                 bcma_chipco_pll_write(cc, 0x3, 0x00000000);
4554                                 bcma_chipco_pll_write(cc, 0x4, 0x2001E924);
4555                                 bcma_chipco_pll_write(cc, 0x5, 0x88888815);
4556                         } else {
4557                                 bcma_chipco_pll_write(cc, 0x0, 0x11100060);
4558                                 bcma_chipco_pll_write(cc, 0x1, 0x080c0c06);
4559                                 bcma_chipco_pll_write(cc, 0x2, 0x03000000);
4560                                 bcma_chipco_pll_write(cc, 0x3, 0x00000000);
4561                                 bcma_chipco_pll_write(cc, 0x4, 0x200005c0);
4562                                 bcma_chipco_pll_write(cc, 0x5, 0x88888815);
4563                         }
4564                         pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD |
4565                                   BCMA_CC_PMU_CTL_NOILPONW;
4566                 } else if (dev->dev->chip_id == 0x4322 ||
4567                            dev->dev->chip_id == 0x4340 ||
4568                            dev->dev->chip_id == 0x4341) {
4569                         bcma_chipco_pll_write(cc, 0x0, 0x11100070);
4570                         bcma_chipco_pll_write(cc, 0x1, 0x1014140a);
4571                         bcma_chipco_pll_write(cc, 0x5, 0x88888854);
4572                         if (avoid)
4573                                 bcma_chipco_pll_write(cc, 0x2, 0x05201828);
4574                         else
4575                                 bcma_chipco_pll_write(cc, 0x2, 0x05001828);
4576                         pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD;
4577                 } else {
4578                         return;
4579                 }
4580                 bcma_cc_set32(cc, BCMA_CC_PMU_CTL, pmu_ctl);
4581                 break;
4582 #endif
4583 #ifdef CONFIG_B43_SSB
4584         case B43_BUS_SSB:
4585                 /* FIXME */
4586                 break;
4587 #endif
4588         }
4589 }
4590
4591 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ChanspecSetup */
4592 static void b43_nphy_channel_setup(struct b43_wldev *dev,
4593                                 const struct b43_phy_n_sfo_cfg *e,
4594                                 struct ieee80211_channel *new_channel)
4595 {
4596         struct b43_phy *phy = &dev->phy;
4597         struct b43_phy_n *nphy = dev->phy.n;
4598         int ch = new_channel->hw_value;
4599
4600         u16 old_band_5ghz;
4601         u32 tmp32;
4602
4603         old_band_5ghz =
4604                 b43_phy_read(dev, B43_NPHY_BANDCTL) & B43_NPHY_BANDCTL_5GHZ;
4605         if (new_channel->band == IEEE80211_BAND_5GHZ && !old_band_5ghz) {
4606                 tmp32 = b43_read32(dev, B43_MMIO_PSM_PHY_HDR);
4607                 b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32 | 4);
4608                 b43_phy_set(dev, B43_PHY_B_BBCFG, 0xC000);
4609                 b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32);
4610                 b43_phy_set(dev, B43_NPHY_BANDCTL, B43_NPHY_BANDCTL_5GHZ);
4611         } else if (new_channel->band == IEEE80211_BAND_2GHZ && old_band_5ghz) {
4612                 b43_phy_mask(dev, B43_NPHY_BANDCTL, ~B43_NPHY_BANDCTL_5GHZ);
4613                 tmp32 = b43_read32(dev, B43_MMIO_PSM_PHY_HDR);
4614                 b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32 | 4);
4615                 b43_phy_mask(dev, B43_PHY_B_BBCFG, 0x3FFF);
4616                 b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32);
4617         }
4618
4619         b43_chantab_phy_upload(dev, e);
4620
4621         if (new_channel->hw_value == 14) {
4622                 b43_nphy_classifier(dev, 2, 0);
4623                 b43_phy_set(dev, B43_PHY_B_TEST, 0x0800);
4624         } else {
4625                 b43_nphy_classifier(dev, 2, 2);
4626                 if (new_channel->band == IEEE80211_BAND_2GHZ)
4627                         b43_phy_mask(dev, B43_PHY_B_TEST, ~0x840);
4628         }
4629
4630         if (!nphy->txpwrctrl)
4631                 b43_nphy_tx_power_fix(dev);
4632
4633         if (dev->phy.rev < 3)
4634                 b43_nphy_adjust_lna_gain_table(dev);
4635
4636         b43_nphy_tx_lp_fbw(dev);
4637
4638         if (dev->phy.rev >= 3 &&
4639             dev->phy.n->spur_avoid != B43_SPUR_AVOID_DISABLE) {
4640                 bool avoid = false;
4641                 if (dev->phy.n->spur_avoid == B43_SPUR_AVOID_FORCE) {
4642                         avoid = true;
4643                 } else if (!b43_channel_type_is_40mhz(phy->channel_type)) {
4644                         if ((ch >= 5 && ch <= 8) || ch == 13 || ch == 14)
4645                                 avoid = true;
4646                 } else { /* 40MHz */
4647                         if (nphy->aband_spurwar_en &&
4648                             (ch == 38 || ch == 102 || ch == 118))
4649                                 avoid = dev->dev->chip_id == 0x4716;
4650                 }
4651
4652                 b43_nphy_pmu_spur_avoid(dev, avoid);
4653
4654                 if (dev->dev->chip_id == 43222 || dev->dev->chip_id == 43224 ||
4655                     dev->dev->chip_id == 43225) {
4656                         b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW,
4657                                     avoid ? 0x5341 : 0x8889);
4658                         b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x8);
4659                 }
4660
4661                 if (dev->phy.rev == 3 || dev->phy.rev == 4)
4662                         ; /* TODO: reset PLL */
4663
4664                 if (avoid)
4665                         b43_phy_set(dev, B43_NPHY_BBCFG, B43_NPHY_BBCFG_RSTRX);
4666                 else
4667                         b43_phy_mask(dev, B43_NPHY_BBCFG,
4668                                      ~B43_NPHY_BBCFG_RSTRX & 0xFFFF);
4669
4670                 b43_nphy_reset_cca(dev);
4671
4672                 /* wl sets useless phy_isspuravoid here */
4673         }
4674
4675         b43_phy_write(dev, B43_NPHY_NDATAT_DUP40, 0x3830);
4676
4677         if (phy->rev >= 3)
4678                 b43_nphy_spur_workaround(dev);
4679 }
4680
4681 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetChanspec */
4682 static int b43_nphy_set_channel(struct b43_wldev *dev,
4683                                 struct ieee80211_channel *channel,
4684                                 enum nl80211_channel_type channel_type)
4685 {
4686         struct b43_phy *phy = &dev->phy;
4687
4688         const struct b43_nphy_channeltab_entry_rev2 *tabent_r2 = NULL;
4689         const struct b43_nphy_channeltab_entry_rev3 *tabent_r3 = NULL;
4690
4691         u8 tmp;
4692
4693         if (dev->phy.rev >= 3) {
4694                 tabent_r3 = b43_nphy_get_chantabent_rev3(dev,
4695                                                         channel->center_freq);
4696                 if (!tabent_r3)
4697                         return -ESRCH;
4698         } else {
4699                 tabent_r2 = b43_nphy_get_chantabent_rev2(dev,
4700                                                         channel->hw_value);
4701                 if (!tabent_r2)
4702                         return -ESRCH;
4703         }
4704
4705         /* Channel is set later in common code, but we need to set it on our
4706            own to let this function's subcalls work properly. */
4707         phy->channel = channel->hw_value;
4708         phy->channel_freq = channel->center_freq;
4709
4710         if (b43_channel_type_is_40mhz(phy->channel_type) !=
4711                 b43_channel_type_is_40mhz(channel_type))
4712                 ; /* TODO: BMAC BW Set (channel_type) */
4713
4714         if (channel_type == NL80211_CHAN_HT40PLUS)
4715                 b43_phy_set(dev, B43_NPHY_RXCTL,
4716                                 B43_NPHY_RXCTL_BSELU20);
4717         else if (channel_type == NL80211_CHAN_HT40MINUS)
4718                 b43_phy_mask(dev, B43_NPHY_RXCTL,
4719                                 ~B43_NPHY_RXCTL_BSELU20);
4720
4721         if (dev->phy.rev >= 3) {
4722                 tmp = (channel->band == IEEE80211_BAND_5GHZ) ? 4 : 0;
4723                 b43_radio_maskset(dev, 0x08, 0xFFFB, tmp);
4724                 b43_radio_2056_setup(dev, tabent_r3);
4725                 b43_nphy_channel_setup(dev, &(tabent_r3->phy_regs), channel);
4726         } else {
4727                 tmp = (channel->band == IEEE80211_BAND_5GHZ) ? 0x0020 : 0x0050;
4728                 b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, tmp);
4729                 b43_radio_2055_setup(dev, tabent_r2);
4730                 b43_nphy_channel_setup(dev, &(tabent_r2->phy_regs), channel);
4731         }
4732
4733         return 0;
4734 }
4735
4736 /**************************************************
4737  * Basic PHY ops.
4738  **************************************************/
4739
4740 static int b43_nphy_op_allocate(struct b43_wldev *dev)
4741 {
4742         struct b43_phy_n *nphy;
4743
4744         nphy = kzalloc(sizeof(*nphy), GFP_KERNEL);
4745         if (!nphy)
4746                 return -ENOMEM;
4747         dev->phy.n = nphy;
4748
4749         return 0;
4750 }
4751
4752 static void b43_nphy_op_prepare_structs(struct b43_wldev *dev)
4753 {
4754         struct b43_phy *phy = &dev->phy;
4755         struct b43_phy_n *nphy = phy->n;
4756         struct ssb_sprom *sprom = dev->dev->bus_sprom;
4757
4758         memset(nphy, 0, sizeof(*nphy));
4759
4760         nphy->hang_avoid = (phy->rev == 3 || phy->rev == 4);
4761         nphy->spur_avoid = (phy->rev >= 3) ?
4762                                 B43_SPUR_AVOID_AUTO : B43_SPUR_AVOID_DISABLE;
4763         nphy->gain_boost = true; /* this way we follow wl, assume it is true */
4764         nphy->txrx_chain = 2; /* sth different than 0 and 1 for now */
4765         nphy->phyrxchain = 3; /* to avoid b43_nphy_set_rx_core_state like wl */
4766         nphy->perical = 2; /* avoid additional rssi cal on init (like wl) */
4767         /* 128 can mean disabled-by-default state of TX pwr ctl. Max value is
4768          * 0x7f == 127 and we check for 128 when restoring TX pwr ctl. */
4769         nphy->tx_pwr_idx[0] = 128;
4770         nphy->tx_pwr_idx[1] = 128;
4771
4772         /* Hardware TX power control and 5GHz power gain */
4773         nphy->txpwrctrl = false;
4774         nphy->pwg_gain_5ghz = false;
4775         if (dev->phy.rev >= 3 ||
4776             (dev->dev->board_vendor == PCI_VENDOR_ID_APPLE &&
4777              (dev->dev->core_rev == 11 || dev->dev->core_rev == 12))) {
4778                 nphy->txpwrctrl = true;
4779                 nphy->pwg_gain_5ghz = true;
4780         } else if (sprom->revision >= 4) {
4781                 if (dev->phy.rev >= 2 &&
4782                     (sprom->boardflags2_lo & B43_BFL2_TXPWRCTRL_EN)) {
4783                         nphy->txpwrctrl = true;
4784 #ifdef CONFIG_B43_SSB
4785                         if (dev->dev->bus_type == B43_BUS_SSB &&
4786                             dev->dev->sdev->bus->bustype == SSB_BUSTYPE_PCI) {
4787                                 struct pci_dev *pdev =
4788                                         dev->dev->sdev->bus->host_pci;
4789                                 if (pdev->device == 0x4328 ||
4790                                     pdev->device == 0x432a)
4791                                         nphy->pwg_gain_5ghz = true;
4792                         }
4793 #endif
4794                 } else if (sprom->boardflags2_lo & B43_BFL2_5G_PWRGAIN) {
4795                         nphy->pwg_gain_5ghz = true;
4796                 }
4797         }
4798
4799         if (dev->phy.rev >= 3) {
4800                 nphy->ipa2g_on = sprom->fem.ghz2.extpa_gain == 2;
4801                 nphy->ipa5g_on = sprom->fem.ghz5.extpa_gain == 2;
4802         }
4803 }
4804
4805 static void b43_nphy_op_free(struct b43_wldev *dev)
4806 {
4807         struct b43_phy *phy = &dev->phy;
4808         struct b43_phy_n *nphy = phy->n;
4809
4810         kfree(nphy);
4811         phy->n = NULL;
4812 }
4813
4814 static int b43_nphy_op_init(struct b43_wldev *dev)
4815 {
4816         return b43_phy_initn(dev);
4817 }
4818
4819 static inline void check_phyreg(struct b43_wldev *dev, u16 offset)
4820 {
4821 #if B43_DEBUG
4822         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
4823                 /* OFDM registers are onnly available on A/G-PHYs */
4824                 b43err(dev->wl, "Invalid OFDM PHY access at "
4825                        "0x%04X on N-PHY\n", offset);
4826                 dump_stack();
4827         }
4828         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
4829                 /* Ext-G registers are only available on G-PHYs */
4830                 b43err(dev->wl, "Invalid EXT-G PHY access at "
4831                        "0x%04X on N-PHY\n", offset);
4832                 dump_stack();
4833         }
4834 #endif /* B43_DEBUG */
4835 }
4836
4837 static u16 b43_nphy_op_read(struct b43_wldev *dev, u16 reg)
4838 {
4839         check_phyreg(dev, reg);
4840         b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
4841         return b43_read16(dev, B43_MMIO_PHY_DATA);
4842 }
4843
4844 static void b43_nphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
4845 {
4846         check_phyreg(dev, reg);
4847         b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
4848         b43_write16(dev, B43_MMIO_PHY_DATA, value);
4849 }
4850
4851 static void b43_nphy_op_maskset(struct b43_wldev *dev, u16 reg, u16 mask,
4852                                  u16 set)
4853 {
4854         check_phyreg(dev, reg);
4855         b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
4856         b43_maskset16(dev, B43_MMIO_PHY_DATA, mask, set);
4857 }
4858
4859 static u16 b43_nphy_op_radio_read(struct b43_wldev *dev, u16 reg)
4860 {
4861         /* Register 1 is a 32-bit register. */
4862         B43_WARN_ON(reg == 1);
4863         /* N-PHY needs 0x100 for read access */
4864         reg |= 0x100;
4865
4866         b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
4867         return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
4868 }
4869
4870 static void b43_nphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
4871 {
4872         /* Register 1 is a 32-bit register. */
4873         B43_WARN_ON(reg == 1);
4874
4875         b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
4876         b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
4877 }
4878
4879 /* http://bcm-v4.sipsolutions.net/802.11/Radio/Switch%20Radio */
4880 static void b43_nphy_op_software_rfkill(struct b43_wldev *dev,
4881                                         bool blocked)
4882 {
4883         if (b43_read32(dev, B43_MMIO_MACCTL) & B43_MACCTL_ENABLED)
4884                 b43err(dev->wl, "MAC not suspended\n");
4885
4886         if (blocked) {
4887                 b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
4888                                 ~B43_NPHY_RFCTL_CMD_CHIP0PU);
4889                 if (dev->phy.rev >= 3) {
4890                         b43_radio_mask(dev, 0x09, ~0x2);
4891
4892                         b43_radio_write(dev, 0x204D, 0);
4893                         b43_radio_write(dev, 0x2053, 0);
4894                         b43_radio_write(dev, 0x2058, 0);
4895                         b43_radio_write(dev, 0x205E, 0);
4896                         b43_radio_mask(dev, 0x2062, ~0xF0);
4897                         b43_radio_write(dev, 0x2064, 0);
4898
4899                         b43_radio_write(dev, 0x304D, 0);
4900                         b43_radio_write(dev, 0x3053, 0);
4901                         b43_radio_write(dev, 0x3058, 0);
4902                         b43_radio_write(dev, 0x305E, 0);
4903                         b43_radio_mask(dev, 0x3062, ~0xF0);
4904                         b43_radio_write(dev, 0x3064, 0);
4905                 }
4906         } else {
4907                 if (dev->phy.rev >= 3) {
4908                         b43_radio_init2056(dev);
4909                         b43_switch_channel(dev, dev->phy.channel);
4910                 } else {
4911                         b43_radio_init2055(dev);
4912                 }
4913         }
4914 }
4915
4916 /* http://bcm-v4.sipsolutions.net/802.11/PHY/Anacore */
4917 static void b43_nphy_op_switch_analog(struct b43_wldev *dev, bool on)
4918 {
4919         u16 override = on ? 0x0 : 0x7FFF;
4920         u16 core = on ? 0xD : 0x00FD;
4921
4922         if (dev->phy.rev >= 3) {
4923                 if (on) {
4924                         b43_phy_write(dev, B43_NPHY_AFECTL_C1, core);
4925                         b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, override);
4926                         b43_phy_write(dev, B43_NPHY_AFECTL_C2, core);
4927                         b43_phy_write(dev, B43_NPHY_AFECTL_OVER, override);
4928                 } else {
4929                         b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, override);
4930                         b43_phy_write(dev, B43_NPHY_AFECTL_C1, core);
4931                         b43_phy_write(dev, B43_NPHY_AFECTL_OVER, override);
4932                         b43_phy_write(dev, B43_NPHY_AFECTL_C2, core);
4933                 }
4934         } else {
4935                 b43_phy_write(dev, B43_NPHY_AFECTL_OVER, override);
4936         }
4937 }
4938
4939 static int b43_nphy_op_switch_channel(struct b43_wldev *dev,
4940                                       unsigned int new_channel)
4941 {
4942         struct ieee80211_channel *channel = dev->wl->hw->conf.channel;
4943         enum nl80211_channel_type channel_type = dev->wl->hw->conf.channel_type;
4944
4945         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
4946                 if ((new_channel < 1) || (new_channel > 14))
4947                         return -EINVAL;
4948         } else {
4949                 if (new_channel > 200)
4950                         return -EINVAL;
4951         }
4952
4953         return b43_nphy_set_channel(dev, channel, channel_type);
4954 }
4955
4956 static unsigned int b43_nphy_op_get_default_chan(struct b43_wldev *dev)
4957 {
4958         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
4959                 return 1;
4960         return 36;
4961 }
4962
4963 const struct b43_phy_operations b43_phyops_n = {
4964         .allocate               = b43_nphy_op_allocate,
4965         .free                   = b43_nphy_op_free,
4966         .prepare_structs        = b43_nphy_op_prepare_structs,
4967         .init                   = b43_nphy_op_init,
4968         .phy_read               = b43_nphy_op_read,
4969         .phy_write              = b43_nphy_op_write,
4970         .phy_maskset            = b43_nphy_op_maskset,
4971         .radio_read             = b43_nphy_op_radio_read,
4972         .radio_write            = b43_nphy_op_radio_write,
4973         .software_rfkill        = b43_nphy_op_software_rfkill,
4974         .switch_analog          = b43_nphy_op_switch_analog,
4975         .switch_channel         = b43_nphy_op_switch_channel,
4976         .get_default_chan       = b43_nphy_op_get_default_chan,
4977         .recalc_txpower         = b43_nphy_op_recalc_txpower,
4978         .adjust_txpower         = b43_nphy_op_adjust_txpower,
4979 };