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