drivers/net/wireless/b43/phy.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
   6   Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
   7   Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
   8   Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
   9   Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
  10
  11   This program is free software; you can redistribute it and/or modify
  12   it under the terms of the GNU General Public License as published by
  13   the Free Software Foundation; either version 2 of the License, or
  14   (at your option) any later version.
  15
  16   This program is distributed in the hope that it will be useful,
  17   but WITHOUT ANY WARRANTY; without even the implied warranty of
  18   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19   GNU General Public License for more details.
  20
  21   You should have received a copy of the GNU General Public License
  22   along with this program; see the file COPYING.  If not, write to
  23   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  24   Boston, MA 02110-1301, USA.
  25
  26 */
  27
  28 #include <linux/delay.h>
  29 #include <linux/io.h>
  30 #include <linux/types.h>
  31 #include <linux/bitrev.h>
  32
  33 #include "b43.h"
  34 #include "phy.h"
  35 #include "nphy.h"
  36 #include "main.h"
  37 #include "tables.h"
  38 #include "lo.h"
  39 #include "wa.h"
  40
  41
  42 static const s8 b43_tssi2dbm_b_table[] = {
  43         0x4D, 0x4C, 0x4B, 0x4A,
  44         0x4A, 0x49, 0x48, 0x47,
  45         0x47, 0x46, 0x45, 0x45,
  46         0x44, 0x43, 0x42, 0x42,
  47         0x41, 0x40, 0x3F, 0x3E,
  48         0x3D, 0x3C, 0x3B, 0x3A,
  49         0x39, 0x38, 0x37, 0x36,
  50         0x35, 0x34, 0x32, 0x31,
  51         0x30, 0x2F, 0x2D, 0x2C,
  52         0x2B, 0x29, 0x28, 0x26,
  53         0x25, 0x23, 0x21, 0x1F,
  54         0x1D, 0x1A, 0x17, 0x14,
  55         0x10, 0x0C, 0x06, 0x00,
  56         -7, -7, -7, -7,
  57         -7, -7, -7, -7,
  58         -7, -7, -7, -7,
  59 };
  60
  61 static const s8 b43_tssi2dbm_g_table[] = {
  62         77, 77, 77, 76,
  63         76, 76, 75, 75,
  64         74, 74, 73, 73,
  65         73, 72, 72, 71,
  66         71, 70, 70, 69,
  67         68, 68, 67, 67,
  68         66, 65, 65, 64,
  69         63, 63, 62, 61,
  70         60, 59, 58, 57,
  71         56, 55, 54, 53,
  72         52, 50, 49, 47,
  73         45, 43, 40, 37,
  74         33, 28, 22, 14,
  75         5, -7, -20, -20,
  76         -20, -20, -20, -20,
  77         -20, -20, -20, -20,
  78 };
  79
  80 const u8 b43_radio_channel_codes_bg[] = {
  81         12, 17, 22, 27,
  82         32, 37, 42, 47,
  83         52, 57, 62, 67,
  84         72, 84,
  85 };
  86
  87 #define bitrev4(tmp) (bitrev8(tmp) >> 4)
  88 static void b43_phy_initg(struct b43_wldev *dev);
  89
  90 static void generate_rfatt_list(struct b43_wldev *dev,
  91                                 struct b43_rfatt_list *list)
  92 {
  93         struct b43_phy *phy = &dev->phy;
  94
  95         /* APHY.rev < 5 || GPHY.rev < 6 */
  96         static const struct b43_rfatt rfatt_0[] = {
  97                 {.att = 3,.with_padmix = 0,},
  98                 {.att = 1,.with_padmix = 0,},
  99                 {.att = 5,.with_padmix = 0,},
 100                 {.att = 7,.with_padmix = 0,},
 101                 {.att = 9,.with_padmix = 0,},
 102                 {.att = 2,.with_padmix = 0,},
 103                 {.att = 0,.with_padmix = 0,},
 104                 {.att = 4,.with_padmix = 0,},
 105                 {.att = 6,.with_padmix = 0,},
 106                 {.att = 8,.with_padmix = 0,},
 107                 {.att = 1,.with_padmix = 1,},
 108                 {.att = 2,.with_padmix = 1,},
 109                 {.att = 3,.with_padmix = 1,},
 110                 {.att = 4,.with_padmix = 1,},
 111         };
 112         /* Radio.rev == 8 && Radio.version == 0x2050 */
 113         static const struct b43_rfatt rfatt_1[] = {
 114                 {.att = 2,.with_padmix = 1,},
 115                 {.att = 4,.with_padmix = 1,},
 116                 {.att = 6,.with_padmix = 1,},
 117                 {.att = 8,.with_padmix = 1,},
 118                 {.att = 10,.with_padmix = 1,},
 119                 {.att = 12,.with_padmix = 1,},
 120                 {.att = 14,.with_padmix = 1,},
 121         };
 122         /* Otherwise */
 123         static const struct b43_rfatt rfatt_2[] = {
 124                 {.att = 0,.with_padmix = 1,},
 125                 {.att = 2,.with_padmix = 1,},
 126                 {.att = 4,.with_padmix = 1,},
 127                 {.att = 6,.with_padmix = 1,},
 128                 {.att = 8,.with_padmix = 1,},
 129                 {.att = 9,.with_padmix = 1,},
 130                 {.att = 9,.with_padmix = 1,},
 131         };
 132
 133         if (!b43_has_hardware_pctl(phy)) {
 134                 /* Software pctl */
 135                 list->list = rfatt_0;
 136                 list->len = ARRAY_SIZE(rfatt_0);
 137                 list->min_val = 0;
 138                 list->max_val = 9;
 139                 return;
 140         }
 141         if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
 142                 /* Hardware pctl */
 143                 list->list = rfatt_1;
 144                 list->len = ARRAY_SIZE(rfatt_1);
 145                 list->min_val = 0;
 146                 list->max_val = 14;
 147                 return;
 148         }
 149         /* Hardware pctl */
 150         list->list = rfatt_2;
 151         list->len = ARRAY_SIZE(rfatt_2);
 152         list->min_val = 0;
 153         list->max_val = 9;
 154 }
 155
 156 static void generate_bbatt_list(struct b43_wldev *dev,
 157                                 struct b43_bbatt_list *list)
 158 {
 159         static const struct b43_bbatt bbatt_0[] = {
 160                 {.att = 0,},
 161                 {.att = 1,},
 162                 {.att = 2,},
 163                 {.att = 3,},
 164                 {.att = 4,},
 165                 {.att = 5,},
 166                 {.att = 6,},
 167                 {.att = 7,},
 168                 {.att = 8,},
 169         };
 170
 171         list->list = bbatt_0;
 172         list->len = ARRAY_SIZE(bbatt_0);
 173         list->min_val = 0;
 174         list->max_val = 8;
 175 }
 176
 177 bool b43_has_hardware_pctl(struct b43_phy *phy)
 178 {
 179         if (!phy->hardware_power_control)
 180                 return 0;
 181         switch (phy->type) {
 182         case B43_PHYTYPE_A:
 183                 if (phy->rev >= 5)
 184                         return 1;
 185                 break;
 186         case B43_PHYTYPE_G:
 187                 if (phy->rev >= 6)
 188                         return 1;
 189                 break;
 190         default:
 191                 B43_WARN_ON(1);
 192         }
 193         return 0;
 194 }
 195
 196 static void b43_shm_clear_tssi(struct b43_wldev *dev)
 197 {
 198         struct b43_phy *phy = &dev->phy;
 199
 200         switch (phy->type) {
 201         case B43_PHYTYPE_A:
 202                 b43_shm_write16(dev, B43_SHM_SHARED, 0x0068, 0x7F7F);
 203                 b43_shm_write16(dev, B43_SHM_SHARED, 0x006a, 0x7F7F);
 204                 break;
 205         case B43_PHYTYPE_B:
 206         case B43_PHYTYPE_G:
 207                 b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F);
 208                 b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F);
 209                 b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F);
 210                 b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F);
 211                 break;
 212         }
 213 }
 214
 215 /* Lock the PHY registers against concurrent access from the microcode.
 216  * This lock is nonrecursive. */
 217 void b43_phy_lock(struct b43_wldev *dev)
 218 {
 219 #if B43_DEBUG
 220         B43_WARN_ON(dev->phy.phy_locked);
 221         dev->phy.phy_locked = 1;
 222 #endif
 223         B43_WARN_ON(dev->dev->id.revision < 3);
 224
 225         if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
 226                 b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
 227 }
 228
 229 void b43_phy_unlock(struct b43_wldev *dev)
 230 {
 231 #if B43_DEBUG
 232         B43_WARN_ON(!dev->phy.phy_locked);
 233         dev->phy.phy_locked = 0;
 234 #endif
 235         B43_WARN_ON(dev->dev->id.revision < 3);
 236
 237         if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
 238                 b43_power_saving_ctl_bits(dev, 0);
 239 }
 240
 241 /* Different PHYs require different register routing flags.
 242  * This adjusts (and does sanity checks on) the routing flags.
 243  */
 244 static inline u16 adjust_phyreg_for_phytype(struct b43_phy *phy,
 245                                             u16 offset, struct b43_wldev *dev)
 246 {
 247         if (phy->type == B43_PHYTYPE_A) {
 248                 /* OFDM registers are base-registers for the A-PHY. */
 249                 if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
 250                         offset &= ~B43_PHYROUTE;
 251                         offset |= B43_PHYROUTE_BASE;
 252                 }
 253         }
 254
 255 #if B43_DEBUG
 256         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
 257                 /* Ext-G registers are only available on G-PHYs */
 258                 if (phy->type != B43_PHYTYPE_G) {
 259                         b43err(dev->wl, "Invalid EXT-G PHY access at "
 260                                "0x%04X on PHY type %u\n", offset, phy->type);
 261                         dump_stack();
 262                 }
 263         }
 264         if ((offset & B43_PHYROUTE) == B43_PHYROUTE_N_BMODE) {
 265                 /* N-BMODE registers are only available on N-PHYs */
 266                 if (phy->type != B43_PHYTYPE_N) {
 267                         b43err(dev->wl, "Invalid N-BMODE PHY access at "
 268                                "0x%04X on PHY type %u\n", offset, phy->type);
 269                         dump_stack();
 270                 }
 271         }
 272 #endif /* B43_DEBUG */
 273
 274         return offset;
 275 }
 276
 277 u16 b43_phy_read(struct b43_wldev * dev, u16 offset)
 278 {
 279         struct b43_phy *phy = &dev->phy;
 280
 281         offset = adjust_phyreg_for_phytype(phy, offset, dev);
 282         b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
 283         return b43_read16(dev, B43_MMIO_PHY_DATA);
 284 }
 285
 286 void b43_phy_write(struct b43_wldev *dev, u16 offset, u16 val)
 287 {
 288         struct b43_phy *phy = &dev->phy;
 289
 290         offset = adjust_phyreg_for_phytype(phy, offset, dev);
 291         b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
 292         b43_write16(dev, B43_MMIO_PHY_DATA, val);
 293 }
 294
 295 void b43_phy_mask(struct b43_wldev *dev, u16 offset, u16 mask)
 296 {
 297         b43_phy_write(dev, offset,
 298                       b43_phy_read(dev, offset) & mask);
 299 }
 300
 301 void b43_phy_set(struct b43_wldev *dev, u16 offset, u16 set)
 302 {
 303         b43_phy_write(dev, offset,
 304                       b43_phy_read(dev, offset) | set);
 305 }
 306
 307 void b43_phy_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
 308 {
 309         b43_phy_write(dev, offset,
 310                       (b43_phy_read(dev, offset) & mask) | set);
 311 }
 312
 313 /* Adjust the transmission power output (G-PHY) */
 314 void b43_set_txpower_g(struct b43_wldev *dev,
 315                        const struct b43_bbatt *bbatt,
 316                        const struct b43_rfatt *rfatt, u8 tx_control)
 317 {
 318         struct b43_phy *phy = &dev->phy;
 319         struct b43_txpower_lo_control *lo = phy->lo_control;
 320         u16 bb, rf;
 321         u16 tx_bias, tx_magn;
 322
 323         bb = bbatt->att;
 324         rf = rfatt->att;
 325         tx_bias = lo->tx_bias;
 326         tx_magn = lo->tx_magn;
 327         if (unlikely(tx_bias == 0xFF))
 328                 tx_bias = 0;
 329
 330         /* Save the values for later */
 331         phy->tx_control = tx_control;
 332         memcpy(&phy->rfatt, rfatt, sizeof(*rfatt));
 333         phy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
 334         memcpy(&phy->bbatt, bbatt, sizeof(*bbatt));
 335
 336         if (b43_debug(dev, B43_DBG_XMITPOWER)) {
 337                 b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
 338                        "rfatt(%u), tx_control(0x%02X), "
 339                        "tx_bias(0x%02X), tx_magn(0x%02X)\n",
 340                        bb, rf, tx_control, tx_bias, tx_magn);
 341         }
 342
 343         b43_phy_set_baseband_attenuation(dev, bb);
 344         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
 345         if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
 346                 b43_radio_write16(dev, 0x43,
 347                                   (rf & 0x000F) | (tx_control & 0x0070));
 348         } else {
 349                 b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
 350                                               & 0xFFF0) | (rf & 0x000F));
 351                 b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
 352                                               & ~0x0070) | (tx_control &
 353                                                             0x0070));
 354         }
 355         if (has_tx_magnification(phy)) {
 356                 b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
 357         } else {
 358                 b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
 359                                               & 0xFFF0) | (tx_bias & 0x000F));
 360         }
 361         if (phy->type == B43_PHYTYPE_G)
 362                 b43_lo_g_adjust(dev);
 363 }
 364
 365 static void default_baseband_attenuation(struct b43_wldev *dev,
 366                                          struct b43_bbatt *bb)
 367 {
 368         struct b43_phy *phy = &dev->phy;
 369
 370         if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
 371                 bb->att = 0;
 372         else
 373                 bb->att = 2;
 374 }
 375
 376 static void default_radio_attenuation(struct b43_wldev *dev,
 377                                       struct b43_rfatt *rf)
 378 {
 379         struct ssb_bus *bus = dev->dev->bus;
 380         struct b43_phy *phy = &dev->phy;
 381
 382         rf->with_padmix = 0;
 383
 384         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM &&
 385             bus->boardinfo.type == SSB_BOARD_BCM4309G) {
 386                 if (bus->boardinfo.rev < 0x43) {
 387                         rf->att = 2;
 388                         return;
 389                 } else if (bus->boardinfo.rev < 0x51) {
 390                         rf->att = 3;
 391                         return;
 392                 }
 393         }
 394
 395         if (phy->type == B43_PHYTYPE_A) {
 396                 rf->att = 0x60;
 397                 return;
 398         }
 399
 400         switch (phy->radio_ver) {
 401         case 0x2053:
 402                 switch (phy->radio_rev) {
 403                 case 1:
 404                         rf->att = 6;
 405                         return;
 406                 }
 407                 break;
 408         case 0x2050:
 409                 switch (phy->radio_rev) {
 410                 case 0:
 411                         rf->att = 5;
 412                         return;
 413                 case 1:
 414                         if (phy->type == B43_PHYTYPE_G) {
 415                                 if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
 416                                     && bus->boardinfo.type == SSB_BOARD_BCM4309G
 417                                     && bus->boardinfo.rev >= 30)
 418                                         rf->att = 3;
 419                                 else if (bus->boardinfo.vendor ==
 420                                          SSB_BOARDVENDOR_BCM
 421                                          && bus->boardinfo.type ==
 422                                          SSB_BOARD_BU4306)
 423                                         rf->att = 3;
 424                                 else
 425                                         rf->att = 1;
 426                         } else {
 427                                 if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
 428                                     && bus->boardinfo.type == SSB_BOARD_BCM4309G
 429                                     && bus->boardinfo.rev >= 30)
 430                                         rf->att = 7;
 431                                 else
 432                                         rf->att = 6;
 433                         }
 434                         return;
 435                 case 2:
 436                         if (phy->type == B43_PHYTYPE_G) {
 437                                 if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
 438                                     && bus->boardinfo.type == SSB_BOARD_BCM4309G
 439                                     && bus->boardinfo.rev >= 30)
 440                                         rf->att = 3;
 441                                 else if (bus->boardinfo.vendor ==
 442                                          SSB_BOARDVENDOR_BCM
 443                                          && bus->boardinfo.type ==
 444                                          SSB_BOARD_BU4306)
 445                                         rf->att = 5;
 446                                 else if (bus->chip_id == 0x4320)
 447                                         rf->att = 4;
 448                                 else
 449                                         rf->att = 3;
 450                         } else
 451                                 rf->att = 6;
 452                         return;
 453                 case 3:
 454                         rf->att = 5;
 455                         return;
 456                 case 4:
 457                 case 5:
 458                         rf->att = 1;
 459                         return;
 460                 case 6:
 461                 case 7:
 462                         rf->att = 5;
 463                         return;
 464                 case 8:
 465                         rf->att = 0xA;
 466                         rf->with_padmix = 1;
 467                         return;
 468                 case 9:
 469                 default:
 470                         rf->att = 5;
 471                         return;
 472                 }
 473         }
 474         rf->att = 5;
 475 }
 476
 477 static u16 default_tx_control(struct b43_wldev *dev)
 478 {
 479         struct b43_phy *phy = &dev->phy;
 480
 481         if (phy->radio_ver != 0x2050)
 482                 return 0;
 483         if (phy->radio_rev == 1)
 484                 return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
 485         if (phy->radio_rev < 6)
 486                 return B43_TXCTL_PA2DB;
 487         if (phy->radio_rev == 8)
 488                 return B43_TXCTL_TXMIX;
 489         return 0;
 490 }
 491
 492 /* This func is called "PHY calibrate" in the specs... */
 493 void b43_phy_early_init(struct b43_wldev *dev)
 494 {
 495         struct b43_phy *phy = &dev->phy;
 496         struct b43_txpower_lo_control *lo = phy->lo_control;
 497
 498         default_baseband_attenuation(dev, &phy->bbatt);
 499         default_radio_attenuation(dev, &phy->rfatt);
 500         phy->tx_control = (default_tx_control(dev) << 4);
 501
 502         /* Commit previous writes */
 503         b43_read32(dev, B43_MMIO_MACCTL);
 504
 505         if (phy->type == B43_PHYTYPE_B || phy->type == B43_PHYTYPE_G) {
 506                 generate_rfatt_list(dev, &lo->rfatt_list);
 507                 generate_bbatt_list(dev, &lo->bbatt_list);
 508         }
 509         if (phy->type == B43_PHYTYPE_G && phy->rev == 1) {
 510                 /* Workaround: Temporarly disable gmode through the early init
 511                  * phase, as the gmode stuff is not needed for phy rev 1 */
 512                 phy->gmode = 0;
 513                 b43_wireless_core_reset(dev, 0);
 514                 b43_phy_initg(dev);
 515                 phy->gmode = 1;
 516                 b43_wireless_core_reset(dev, B43_TMSLOW_GMODE);
 517         }
 518 }
 519
 520 /* GPHY_TSSI_Power_Lookup_Table_Init */
 521 static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
 522 {
 523         struct b43_phy *phy = &dev->phy;
 524         int i;
 525         u16 value;
 526
 527         for (i = 0; i < 32; i++)
 528                 b43_ofdmtab_write16(dev, 0x3C20, i, phy->tssi2dbm[i]);
 529         for (i = 32; i < 64; i++)
 530                 b43_ofdmtab_write16(dev, 0x3C00, i - 32, phy->tssi2dbm[i]);
 531         for (i = 0; i < 64; i += 2) {
 532                 value = (u16) phy->tssi2dbm[i];
 533                 value |= ((u16) phy->tssi2dbm[i + 1]) << 8;
 534                 b43_phy_write(dev, 0x380 + (i / 2), value);
 535         }
 536 }
 537
 538 /* GPHY_Gain_Lookup_Table_Init */
 539 static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
 540 {
 541         struct b43_phy *phy = &dev->phy;
 542         struct b43_txpower_lo_control *lo = phy->lo_control;
 543         u16 nr_written = 0;
 544         u16 tmp;
 545         u8 rf, bb;
 546
 547         for (rf = 0; rf < lo->rfatt_list.len; rf++) {
 548                 for (bb = 0; bb < lo->bbatt_list.len; bb++) {
 549                         if (nr_written >= 0x40)
 550                                 return;
 551                         tmp = lo->bbatt_list.list[bb].att;
 552                         tmp <<= 8;
 553                         if (phy->radio_rev == 8)
 554                                 tmp |= 0x50;
 555                         else
 556                                 tmp |= 0x40;
 557                         tmp |= lo->rfatt_list.list[rf].att;
 558                         b43_phy_write(dev, 0x3C0 + nr_written, tmp);
 559                         nr_written++;
 560                 }
 561         }
 562 }
 563
 564 static void hardware_pctl_init_aphy(struct b43_wldev *dev)
 565 {
 566         //TODO
 567 }
 568
 569 static void hardware_pctl_init_gphy(struct b43_wldev *dev)
 570 {
 571         struct b43_phy *phy = &dev->phy;
 572
 573         b43_phy_write(dev, 0x0036, (b43_phy_read(dev, 0x0036) & 0xFFC0)
 574                       | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
 575         b43_phy_write(dev, 0x0478, (b43_phy_read(dev, 0x0478) & 0xFF00)
 576                       | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
 577         b43_gphy_tssi_power_lt_init(dev);
 578         b43_gphy_gain_lt_init(dev);
 579         b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) & 0xFFBF);
 580         b43_phy_write(dev, 0x0014, 0x0000);
 581
 582         B43_WARN_ON(phy->rev < 6);
 583         b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
 584                       | 0x0800);
 585         b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
 586                       & 0xFEFF);
 587         b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801)
 588                       & 0xFFBF);
 589
 590         b43_gphy_dc_lt_init(dev, 1);
 591 }
 592
 593 /* HardwarePowerControl init for A and G PHY */
 594 static void b43_hardware_pctl_init(struct b43_wldev *dev)
 595 {
 596         struct b43_phy *phy = &dev->phy;
 597
 598         if (!b43_has_hardware_pctl(phy)) {
 599                 /* No hardware power control */
 600                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_HWPCTL);
 601                 return;
 602         }
 603         /* Init the hwpctl related hardware */
 604         switch (phy->type) {
 605         case B43_PHYTYPE_A:
 606                 hardware_pctl_init_aphy(dev);
 607                 break;
 608         case B43_PHYTYPE_G:
 609                 hardware_pctl_init_gphy(dev);
 610                 break;
 611         default:
 612                 B43_WARN_ON(1);
 613         }
 614         /* Enable hardware pctl in firmware. */
 615         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_HWPCTL);
 616 }
 617
 618 static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
 619 {
 620         struct b43_phy *phy = &dev->phy;
 621
 622         if (!b43_has_hardware_pctl(phy)) {
 623                 b43_phy_write(dev, 0x047A, 0xC111);
 624                 return;
 625         }
 626
 627         b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036) & 0xFEFF);
 628         b43_phy_write(dev, 0x002F, 0x0202);
 629         b43_phy_write(dev, 0x047C, b43_phy_read(dev, 0x047C) | 0x0002);
 630         b43_phy_write(dev, 0x047A, b43_phy_read(dev, 0x047A) | 0xF000);
 631         if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
 632                 b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
 633                                             & 0xFF0F) | 0x0010);
 634                 b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
 635                               | 0x8000);
 636                 b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
 637                                             & 0xFFC0) | 0x0010);
 638                 b43_phy_write(dev, 0x002E, 0xC07F);
 639                 b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
 640                               | 0x0400);
 641         } else {
 642                 b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
 643                               | 0x0200);
 644                 b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
 645                               | 0x0400);
 646                 b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
 647                               & 0x7FFF);
 648                 b43_phy_write(dev, 0x004F, b43_phy_read(dev, 0x004F)
 649                               & 0xFFFE);
 650                 b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
 651                                             & 0xFFC0) | 0x0010);
 652                 b43_phy_write(dev, 0x002E, 0xC07F);
 653                 b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
 654                                             & 0xFF0F) | 0x0010);
 655         }
 656 }
 657
 658 /* Intialize B/G PHY power control
 659  * as described in http://bcm-specs.sipsolutions.net/InitPowerControl
 660  */
 661 static void b43_phy_init_pctl(struct b43_wldev *dev)
 662 {
 663         struct ssb_bus *bus = dev->dev->bus;
 664         struct b43_phy *phy = &dev->phy;
 665         struct b43_rfatt old_rfatt;
 666         struct b43_bbatt old_bbatt;
 667         u8 old_tx_control = 0;
 668
 669         if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
 670             (bus->boardinfo.type == SSB_BOARD_BU4306))
 671                 return;
 672
 673         b43_phy_write(dev, 0x0028, 0x8018);
 674
 675         /* This does something with the Analog... */
 676         b43_write16(dev, B43_MMIO_PHY0, b43_read16(dev, B43_MMIO_PHY0)
 677                     & 0xFFDF);
 678
 679         if (phy->type == B43_PHYTYPE_G && !phy->gmode)
 680                 return;
 681         b43_hardware_pctl_early_init(dev);
 682         if (phy->cur_idle_tssi == 0) {
 683                 if (phy->radio_ver == 0x2050 && phy->analog == 0) {
 684                         b43_radio_write16(dev, 0x0076,
 685                                           (b43_radio_read16(dev, 0x0076)
 686                                            & 0x00F7) | 0x0084);
 687                 } else {
 688                         struct b43_rfatt rfatt;
 689                         struct b43_bbatt bbatt;
 690
 691                         memcpy(&old_rfatt, &phy->rfatt, sizeof(old_rfatt));
 692                         memcpy(&old_bbatt, &phy->bbatt, sizeof(old_bbatt));
 693                         old_tx_control = phy->tx_control;
 694
 695                         bbatt.att = 11;
 696                         if (phy->radio_rev == 8) {
 697                                 rfatt.att = 15;
 698                                 rfatt.with_padmix = 1;
 699                         } else {
 700                                 rfatt.att = 9;
 701                                 rfatt.with_padmix = 0;
 702                         }
 703                         b43_set_txpower_g(dev, &bbatt, &rfatt, 0);
 704                 }
 705                 b43_dummy_transmission(dev);
 706                 phy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
 707                 if (B43_DEBUG) {
 708                         /* Current-Idle-TSSI sanity check. */
 709                         if (abs(phy->cur_idle_tssi - phy->tgt_idle_tssi) >= 20) {
 710                                 b43dbg(dev->wl,
 711                                        "!WARNING! Idle-TSSI phy->cur_idle_tssi "
 712                                        "measuring failed. (cur=%d, tgt=%d). Disabling TX power "
 713                                        "adjustment.\n", phy->cur_idle_tssi,
 714                                        phy->tgt_idle_tssi);
 715                                 phy->cur_idle_tssi = 0;
 716                         }
 717                 }
 718                 if (phy->radio_ver == 0x2050 && phy->analog == 0) {
 719                         b43_radio_write16(dev, 0x0076,
 720                                           b43_radio_read16(dev, 0x0076)
 721                                           & 0xFF7B);
 722                 } else {
 723                         b43_set_txpower_g(dev, &old_bbatt,
 724                                           &old_rfatt, old_tx_control);
 725                 }
 726         }
 727         b43_hardware_pctl_init(dev);
 728         b43_shm_clear_tssi(dev);
 729 }
 730
 731 static void b43_phy_rssiagc(struct b43_wldev *dev, u8 enable)
 732 {
 733         int i;
 734
 735         if (dev->phy.rev < 3) {
 736                 if (enable)
 737                         for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
 738                                 b43_ofdmtab_write16(dev,
 739                                         B43_OFDMTAB_LNAHPFGAIN1, i, 0xFFF8);
 740                                 b43_ofdmtab_write16(dev,
 741                                         B43_OFDMTAB_WRSSI, i, 0xFFF8);
 742                         }
 743                 else
 744                         for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
 745                                 b43_ofdmtab_write16(dev,
 746                                         B43_OFDMTAB_LNAHPFGAIN1, i, b43_tab_rssiagc1[i]);
 747                                 b43_ofdmtab_write16(dev,
 748                                         B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc1[i]);
 749                         }
 750         } else {
 751                 if (enable)
 752                         for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++)
 753                                 b43_ofdmtab_write16(dev,
 754                                         B43_OFDMTAB_WRSSI, i, 0x0820);
 755                 else
 756                         for (i = 0; i < B43_TAB_RSSIAGC2_SIZE; i++)
 757                                 b43_ofdmtab_write16(dev,
 758                                         B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc2[i]);
 759         }
 760 }
 761
 762 static void b43_phy_ww(struct b43_wldev *dev)
 763 {
 764         u16 b, curr_s, best_s = 0xFFFF;
 765         int i;
 766
 767         b43_phy_write(dev, B43_PHY_CRS0,
 768                 b43_phy_read(dev, B43_PHY_CRS0) & ~B43_PHY_CRS0_EN);
 769         b43_phy_write(dev, B43_PHY_OFDM(0x1B),
 770                 b43_phy_read(dev, B43_PHY_OFDM(0x1B)) | 0x1000);
 771         b43_phy_write(dev, B43_PHY_OFDM(0x82),
 772                 (b43_phy_read(dev, B43_PHY_OFDM(0x82)) & 0xF0FF) | 0x0300);
 773         b43_radio_write16(dev, 0x0009,
 774                 b43_radio_read16(dev, 0x0009) | 0x0080);
 775         b43_radio_write16(dev, 0x0012,
 776                 (b43_radio_read16(dev, 0x0012) & 0xFFFC) | 0x0002);
 777         b43_wa_initgains(dev);
 778         b43_phy_write(dev, B43_PHY_OFDM(0xBA), 0x3ED5);
 779         b = b43_phy_read(dev, B43_PHY_PWRDOWN);
 780         b43_phy_write(dev, B43_PHY_PWRDOWN, (b & 0xFFF8) | 0x0005);
 781         b43_radio_write16(dev, 0x0004,
 782                 b43_radio_read16(dev, 0x0004) | 0x0004);
 783         for (i = 0x10; i <= 0x20; i++) {
 784                 b43_radio_write16(dev, 0x0013, i);
 785                 curr_s = b43_phy_read(dev, B43_PHY_OTABLEQ) & 0x00FF;
 786                 if (!curr_s) {
 787                         best_s = 0x0000;
 788                         break;
 789                 } else if (curr_s >= 0x0080)
 790                         curr_s = 0x0100 - curr_s;
 791                 if (curr_s < best_s)
 792                         best_s = curr_s;
 793         }
 794         b43_phy_write(dev, B43_PHY_PWRDOWN, b);
 795         b43_radio_write16(dev, 0x0004,
 796                 b43_radio_read16(dev, 0x0004) & 0xFFFB);
 797         b43_radio_write16(dev, 0x0013, best_s);
 798         b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1_R1, 0, 0xFFEC);
 799         b43_phy_write(dev, B43_PHY_OFDM(0xB7), 0x1E80);
 800         b43_phy_write(dev, B43_PHY_OFDM(0xB6), 0x1C00);
 801         b43_phy_write(dev, B43_PHY_OFDM(0xB5), 0x0EC0);
 802         b43_phy_write(dev, B43_PHY_OFDM(0xB2), 0x00C0);
 803         b43_phy_write(dev, B43_PHY_OFDM(0xB9), 0x1FFF);
 804         b43_phy_write(dev, B43_PHY_OFDM(0xBB),
 805                 (b43_phy_read(dev, B43_PHY_OFDM(0xBB)) & 0xF000) | 0x0053);
 806         b43_phy_write(dev, B43_PHY_OFDM61,
 807                 (b43_phy_read(dev, B43_PHY_OFDM61) & 0xFE1F) | 0x0120);
 808         b43_phy_write(dev, B43_PHY_OFDM(0x13),
 809                 (b43_phy_read(dev, B43_PHY_OFDM(0x13)) & 0x0FFF) | 0x3000);
 810         b43_phy_write(dev, B43_PHY_OFDM(0x14),
 811                 (b43_phy_read(dev, B43_PHY_OFDM(0x14)) & 0x0FFF) | 0x3000);
 812         b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 6, 0x0017);
 813         for (i = 0; i < 6; i++)
 814                 b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, i, 0x000F);
 815         b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0D, 0x000E);
 816         b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0E, 0x0011);
 817         b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0F, 0x0013);
 818         b43_phy_write(dev, B43_PHY_OFDM(0x33), 0x5030);
 819         b43_phy_write(dev, B43_PHY_CRS0,
 820                 b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
 821 }
 822
 823 /* Initialize APHY. This is also called for the GPHY in some cases. */
 824 static void b43_phy_inita(struct b43_wldev *dev)
 825 {
 826         struct ssb_bus *bus = dev->dev->bus;
 827         struct b43_phy *phy = &dev->phy;
 828
 829         might_sleep();
 830
 831         if (phy->rev >= 6) {
 832                 if (phy->type == B43_PHYTYPE_A)
 833                         b43_phy_write(dev, B43_PHY_OFDM(0x1B),
 834                                 b43_phy_read(dev, B43_PHY_OFDM(0x1B)) & ~0x1000);
 835                 if (b43_phy_read(dev, B43_PHY_ENCORE) & B43_PHY_ENCORE_EN)
 836                         b43_phy_write(dev, B43_PHY_ENCORE,
 837                                 b43_phy_read(dev, B43_PHY_ENCORE) | 0x0010);
 838                 else
 839                         b43_phy_write(dev, B43_PHY_ENCORE,
 840                                 b43_phy_read(dev, B43_PHY_ENCORE) & ~0x1010);
 841         }
 842
 843         b43_wa_all(dev);
 844
 845         if (phy->type == B43_PHYTYPE_A) {
 846                 if (phy->gmode && (phy->rev < 3))
 847                         b43_phy_write(dev, 0x0034,
 848                                 b43_phy_read(dev, 0x0034) | 0x0001);
 849                 b43_phy_rssiagc(dev, 0);
 850
 851                 b43_phy_write(dev, B43_PHY_CRS0,
 852                         b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
 853
 854                 b43_radio_init2060(dev);
 855
 856                 if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
 857                     ((bus->boardinfo.type == SSB_BOARD_BU4306) ||
 858                      (bus->boardinfo.type == SSB_BOARD_BU4309))) {
 859                         ; //TODO: A PHY LO
 860                 }
 861
 862                 if (phy->rev >= 3)
 863                         b43_phy_ww(dev);
 864
 865                 hardware_pctl_init_aphy(dev);
 866
 867                 //TODO: radar detection
 868         }
 869
 870         if ((phy->type == B43_PHYTYPE_G) &&
 871             (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)) {
 872                 b43_phy_write(dev, B43_PHY_OFDM(0x6E),
 873                                   (b43_phy_read(dev, B43_PHY_OFDM(0x6E))
 874                                    & 0xE000) | 0x3CF);
 875         }
 876 }
 877
 878 static void b43_phy_initb5(struct b43_wldev *dev)
 879 {
 880         struct ssb_bus *bus = dev->dev->bus;
 881         struct b43_phy *phy = &dev->phy;
 882         u16 offset, value;
 883         u8 old_channel;
 884
 885         if (phy->analog == 1) {
 886                 b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A)
 887                                   | 0x0050);
 888         }
 889         if ((bus->boardinfo.vendor != SSB_BOARDVENDOR_BCM) &&
 890             (bus->boardinfo.type != SSB_BOARD_BU4306)) {
 891                 value = 0x2120;
 892                 for (offset = 0x00A8; offset < 0x00C7; offset++) {
 893                         b43_phy_write(dev, offset, value);
 894                         value += 0x202;
 895                 }
 896         }
 897         b43_phy_write(dev, 0x0035, (b43_phy_read(dev, 0x0035) & 0xF0FF)
 898                       | 0x0700);
 899         if (phy->radio_ver == 0x2050)
 900                 b43_phy_write(dev, 0x0038, 0x0667);
 901
 902         if (phy->gmode || phy->rev >= 2) {
 903                 if (phy->radio_ver == 0x2050) {
 904                         b43_radio_write16(dev, 0x007A,
 905                                           b43_radio_read16(dev, 0x007A)
 906                                           | 0x0020);
 907                         b43_radio_write16(dev, 0x0051,
 908                                           b43_radio_read16(dev, 0x0051)
 909                                           | 0x0004);
 910                 }
 911                 b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
 912
 913                 b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
 914                 b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
 915
 916                 b43_phy_write(dev, 0x001C, 0x186A);
 917
 918                 b43_phy_write(dev, 0x0013,
 919                               (b43_phy_read(dev, 0x0013) & 0x00FF) | 0x1900);
 920                 b43_phy_write(dev, 0x0035,
 921                               (b43_phy_read(dev, 0x0035) & 0xFFC0) | 0x0064);
 922                 b43_phy_write(dev, 0x005D,
 923                               (b43_phy_read(dev, 0x005D) & 0xFF80) | 0x000A);
 924         }
 925
 926         if (dev->bad_frames_preempt) {
 927                 b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
 928                               b43_phy_read(dev,
 929                                            B43_PHY_RADIO_BITFIELD) | (1 << 11));
 930         }
 931
 932         if (phy->analog == 1) {
 933                 b43_phy_write(dev, 0x0026, 0xCE00);
 934                 b43_phy_write(dev, 0x0021, 0x3763);
 935                 b43_phy_write(dev, 0x0022, 0x1BC3);
 936                 b43_phy_write(dev, 0x0023, 0x06F9);
 937                 b43_phy_write(dev, 0x0024, 0x037E);
 938         } else
 939                 b43_phy_write(dev, 0x0026, 0xCC00);
 940         b43_phy_write(dev, 0x0030, 0x00C6);
 941         b43_write16(dev, 0x03EC, 0x3F22);
 942
 943         if (phy->analog == 1)
 944                 b43_phy_write(dev, 0x0020, 0x3E1C);
 945         else
 946                 b43_phy_write(dev, 0x0020, 0x301C);
 947
 948         if (phy->analog == 0)
 949                 b43_write16(dev, 0x03E4, 0x3000);
 950
 951         old_channel = phy->channel;
 952         /* Force to channel 7, even if not supported. */
 953         b43_radio_selectchannel(dev, 7, 0);
 954
 955         if (phy->radio_ver != 0x2050) {
 956                 b43_radio_write16(dev, 0x0075, 0x0080);
 957                 b43_radio_write16(dev, 0x0079, 0x0081);
 958         }
 959
 960         b43_radio_write16(dev, 0x0050, 0x0020);
 961         b43_radio_write16(dev, 0x0050, 0x0023);
 962
 963         if (phy->radio_ver == 0x2050) {
 964                 b43_radio_write16(dev, 0x0050, 0x0020);
 965                 b43_radio_write16(dev, 0x005A, 0x0070);
 966         }
 967
 968         b43_radio_write16(dev, 0x005B, 0x007B);
 969         b43_radio_write16(dev, 0x005C, 0x00B0);
 970
 971         b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0007);
 972
 973         b43_radio_selectchannel(dev, old_channel, 0);
 974
 975         b43_phy_write(dev, 0x0014, 0x0080);
 976         b43_phy_write(dev, 0x0032, 0x00CA);
 977         b43_phy_write(dev, 0x002A, 0x88A3);
 978
 979         b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
 980
 981         if (phy->radio_ver == 0x2050)
 982                 b43_radio_write16(dev, 0x005D, 0x000D);
 983
 984         b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
 985 }
 986
 987 static void b43_phy_initb6(struct b43_wldev *dev)
 988 {
 989         struct b43_phy *phy = &dev->phy;
 990         u16 offset, val;
 991         u8 old_channel;
 992
 993         b43_phy_write(dev, 0x003E, 0x817A);
 994         b43_radio_write16(dev, 0x007A,
 995                           (b43_radio_read16(dev, 0x007A) | 0x0058));
 996         if (phy->radio_rev == 4 || phy->radio_rev == 5) {
 997                 b43_radio_write16(dev, 0x51, 0x37);
 998                 b43_radio_write16(dev, 0x52, 0x70);
 999                 b43_radio_write16(dev, 0x53, 0xB3);
1000                 b43_radio_write16(dev, 0x54, 0x9B);
1001                 b43_radio_write16(dev, 0x5A, 0x88);
1002                 b43_radio_write16(dev, 0x5B, 0x88);
1003                 b43_radio_write16(dev, 0x5D, 0x88);
1004                 b43_radio_write16(dev, 0x5E, 0x88);
1005                 b43_radio_write16(dev, 0x7D, 0x88);
1006                 b43_hf_write(dev, b43_hf_read(dev)
1007                              | B43_HF_TSSIRPSMW);
1008         }
1009         B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);        /* We had code for these revs here... */
1010         if (phy->radio_rev == 8) {
1011                 b43_radio_write16(dev, 0x51, 0);
1012                 b43_radio_write16(dev, 0x52, 0x40);
1013                 b43_radio_write16(dev, 0x53, 0xB7);
1014                 b43_radio_write16(dev, 0x54, 0x98);
1015                 b43_radio_write16(dev, 0x5A, 0x88);
1016                 b43_radio_write16(dev, 0x5B, 0x6B);
1017                 b43_radio_write16(dev, 0x5C, 0x0F);
1018                 if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_ALTIQ) {
1019                         b43_radio_write16(dev, 0x5D, 0xFA);
1020                         b43_radio_write16(dev, 0x5E, 0xD8);
1021                 } else {
1022                         b43_radio_write16(dev, 0x5D, 0xF5);
1023                         b43_radio_write16(dev, 0x5E, 0xB8);
1024                 }
1025                 b43_radio_write16(dev, 0x0073, 0x0003);
1026                 b43_radio_write16(dev, 0x007D, 0x00A8);
1027                 b43_radio_write16(dev, 0x007C, 0x0001);
1028                 b43_radio_write16(dev, 0x007E, 0x0008);
1029         }
1030         val = 0x1E1F;
1031         for (offset = 0x0088; offset < 0x0098; offset++) {
1032                 b43_phy_write(dev, offset, val);
1033                 val -= 0x0202;
1034         }
1035         val = 0x3E3F;
1036         for (offset = 0x0098; offset < 0x00A8; offset++) {
1037                 b43_phy_write(dev, offset, val);
1038                 val -= 0x0202;
1039         }
1040         val = 0x2120;
1041         for (offset = 0x00A8; offset < 0x00C8; offset++) {
1042                 b43_phy_write(dev, offset, (val & 0x3F3F));
1043                 val += 0x0202;
1044         }
1045         if (phy->type == B43_PHYTYPE_G) {
1046                 b43_radio_write16(dev, 0x007A,
1047                                   b43_radio_read16(dev, 0x007A) | 0x0020);
1048                 b43_radio_write16(dev, 0x0051,
1049                                   b43_radio_read16(dev, 0x0051) | 0x0004);
1050                 b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
1051                 b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
1052                 b43_phy_write(dev, 0x5B, 0);
1053                 b43_phy_write(dev, 0x5C, 0);
1054         }
1055
1056         old_channel = phy->channel;
1057         if (old_channel >= 8)
1058                 b43_radio_selectchannel(dev, 1, 0);
1059         else
1060                 b43_radio_selectchannel(dev, 13, 0);
1061
1062         b43_radio_write16(dev, 0x0050, 0x0020);
1063         b43_radio_write16(dev, 0x0050, 0x0023);
1064         udelay(40);
1065         if (phy->radio_rev < 6 || phy->radio_rev == 8) {
1066                 b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
1067                                               | 0x0002));
1068                 b43_radio_write16(dev, 0x50, 0x20);
1069         }
1070         if (phy->radio_rev <= 2) {
1071                 b43_radio_write16(dev, 0x7C, 0x20);
1072                 b43_radio_write16(dev, 0x5A, 0x70);
1073                 b43_radio_write16(dev, 0x5B, 0x7B);
1074                 b43_radio_write16(dev, 0x5C, 0xB0);
1075         }
1076         b43_radio_write16(dev, 0x007A,
1077                           (b43_radio_read16(dev, 0x007A) & 0x00F8) | 0x0007);
1078
1079         b43_radio_selectchannel(dev, old_channel, 0);
1080
1081         b43_phy_write(dev, 0x0014, 0x0200);
1082         if (phy->radio_rev >= 6)
1083                 b43_phy_write(dev, 0x2A, 0x88C2);
1084         else
1085                 b43_phy_write(dev, 0x2A, 0x8AC0);
1086         b43_phy_write(dev, 0x0038, 0x0668);
1087         b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
1088         if (phy->radio_rev <= 5) {
1089                 b43_phy_write(dev, 0x5D, (b43_phy_read(dev, 0x5D)
1090                                           & 0xFF80) | 0x0003);
1091         }
1092         if (phy->radio_rev <= 2)
1093                 b43_radio_write16(dev, 0x005D, 0x000D);
1094
1095         if (phy->analog == 4) {
1096                 b43_write16(dev, 0x3E4, 9);
1097                 b43_phy_write(dev, 0x61, b43_phy_read(dev, 0x61)
1098                               & 0x0FFF);
1099         } else {
1100                 b43_phy_write(dev, 0x0002, (b43_phy_read(dev, 0x0002) & 0xFFC0)
1101                               | 0x0004);
1102         }
1103         if (phy->type == B43_PHYTYPE_B)
1104                 B43_WARN_ON(1);
1105         else if (phy->type == B43_PHYTYPE_G)
1106                 b43_write16(dev, 0x03E6, 0x0);
1107 }
1108
1109 static void b43_calc_loopback_gain(struct b43_wldev *dev)
1110 {
1111         struct b43_phy *phy = &dev->phy;
1112         u16 backup_phy[16] = { 0 };
1113         u16 backup_radio[3];
1114         u16 backup_bband;
1115         u16 i, j, loop_i_max;
1116         u16 trsw_rx;
1117         u16 loop1_outer_done, loop1_inner_done;
1118
1119         backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
1120         backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
1121         backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
1122         backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
1123         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1124                 backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
1125                 backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
1126         }
1127         backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
1128         backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
1129         backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
1130         backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
1131         backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
1132         backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
1133         backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
1134         backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
1135         backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
1136         backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1137         backup_bband = phy->bbatt.att;
1138         backup_radio[0] = b43_radio_read16(dev, 0x52);
1139         backup_radio[1] = b43_radio_read16(dev, 0x43);
1140         backup_radio[2] = b43_radio_read16(dev, 0x7A);
1141
1142         b43_phy_write(dev, B43_PHY_CRS0,
1143                       b43_phy_read(dev, B43_PHY_CRS0) & 0x3FFF);
1144         b43_phy_write(dev, B43_PHY_CCKBBANDCFG,
1145                       b43_phy_read(dev, B43_PHY_CCKBBANDCFG) | 0x8000);
1146         b43_phy_write(dev, B43_PHY_RFOVER,
1147                       b43_phy_read(dev, B43_PHY_RFOVER) | 0x0002);
1148         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1149                       b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFD);
1150         b43_phy_write(dev, B43_PHY_RFOVER,
1151                       b43_phy_read(dev, B43_PHY_RFOVER) | 0x0001);
1152         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1153                       b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFE);
1154         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1155                 b43_phy_write(dev, B43_PHY_ANALOGOVER,
1156                               b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0001);
1157                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
1158                               b43_phy_read(dev,
1159                                            B43_PHY_ANALOGOVERVAL) & 0xFFFE);
1160                 b43_phy_write(dev, B43_PHY_ANALOGOVER,
1161                               b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0002);
1162                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
1163                               b43_phy_read(dev,
1164                                            B43_PHY_ANALOGOVERVAL) & 0xFFFD);
1165         }
1166         b43_phy_write(dev, B43_PHY_RFOVER,
1167                       b43_phy_read(dev, B43_PHY_RFOVER) | 0x000C);
1168         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1169                       b43_phy_read(dev, B43_PHY_RFOVERVAL) | 0x000C);
1170         b43_phy_write(dev, B43_PHY_RFOVER,
1171                       b43_phy_read(dev, B43_PHY_RFOVER) | 0x0030);
1172         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1173                       (b43_phy_read(dev, B43_PHY_RFOVERVAL)
1174                        & 0xFFCF) | 0x10);
1175
1176         b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0780);
1177         b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
1178         b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
1179
1180         b43_phy_write(dev, B43_PHY_CCK(0x0A),
1181                       b43_phy_read(dev, B43_PHY_CCK(0x0A)) | 0x2000);
1182         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1183                 b43_phy_write(dev, B43_PHY_ANALOGOVER,
1184                               b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0004);
1185                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
1186                               b43_phy_read(dev,
1187                                            B43_PHY_ANALOGOVERVAL) & 0xFFFB);
1188         }
1189         b43_phy_write(dev, B43_PHY_CCK(0x03),
1190                       (b43_phy_read(dev, B43_PHY_CCK(0x03))
1191                        & 0xFF9F) | 0x40);
1192
1193         if (phy->radio_rev == 8) {
1194                 b43_radio_write16(dev, 0x43, 0x000F);
1195         } else {
1196                 b43_radio_write16(dev, 0x52, 0);
1197                 b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
1198                                               & 0xFFF0) | 0x9);
1199         }
1200         b43_phy_set_baseband_attenuation(dev, 11);
1201
1202         if (phy->rev >= 3)
1203                 b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
1204         else
1205                 b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
1206         b43_phy_write(dev, B43_PHY_LO_CTL, 0);
1207
1208         b43_phy_write(dev, B43_PHY_CCK(0x2B),
1209                       (b43_phy_read(dev, B43_PHY_CCK(0x2B))
1210                        & 0xFFC0) | 0x01);
1211         b43_phy_write(dev, B43_PHY_CCK(0x2B),
1212                       (b43_phy_read(dev, B43_PHY_CCK(0x2B))
1213                        & 0xC0FF) | 0x800);
1214
1215         b43_phy_write(dev, B43_PHY_RFOVER,
1216                       b43_phy_read(dev, B43_PHY_RFOVER) | 0x0100);
1217         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1218                       b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xCFFF);
1219
1220         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_EXTLNA) {
1221                 if (phy->rev >= 7) {
1222                         b43_phy_write(dev, B43_PHY_RFOVER,
1223                                       b43_phy_read(dev, B43_PHY_RFOVER)
1224                                       | 0x0800);
1225                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1226                                       b43_phy_read(dev, B43_PHY_RFOVERVAL)
1227                                       | 0x8000);
1228                 }
1229         }
1230         b43_radio_write16(dev, 0x7A, b43_radio_read16(dev, 0x7A)
1231                           & 0x00F7);
1232
1233         j = 0;
1234         loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
1235         for (i = 0; i < loop_i_max; i++) {
1236                 for (j = 0; j < 16; j++) {
1237                         b43_radio_write16(dev, 0x43, i);
1238                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1239                                       (b43_phy_read(dev, B43_PHY_RFOVERVAL)
1240                                        & 0xF0FF) | (j << 8));
1241                         b43_phy_write(dev, B43_PHY_PGACTL,
1242                                       (b43_phy_read(dev, B43_PHY_PGACTL)
1243                                        & 0x0FFF) | 0xA000);
1244                         b43_phy_write(dev, B43_PHY_PGACTL,
1245                                       b43_phy_read(dev, B43_PHY_PGACTL)
1246                                       | 0xF000);
1247                         udelay(20);
1248                         if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1249                                 goto exit_loop1;
1250                 }
1251         }
1252       exit_loop1:
1253         loop1_outer_done = i;
1254         loop1_inner_done = j;
1255         if (j >= 8) {
1256                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
1257                               b43_phy_read(dev, B43_PHY_RFOVERVAL)
1258                               | 0x30);
1259                 trsw_rx = 0x1B;
1260                 for (j = j - 8; j < 16; j++) {
1261                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1262                                       (b43_phy_read(dev, B43_PHY_RFOVERVAL)
1263                                        & 0xF0FF) | (j << 8));
1264                         b43_phy_write(dev, B43_PHY_PGACTL,
1265                                       (b43_phy_read(dev, B43_PHY_PGACTL)
1266                                        & 0x0FFF) | 0xA000);
1267                         b43_phy_write(dev, B43_PHY_PGACTL,
1268                                       b43_phy_read(dev, B43_PHY_PGACTL)
1269                                       | 0xF000);
1270                         udelay(20);
1271                         trsw_rx -= 3;
1272                         if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1273                                 goto exit_loop2;
1274                 }
1275         } else
1276                 trsw_rx = 0x18;
1277       exit_loop2:
1278
1279         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1280                 b43_phy_write(dev, B43_PHY_ANALOGOVER, backup_phy[4]);
1281                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, backup_phy[5]);
1282         }
1283         b43_phy_write(dev, B43_PHY_CCK(0x5A), backup_phy[6]);
1284         b43_phy_write(dev, B43_PHY_CCK(0x59), backup_phy[7]);
1285         b43_phy_write(dev, B43_PHY_CCK(0x58), backup_phy[8]);
1286         b43_phy_write(dev, B43_PHY_CCK(0x0A), backup_phy[9]);
1287         b43_phy_write(dev, B43_PHY_CCK(0x03), backup_phy[10]);
1288         b43_phy_write(dev, B43_PHY_LO_MASK, backup_phy[11]);
1289         b43_phy_write(dev, B43_PHY_LO_CTL, backup_phy[12]);
1290         b43_phy_write(dev, B43_PHY_CCK(0x2B), backup_phy[13]);
1291         b43_phy_write(dev, B43_PHY_PGACTL, backup_phy[14]);
1292
1293         b43_phy_set_baseband_attenuation(dev, backup_bband);
1294
1295         b43_radio_write16(dev, 0x52, backup_radio[0]);
1296         b43_radio_write16(dev, 0x43, backup_radio[1]);
1297         b43_radio_write16(dev, 0x7A, backup_radio[2]);
1298
1299         b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2] | 0x0003);
1300         udelay(10);
1301         b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2]);
1302         b43_phy_write(dev, B43_PHY_RFOVERVAL, backup_phy[3]);
1303         b43_phy_write(dev, B43_PHY_CRS0, backup_phy[0]);
1304         b43_phy_write(dev, B43_PHY_CCKBBANDCFG, backup_phy[1]);
1305
1306         phy->max_lb_gain =
1307             ((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
1308         phy->trsw_rx_gain = trsw_rx * 2;
1309 }
1310
1311 static void b43_phy_initg(struct b43_wldev *dev)
1312 {
1313         struct b43_phy *phy = &dev->phy;
1314         u16 tmp;
1315
1316         if (phy->rev == 1)
1317                 b43_phy_initb5(dev);
1318         else
1319                 b43_phy_initb6(dev);
1320
1321         if (phy->rev >= 2 || phy->gmode)
1322                 b43_phy_inita(dev);
1323
1324         if (phy->rev >= 2) {
1325                 b43_phy_write(dev, B43_PHY_ANALOGOVER, 0);
1326                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, 0);
1327         }
1328         if (phy->rev == 2) {
1329                 b43_phy_write(dev, B43_PHY_RFOVER, 0);
1330                 b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
1331         }
1332         if (phy->rev > 5) {
1333                 b43_phy_write(dev, B43_PHY_RFOVER, 0x400);
1334                 b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
1335         }
1336         if (phy->gmode || phy->rev >= 2) {
1337                 tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
1338                 tmp &= B43_PHYVER_VERSION;
1339                 if (tmp == 3 || tmp == 5) {
1340                         b43_phy_write(dev, B43_PHY_OFDM(0xC2), 0x1816);
1341                         b43_phy_write(dev, B43_PHY_OFDM(0xC3), 0x8006);
1342                 }
1343                 if (tmp == 5) {
1344                         b43_phy_write(dev, B43_PHY_OFDM(0xCC),
1345                                       (b43_phy_read(dev, B43_PHY_OFDM(0xCC))
1346                                        & 0x00FF) | 0x1F00);
1347                 }
1348         }
1349         if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
1350                 b43_phy_write(dev, B43_PHY_OFDM(0x7E), 0x78);
1351         if (phy->radio_rev == 8) {
1352                 b43_phy_write(dev, B43_PHY_EXTG(0x01),
1353                               b43_phy_read(dev, B43_PHY_EXTG(0x01))
1354                               | 0x80);
1355                 b43_phy_write(dev, B43_PHY_OFDM(0x3E),
1356                               b43_phy_read(dev, B43_PHY_OFDM(0x3E))
1357                               | 0x4);
1358         }
1359         if (has_loopback_gain(phy))
1360                 b43_calc_loopback_gain(dev);
1361
1362         if (phy->radio_rev != 8) {
1363                 if (phy->initval == 0xFFFF)
1364                         phy->initval = b43_radio_init2050(dev);
1365                 else
1366                         b43_radio_write16(dev, 0x0078, phy->initval);
1367         }
1368         b43_lo_g_init(dev);
1369         if (has_tx_magnification(phy)) {
1370                 b43_radio_write16(dev, 0x52,
1371                                   (b43_radio_read16(dev, 0x52) & 0xFF00)
1372                                   | phy->lo_control->tx_bias | phy->
1373                                   lo_control->tx_magn);
1374         } else {
1375                 b43_radio_write16(dev, 0x52,
1376                                   (b43_radio_read16(dev, 0x52) & 0xFFF0)
1377                                   | phy->lo_control->tx_bias);
1378         }
1379         if (phy->rev >= 6) {
1380                 b43_phy_write(dev, B43_PHY_CCK(0x36),
1381                               (b43_phy_read(dev, B43_PHY_CCK(0x36))
1382                                & 0x0FFF) | (phy->lo_control->
1383                                             tx_bias << 12));
1384         }
1385         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
1386                 b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
1387         else
1388                 b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
1389         if (phy->rev < 2)
1390                 b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
1391         else
1392                 b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
1393         if (phy->gmode || phy->rev >= 2) {
1394                 b43_lo_g_adjust(dev);
1395                 b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
1396         }
1397
1398         if (!(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
1399                 /* The specs state to update the NRSSI LT with
1400                  * the value 0x7FFFFFFF here. I think that is some weird
1401                  * compiler optimization in the original driver.
1402                  * Essentially, what we do here is resetting all NRSSI LT
1403                  * entries to -32 (see the clamp_val() in nrssi_hw_update())
1404                  */
1405                 b43_nrssi_hw_update(dev, 0xFFFF);       //FIXME?
1406                 b43_calc_nrssi_threshold(dev);
1407         } else if (phy->gmode || phy->rev >= 2) {
1408                 if (phy->nrssi[0] == -1000) {
1409                         B43_WARN_ON(phy->nrssi[1] != -1000);
1410                         b43_calc_nrssi_slope(dev);
1411                 } else
1412                         b43_calc_nrssi_threshold(dev);
1413         }
1414         if (phy->radio_rev == 8)
1415                 b43_phy_write(dev, B43_PHY_EXTG(0x05), 0x3230);
1416         b43_phy_init_pctl(dev);
1417         /* FIXME: The spec says in the following if, the 0 should be replaced
1418            'if OFDM may not be used in the current locale'
1419            but OFDM is legal everywhere */
1420         if ((dev->dev->bus->chip_id == 0x4306
1421              && dev->dev->bus->chip_package == 2) || 0) {
1422                 b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
1423                               & 0xBFFF);
1424                 b43_phy_write(dev, B43_PHY_OFDM(0xC3),
1425                               b43_phy_read(dev, B43_PHY_OFDM(0xC3))
1426                               & 0x7FFF);
1427         }
1428 }
1429
1430 /* Set the baseband attenuation value on chip. */
1431 void b43_phy_set_baseband_attenuation(struct b43_wldev *dev,
1432                                       u16 baseband_attenuation)
1433 {
1434         struct b43_phy *phy = &dev->phy;
1435
1436         if (phy->analog == 0) {
1437                 b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
1438                                                  & 0xFFF0) |
1439                             baseband_attenuation);
1440         } else if (phy->analog > 1) {
1441                 b43_phy_write(dev, B43_PHY_DACCTL,
1442                               (b43_phy_read(dev, B43_PHY_DACCTL)
1443                                & 0xFFC3) | (baseband_attenuation << 2));
1444         } else {
1445                 b43_phy_write(dev, B43_PHY_DACCTL,
1446                               (b43_phy_read(dev, B43_PHY_DACCTL)
1447                                & 0xFF87) | (baseband_attenuation << 3));
1448         }
1449 }
1450
1451 /* http://bcm-specs.sipsolutions.net/EstimatePowerOut
1452  * This function converts a TSSI value to dBm in Q5.2
1453  */
1454 static s8 b43_phy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
1455 {
1456         struct b43_phy *phy = &dev->phy;
1457         s8 dbm = 0;
1458         s32 tmp;
1459
1460         tmp = (phy->tgt_idle_tssi - phy->cur_idle_tssi + tssi);
1461
1462         switch (phy->type) {
1463         case B43_PHYTYPE_A:
1464                 tmp += 0x80;
1465                 tmp = clamp_val(tmp, 0x00, 0xFF);
1466                 dbm = phy->tssi2dbm[tmp];
1467                 //TODO: There's a FIXME on the specs
1468                 break;
1469         case B43_PHYTYPE_B:
1470         case B43_PHYTYPE_G:
1471                 tmp = clamp_val(tmp, 0x00, 0x3F);
1472                 dbm = phy->tssi2dbm[tmp];
1473                 break;
1474         default:
1475                 B43_WARN_ON(1);
1476         }
1477
1478         return dbm;
1479 }
1480
1481 void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
1482                                      int *_bbatt, int *_rfatt)
1483 {
1484         int rfatt = *_rfatt;
1485         int bbatt = *_bbatt;
1486         struct b43_txpower_lo_control *lo = dev->phy.lo_control;
1487
1488         /* Get baseband and radio attenuation values into their permitted ranges.
1489          * Radio attenuation affects power level 4 times as much as baseband. */
1490
1491         /* Range constants */
1492         const int rf_min = lo->rfatt_list.min_val;
1493         const int rf_max = lo->rfatt_list.max_val;
1494         const int bb_min = lo->bbatt_list.min_val;
1495         const int bb_max = lo->bbatt_list.max_val;
1496
1497         while (1) {
1498                 if (rfatt > rf_max && bbatt > bb_max - 4)
1499                         break;  /* Can not get it into ranges */
1500                 if (rfatt < rf_min && bbatt < bb_min + 4)
1501                         break;  /* Can not get it into ranges */
1502                 if (bbatt > bb_max && rfatt > rf_max - 1)
1503                         break;  /* Can not get it into ranges */
1504                 if (bbatt < bb_min && rfatt < rf_min + 1)
1505                         break;  /* Can not get it into ranges */
1506
1507                 if (bbatt > bb_max) {
1508                         bbatt -= 4;
1509                         rfatt += 1;
1510                         continue;
1511                 }
1512                 if (bbatt < bb_min) {
1513                         bbatt += 4;
1514                         rfatt -= 1;
1515                         continue;
1516                 }
1517                 if (rfatt > rf_max) {
1518                         rfatt -= 1;
1519                         bbatt += 4;
1520                         continue;
1521                 }
1522                 if (rfatt < rf_min) {
1523                         rfatt += 1;
1524                         bbatt -= 4;
1525                         continue;
1526                 }
1527                 break;
1528         }
1529
1530         *_rfatt = clamp_val(rfatt, rf_min, rf_max);
1531         *_bbatt = clamp_val(bbatt, bb_min, bb_max);
1532 }
1533
1534 /* http://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
1535 void b43_phy_xmitpower(struct b43_wldev *dev)
1536 {
1537         struct ssb_bus *bus = dev->dev->bus;
1538         struct b43_phy *phy = &dev->phy;
1539
1540         if (phy->cur_idle_tssi == 0)
1541                 return;
1542         if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
1543             (bus->boardinfo.type == SSB_BOARD_BU4306))
1544                 return;
1545 #ifdef CONFIG_B43_DEBUG
1546         if (phy->manual_txpower_control)
1547                 return;
1548 #endif
1549
1550         switch (phy->type) {
1551         case B43_PHYTYPE_A:{
1552
1553                         //TODO: Nothing for A PHYs yet :-/
1554
1555                         break;
1556                 }
1557         case B43_PHYTYPE_B:
1558         case B43_PHYTYPE_G:{
1559                         u16 tmp;
1560                         s8 v0, v1, v2, v3;
1561                         s8 average;
1562                         int max_pwr;
1563                         int desired_pwr, estimated_pwr, pwr_adjust;
1564                         int rfatt_delta, bbatt_delta;
1565                         int rfatt, bbatt;
1566                         u8 tx_control;
1567
1568                         tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x0058);
1569                         v0 = (s8) (tmp & 0x00FF);
1570                         v1 = (s8) ((tmp & 0xFF00) >> 8);
1571                         tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x005A);
1572                         v2 = (s8) (tmp & 0x00FF);
1573                         v3 = (s8) ((tmp & 0xFF00) >> 8);
1574                         tmp = 0;
1575
1576                         if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
1577                             || v3 == 0x7F) {
1578                                 tmp =
1579                                     b43_shm_read16(dev, B43_SHM_SHARED, 0x0070);
1580                                 v0 = (s8) (tmp & 0x00FF);
1581                                 v1 = (s8) ((tmp & 0xFF00) >> 8);
1582                                 tmp =
1583                                     b43_shm_read16(dev, B43_SHM_SHARED, 0x0072);
1584                                 v2 = (s8) (tmp & 0x00FF);
1585                                 v3 = (s8) ((tmp & 0xFF00) >> 8);
1586                                 if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
1587                                     || v3 == 0x7F)
1588                                         return;
1589                                 v0 = (v0 + 0x20) & 0x3F;
1590                                 v1 = (v1 + 0x20) & 0x3F;
1591                                 v2 = (v2 + 0x20) & 0x3F;
1592                                 v3 = (v3 + 0x20) & 0x3F;
1593                                 tmp = 1;
1594                         }
1595                         b43_shm_clear_tssi(dev);
1596
1597                         average = (v0 + v1 + v2 + v3 + 2) / 4;
1598
1599                         if (tmp
1600                             && (b43_shm_read16(dev, B43_SHM_SHARED, 0x005E) &
1601                                 0x8))
1602                                 average -= 13;
1603
1604                         estimated_pwr =
1605                             b43_phy_estimate_power_out(dev, average);
1606
1607                         max_pwr = dev->dev->bus->sprom.maxpwr_bg;
1608                         if ((dev->dev->bus->sprom.boardflags_lo
1609                             & B43_BFL_PACTRL) && (phy->type == B43_PHYTYPE_G))
1610                                 max_pwr -= 0x3;
1611                         if (unlikely(max_pwr <= 0)) {
1612                                 b43warn(dev->wl,
1613                                         "Invalid max-TX-power value in SPROM.\n");
1614                                 max_pwr = 60;   /* fake it */
1615                                 dev->dev->bus->sprom.maxpwr_bg = max_pwr;
1616                         }
1617
1618                         /*TODO:
1619                            max_pwr = min(REG - dev->dev->bus->sprom.antennagain_bgphy - 0x6, max_pwr)
1620                            where REG is the max power as per the regulatory domain
1621                          */
1622
1623                         /* Get desired power (in Q5.2) */
1624                         desired_pwr = INT_TO_Q52(phy->power_level);
1625                         /* And limit it. max_pwr already is Q5.2 */
1626                         desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
1627                         if (b43_debug(dev, B43_DBG_XMITPOWER)) {
1628                                 b43dbg(dev->wl,
1629                                        "Current TX power output: " Q52_FMT
1630                                        " dBm, " "Desired TX power output: "
1631                                        Q52_FMT " dBm\n", Q52_ARG(estimated_pwr),
1632                                        Q52_ARG(desired_pwr));
1633                         }
1634
1635                         /* Calculate the adjustment delta. */
1636                         pwr_adjust = desired_pwr - estimated_pwr;
1637
1638                         /* RF attenuation delta. */
1639                         rfatt_delta = ((pwr_adjust + 7) / 8);
1640                         /* Lower attenuation => Bigger power output. Negate it. */
1641                         rfatt_delta = -rfatt_delta;
1642
1643                         /* Baseband attenuation delta. */
1644                         bbatt_delta = pwr_adjust / 2;
1645                         /* Lower attenuation => Bigger power output. Negate it. */
1646                         bbatt_delta = -bbatt_delta;
1647                         /* RF att affects power level 4 times as much as
1648                          * Baseband attennuation. Subtract it. */
1649                         bbatt_delta -= 4 * rfatt_delta;
1650
1651                         /* So do we finally need to adjust something? */
1652                         if ((rfatt_delta == 0) && (bbatt_delta == 0))
1653                                 return;
1654
1655                         /* Calculate the new attenuation values. */
1656                         bbatt = phy->bbatt.att;
1657                         bbatt += bbatt_delta;
1658                         rfatt = phy->rfatt.att;
1659                         rfatt += rfatt_delta;
1660
1661                         b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
1662                         tx_control = phy->tx_control;
1663                         if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
1664                                 if (rfatt <= 1) {
1665                                         if (tx_control == 0) {
1666                                                 tx_control =
1667                                                     B43_TXCTL_PA2DB |
1668                                                     B43_TXCTL_TXMIX;
1669                                                 rfatt += 2;
1670                                                 bbatt += 2;
1671                                         } else if (dev->dev->bus->sprom.
1672                                                    boardflags_lo &
1673                                                    B43_BFL_PACTRL) {
1674                                                 bbatt += 4 * (rfatt - 2);
1675                                                 rfatt = 2;
1676                                         }
1677                                 } else if (rfatt > 4 && tx_control) {
1678                                         tx_control = 0;
1679                                         if (bbatt < 3) {
1680                                                 rfatt -= 3;
1681                                                 bbatt += 2;
1682                                         } else {
1683                                                 rfatt -= 2;
1684                                                 bbatt -= 2;
1685                                         }
1686                                 }
1687                         }
1688                         /* Save the control values */
1689                         phy->tx_control = tx_control;
1690                         b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
1691                         phy->rfatt.att = rfatt;
1692                         phy->bbatt.att = bbatt;
1693
1694                         /* Adjust the hardware */
1695                         b43_phy_lock(dev);
1696                         b43_radio_lock(dev);
1697                         b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt,
1698                                           phy->tx_control);
1699                         b43_radio_unlock(dev);
1700                         b43_phy_unlock(dev);
1701                         break;
1702                 }
1703         case B43_PHYTYPE_N:
1704                 b43_nphy_xmitpower(dev);
1705                 break;
1706         default:
1707                 B43_WARN_ON(1);
1708         }
1709 }
1710
1711 static inline s32 b43_tssi2dbm_ad(s32 num, s32 den)
1712 {
1713         if (num < 0)
1714                 return num / den;
1715         else
1716                 return (num + den / 2) / den;
1717 }
1718
1719 static inline
1720     s8 b43_tssi2dbm_entry(s8 entry[], u8 index, s16 pab0, s16 pab1, s16 pab2)
1721 {
1722         s32 m1, m2, f = 256, q, delta;
1723         s8 i = 0;
1724
1725         m1 = b43_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
1726         m2 = max(b43_tssi2dbm_ad(32768 + index * pab2, 256), 1);
1727         do {
1728                 if (i > 15)
1729                         return -EINVAL;
1730                 q = b43_tssi2dbm_ad(f * 4096 -
1731                                     b43_tssi2dbm_ad(m2 * f, 16) * f, 2048);
1732                 delta = abs(q - f);
1733                 f = q;
1734                 i++;
1735         } while (delta >= 2);
1736         entry[index] = clamp_val(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
1737         return 0;
1738 }
1739
1740 /* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
1741 int b43_phy_init_tssi2dbm_table(struct b43_wldev *dev)
1742 {
1743         struct b43_phy *phy = &dev->phy;
1744         s16 pab0, pab1, pab2;
1745         u8 idx;
1746         s8 *dyn_tssi2dbm;
1747
1748         if (phy->type == B43_PHYTYPE_A) {
1749                 pab0 = (s16) (dev->dev->bus->sprom.pa1b0);
1750                 pab1 = (s16) (dev->dev->bus->sprom.pa1b1);
1751                 pab2 = (s16) (dev->dev->bus->sprom.pa1b2);
1752         } else {
1753                 pab0 = (s16) (dev->dev->bus->sprom.pa0b0);
1754                 pab1 = (s16) (dev->dev->bus->sprom.pa0b1);
1755                 pab2 = (s16) (dev->dev->bus->sprom.pa0b2);
1756         }
1757
1758         if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
1759                 phy->tgt_idle_tssi = 0x34;
1760                 phy->tssi2dbm = b43_tssi2dbm_b_table;
1761                 return 0;
1762         }
1763
1764         if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
1765             pab0 != -1 && pab1 != -1 && pab2 != -1) {
1766                 /* The pabX values are set in SPROM. Use them. */
1767                 if (phy->type == B43_PHYTYPE_A) {
1768                         if ((s8) dev->dev->bus->sprom.itssi_a != 0 &&
1769                             (s8) dev->dev->bus->sprom.itssi_a != -1)
1770                                 phy->tgt_idle_tssi =
1771                                     (s8) (dev->dev->bus->sprom.itssi_a);
1772                         else
1773                                 phy->tgt_idle_tssi = 62;
1774                 } else {
1775                         if ((s8) dev->dev->bus->sprom.itssi_bg != 0 &&
1776                             (s8) dev->dev->bus->sprom.itssi_bg != -1)
1777                                 phy->tgt_idle_tssi =
1778                                     (s8) (dev->dev->bus->sprom.itssi_bg);
1779                         else
1780                                 phy->tgt_idle_tssi = 62;
1781                 }
1782                 dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
1783                 if (dyn_tssi2dbm == NULL) {
1784                         b43err(dev->wl, "Could not allocate memory "
1785                                "for tssi2dbm table\n");
1786                         return -ENOMEM;
1787                 }
1788                 for (idx = 0; idx < 64; idx++)
1789                         if (b43_tssi2dbm_entry
1790                             (dyn_tssi2dbm, idx, pab0, pab1, pab2)) {
1791                                 phy->tssi2dbm = NULL;
1792                                 b43err(dev->wl, "Could not generate "
1793                                        "tssi2dBm table\n");
1794                                 kfree(dyn_tssi2dbm);
1795                                 return -ENODEV;
1796                         }
1797                 phy->tssi2dbm = dyn_tssi2dbm;
1798                 phy->dyn_tssi_tbl = 1;
1799         } else {
1800                 /* pabX values not set in SPROM. */
1801                 switch (phy->type) {
1802                 case B43_PHYTYPE_A:
1803                         /* APHY needs a generated table. */
1804                         phy->tssi2dbm = NULL;
1805                         b43err(dev->wl, "Could not generate tssi2dBm "
1806                                "table (wrong SPROM info)!\n");
1807                         return -ENODEV;
1808                 case B43_PHYTYPE_B:
1809                         phy->tgt_idle_tssi = 0x34;
1810                         phy->tssi2dbm = b43_tssi2dbm_b_table;
1811                         break;
1812                 case B43_PHYTYPE_G:
1813                         phy->tgt_idle_tssi = 0x34;
1814                         phy->tssi2dbm = b43_tssi2dbm_g_table;
1815                         break;
1816                 }
1817         }
1818
1819         return 0;
1820 }
1821
1822 int b43_phy_init(struct b43_wldev *dev)
1823 {
1824         struct b43_phy *phy = &dev->phy;
1825         bool unsupported = 0;
1826         int err = 0;
1827
1828         switch (phy->type) {
1829         case B43_PHYTYPE_A:
1830                 if (phy->rev == 2 || phy->rev == 3)
1831                         b43_phy_inita(dev);
1832                 else
1833                         unsupported = 1;
1834                 break;
1835         case B43_PHYTYPE_G:
1836                 b43_phy_initg(dev);
1837                 break;
1838         case B43_PHYTYPE_N:
1839                 err = b43_phy_initn(dev);
1840                 break;
1841         default:
1842                 unsupported = 1;
1843         }
1844         if (unsupported)
1845                 b43err(dev->wl, "Unknown PHYTYPE found\n");
1846
1847         return err;
1848 }
1849
1850 void b43_set_rx_antenna(struct b43_wldev *dev, int antenna)
1851 {
1852         struct b43_phy *phy = &dev->phy;
1853         u64 hf;
1854         u16 tmp;
1855         int autodiv = 0;
1856
1857         if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
1858                 autodiv = 1;
1859
1860         hf = b43_hf_read(dev);
1861         hf &= ~B43_HF_ANTDIVHELP;
1862         b43_hf_write(dev, hf);
1863
1864         switch (phy->type) {
1865         case B43_PHYTYPE_A:
1866         case B43_PHYTYPE_G:
1867                 tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
1868                 tmp &= ~B43_PHY_BBANDCFG_RXANT;
1869                 tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
1870                     << B43_PHY_BBANDCFG_RXANT_SHIFT;
1871                 b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
1872
1873                 if (autodiv) {
1874                         tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
1875                         if (antenna == B43_ANTENNA_AUTO0)
1876                                 tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
1877                         else
1878                                 tmp |= B43_PHY_ANTDWELL_AUTODIV1;
1879                         b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
1880                 }
1881                 if (phy->type == B43_PHYTYPE_G) {
1882                         tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
1883                         if (autodiv)
1884                                 tmp |= B43_PHY_ANTWRSETT_ARXDIV;
1885                         else
1886                                 tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
1887                         b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
1888                         if (phy->rev >= 2) {
1889                                 tmp = b43_phy_read(dev, B43_PHY_OFDM61);
1890                                 tmp |= B43_PHY_OFDM61_10;
1891                                 b43_phy_write(dev, B43_PHY_OFDM61, tmp);
1892
1893                                 tmp =
1894                                     b43_phy_read(dev, B43_PHY_DIVSRCHGAINBACK);
1895                                 tmp = (tmp & 0xFF00) | 0x15;
1896                                 b43_phy_write(dev, B43_PHY_DIVSRCHGAINBACK,
1897                                               tmp);
1898
1899                                 if (phy->rev == 2) {
1900                                         b43_phy_write(dev, B43_PHY_ADIVRELATED,
1901                                                       8);
1902                                 } else {
1903                                         tmp =
1904                                             b43_phy_read(dev,
1905                                                          B43_PHY_ADIVRELATED);
1906                                         tmp = (tmp & 0xFF00) | 8;
1907                                         b43_phy_write(dev, B43_PHY_ADIVRELATED,
1908                                                       tmp);
1909                                 }
1910                         }
1911                         if (phy->rev >= 6)
1912                                 b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
1913                 } else {
1914                         if (phy->rev < 3) {
1915                                 tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
1916                                 tmp = (tmp & 0xFF00) | 0x24;
1917                                 b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
1918                         } else {
1919                                 tmp = b43_phy_read(dev, B43_PHY_OFDM61);
1920                                 tmp |= 0x10;
1921                                 b43_phy_write(dev, B43_PHY_OFDM61, tmp);
1922                                 if (phy->analog == 3) {
1923                                         b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
1924                                                       0x1D);
1925                                         b43_phy_write(dev, B43_PHY_ADIVRELATED,
1926                                                       8);
1927                                 } else {
1928                                         b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
1929                                                       0x3A);
1930                                         tmp =
1931                                             b43_phy_read(dev,
1932                                                          B43_PHY_ADIVRELATED);
1933                                         tmp = (tmp & 0xFF00) | 8;
1934                                         b43_phy_write(dev, B43_PHY_ADIVRELATED,
1935                                                       tmp);
1936                                 }
1937                         }
1938                 }
1939                 break;
1940         case B43_PHYTYPE_B:
1941                 tmp = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
1942                 tmp &= ~B43_PHY_BBANDCFG_RXANT;
1943                 tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
1944                     << B43_PHY_BBANDCFG_RXANT_SHIFT;
1945                 b43_phy_write(dev, B43_PHY_CCKBBANDCFG, tmp);
1946                 break;
1947         case B43_PHYTYPE_N:
1948                 b43_nphy_set_rxantenna(dev, antenna);
1949                 break;
1950         default:
1951                 B43_WARN_ON(1);
1952         }
1953
1954         hf |= B43_HF_ANTDIVHELP;
1955         b43_hf_write(dev, hf);
1956 }
1957
1958 /* Get the freq, as it has to be written to the device. */
1959 static inline u16 channel2freq_bg(u8 channel)
1960 {
1961         B43_WARN_ON(!(channel >= 1 && channel <= 14));
1962
1963         return b43_radio_channel_codes_bg[channel - 1];
1964 }
1965
1966 /* Get the freq, as it has to be written to the device. */
1967 static inline u16 channel2freq_a(u8 channel)
1968 {
1969         B43_WARN_ON(channel > 200);
1970
1971         return (5000 + 5 * channel);
1972 }
1973
1974 void b43_radio_lock(struct b43_wldev *dev)
1975 {
1976         u32 macctl;
1977
1978         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1979         B43_WARN_ON(macctl & B43_MACCTL_RADIOLOCK);
1980         macctl |= B43_MACCTL_RADIOLOCK;
1981         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1982         /* Commit the write and wait for the device
1983          * to exit any radio register access. */
1984         b43_read32(dev, B43_MMIO_MACCTL);
1985         udelay(10);
1986 }
1987
1988 void b43_radio_unlock(struct b43_wldev *dev)
1989 {
1990         u32 macctl;
1991
1992         /* Commit any write */
1993         b43_read16(dev, B43_MMIO_PHY_VER);
1994         /* unlock */
1995         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1996         B43_WARN_ON(!(macctl & B43_MACCTL_RADIOLOCK));
1997         macctl &= ~B43_MACCTL_RADIOLOCK;
1998         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1999 }
2000
2001 u16 b43_radio_read16(struct b43_wldev *dev, u16 offset)
2002 {
2003         struct b43_phy *phy = &dev->phy;
2004
2005         /* Offset 1 is a 32-bit register. */
2006         B43_WARN_ON(offset == 1);
2007
2008         switch (phy->type) {
2009         case B43_PHYTYPE_A:
2010                 offset |= 0x40;
2011                 break;
2012         case B43_PHYTYPE_B:
2013                 if (phy->radio_ver == 0x2053) {
2014                         if (offset < 0x70)
2015                                 offset += 0x80;
2016                         else if (offset < 0x80)
2017                                 offset += 0x70;
2018                 } else if (phy->radio_ver == 0x2050) {
2019                         offset |= 0x80;
2020                 } else
2021                         B43_WARN_ON(1);
2022                 break;
2023         case B43_PHYTYPE_G:
2024                 offset |= 0x80;
2025                 break;
2026         case B43_PHYTYPE_N:
2027                 offset |= 0x100;
2028                 break;
2029         case B43_PHYTYPE_LP:
2030                 /* No adjustment required. */
2031                 break;
2032         default:
2033                 B43_WARN_ON(1);
2034         }
2035
2036         b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
2037         return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
2038 }
2039
2040 void b43_radio_write16(struct b43_wldev *dev, u16 offset, u16 val)
2041 {
2042         /* Offset 1 is a 32-bit register. */
2043         B43_WARN_ON(offset == 1);
2044
2045         b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
2046         b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, val);
2047 }
2048
2049 void b43_radio_mask(struct b43_wldev *dev, u16 offset, u16 mask)
2050 {
2051         b43_radio_write16(dev, offset,
2052                           b43_radio_read16(dev, offset) & mask);
2053 }
2054
2055 void b43_radio_set(struct b43_wldev *dev, u16 offset, u16 set)
2056 {
2057         b43_radio_write16(dev, offset,
2058                           b43_radio_read16(dev, offset) | set);
2059 }
2060
2061 void b43_radio_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
2062 {
2063         b43_radio_write16(dev, offset,
2064                           (b43_radio_read16(dev, offset) & mask) | set);
2065 }
2066
2067 static void b43_set_all_gains(struct b43_wldev *dev,
2068                               s16 first, s16 second, s16 third)
2069 {
2070         struct b43_phy *phy = &dev->phy;
2071         u16 i;
2072         u16 start = 0x08, end = 0x18;
2073         u16 tmp;
2074         u16 table;
2075
2076         if (phy->rev <= 1) {
2077                 start = 0x10;
2078                 end = 0x20;
2079         }
2080
2081         table = B43_OFDMTAB_GAINX;
2082         if (phy->rev <= 1)
2083                 table = B43_OFDMTAB_GAINX_R1;
2084         for (i = 0; i < 4; i++)
2085                 b43_ofdmtab_write16(dev, table, i, first);
2086
2087         for (i = start; i < end; i++)
2088                 b43_ofdmtab_write16(dev, table, i, second);
2089
2090         if (third != -1) {
2091                 tmp = ((u16) third << 14) | ((u16) third << 6);
2092                 b43_phy_write(dev, 0x04A0,
2093                               (b43_phy_read(dev, 0x04A0) & 0xBFBF) | tmp);
2094                 b43_phy_write(dev, 0x04A1,
2095                               (b43_phy_read(dev, 0x04A1) & 0xBFBF) | tmp);
2096                 b43_phy_write(dev, 0x04A2,
2097                               (b43_phy_read(dev, 0x04A2) & 0xBFBF) | tmp);
2098         }
2099         b43_dummy_transmission(dev);
2100 }
2101
2102 static void b43_set_original_gains(struct b43_wldev *dev)
2103 {
2104         struct b43_phy *phy = &dev->phy;
2105         u16 i, tmp;
2106         u16 table;
2107         u16 start = 0x0008, end = 0x0018;
2108
2109         if (phy->rev <= 1) {
2110                 start = 0x0010;
2111                 end = 0x0020;
2112         }
2113
2114         table = B43_OFDMTAB_GAINX;
2115         if (phy->rev <= 1)
2116                 table = B43_OFDMTAB_GAINX_R1;
2117         for (i = 0; i < 4; i++) {
2118                 tmp = (i & 0xFFFC);
2119                 tmp |= (i & 0x0001) << 1;
2120                 tmp |= (i & 0x0002) >> 1;
2121
2122                 b43_ofdmtab_write16(dev, table, i, tmp);
2123         }
2124
2125         for (i = start; i < end; i++)
2126                 b43_ofdmtab_write16(dev, table, i, i - start);
2127
2128         b43_phy_write(dev, 0x04A0,
2129                       (b43_phy_read(dev, 0x04A0) & 0xBFBF) | 0x4040);
2130         b43_phy_write(dev, 0x04A1,
2131                       (b43_phy_read(dev, 0x04A1) & 0xBFBF) | 0x4040);
2132         b43_phy_write(dev, 0x04A2,
2133                       (b43_phy_read(dev, 0x04A2) & 0xBFBF) | 0x4000);
2134         b43_dummy_transmission(dev);
2135 }
2136
2137 /* Synthetic PU workaround */
2138 static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
2139 {
2140         struct b43_phy *phy = &dev->phy;
2141
2142         might_sleep();
2143
2144         if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
2145                 /* We do not need the workaround. */
2146                 return;
2147         }
2148
2149         if (channel <= 10) {
2150                 b43_write16(dev, B43_MMIO_CHANNEL,
2151                             channel2freq_bg(channel + 4));
2152         } else {
2153                 b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
2154         }
2155         msleep(1);
2156         b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
2157 }
2158
2159 u8 b43_radio_aci_detect(struct b43_wldev *dev, u8 channel)
2160 {
2161         struct b43_phy *phy = &dev->phy;
2162         u8 ret = 0;
2163         u16 saved, rssi, temp;
2164         int i, j = 0;
2165
2166         saved = b43_phy_read(dev, 0x0403);
2167         b43_radio_selectchannel(dev, channel, 0);
2168         b43_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
2169         if (phy->aci_hw_rssi)
2170                 rssi = b43_phy_read(dev, 0x048A) & 0x3F;
2171         else
2172                 rssi = saved & 0x3F;
2173         /* clamp temp to signed 5bit */
2174         if (rssi > 32)
2175                 rssi -= 64;
2176         for (i = 0; i < 100; i++) {
2177                 temp = (b43_phy_read(dev, 0x047F) >> 8) & 0x3F;
2178                 if (temp > 32)
2179                         temp -= 64;
2180                 if (temp < rssi)
2181                         j++;
2182                 if (j >= 20)
2183                         ret = 1;
2184         }
2185         b43_phy_write(dev, 0x0403, saved);
2186
2187         return ret;
2188 }
2189
2190 u8 b43_radio_aci_scan(struct b43_wldev * dev)
2191 {
2192         struct b43_phy *phy = &dev->phy;
2193         u8 ret[13];
2194         unsigned int channel = phy->channel;
2195         unsigned int i, j, start, end;
2196
2197         if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
2198                 return 0;
2199
2200         b43_phy_lock(dev);
2201         b43_radio_lock(dev);
2202         b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
2203         b43_phy_write(dev, B43_PHY_G_CRS,
2204                       b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
2205         b43_set_all_gains(dev, 3, 8, 1);
2206
2207         start = (channel - 5 > 0) ? channel - 5 : 1;
2208         end = (channel + 5 < 14) ? channel + 5 : 13;
2209
2210         for (i = start; i <= end; i++) {
2211                 if (abs(channel - i) > 2)
2212                         ret[i - 1] = b43_radio_aci_detect(dev, i);
2213         }
2214         b43_radio_selectchannel(dev, channel, 0);
2215         b43_phy_write(dev, 0x0802,
2216                       (b43_phy_read(dev, 0x0802) & 0xFFFC) | 0x0003);
2217         b43_phy_write(dev, 0x0403, b43_phy_read(dev, 0x0403) & 0xFFF8);
2218         b43_phy_write(dev, B43_PHY_G_CRS,
2219                       b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
2220         b43_set_original_gains(dev);
2221         for (i = 0; i < 13; i++) {
2222                 if (!ret[i])
2223                         continue;
2224                 end = (i + 5 < 13) ? i + 5 : 13;
2225                 for (j = i; j < end; j++)
2226                         ret[j] = 1;
2227         }
2228         b43_radio_unlock(dev);
2229         b43_phy_unlock(dev);
2230
2231         return ret[channel - 1];
2232 }
2233
2234 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
2235 void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
2236 {
2237         b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
2238         mmiowb();
2239         b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
2240 }
2241
2242 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
2243 s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
2244 {
2245         u16 val;
2246
2247         b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
2248         val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
2249
2250         return (s16) val;
2251 }
2252
2253 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
2254 void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
2255 {
2256         u16 i;
2257         s16 tmp;
2258
2259         for (i = 0; i < 64; i++) {
2260                 tmp = b43_nrssi_hw_read(dev, i);
2261                 tmp -= val;
2262                 tmp = clamp_val(tmp, -32, 31);
2263                 b43_nrssi_hw_write(dev, i, tmp);
2264         }
2265 }
2266
2267 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
2268 void b43_nrssi_mem_update(struct b43_wldev *dev)
2269 {
2270         struct b43_phy *phy = &dev->phy;
2271         s16 i, delta;
2272         s32 tmp;
2273
2274         delta = 0x1F - phy->nrssi[0];
2275         for (i = 0; i < 64; i++) {
2276                 tmp = (i - delta) * phy->nrssislope;
2277                 tmp /= 0x10000;
2278                 tmp += 0x3A;
2279                 tmp = clamp_val(tmp, 0, 0x3F);
2280                 phy->nrssi_lt[i] = tmp;
2281         }
2282 }
2283
2284 static void b43_calc_nrssi_offset(struct b43_wldev *dev)
2285 {
2286         struct b43_phy *phy = &dev->phy;
2287         u16 backup[20] = { 0 };
2288         s16 v47F;
2289         u16 i;
2290         u16 saved = 0xFFFF;
2291
2292         backup[0] = b43_phy_read(dev, 0x0001);
2293         backup[1] = b43_phy_read(dev, 0x0811);
2294         backup[2] = b43_phy_read(dev, 0x0812);
2295         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
2296                 backup[3] = b43_phy_read(dev, 0x0814);
2297                 backup[4] = b43_phy_read(dev, 0x0815);
2298         }
2299         backup[5] = b43_phy_read(dev, 0x005A);
2300         backup[6] = b43_phy_read(dev, 0x0059);
2301         backup[7] = b43_phy_read(dev, 0x0058);
2302         backup[8] = b43_phy_read(dev, 0x000A);
2303         backup[9] = b43_phy_read(dev, 0x0003);
2304         backup[10] = b43_radio_read16(dev, 0x007A);
2305         backup[11] = b43_radio_read16(dev, 0x0043);
2306
2307         b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) & 0x7FFF);
2308         b43_phy_write(dev, 0x0001,
2309                       (b43_phy_read(dev, 0x0001) & 0x3FFF) | 0x4000);
2310         b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
2311         b43_phy_write(dev, 0x0812,
2312                       (b43_phy_read(dev, 0x0812) & 0xFFF3) | 0x0004);
2313         b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
2314         if (phy->rev >= 6) {
2315                 backup[12] = b43_phy_read(dev, 0x002E);
2316                 backup[13] = b43_phy_read(dev, 0x002F);
2317                 backup[14] = b43_phy_read(dev, 0x080F);
2318                 backup[15] = b43_phy_read(dev, 0x0810);
2319                 backup[16] = b43_phy_read(dev, 0x0801);
2320                 backup[17] = b43_phy_read(dev, 0x0060);
2321                 backup[18] = b43_phy_read(dev, 0x0014);
2322                 backup[19] = b43_phy_read(dev, 0x0478);
2323
2324                 b43_phy_write(dev, 0x002E, 0);
2325                 b43_phy_write(dev, 0x002F, 0);
2326                 b43_phy_write(dev, 0x080F, 0);
2327                 b43_phy_write(dev, 0x0810, 0);
2328                 b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478) | 0x0100);
2329                 b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801) | 0x0040);
2330                 b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) | 0x0040);
2331                 b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) | 0x0200);
2332         }
2333         b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0070);
2334         b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0080);
2335         udelay(30);
2336
2337         v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
2338         if (v47F >= 0x20)
2339                 v47F -= 0x40;
2340         if (v47F == 31) {
2341                 for (i = 7; i >= 4; i--) {
2342                         b43_radio_write16(dev, 0x007B, i);
2343                         udelay(20);
2344                         v47F =
2345                             (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
2346                         if (v47F >= 0x20)
2347                                 v47F -= 0x40;
2348                         if (v47F < 31 && saved == 0xFFFF)
2349                                 saved = i;
2350                 }
2351                 if (saved == 0xFFFF)
2352                         saved = 4;
2353         } else {
2354                 b43_radio_write16(dev, 0x007A,
2355                                   b43_radio_read16(dev, 0x007A) & 0x007F);
2356                 if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
2357                         b43_phy_write(dev, 0x0814,
2358                                       b43_phy_read(dev, 0x0814) | 0x0001);
2359                         b43_phy_write(dev, 0x0815,
2360                                       b43_phy_read(dev, 0x0815) & 0xFFFE);
2361                 }
2362                 b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
2363                 b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x000C);
2364                 b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x0030);
2365                 b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x0030);
2366                 b43_phy_write(dev, 0x005A, 0x0480);
2367                 b43_phy_write(dev, 0x0059, 0x0810);
2368                 b43_phy_write(dev, 0x0058, 0x000D);
2369                 if (phy->rev == 0) {
2370                         b43_phy_write(dev, 0x0003, 0x0122);
2371                 } else {
2372                         b43_phy_write(dev, 0x000A, b43_phy_read(dev, 0x000A)
2373                                       | 0x2000);
2374                 }
2375                 if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
2376                         b43_phy_write(dev, 0x0814,
2377                                       b43_phy_read(dev, 0x0814) | 0x0004);
2378                         b43_phy_write(dev, 0x0815,
2379                                       b43_phy_read(dev, 0x0815) & 0xFFFB);
2380                 }
2381                 b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) & 0xFF9F)
2382                               | 0x0040);
2383                 b43_radio_write16(dev, 0x007A,
2384                                   b43_radio_read16(dev, 0x007A) | 0x000F);
2385                 b43_set_all_gains(dev, 3, 0, 1);
2386                 b43_radio_write16(dev, 0x0043, (b43_radio_read16(dev, 0x0043)
2387                                                 & 0x00F0) | 0x000F);
2388                 udelay(30);
2389                 v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
2390                 if (v47F >= 0x20)
2391                         v47F -= 0x40;
2392                 if (v47F == -32) {
2393                         for (i = 0; i < 4; i++) {
2394                                 b43_radio_write16(dev, 0x007B, i);
2395                                 udelay(20);
2396                                 v47F =
2397                                     (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
2398                                            0x003F);
2399                                 if (v47F >= 0x20)
2400                                         v47F -= 0x40;
2401                                 if (v47F > -31 && saved == 0xFFFF)
2402                                         saved = i;
2403                         }
2404                         if (saved == 0xFFFF)
2405                                 saved = 3;
2406                 } else
2407                         saved = 0;
2408         }
2409         b43_radio_write16(dev, 0x007B, saved);
2410
2411         if (phy->rev >= 6) {
2412                 b43_phy_write(dev, 0x002E, backup[12]);
2413                 b43_phy_write(dev, 0x002F, backup[13]);
2414                 b43_phy_write(dev, 0x080F, backup[14]);
2415                 b43_phy_write(dev, 0x0810, backup[15]);
2416         }
2417         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
2418                 b43_phy_write(dev, 0x0814, backup[3]);
2419                 b43_phy_write(dev, 0x0815, backup[4]);
2420         }
2421         b43_phy_write(dev, 0x005A, backup[5]);
2422         b43_phy_write(dev, 0x0059, backup[6]);
2423         b43_phy_write(dev, 0x0058, backup[7]);
2424         b43_phy_write(dev, 0x000A, backup[8]);
2425         b43_phy_write(dev, 0x0003, backup[9]);
2426         b43_radio_write16(dev, 0x0043, backup[11]);
2427         b43_radio_write16(dev, 0x007A, backup[10]);
2428         b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
2429         b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) | 0x8000);
2430         b43_set_original_gains(dev);
2431         if (phy->rev >= 6) {
2432                 b43_phy_write(dev, 0x0801, backup[16]);
2433                 b43_phy_write(dev, 0x0060, backup[17]);
2434                 b43_phy_write(dev, 0x0014, backup[18]);
2435                 b43_phy_write(dev, 0x0478, backup[19]);
2436         }
2437         b43_phy_write(dev, 0x0001, backup[0]);
2438         b43_phy_write(dev, 0x0812, backup[2]);
2439         b43_phy_write(dev, 0x0811, backup[1]);
2440 }
2441
2442 void b43_calc_nrssi_slope(struct b43_wldev *dev)
2443 {
2444         struct b43_phy *phy = &dev->phy;
2445         u16 backup[18] = { 0 };
2446         u16 tmp;
2447         s16 nrssi0, nrssi1;
2448
2449         switch (phy->type) {
2450         case B43_PHYTYPE_B:
2451                 backup[0] = b43_radio_read16(dev, 0x007A);
2452                 backup[1] = b43_radio_read16(dev, 0x0052);
2453                 backup[2] = b43_radio_read16(dev, 0x0043);
2454                 backup[3] = b43_phy_read(dev, 0x0030);
2455                 backup[4] = b43_phy_read(dev, 0x0026);
2456                 backup[5] = b43_phy_read(dev, 0x0015);
2457                 backup[6] = b43_phy_read(dev, 0x002A);
2458                 backup[7] = b43_phy_read(dev, 0x0020);
2459                 backup[8] = b43_phy_read(dev, 0x005A);
2460                 backup[9] = b43_phy_read(dev, 0x0059);
2461                 backup[10] = b43_phy_read(dev, 0x0058);
2462                 backup[11] = b43_read16(dev, 0x03E2);
2463                 backup[12] = b43_read16(dev, 0x03E6);
2464                 backup[13] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
2465
2466                 tmp = b43_radio_read16(dev, 0x007A);
2467                 tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
2468                 b43_radio_write16(dev, 0x007A, tmp);
2469                 b43_phy_write(dev, 0x0030, 0x00FF);
2470                 b43_write16(dev, 0x03EC, 0x7F7F);
2471                 b43_phy_write(dev, 0x0026, 0x0000);
2472                 b43_phy_write(dev, 0x0015, b43_phy_read(dev, 0x0015) | 0x0020);
2473                 b43_phy_write(dev, 0x002A, 0x08A3);
2474                 b43_radio_write16(dev, 0x007A,
2475                                   b43_radio_read16(dev, 0x007A) | 0x0080);
2476
2477                 nrssi0 = (s16) b43_phy_read(dev, 0x0027);
2478                 b43_radio_write16(dev, 0x007A,
2479                                   b43_radio_read16(dev, 0x007A) & 0x007F);
2480                 if (phy->rev >= 2) {
2481                         b43_write16(dev, 0x03E6, 0x0040);
2482                 } else if (phy->rev == 0) {
2483                         b43_write16(dev, 0x03E6, 0x0122);
2484                 } else {
2485                         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2486                                     b43_read16(dev,
2487                                                B43_MMIO_CHANNEL_EXT) & 0x2000);
2488                 }
2489                 b43_phy_write(dev, 0x0020, 0x3F3F);
2490                 b43_phy_write(dev, 0x0015, 0xF330);
2491                 b43_radio_write16(dev, 0x005A, 0x0060);
2492                 b43_radio_write16(dev, 0x0043,
2493                                   b43_radio_read16(dev, 0x0043) & 0x00F0);
2494                 b43_phy_write(dev, 0x005A, 0x0480);
2495                 b43_phy_write(dev, 0x0059, 0x0810);
2496                 b43_phy_write(dev, 0x0058, 0x000D);
2497                 udelay(20);
2498
2499                 nrssi1 = (s16) b43_phy_read(dev, 0x0027);
2500                 b43_phy_write(dev, 0x0030, backup[3]);
2501                 b43_radio_write16(dev, 0x007A, backup[0]);
2502                 b43_write16(dev, 0x03E2, backup[11]);
2503                 b43_phy_write(dev, 0x0026, backup[4]);
2504                 b43_phy_write(dev, 0x0015, backup[5]);
2505                 b43_phy_write(dev, 0x002A, backup[6]);
2506                 b43_synth_pu_workaround(dev, phy->channel);
2507                 if (phy->rev != 0)
2508                         b43_write16(dev, 0x03F4, backup[13]);
2509
2510                 b43_phy_write(dev, 0x0020, backup[7]);
2511                 b43_phy_write(dev, 0x005A, backup[8]);
2512                 b43_phy_write(dev, 0x0059, backup[9]);
2513                 b43_phy_write(dev, 0x0058, backup[10]);
2514                 b43_radio_write16(dev, 0x0052, backup[1]);
2515                 b43_radio_write16(dev, 0x0043, backup[2]);
2516
2517                 if (nrssi0 == nrssi1)
2518                         phy->nrssislope = 0x00010000;
2519                 else
2520                         phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
2521
2522                 if (nrssi0 <= -4) {
2523                         phy->nrssi[0] = nrssi0;
2524                         phy->nrssi[1] = nrssi1;
2525                 }
2526                 break;
2527         case B43_PHYTYPE_G:
2528                 if (phy->radio_rev >= 9)
2529                         return;
2530                 if (phy->radio_rev == 8)
2531                         b43_calc_nrssi_offset(dev);
2532
2533                 b43_phy_write(dev, B43_PHY_G_CRS,
2534                               b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
2535                 b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
2536                 backup[7] = b43_read16(dev, 0x03E2);
2537                 b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
2538                 backup[0] = b43_radio_read16(dev, 0x007A);
2539                 backup[1] = b43_radio_read16(dev, 0x0052);
2540                 backup[2] = b43_radio_read16(dev, 0x0043);
2541                 backup[3] = b43_phy_read(dev, 0x0015);
2542                 backup[4] = b43_phy_read(dev, 0x005A);
2543                 backup[5] = b43_phy_read(dev, 0x0059);
2544                 backup[6] = b43_phy_read(dev, 0x0058);
2545                 backup[8] = b43_read16(dev, 0x03E6);
2546                 backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
2547                 if (phy->rev >= 3) {
2548                         backup[10] = b43_phy_read(dev, 0x002E);
2549                         backup[11] = b43_phy_read(dev, 0x002F);
2550                         backup[12] = b43_phy_read(dev, 0x080F);
2551                         backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
2552                         backup[14] = b43_phy_read(dev, 0x0801);
2553                         backup[15] = b43_phy_read(dev, 0x0060);
2554                         backup[16] = b43_phy_read(dev, 0x0014);
2555                         backup[17] = b43_phy_read(dev, 0x0478);
2556                         b43_phy_write(dev, 0x002E, 0);
2557                         b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
2558                         switch (phy->rev) {
2559                         case 4:
2560                         case 6:
2561                         case 7:
2562                                 b43_phy_write(dev, 0x0478,
2563                                               b43_phy_read(dev, 0x0478)
2564                                               | 0x0100);
2565                                 b43_phy_write(dev, 0x0801,
2566                                               b43_phy_read(dev, 0x0801)
2567                                               | 0x0040);
2568                                 break;
2569                         case 3:
2570                         case 5:
2571                                 b43_phy_write(dev, 0x0801,
2572                                               b43_phy_read(dev, 0x0801)
2573                                               & 0xFFBF);
2574                                 break;
2575                         }
2576                         b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060)
2577                                       | 0x0040);
2578                         b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014)
2579                                       | 0x0200);
2580                 }
2581                 b43_radio_write16(dev, 0x007A,
2582                                   b43_radio_read16(dev, 0x007A) | 0x0070);
2583                 b43_set_all_gains(dev, 0, 8, 0);
2584                 b43_radio_write16(dev, 0x007A,
2585                                   b43_radio_read16(dev, 0x007A) & 0x00F7);
2586                 if (phy->rev >= 2) {
2587                         b43_phy_write(dev, 0x0811,
2588                                       (b43_phy_read(dev, 0x0811) & 0xFFCF) |
2589                                       0x0030);
2590                         b43_phy_write(dev, 0x0812,
2591                                       (b43_phy_read(dev, 0x0812) & 0xFFCF) |
2592                                       0x0010);
2593                 }
2594                 b43_radio_write16(dev, 0x007A,
2595                                   b43_radio_read16(dev, 0x007A) | 0x0080);
2596                 udelay(20);
2597
2598                 nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
2599                 if (nrssi0 >= 0x0020)
2600                         nrssi0 -= 0x0040;
2601
2602                 b43_radio_write16(dev, 0x007A,
2603                                   b43_radio_read16(dev, 0x007A) & 0x007F);
2604                 if (phy->rev >= 2) {
2605                         b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003)
2606                                                     & 0xFF9F) | 0x0040);
2607                 }
2608
2609                 b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2610                             b43_read16(dev, B43_MMIO_CHANNEL_EXT)
2611                             | 0x2000);
2612                 b43_radio_write16(dev, 0x007A,
2613                                   b43_radio_read16(dev, 0x007A) | 0x000F);
2614                 b43_phy_write(dev, 0x0015, 0xF330);
2615                 if (phy->rev >= 2) {
2616                         b43_phy_write(dev, 0x0812,
2617                                       (b43_phy_read(dev, 0x0812) & 0xFFCF) |
2618                                       0x0020);
2619                         b43_phy_write(dev, 0x0811,
2620                                       (b43_phy_read(dev, 0x0811) & 0xFFCF) |
2621                                       0x0020);
2622                 }
2623
2624                 b43_set_all_gains(dev, 3, 0, 1);
2625                 if (phy->radio_rev == 8) {
2626                         b43_radio_write16(dev, 0x0043, 0x001F);
2627                 } else {
2628                         tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
2629                         b43_radio_write16(dev, 0x0052, tmp | 0x0060);
2630                         tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
2631                         b43_radio_write16(dev, 0x0043, tmp | 0x0009);
2632                 }
2633                 b43_phy_write(dev, 0x005A, 0x0480);
2634                 b43_phy_write(dev, 0x0059, 0x0810);
2635                 b43_phy_write(dev, 0x0058, 0x000D);
2636                 udelay(20);
2637                 nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
2638                 if (nrssi1 >= 0x0020)
2639                         nrssi1 -= 0x0040;
2640                 if (nrssi0 == nrssi1)
2641                         phy->nrssislope = 0x00010000;
2642                 else
2643                         phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
2644                 if (nrssi0 >= -4) {
2645                         phy->nrssi[0] = nrssi1;
2646                         phy->nrssi[1] = nrssi0;
2647                 }
2648                 if (phy->rev >= 3) {
2649                         b43_phy_write(dev, 0x002E, backup[10]);
2650                         b43_phy_write(dev, 0x002F, backup[11]);
2651                         b43_phy_write(dev, 0x080F, backup[12]);
2652                         b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
2653                 }
2654                 if (phy->rev >= 2) {
2655                         b43_phy_write(dev, 0x0812,
2656                                       b43_phy_read(dev, 0x0812) & 0xFFCF);
2657                         b43_phy_write(dev, 0x0811,
2658                                       b43_phy_read(dev, 0x0811) & 0xFFCF);
2659                 }
2660
2661                 b43_radio_write16(dev, 0x007A, backup[0]);
2662                 b43_radio_write16(dev, 0x0052, backup[1]);
2663                 b43_radio_write16(dev, 0x0043, backup[2]);
2664                 b43_write16(dev, 0x03E2, backup[7]);
2665                 b43_write16(dev, 0x03E6, backup[8]);
2666                 b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
2667                 b43_phy_write(dev, 0x0015, backup[3]);
2668                 b43_phy_write(dev, 0x005A, backup[4]);
2669                 b43_phy_write(dev, 0x0059, backup[5]);
2670                 b43_phy_write(dev, 0x0058, backup[6]);
2671                 b43_synth_pu_workaround(dev, phy->channel);
2672                 b43_phy_write(dev, 0x0802,
2673                               b43_phy_read(dev, 0x0802) | (0x0001 | 0x0002));
2674                 b43_set_original_gains(dev);
2675                 b43_phy_write(dev, B43_PHY_G_CRS,
2676                               b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
2677                 if (phy->rev >= 3) {
2678                         b43_phy_write(dev, 0x0801, backup[14]);
2679                         b43_phy_write(dev, 0x0060, backup[15]);
2680                         b43_phy_write(dev, 0x0014, backup[16]);
2681                         b43_phy_write(dev, 0x0478, backup[17]);
2682                 }
2683                 b43_nrssi_mem_update(dev);
2684                 b43_calc_nrssi_threshold(dev);
2685                 break;
2686         default:
2687                 B43_WARN_ON(1);
2688         }
2689 }
2690
2691 void b43_calc_nrssi_threshold(struct b43_wldev *dev)
2692 {
2693         struct b43_phy *phy = &dev->phy;
2694         s32 threshold;
2695         s32 a, b;
2696         s16 tmp16;
2697         u16 tmp_u16;
2698
2699         switch (phy->type) {
2700         case B43_PHYTYPE_B:{
2701                         if (phy->radio_ver != 0x2050)
2702                                 return;
2703                         if (!
2704                             (dev->dev->bus->sprom.
2705                              boardflags_lo & B43_BFL_RSSI))
2706                                 return;
2707
2708                         if (phy->radio_rev >= 6) {
2709                                 threshold =
2710                                     (phy->nrssi[1] - phy->nrssi[0]) * 32;
2711                                 threshold += 20 * (phy->nrssi[0] + 1);
2712                                 threshold /= 40;
2713                         } else
2714                                 threshold = phy->nrssi[1] - 5;
2715
2716                         threshold = clamp_val(threshold, 0, 0x3E);
2717                         b43_phy_read(dev, 0x0020);      /* dummy read */
2718                         b43_phy_write(dev, 0x0020,
2719                                       (((u16) threshold) << 8) | 0x001C);
2720
2721                         if (phy->radio_rev >= 6) {
2722                                 b43_phy_write(dev, 0x0087, 0x0E0D);
2723                                 b43_phy_write(dev, 0x0086, 0x0C0B);
2724                                 b43_phy_write(dev, 0x0085, 0x0A09);
2725                                 b43_phy_write(dev, 0x0084, 0x0808);
2726                                 b43_phy_write(dev, 0x0083, 0x0808);
2727                                 b43_phy_write(dev, 0x0082, 0x0604);
2728                                 b43_phy_write(dev, 0x0081, 0x0302);
2729                                 b43_phy_write(dev, 0x0080, 0x0100);
2730                         }
2731                         break;
2732                 }
2733         case B43_PHYTYPE_G:
2734                 if (!phy->gmode ||
2735                     !(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
2736                         tmp16 = b43_nrssi_hw_read(dev, 0x20);
2737                         if (tmp16 >= 0x20)
2738                                 tmp16 -= 0x40;
2739                         if (tmp16 < 3) {
2740                                 b43_phy_write(dev, 0x048A,
2741                                               (b43_phy_read(dev, 0x048A)
2742                                                & 0xF000) | 0x09EB);
2743                         } else {
2744                                 b43_phy_write(dev, 0x048A,
2745                                               (b43_phy_read(dev, 0x048A)
2746                                                & 0xF000) | 0x0AED);
2747                         }
2748                 } else {
2749                         if (phy->interfmode == B43_INTERFMODE_NONWLAN) {
2750                                 a = 0xE;
2751                                 b = 0xA;
2752                         } else if (!phy->aci_wlan_automatic && phy->aci_enable) {
2753                                 a = 0x13;
2754                                 b = 0x12;
2755                         } else {
2756                                 a = 0xE;
2757                                 b = 0x11;
2758                         }
2759
2760                         a = a * (phy->nrssi[1] - phy->nrssi[0]);
2761                         a += (phy->nrssi[0] << 6);
2762                         if (a < 32)
2763                                 a += 31;
2764                         else
2765                                 a += 32;
2766                         a = a >> 6;
2767                         a = clamp_val(a, -31, 31);
2768
2769                         b = b * (phy->nrssi[1] - phy->nrssi[0]);
2770                         b += (phy->nrssi[0] << 6);
2771                         if (b < 32)
2772                                 b += 31;
2773                         else
2774                                 b += 32;
2775                         b = b >> 6;
2776                         b = clamp_val(b, -31, 31);
2777
2778                         tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
2779                         tmp_u16 |= ((u32) b & 0x0000003F);
2780                         tmp_u16 |= (((u32) a & 0x0000003F) << 6);
2781                         b43_phy_write(dev, 0x048A, tmp_u16);
2782                 }
2783                 break;
2784         default:
2785                 B43_WARN_ON(1);
2786         }
2787 }
2788
2789 /* Stack implementation to save/restore values from the
2790  * interference mitigation code.
2791  * It is save to restore values in random order.
2792  */
2793 static void _stack_save(u32 * _stackptr, size_t * stackidx,
2794                         u8 id, u16 offset, u16 value)
2795 {
2796         u32 *stackptr = &(_stackptr[*stackidx]);
2797
2798         B43_WARN_ON(offset & 0xF000);
2799         B43_WARN_ON(id & 0xF0);
2800         *stackptr = offset;
2801         *stackptr |= ((u32) id) << 12;
2802         *stackptr |= ((u32) value) << 16;
2803         (*stackidx)++;
2804         B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
2805 }
2806
2807 static u16 _stack_restore(u32 * stackptr, u8 id, u16 offset)
2808 {
2809         size_t i;
2810
2811         B43_WARN_ON(offset & 0xF000);
2812         B43_WARN_ON(id & 0xF0);
2813         for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
2814                 if ((*stackptr & 0x00000FFF) != offset)
2815                         continue;
2816                 if (((*stackptr & 0x0000F000) >> 12) != id)
2817                         continue;
2818                 return ((*stackptr & 0xFFFF0000) >> 16);
2819         }
2820         B43_WARN_ON(1);
2821
2822         return 0;
2823 }
2824
2825 #define phy_stacksave(offset)                                   \
2826         do {                                                    \
2827                 _stack_save(stack, &stackidx, 0x1, (offset),    \
2828                             b43_phy_read(dev, (offset)));       \
2829         } while (0)
2830 #define phy_stackrestore(offset)                                \
2831         do {                                                    \
2832                 b43_phy_write(dev, (offset),            \
2833                                   _stack_restore(stack, 0x1,    \
2834                                                  (offset)));    \
2835         } while (0)
2836 #define radio_stacksave(offset)                                         \
2837         do {                                                            \
2838                 _stack_save(stack, &stackidx, 0x2, (offset),            \
2839                             b43_radio_read16(dev, (offset)));   \
2840         } while (0)
2841 #define radio_stackrestore(offset)                                      \
2842         do {                                                            \
2843                 b43_radio_write16(dev, (offset),                        \
2844                                       _stack_restore(stack, 0x2,        \
2845                                                      (offset)));        \
2846         } while (0)
2847 #define ofdmtab_stacksave(table, offset)                        \
2848         do {                                                    \
2849                 _stack_save(stack, &stackidx, 0x3, (offset)|(table),    \
2850                             b43_ofdmtab_read16(dev, (table), (offset)));        \
2851         } while (0)
2852 #define ofdmtab_stackrestore(table, offset)                     \
2853         do {                                                    \
2854                 b43_ofdmtab_write16(dev, (table),       (offset),       \
2855                                   _stack_restore(stack, 0x3,    \
2856                                                  (offset)|(table)));    \
2857         } while (0)
2858
2859 static void
2860 b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
2861 {
2862         struct b43_phy *phy = &dev->phy;
2863         u16 tmp, flipped;
2864         size_t stackidx = 0;
2865         u32 *stack = phy->interfstack;
2866
2867         switch (mode) {
2868         case B43_INTERFMODE_NONWLAN:
2869                 if (phy->rev != 1) {
2870                         b43_phy_write(dev, 0x042B,
2871                                       b43_phy_read(dev, 0x042B) | 0x0800);
2872                         b43_phy_write(dev, B43_PHY_G_CRS,
2873                                       b43_phy_read(dev,
2874                                                    B43_PHY_G_CRS) & ~0x4000);
2875                         break;
2876                 }
2877                 radio_stacksave(0x0078);
2878                 tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
2879                 B43_WARN_ON(tmp > 15);
2880                 flipped = bitrev4(tmp);
2881                 if (flipped < 10 && flipped >= 8)
2882                         flipped = 7;
2883                 else if (flipped >= 10)
2884                         flipped -= 3;
2885                 flipped = (bitrev4(flipped) << 1) | 0x0020;
2886                 b43_radio_write16(dev, 0x0078, flipped);
2887
2888                 b43_calc_nrssi_threshold(dev);
2889
2890                 phy_stacksave(0x0406);
2891                 b43_phy_write(dev, 0x0406, 0x7E28);
2892
2893                 b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x0800);
2894                 b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
2895                               b43_phy_read(dev,
2896                                            B43_PHY_RADIO_BITFIELD) | 0x1000);
2897
2898                 phy_stacksave(0x04A0);
2899                 b43_phy_write(dev, 0x04A0,
2900                               (b43_phy_read(dev, 0x04A0) & 0xC0C0) | 0x0008);
2901                 phy_stacksave(0x04A1);
2902                 b43_phy_write(dev, 0x04A1,
2903                               (b43_phy_read(dev, 0x04A1) & 0xC0C0) | 0x0605);
2904                 phy_stacksave(0x04A2);
2905                 b43_phy_write(dev, 0x04A2,
2906                               (b43_phy_read(dev, 0x04A2) & 0xC0C0) | 0x0204);
2907                 phy_stacksave(0x04A8);
2908                 b43_phy_write(dev, 0x04A8,
2909                               (b43_phy_read(dev, 0x04A8) & 0xC0C0) | 0x0803);
2910                 phy_stacksave(0x04AB);
2911                 b43_phy_write(dev, 0x04AB,
2912                               (b43_phy_read(dev, 0x04AB) & 0xC0C0) | 0x0605);
2913
2914                 phy_stacksave(0x04A7);
2915                 b43_phy_write(dev, 0x04A7, 0x0002);
2916                 phy_stacksave(0x04A3);
2917                 b43_phy_write(dev, 0x04A3, 0x287A);
2918                 phy_stacksave(0x04A9);
2919                 b43_phy_write(dev, 0x04A9, 0x2027);
2920                 phy_stacksave(0x0493);
2921                 b43_phy_write(dev, 0x0493, 0x32F5);
2922                 phy_stacksave(0x04AA);
2923                 b43_phy_write(dev, 0x04AA, 0x2027);
2924                 phy_stacksave(0x04AC);
2925                 b43_phy_write(dev, 0x04AC, 0x32F5);
2926                 break;
2927         case B43_INTERFMODE_MANUALWLAN:
2928                 if (b43_phy_read(dev, 0x0033) & 0x0800)
2929                         break;
2930
2931                 phy->aci_enable = 1;
2932
2933                 phy_stacksave(B43_PHY_RADIO_BITFIELD);
2934                 phy_stacksave(B43_PHY_G_CRS);
2935                 if (phy->rev < 2) {
2936                         phy_stacksave(0x0406);
2937                 } else {
2938                         phy_stacksave(0x04C0);
2939                         phy_stacksave(0x04C1);
2940                 }
2941                 phy_stacksave(0x0033);
2942                 phy_stacksave(0x04A7);
2943                 phy_stacksave(0x04A3);
2944                 phy_stacksave(0x04A9);
2945                 phy_stacksave(0x04AA);
2946                 phy_stacksave(0x04AC);
2947                 phy_stacksave(0x0493);
2948                 phy_stacksave(0x04A1);
2949                 phy_stacksave(0x04A0);
2950                 phy_stacksave(0x04A2);
2951                 phy_stacksave(0x048A);
2952                 phy_stacksave(0x04A8);
2953                 phy_stacksave(0x04AB);
2954                 if (phy->rev == 2) {
2955                         phy_stacksave(0x04AD);
2956                         phy_stacksave(0x04AE);
2957                 } else if (phy->rev >= 3) {
2958                         phy_stacksave(0x04AD);
2959                         phy_stacksave(0x0415);
2960                         phy_stacksave(0x0416);
2961                         phy_stacksave(0x0417);
2962                         ofdmtab_stacksave(0x1A00, 0x2);
2963                         ofdmtab_stacksave(0x1A00, 0x3);
2964                 }
2965                 phy_stacksave(0x042B);
2966                 phy_stacksave(0x048C);
2967
2968                 b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
2969                               b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
2970                               & ~0x1000);
2971                 b43_phy_write(dev, B43_PHY_G_CRS,
2972                               (b43_phy_read(dev, B43_PHY_G_CRS)
2973                                & 0xFFFC) | 0x0002);
2974
2975                 b43_phy_write(dev, 0x0033, 0x0800);
2976                 b43_phy_write(dev, 0x04A3, 0x2027);
2977                 b43_phy_write(dev, 0x04A9, 0x1CA8);
2978                 b43_phy_write(dev, 0x0493, 0x287A);
2979                 b43_phy_write(dev, 0x04AA, 0x1CA8);
2980                 b43_phy_write(dev, 0x04AC, 0x287A);
2981
2982                 b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
2983                                             & 0xFFC0) | 0x001A);
2984                 b43_phy_write(dev, 0x04A7, 0x000D);
2985
2986                 if (phy->rev < 2) {
2987                         b43_phy_write(dev, 0x0406, 0xFF0D);
2988                 } else if (phy->rev == 2) {
2989                         b43_phy_write(dev, 0x04C0, 0xFFFF);
2990                         b43_phy_write(dev, 0x04C1, 0x00A9);
2991                 } else {
2992                         b43_phy_write(dev, 0x04C0, 0x00C1);
2993                         b43_phy_write(dev, 0x04C1, 0x0059);
2994                 }
2995
2996                 b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
2997                                             & 0xC0FF) | 0x1800);
2998                 b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
2999                                             & 0xFFC0) | 0x0015);
3000                 b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
3001                                             & 0xCFFF) | 0x1000);
3002                 b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
3003                                             & 0xF0FF) | 0x0A00);
3004                 b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
3005                                             & 0xCFFF) | 0x1000);
3006                 b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
3007                                             & 0xF0FF) | 0x0800);
3008                 b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
3009                                             & 0xFFCF) | 0x0010);
3010                 b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
3011                                             & 0xFFF0) | 0x0005);
3012                 b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
3013                                             & 0xFFCF) | 0x0010);
3014                 b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
3015                                             & 0xFFF0) | 0x0006);
3016                 b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
3017                                             & 0xF0FF) | 0x0800);
3018                 b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
3019                                             & 0xF0FF) | 0x0500);
3020                 b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
3021                                             & 0xFFF0) | 0x000B);
3022
3023                 if (phy->rev >= 3) {
3024                         b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
3025                                       & ~0x8000);
3026                         b43_phy_write(dev, 0x0415, (b43_phy_read(dev, 0x0415)
3027                                                     & 0x8000) | 0x36D8);
3028                         b43_phy_write(dev, 0x0416, (b43_phy_read(dev, 0x0416)
3029                                                     & 0x8000) | 0x36D8);
3030                         b43_phy_write(dev, 0x0417, (b43_phy_read(dev, 0x0417)
3031                                                     & 0xFE00) | 0x016D);
3032                 } else {
3033                         b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
3034                                       | 0x1000);
3035                         b43_phy_write(dev, 0x048A, (b43_phy_read(dev, 0x048A)
3036                                                     & 0x9FFF) | 0x2000);
3037                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
3038                 }
3039                 if (phy->rev >= 2) {
3040                         b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B)
3041                                       | 0x0800);
3042                 }
3043                 b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
3044                                             & 0xF0FF) | 0x0200);
3045                 if (phy->rev == 2) {
3046                         b43_phy_write(dev, 0x04AE, (b43_phy_read(dev, 0x04AE)
3047                                                     & 0xFF00) | 0x007F);
3048                         b43_phy_write(dev, 0x04AD, (b43_phy_read(dev, 0x04AD)
3049                                                     & 0x00FF) | 0x1300);
3050                 } else if (phy->rev >= 6) {
3051                         b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
3052                         b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
3053                         b43_phy_write(dev, 0x04AD, b43_phy_read(dev, 0x04AD)
3054                                       & 0x00FF);
3055                 }
3056                 b43_calc_nrssi_slope(dev);
3057                 break;
3058         default:
3059                 B43_WARN_ON(1);
3060         }
3061 }
3062
3063 static void
3064 b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
3065 {
3066         struct b43_phy *phy = &dev->phy;
3067         u32 *stack = phy->interfstack;
3068
3069         switch (mode) {
3070         case B43_INTERFMODE_NONWLAN:
3071                 if (phy->rev != 1) {
3072                         b43_phy_write(dev, 0x042B,
3073                                       b43_phy_read(dev, 0x042B) & ~0x0800);
3074                         b43_phy_write(dev, B43_PHY_G_CRS,
3075                                       b43_phy_read(dev,
3076                                                    B43_PHY_G_CRS) | 0x4000);
3077                         break;
3078                 }
3079                 radio_stackrestore(0x0078);
3080                 b43_calc_nrssi_threshold(dev);
3081                 phy_stackrestore(0x0406);
3082                 b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) & ~0x0800);
3083                 if (!dev->bad_frames_preempt) {
3084                         b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
3085                                       b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
3086                                       & ~(1 << 11));
3087                 }
3088                 b43_phy_write(dev, B43_PHY_G_CRS,
3089                               b43_phy_read(dev, B43_PHY_G_CRS) | 0x4000);
3090                 phy_stackrestore(0x04A0);
3091                 phy_stackrestore(0x04A1);
3092                 phy_stackrestore(0x04A2);
3093                 phy_stackrestore(0x04A8);
3094                 phy_stackrestore(0x04AB);
3095                 phy_stackrestore(0x04A7);
3096                 phy_stackrestore(0x04A3);
3097                 phy_stackrestore(0x04A9);
3098                 phy_stackrestore(0x0493);
3099                 phy_stackrestore(0x04AA);
3100                 phy_stackrestore(0x04AC);
3101                 break;
3102         case B43_INTERFMODE_MANUALWLAN:
3103                 if (!(b43_phy_read(dev, 0x0033) & 0x0800))
3104                         break;
3105
3106                 phy->aci_enable = 0;
3107
3108                 phy_stackrestore(B43_PHY_RADIO_BITFIELD);
3109                 phy_stackrestore(B43_PHY_G_CRS);
3110                 phy_stackrestore(0x0033);
3111                 phy_stackrestore(0x04A3);
3112                 phy_stackrestore(0x04A9);
3113                 phy_stackrestore(0x0493);
3114                 phy_stackrestore(0x04AA);
3115                 phy_stackrestore(0x04AC);
3116                 phy_stackrestore(0x04A0);
3117                 phy_stackrestore(0x04A7);
3118                 if (phy->rev >= 2) {
3119                         phy_stackrestore(0x04C0);
3120                         phy_stackrestore(0x04C1);
3121                 } else
3122                         phy_stackrestore(0x0406);
3123                 phy_stackrestore(0x04A1);
3124                 phy_stackrestore(0x04AB);
3125                 phy_stackrestore(0x04A8);
3126                 if (phy->rev == 2) {
3127                         phy_stackrestore(0x04AD);
3128                         phy_stackrestore(0x04AE);
3129                 } else if (phy->rev >= 3) {
3130                         phy_stackrestore(0x04AD);
3131                         phy_stackrestore(0x0415);
3132                         phy_stackrestore(0x0416);
3133                         phy_stackrestore(0x0417);
3134                         ofdmtab_stackrestore(0x1A00, 0x2);
3135                         ofdmtab_stackrestore(0x1A00, 0x3);
3136                 }
3137                 phy_stackrestore(0x04A2);
3138                 phy_stackrestore(0x048A);
3139                 phy_stackrestore(0x042B);
3140                 phy_stackrestore(0x048C);
3141                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
3142                 b43_calc_nrssi_slope(dev);
3143                 break;
3144         default:
3145                 B43_WARN_ON(1);
3146         }
3147 }
3148
3149 #undef phy_stacksave
3150 #undef phy_stackrestore
3151 #undef radio_stacksave
3152 #undef radio_stackrestore
3153 #undef ofdmtab_stacksave
3154 #undef ofdmtab_stackrestore
3155
3156 int b43_radio_set_interference_mitigation(struct b43_wldev *dev, int mode)
3157 {
3158         struct b43_phy *phy = &dev->phy;
3159         int currentmode;
3160
3161         if ((phy->type != B43_PHYTYPE_G) || (phy->rev == 0) || (!phy->gmode))
3162                 return -ENODEV;
3163
3164         phy->aci_wlan_automatic = 0;
3165         switch (mode) {
3166         case B43_INTERFMODE_AUTOWLAN:
3167                 phy->aci_wlan_automatic = 1;
3168                 if (phy->aci_enable)
3169                         mode = B43_INTERFMODE_MANUALWLAN;
3170                 else
3171                         mode = B43_INTERFMODE_NONE;
3172                 break;
3173         case B43_INTERFMODE_NONE:
3174         case B43_INTERFMODE_NONWLAN:
3175         case B43_INTERFMODE_MANUALWLAN:
3176                 break;
3177         default:
3178                 return -EINVAL;
3179         }
3180
3181         currentmode = phy->interfmode;
3182         if (currentmode == mode)
3183                 return 0;
3184         if (currentmode != B43_INTERFMODE_NONE)
3185                 b43_radio_interference_mitigation_disable(dev, currentmode);
3186
3187         if (mode == B43_INTERFMODE_NONE) {
3188                 phy->aci_enable = 0;
3189                 phy->aci_hw_rssi = 0;
3190         } else
3191                 b43_radio_interference_mitigation_enable(dev, mode);
3192         phy->interfmode = mode;
3193
3194         return 0;
3195 }
3196
3197 static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
3198 {
3199         u16 reg, index, ret;
3200
3201         static const u8 rcc_table[] = {
3202                 0x02, 0x03, 0x01, 0x0F,
3203                 0x06, 0x07, 0x05, 0x0F,
3204                 0x0A, 0x0B, 0x09, 0x0F,
3205                 0x0E, 0x0F, 0x0D, 0x0F,
3206         };
3207
3208         reg = b43_radio_read16(dev, 0x60);
3209         index = (reg & 0x001E) >> 1;
3210         ret = rcc_table[index] << 1;
3211         ret |= (reg & 0x0001);
3212         ret |= 0x0020;
3213
3214         return ret;
3215 }
3216
3217 #define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
3218 static u16 radio2050_rfover_val(struct b43_wldev *dev,
3219                                 u16 phy_register, unsigned int lpd)
3220 {
3221         struct b43_phy *phy = &dev->phy;
3222         struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
3223
3224         if (!phy->gmode)
3225                 return 0;
3226
3227         if (has_loopback_gain(phy)) {
3228                 int max_lb_gain = phy->max_lb_gain;
3229                 u16 extlna;
3230                 u16 i;
3231
3232                 if (phy->radio_rev == 8)
3233                         max_lb_gain += 0x3E;
3234                 else
3235                         max_lb_gain += 0x26;
3236                 if (max_lb_gain >= 0x46) {
3237                         extlna = 0x3000;
3238                         max_lb_gain -= 0x46;
3239                 } else if (max_lb_gain >= 0x3A) {
3240                         extlna = 0x1000;
3241                         max_lb_gain -= 0x3A;
3242                 } else if (max_lb_gain >= 0x2E) {
3243                         extlna = 0x2000;
3244                         max_lb_gain -= 0x2E;
3245                 } else {
3246                         extlna = 0;
3247                         max_lb_gain -= 0x10;
3248                 }
3249
3250                 for (i = 0; i < 16; i++) {
3251                         max_lb_gain -= (i * 6);
3252                         if (max_lb_gain < 6)
3253                                 break;
3254                 }
3255
3256                 if ((phy->rev < 7) ||
3257                     !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
3258                         if (phy_register == B43_PHY_RFOVER) {
3259                                 return 0x1B3;
3260                         } else if (phy_register == B43_PHY_RFOVERVAL) {
3261                                 extlna |= (i << 8);
3262                                 switch (lpd) {
3263                                 case LPD(0, 1, 1):
3264                                         return 0x0F92;
3265                                 case LPD(0, 0, 1):
3266                                 case LPD(1, 0, 1):
3267                                         return (0x0092 | extlna);
3268                                 case LPD(1, 0, 0):
3269                                         return (0x0093 | extlna);
3270                                 }
3271                                 B43_WARN_ON(1);
3272                         }
3273                         B43_WARN_ON(1);
3274                 } else {
3275                         if (phy_register == B43_PHY_RFOVER) {
3276                                 return 0x9B3;
3277                         } else if (phy_register == B43_PHY_RFOVERVAL) {
3278                                 if (extlna)
3279                                         extlna |= 0x8000;
3280                                 extlna |= (i << 8);
3281                                 switch (lpd) {
3282                                 case LPD(0, 1, 1):
3283                                         return 0x8F92;
3284                                 case LPD(0, 0, 1):
3285                                         return (0x8092 | extlna);
3286                                 case LPD(1, 0, 1):
3287                                         return (0x2092 | extlna);
3288                                 case LPD(1, 0, 0):
3289                                         return (0x2093 | extlna);
3290                                 }
3291                                 B43_WARN_ON(1);
3292                         }
3293                         B43_WARN_ON(1);
3294                 }
3295         } else {
3296                 if ((phy->rev < 7) ||
3297                     !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
3298                         if (phy_register == B43_PHY_RFOVER) {
3299                                 return 0x1B3;
3300                         } else if (phy_register == B43_PHY_RFOVERVAL) {
3301                                 switch (lpd) {
3302                                 case LPD(0, 1, 1):
3303                                         return 0x0FB2;
3304                                 case LPD(0, 0, 1):
3305                                         return 0x00B2;
3306                                 case LPD(1, 0, 1):
3307                                         return 0x30B2;
3308                                 case LPD(1, 0, 0):
3309                                         return 0x30B3;
3310                                 }
3311                                 B43_WARN_ON(1);
3312                         }
3313                         B43_WARN_ON(1);
3314                 } else {
3315                         if (phy_register == B43_PHY_RFOVER) {
3316                                 return 0x9B3;
3317                         } else if (phy_register == B43_PHY_RFOVERVAL) {
3318                                 switch (lpd) {
3319                                 case LPD(0, 1, 1):
3320                                         return 0x8FB2;
3321                                 case LPD(0, 0, 1):
3322                                         return 0x80B2;
3323                                 case LPD(1, 0, 1):
3324                                         return 0x20B2;
3325                                 case LPD(1, 0, 0):
3326                                         return 0x20B3;
3327                                 }
3328                                 B43_WARN_ON(1);
3329                         }
3330                         B43_WARN_ON(1);
3331                 }
3332         }
3333         return 0;
3334 }
3335
3336 struct init2050_saved_values {
3337         /* Core registers */
3338         u16 reg_3EC;
3339         u16 reg_3E6;
3340         u16 reg_3F4;
3341         /* Radio registers */
3342         u16 radio_43;
3343         u16 radio_51;
3344         u16 radio_52;
3345         /* PHY registers */
3346         u16 phy_pgactl;
3347         u16 phy_cck_5A;
3348         u16 phy_cck_59;
3349         u16 phy_cck_58;
3350         u16 phy_cck_30;
3351         u16 phy_rfover;
3352         u16 phy_rfoverval;
3353         u16 phy_analogover;
3354         u16 phy_analogoverval;
3355         u16 phy_crs0;
3356         u16 phy_classctl;
3357         u16 phy_lo_mask;
3358         u16 phy_lo_ctl;
3359         u16 phy_syncctl;
3360 };
3361
3362 u16 b43_radio_init2050(struct b43_wldev *dev)
3363 {
3364         struct b43_phy *phy = &dev->phy;
3365         struct init2050_saved_values sav;
3366         u16 rcc;
3367         u16 radio78;
3368         u16 ret;
3369         u16 i, j;
3370         u32 tmp1 = 0, tmp2 = 0;
3371
3372         memset(&sav, 0, sizeof(sav));   /* get rid of "may be used uninitialized..." */
3373
3374         sav.radio_43 = b43_radio_read16(dev, 0x43);
3375         sav.radio_51 = b43_radio_read16(dev, 0x51);
3376         sav.radio_52 = b43_radio_read16(dev, 0x52);
3377         sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
3378         sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
3379         sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
3380         sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
3381
3382         if (phy->type == B43_PHYTYPE_B) {
3383                 sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
3384                 sav.reg_3EC = b43_read16(dev, 0x3EC);
3385
3386                 b43_phy_write(dev, B43_PHY_CCK(0x30), 0xFF);
3387                 b43_write16(dev, 0x3EC, 0x3F3F);
3388         } else if (phy->gmode || phy->rev >= 2) {
3389                 sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
3390                 sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
3391                 sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
3392                 sav.phy_analogoverval =
3393                     b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
3394                 sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
3395                 sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
3396
3397                 b43_phy_write(dev, B43_PHY_ANALOGOVER,
3398                               b43_phy_read(dev, B43_PHY_ANALOGOVER)
3399                               | 0x0003);
3400                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
3401                               b43_phy_read(dev, B43_PHY_ANALOGOVERVAL)
3402                               & 0xFFFC);
3403                 b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
3404                               & 0x7FFF);
3405                 b43_phy_write(dev, B43_PHY_CLASSCTL,
3406                               b43_phy_read(dev, B43_PHY_CLASSCTL)
3407                               & 0xFFFC);
3408                 if (has_loopback_gain(phy)) {
3409                         sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
3410                         sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
3411
3412                         if (phy->rev >= 3)
3413                                 b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
3414                         else
3415                                 b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
3416                         b43_phy_write(dev, B43_PHY_LO_CTL, 0);
3417                 }
3418
3419                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3420                               radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
3421                                                    LPD(0, 1, 1)));
3422                 b43_phy_write(dev, B43_PHY_RFOVER,
3423                               radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
3424         }
3425         b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
3426
3427         sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
3428         b43_phy_write(dev, B43_PHY_SYNCCTL, b43_phy_read(dev, B43_PHY_SYNCCTL)
3429                       & 0xFF7F);
3430         sav.reg_3E6 = b43_read16(dev, 0x3E6);
3431         sav.reg_3F4 = b43_read16(dev, 0x3F4);
3432
3433         if (phy->analog == 0) {
3434                 b43_write16(dev, 0x03E6, 0x0122);
3435         } else {
3436                 if (phy->analog >= 2) {
3437                         b43_phy_write(dev, B43_PHY_CCK(0x03),
3438                                       (b43_phy_read(dev, B43_PHY_CCK(0x03))
3439                                        & 0xFFBF) | 0x40);
3440                 }
3441                 b43_write16(dev, B43_MMIO_CHANNEL_EXT,
3442                             (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
3443         }
3444
3445         rcc = b43_radio_core_calibration_value(dev);
3446
3447         if (phy->type == B43_PHYTYPE_B)
3448                 b43_radio_write16(dev, 0x78, 0x26);
3449         if (phy->gmode || phy->rev >= 2) {
3450                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3451                               radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
3452                                                    LPD(0, 1, 1)));
3453         }
3454         b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
3455         b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x1403);
3456         if (phy->gmode || phy->rev >= 2) {
3457                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3458                               radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
3459                                                    LPD(0, 0, 1)));
3460         }
3461         b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
3462         b43_radio_write16(dev, 0x51, b43_radio_read16(dev, 0x51)
3463                           | 0x0004);
3464         if (phy->radio_rev == 8) {
3465                 b43_radio_write16(dev, 0x43, 0x1F);
3466         } else {
3467                 b43_radio_write16(dev, 0x52, 0);
3468                 b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
3469                                               & 0xFFF0) | 0x0009);
3470         }
3471         b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
3472
3473         for (i = 0; i < 16; i++) {
3474                 b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0480);
3475                 b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
3476                 b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
3477                 if (phy->gmode || phy->rev >= 2) {
3478                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
3479                                       radio2050_rfover_val(dev,
3480                                                            B43_PHY_RFOVERVAL,
3481                                                            LPD(1, 0, 1)));
3482                 }
3483                 b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
3484                 udelay(10);
3485                 if (phy->gmode || phy->rev >= 2) {
3486                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
3487                                       radio2050_rfover_val(dev,
3488                                                            B43_PHY_RFOVERVAL,
3489                                                            LPD(1, 0, 1)));
3490                 }
3491                 b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
3492                 udelay(10);
3493                 if (phy->gmode || phy->rev >= 2) {
3494                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
3495                                       radio2050_rfover_val(dev,
3496                                                            B43_PHY_RFOVERVAL,
3497                                                            LPD(1, 0, 0)));
3498                 }
3499                 b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
3500                 udelay(20);
3501                 tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
3502                 b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
3503                 if (phy->gmode || phy->rev >= 2) {
3504                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
3505                                       radio2050_rfover_val(dev,
3506                                                            B43_PHY_RFOVERVAL,
3507                                                            LPD(1, 0, 1)));
3508                 }
3509                 b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
3510         }
3511         udelay(10);
3512
3513         b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
3514         tmp1++;
3515         tmp1 >>= 9;
3516
3517         for (i = 0; i < 16; i++) {
3518                 radio78 = (bitrev4(i) << 1) | 0x0020;
3519                 b43_radio_write16(dev, 0x78, radio78);
3520                 udelay(10);
3521                 for (j = 0; j < 16; j++) {
3522                         b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0D80);
3523                         b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
3524                         b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
3525                         if (phy->gmode || phy->rev >= 2) {
3526                                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3527                                               radio2050_rfover_val(dev,
3528                                                                    B43_PHY_RFOVERVAL,
3529                                                                    LPD(1, 0,
3530                                                                        1)));
3531                         }
3532                         b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
3533                         udelay(10);
3534                         if (phy->gmode || phy->rev >= 2) {
3535                                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3536                                               radio2050_rfover_val(dev,
3537                                                                    B43_PHY_RFOVERVAL,
3538                                                                    LPD(1, 0,
3539                                                                        1)));
3540                         }
3541                         b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
3542                         udelay(10);
3543                         if (phy->gmode || phy->rev >= 2) {
3544                                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3545                                               radio2050_rfover_val(dev,
3546                                                                    B43_PHY_RFOVERVAL,
3547                                                                    LPD(1, 0,
3548                                                                        0)));
3549                         }
3550                         b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
3551                         udelay(10);
3552                         tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
3553                         b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
3554                         if (phy->gmode || phy->rev >= 2) {
3555                                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
3556                                               radio2050_rfover_val(dev,
3557                                                                    B43_PHY_RFOVERVAL,
3558                                                                    LPD(1, 0,
3559                                                                        1)));
3560                         }
3561                         b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
3562                 }
3563                 tmp2++;
3564                 tmp2 >>= 8;
3565                 if (tmp1 < tmp2)
3566                         break;
3567         }
3568
3569         /* Restore the registers */
3570         b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
3571         b43_radio_write16(dev, 0x51, sav.radio_51);
3572         b43_radio_write16(dev, 0x52, sav.radio_52);
3573         b43_radio_write16(dev, 0x43, sav.radio_43);
3574         b43_phy_write(dev, B43_PHY_CCK(0x5A), sav.phy_cck_5A);
3575         b43_phy_write(dev, B43_PHY_CCK(0x59), sav.phy_cck_59);
3576         b43_phy_write(dev, B43_PHY_CCK(0x58), sav.phy_cck_58);
3577         b43_write16(dev, 0x3E6, sav.reg_3E6);
3578         if (phy->analog != 0)
3579                 b43_write16(dev, 0x3F4, sav.reg_3F4);
3580         b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
3581         b43_synth_pu_workaround(dev, phy->channel);
3582         if (phy->type == B43_PHYTYPE_B) {
3583                 b43_phy_write(dev, B43_PHY_CCK(0x30), sav.phy_cck_30);
3584                 b43_write16(dev, 0x3EC, sav.reg_3EC);
3585         } else if (phy->gmode) {
3586                 b43_write16(dev, B43_MMIO_PHY_RADIO,
3587                             b43_read16(dev, B43_MMIO_PHY_RADIO)
3588                             & 0x7FFF);
3589                 b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
3590                 b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
3591                 b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
3592                 b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
3593                               sav.phy_analogoverval);
3594                 b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
3595                 b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
3596                 if (has_loopback_gain(phy)) {
3597                         b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
3598                         b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
3599                 }
3600         }
3601         if (i > 15)
3602                 ret = radio78;
3603         else
3604                 ret = rcc;
3605
3606         return ret;
3607 }
3608
3609 void b43_radio_init2060(struct b43_wldev *dev)
3610 {
3611         int err;
3612
3613         b43_radio_write16(dev, 0x0004, 0x00C0);
3614         b43_radio_write16(dev, 0x0005, 0x0008);
3615         b43_radio_write16(dev, 0x0009, 0x0040);
3616         b43_radio_write16(dev, 0x0005, 0x00AA);
3617         b43_radio_write16(dev, 0x0032, 0x008F);
3618         b43_radio_write16(dev, 0x0006, 0x008F);
3619         b43_radio_write16(dev, 0x0034, 0x008F);
3620         b43_radio_write16(dev, 0x002C, 0x0007);
3621         b43_radio_write16(dev, 0x0082, 0x0080);
3622         b43_radio_write16(dev, 0x0080, 0x0000);
3623         b43_radio_write16(dev, 0x003F, 0x00DA);
3624         b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
3625         b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0010);
3626         b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
3627         b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
3628         msleep(1);              /* delay 400usec */
3629
3630         b43_radio_write16(dev, 0x0081,
3631                           (b43_radio_read16(dev, 0x0081) & ~0x0020) | 0x0010);
3632         msleep(1);              /* delay 400usec */
3633
3634         b43_radio_write16(dev, 0x0005,
3635                           (b43_radio_read16(dev, 0x0005) & ~0x0008) | 0x0008);
3636         b43_radio_write16(dev, 0x0085, b43_radio_read16(dev, 0x0085) & ~0x0010);
3637         b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
3638         b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0040);
3639         b43_radio_write16(dev, 0x0081,
3640                           (b43_radio_read16(dev, 0x0081) & ~0x0040) | 0x0040);
3641         b43_radio_write16(dev, 0x0005,
3642                           (b43_radio_read16(dev, 0x0081) & ~0x0008) | 0x0008);
3643         b43_phy_write(dev, 0x0063, 0xDDC6);
3644         b43_phy_write(dev, 0x0069, 0x07BE);
3645         b43_phy_write(dev, 0x006A, 0x0000);
3646
3647         err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_A, 0);
3648         B43_WARN_ON(err);
3649
3650         msleep(1);
3651 }
3652
3653 static inline u16 freq_r3A_value(u16 frequency)
3654 {
3655         u16 value;
3656
3657         if (frequency < 5091)
3658                 value = 0x0040;
3659         else if (frequency < 5321)
3660                 value = 0x0000;
3661         else if (frequency < 5806)
3662                 value = 0x0080;
3663         else
3664                 value = 0x0040;
3665
3666         return value;
3667 }
3668
3669 void b43_radio_set_tx_iq(struct b43_wldev *dev)
3670 {
3671         static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
3672         static const u8 data_low[5] = { 0x00, 0x01, 0x05, 0x06, 0x0A };
3673         u16 tmp = b43_radio_read16(dev, 0x001E);
3674         int i, j;
3675
3676         for (i = 0; i < 5; i++) {
3677                 for (j = 0; j < 5; j++) {
3678                         if (tmp == (data_high[i] << 4 | data_low[j])) {
3679                                 b43_phy_write(dev, 0x0069,
3680                                               (i - j) << 8 | 0x00C0);
3681                                 return;
3682                         }
3683                 }
3684         }
3685 }
3686
3687 int b43_radio_selectchannel(struct b43_wldev *dev,
3688                             u8 channel, int synthetic_pu_workaround)
3689 {
3690         struct b43_phy *phy = &dev->phy;
3691         u16 r8, tmp;
3692         u16 freq;
3693         u16 channelcookie, savedcookie;
3694         int err = 0;
3695
3696         if (channel == 0xFF) {
3697                 switch (phy->type) {
3698                 case B43_PHYTYPE_A:
3699                         channel = B43_DEFAULT_CHANNEL_A;
3700                         break;
3701                 case B43_PHYTYPE_B:
3702                 case B43_PHYTYPE_G:
3703                         channel = B43_DEFAULT_CHANNEL_BG;
3704                         break;
3705                 case B43_PHYTYPE_N:
3706                         //FIXME check if we are on 2.4GHz or 5GHz and set a default channel.
3707                         channel = 1;
3708                         break;
3709                 default:
3710                         B43_WARN_ON(1);
3711                 }
3712         }
3713
3714         /* First we set the channel radio code to prevent the
3715          * firmware from sending ghost packets.
3716          */
3717         channelcookie = channel;
3718         if (0 /*FIXME on 5Ghz */)
3719                 channelcookie |= 0x100;
3720         //FIXME set 40Mhz flag if required
3721         savedcookie = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN);
3722         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN, channelcookie);
3723
3724         switch (phy->type) {
3725         case B43_PHYTYPE_A:
3726                 if (channel > 200) {
3727                         err = -EINVAL;
3728                         goto out;
3729                 }
3730                 freq = channel2freq_a(channel);
3731
3732                 r8 = b43_radio_read16(dev, 0x0008);
3733                 b43_write16(dev, 0x03F0, freq);
3734                 b43_radio_write16(dev, 0x0008, r8);
3735
3736                 //TODO: write max channel TX power? to Radio 0x2D
3737                 tmp = b43_radio_read16(dev, 0x002E);
3738                 tmp &= 0x0080;
3739                 //TODO: OR tmp with the Power out estimation for this channel?
3740                 b43_radio_write16(dev, 0x002E, tmp);
3741
3742                 if (freq >= 4920 && freq <= 5500) {
3743                         /*
3744                          * r8 = (((freq * 15 * 0xE1FC780F) >> 32) / 29) & 0x0F;
3745                          *    = (freq * 0.025862069
3746                          */
3747                         r8 = 3 * freq / 116;    /* is equal to r8 = freq * 0.025862 */
3748                 }
3749                 b43_radio_write16(dev, 0x0007, (r8 << 4) | r8);
3750                 b43_radio_write16(dev, 0x0020, (r8 << 4) | r8);
3751                 b43_radio_write16(dev, 0x0021, (r8 << 4) | r8);
3752                 b43_radio_write16(dev, 0x0022, (b43_radio_read16(dev, 0x0022)
3753                                                 & 0x000F) | (r8 << 4));
3754                 b43_radio_write16(dev, 0x002A, (r8 << 4));
3755                 b43_radio_write16(dev, 0x002B, (r8 << 4));
3756                 b43_radio_write16(dev, 0x0008, (b43_radio_read16(dev, 0x0008)
3757                                                 & 0x00F0) | (r8 << 4));
3758                 b43_radio_write16(dev, 0x0029, (b43_radio_read16(dev, 0x0029)
3759                                                 & 0xFF0F) | 0x00B0);
3760                 b43_radio_write16(dev, 0x0035, 0x00AA);
3761                 b43_radio_write16(dev, 0x0036, 0x0085);
3762                 b43_radio_write16(dev, 0x003A, (b43_radio_read16(dev, 0x003A)
3763                                                 & 0xFF20) |
3764                                   freq_r3A_value(freq));
3765                 b43_radio_write16(dev, 0x003D,
3766                                   b43_radio_read16(dev, 0x003D) & 0x00FF);
3767                 b43_radio_write16(dev, 0x0081, (b43_radio_read16(dev, 0x0081)
3768                                                 & 0xFF7F) | 0x0080);
3769                 b43_radio_write16(dev, 0x0035,
3770                                   b43_radio_read16(dev, 0x0035) & 0xFFEF);
3771                 b43_radio_write16(dev, 0x0035, (b43_radio_read16(dev, 0x0035)
3772                                                 & 0xFFEF) | 0x0010);
3773                 b43_radio_set_tx_iq(dev);
3774                 //TODO: TSSI2dbm workaround
3775                 b43_phy_xmitpower(dev); //FIXME correct?
3776                 break;
3777         case B43_PHYTYPE_G:
3778                 if ((channel < 1) || (channel > 14)) {
3779                         err = -EINVAL;
3780                         goto out;
3781                 }
3782
3783                 if (synthetic_pu_workaround)
3784                         b43_synth_pu_workaround(dev, channel);
3785
3786                 b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
3787
3788                 if (channel == 14) {
3789                         if (dev->dev->bus->sprom.country_code ==
3790                             SSB_SPROM1CCODE_JAPAN)
3791                                 b43_hf_write(dev,
3792                                              b43_hf_read(dev) & ~B43_HF_ACPR);
3793                         else
3794                                 b43_hf_write(dev,
3795                                              b43_hf_read(dev) | B43_HF_ACPR);
3796                         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
3797                                     b43_read16(dev, B43_MMIO_CHANNEL_EXT)
3798                                     | (1 << 11));
3799                 } else {
3800                         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
3801                                     b43_read16(dev, B43_MMIO_CHANNEL_EXT)
3802                                     & 0xF7BF);
3803                 }
3804                 break;
3805         case B43_PHYTYPE_N:
3806                 err = b43_nphy_selectchannel(dev, channel);
3807                 if (err)
3808                         goto out;
3809                 break;
3810         default:
3811                 B43_WARN_ON(1);
3812         }
3813
3814         phy->channel = channel;
3815         /* Wait for the radio to tune to the channel and stabilize. */
3816         msleep(8);
3817 out:
3818         if (err) {
3819                 b43_shm_write16(dev, B43_SHM_SHARED,
3820                                 B43_SHM_SH_CHAN, savedcookie);
3821         }
3822         return err;
3823 }
3824
3825 void b43_radio_turn_on(struct b43_wldev *dev)
3826 {
3827         struct b43_phy *phy = &dev->phy;
3828         int err;
3829         u8 channel;
3830
3831         might_sleep();
3832
3833         if (phy->radio_on)
3834                 return;
3835
3836         switch (phy->type) {
3837         case B43_PHYTYPE_A:
3838                 b43_radio_write16(dev, 0x0004, 0x00C0);
3839                 b43_radio_write16(dev, 0x0005, 0x0008);
3840                 b43_phy_write(dev, 0x0010, b43_phy_read(dev, 0x0010) & 0xFFF7);
3841                 b43_phy_write(dev, 0x0011, b43_phy_read(dev, 0x0011) & 0xFFF7);
3842                 b43_radio_init2060(dev);
3843                 break;
3844         case B43_PHYTYPE_B:
3845         case B43_PHYTYPE_G:
3846                 b43_phy_write(dev, 0x0015, 0x8000);
3847                 b43_phy_write(dev, 0x0015, 0xCC00);
3848                 b43_phy_write(dev, 0x0015, (phy->gmode ? 0x00C0 : 0x0000));
3849                 if (phy->radio_off_context.valid) {
3850                         /* Restore the RFover values. */
3851                         b43_phy_write(dev, B43_PHY_RFOVER,
3852                                       phy->radio_off_context.rfover);
3853                         b43_phy_write(dev, B43_PHY_RFOVERVAL,
3854                                       phy->radio_off_context.rfoverval);
3855                         phy->radio_off_context.valid = 0;
3856                 }
3857                 channel = phy->channel;
3858                 err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_BG, 1);
3859                 err |= b43_radio_selectchannel(dev, channel, 0);
3860                 B43_WARN_ON(err);
3861                 break;
3862         case B43_PHYTYPE_N:
3863                 b43_nphy_radio_turn_on(dev);
3864                 break;
3865         default:
3866                 B43_WARN_ON(1);
3867         }
3868         phy->radio_on = 1;
3869 }
3870
3871 void b43_radio_turn_off(struct b43_wldev *dev, bool force)
3872 {
3873         struct b43_phy *phy = &dev->phy;
3874
3875         if (!phy->radio_on && !force)
3876                 return;
3877
3878         switch (phy->type) {
3879         case B43_PHYTYPE_N:
3880                 b43_nphy_radio_turn_off(dev);
3881                 break;
3882         case B43_PHYTYPE_A:
3883                 b43_radio_write16(dev, 0x0004, 0x00FF);
3884                 b43_radio_write16(dev, 0x0005, 0x00FB);
3885                 b43_phy_write(dev, 0x0010, b43_phy_read(dev, 0x0010) | 0x0008);
3886                 b43_phy_write(dev, 0x0011, b43_phy_read(dev, 0x0011) | 0x0008);
3887                 break;
3888         case B43_PHYTYPE_G: {
3889                 u16 rfover, rfoverval;
3890
3891                 rfover = b43_phy_read(dev, B43_PHY_RFOVER);
3892                 rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
3893                 if (!force) {
3894                         phy->radio_off_context.rfover = rfover;
3895                         phy->radio_off_context.rfoverval = rfoverval;
3896                         phy->radio_off_context.valid = 1;
3897                 }
3898                 b43_phy_write(dev, B43_PHY_RFOVER, rfover | 0x008C);
3899                 b43_phy_write(dev, B43_PHY_RFOVERVAL, rfoverval & 0xFF73);
3900                 break;
3901         }
3902         default:
3903                 B43_WARN_ON(1);
3904         }
3905         phy->radio_on = 0;
3906 }