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