drivers/net/wireless/broadcom/b43legacy/phy.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3
   4   Broadcom B43legacy wireless driver
   5
   6   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
   7                      Stefano Brivio <stefano.brivio@polimi.it>
   8                      Michael Buesch <m@bues.ch>
   9                      Danny van Dyk <kugelfang@gentoo.org>
  10      Andreas Jaggi <andreas.jaggi@waterwave.ch>
  11   Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
  12
  13   Some parts of the code in this file are derived from the ipw2200
  14   driver  Copyright(c) 2003 - 2004 Intel Corporation.
  15
  16
  17 */
  18
  19 #include <linux/delay.h>
  20 #include <linux/pci.h>
  21 #include <linux/sched.h>
  22 #include <linux/slab.h>
  23 #include <linux/types.h>
  24
  25 #include "b43legacy.h"
  26 #include "phy.h"
  27 #include "main.h"
  28 #include "radio.h"
  29 #include "ilt.h"
  30
  31
  32 static const s8 b43legacy_tssi2dbm_b_table[] = {
  33         0x4D, 0x4C, 0x4B, 0x4A,
  34         0x4A, 0x49, 0x48, 0x47,
  35         0x47, 0x46, 0x45, 0x45,
  36         0x44, 0x43, 0x42, 0x42,
  37         0x41, 0x40, 0x3F, 0x3E,
  38         0x3D, 0x3C, 0x3B, 0x3A,
  39         0x39, 0x38, 0x37, 0x36,
  40         0x35, 0x34, 0x32, 0x31,
  41         0x30, 0x2F, 0x2D, 0x2C,
  42         0x2B, 0x29, 0x28, 0x26,
  43         0x25, 0x23, 0x21, 0x1F,
  44         0x1D, 0x1A, 0x17, 0x14,
  45         0x10, 0x0C, 0x06, 0x00,
  46           -7,   -7,   -7,   -7,
  47           -7,   -7,   -7,   -7,
  48           -7,   -7,   -7,   -7,
  49 };
  50
  51 static const s8 b43legacy_tssi2dbm_g_table[] = {
  52          77,  77,  77,  76,
  53          76,  76,  75,  75,
  54          74,  74,  73,  73,
  55          73,  72,  72,  71,
  56          71,  70,  70,  69,
  57          68,  68,  67,  67,
  58          66,  65,  65,  64,
  59          63,  63,  62,  61,
  60          60,  59,  58,  57,
  61          56,  55,  54,  53,
  62          52,  50,  49,  47,
  63          45,  43,  40,  37,
  64          33,  28,  22,  14,
  65           5,  -7, -20, -20,
  66         -20, -20, -20, -20,
  67         -20, -20, -20, -20,
  68 };
  69
  70 static void b43legacy_phy_initg(struct b43legacy_wldev *dev);
  71
  72
  73 static inline
  74 void b43legacy_voluntary_preempt(void)
  75 {
  76         B43legacy_BUG_ON(!(!in_atomic() && !in_irq() &&
  77                           !in_interrupt() && !irqs_disabled()));
  78 #ifndef CONFIG_PREEMPT
  79         cond_resched();
  80 #endif /* CONFIG_PREEMPT */
  81 }
  82
  83 /* Lock the PHY registers against concurrent access from the microcode.
  84  * This lock is nonrecursive. */
  85 void b43legacy_phy_lock(struct b43legacy_wldev *dev)
  86 {
  87 #if B43legacy_DEBUG
  88         B43legacy_WARN_ON(dev->phy.phy_locked);
  89         dev->phy.phy_locked = 1;
  90 #endif
  91
  92         if (dev->dev->id.revision < 3) {
  93                 b43legacy_mac_suspend(dev);
  94         } else {
  95                 if (!b43legacy_is_mode(dev->wl, NL80211_IFTYPE_AP))
  96                         b43legacy_power_saving_ctl_bits(dev, -1, 1);
  97         }
  98 }
  99
 100 void b43legacy_phy_unlock(struct b43legacy_wldev *dev)
 101 {
 102 #if B43legacy_DEBUG
 103         B43legacy_WARN_ON(!dev->phy.phy_locked);
 104         dev->phy.phy_locked = 0;
 105 #endif
 106
 107         if (dev->dev->id.revision < 3) {
 108                 b43legacy_mac_enable(dev);
 109         } else {
 110                 if (!b43legacy_is_mode(dev->wl, NL80211_IFTYPE_AP))
 111                         b43legacy_power_saving_ctl_bits(dev, -1, -1);
 112         }
 113 }
 114
 115 u16 b43legacy_phy_read(struct b43legacy_wldev *dev, u16 offset)
 116 {
 117         b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
 118         return b43legacy_read16(dev, B43legacy_MMIO_PHY_DATA);
 119 }
 120
 121 void b43legacy_phy_write(struct b43legacy_wldev *dev, u16 offset, u16 val)
 122 {
 123         b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
 124         b43legacy_write16(dev, B43legacy_MMIO_PHY_DATA, val);
 125 }
 126
 127 void b43legacy_phy_calibrate(struct b43legacy_wldev *dev)
 128 {
 129         struct b43legacy_phy *phy = &dev->phy;
 130
 131         b43legacy_read32(dev, B43legacy_MMIO_MACCTL); /* Dummy read. */
 132         if (phy->calibrated)
 133                 return;
 134         if (phy->type == B43legacy_PHYTYPE_G && phy->rev == 1) {
 135                 b43legacy_wireless_core_reset(dev, 0);
 136                 b43legacy_phy_initg(dev);
 137                 b43legacy_wireless_core_reset(dev, B43legacy_TMSLOW_GMODE);
 138         }
 139         phy->calibrated = 1;
 140 }
 141
 142 /* initialize B PHY power control
 143  * as described in http://bcm-specs.sipsolutions.net/InitPowerControl
 144  */
 145 static void b43legacy_phy_init_pctl(struct b43legacy_wldev *dev)
 146 {
 147         struct b43legacy_phy *phy = &dev->phy;
 148         u16 saved_batt = 0;
 149         u16 saved_ratt = 0;
 150         u16 saved_txctl1 = 0;
 151         int must_reset_txpower = 0;
 152
 153         B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
 154                           phy->type == B43legacy_PHYTYPE_G));
 155         if (is_bcm_board_vendor(dev) &&
 156             (dev->dev->bus->boardinfo.type == 0x0416))
 157                 return;
 158
 159         b43legacy_phy_write(dev, 0x0028, 0x8018);
 160         b43legacy_write16(dev, 0x03E6, b43legacy_read16(dev, 0x03E6) & 0xFFDF);
 161
 162         if (phy->type == B43legacy_PHYTYPE_G) {
 163                 if (!phy->gmode)
 164                         return;
 165                 b43legacy_phy_write(dev, 0x047A, 0xC111);
 166         }
 167         if (phy->savedpctlreg != 0xFFFF)
 168                 return;
 169 #ifdef CONFIG_B43LEGACY_DEBUG
 170         if (phy->manual_txpower_control)
 171                 return;
 172 #endif
 173
 174         if (phy->type == B43legacy_PHYTYPE_B &&
 175             phy->rev >= 2 &&
 176             phy->radio_ver == 0x2050)
 177                 b43legacy_radio_write16(dev, 0x0076,
 178                                         b43legacy_radio_read16(dev, 0x0076)
 179                                         | 0x0084);
 180         else {
 181                 saved_batt = phy->bbatt;
 182                 saved_ratt = phy->rfatt;
 183                 saved_txctl1 = phy->txctl1;
 184                 if ((phy->radio_rev >= 6) && (phy->radio_rev <= 8)
 185                     && /*FIXME: incomplete specs for 5 < revision < 9 */ 0)
 186                         b43legacy_radio_set_txpower_bg(dev, 0xB, 0x1F, 0);
 187                 else
 188                         b43legacy_radio_set_txpower_bg(dev, 0xB, 9, 0);
 189                 must_reset_txpower = 1;
 190         }
 191         b43legacy_dummy_transmission(dev);
 192
 193         phy->savedpctlreg = b43legacy_phy_read(dev, B43legacy_PHY_G_PCTL);
 194
 195         if (must_reset_txpower)
 196                 b43legacy_radio_set_txpower_bg(dev, saved_batt, saved_ratt,
 197                                                saved_txctl1);
 198         else
 199                 b43legacy_radio_write16(dev, 0x0076, b43legacy_radio_read16(dev,
 200                                         0x0076) & 0xFF7B);
 201         b43legacy_radio_clear_tssi(dev);
 202 }
 203
 204 static void b43legacy_phy_agcsetup(struct b43legacy_wldev *dev)
 205 {
 206         struct b43legacy_phy *phy = &dev->phy;
 207         u16 offset = 0x0000;
 208
 209         if (phy->rev == 1)
 210                 offset = 0x4C00;
 211
 212         b43legacy_ilt_write(dev, offset, 0x00FE);
 213         b43legacy_ilt_write(dev, offset + 1, 0x000D);
 214         b43legacy_ilt_write(dev, offset + 2, 0x0013);
 215         b43legacy_ilt_write(dev, offset + 3, 0x0019);
 216
 217         if (phy->rev == 1) {
 218                 b43legacy_ilt_write(dev, 0x1800, 0x2710);
 219                 b43legacy_ilt_write(dev, 0x1801, 0x9B83);
 220                 b43legacy_ilt_write(dev, 0x1802, 0x9B83);
 221                 b43legacy_ilt_write(dev, 0x1803, 0x0F8D);
 222                 b43legacy_phy_write(dev, 0x0455, 0x0004);
 223         }
 224
 225         b43legacy_phy_write(dev, 0x04A5, (b43legacy_phy_read(dev, 0x04A5)
 226                                           & 0x00FF) | 0x5700);
 227         b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
 228                                           & 0xFF80) | 0x000F);
 229         b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
 230                                           & 0xC07F) | 0x2B80);
 231         b43legacy_phy_write(dev, 0x048C, (b43legacy_phy_read(dev, 0x048C)
 232                                           & 0xF0FF) | 0x0300);
 233
 234         b43legacy_radio_write16(dev, 0x007A,
 235                                 b43legacy_radio_read16(dev, 0x007A)
 236                                 | 0x0008);
 237
 238         b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
 239                             & 0xFFF0) | 0x0008);
 240         b43legacy_phy_write(dev, 0x04A1, (b43legacy_phy_read(dev, 0x04A1)
 241                             & 0xF0FF) | 0x0600);
 242         b43legacy_phy_write(dev, 0x04A2, (b43legacy_phy_read(dev, 0x04A2)
 243                             & 0xF0FF) | 0x0700);
 244         b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
 245                             & 0xF0FF) | 0x0100);
 246
 247         if (phy->rev == 1)
 248                 b43legacy_phy_write(dev, 0x04A2,
 249                                     (b43legacy_phy_read(dev, 0x04A2)
 250                                     & 0xFFF0) | 0x0007);
 251
 252         b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
 253                             & 0xFF00) | 0x001C);
 254         b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
 255                             & 0xC0FF) | 0x0200);
 256         b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
 257                             & 0xFF00) | 0x001C);
 258         b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
 259                             & 0xFF00) | 0x0020);
 260         b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
 261                             & 0xC0FF) | 0x0200);
 262         b43legacy_phy_write(dev, 0x0482, (b43legacy_phy_read(dev, 0x0482)
 263                             & 0xFF00) | 0x002E);
 264         b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
 265                             & 0x00FF) | 0x1A00);
 266         b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
 267                             & 0xFF00) | 0x0028);
 268         b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
 269                             & 0x00FF) | 0x2C00);
 270
 271         if (phy->rev == 1) {
 272                 b43legacy_phy_write(dev, 0x0430, 0x092B);
 273                 b43legacy_phy_write(dev, 0x041B,
 274                                     (b43legacy_phy_read(dev, 0x041B)
 275                                     & 0xFFE1) | 0x0002);
 276         } else {
 277                 b43legacy_phy_write(dev, 0x041B,
 278                                     b43legacy_phy_read(dev, 0x041B) & 0xFFE1);
 279                 b43legacy_phy_write(dev, 0x041F, 0x287A);
 280                 b43legacy_phy_write(dev, 0x0420,
 281                                     (b43legacy_phy_read(dev, 0x0420)
 282                                     & 0xFFF0) | 0x0004);
 283         }
 284
 285         if (phy->rev > 2) {
 286                 b43legacy_phy_write(dev, 0x0422, 0x287A);
 287                 b43legacy_phy_write(dev, 0x0420,
 288                                     (b43legacy_phy_read(dev, 0x0420)
 289                                     & 0x0FFF) | 0x3000);
 290         }
 291
 292         b43legacy_phy_write(dev, 0x04A8, (b43legacy_phy_read(dev, 0x04A8)
 293                             & 0x8080) | 0x7874);
 294         b43legacy_phy_write(dev, 0x048E, 0x1C00);
 295
 296         if (phy->rev == 1) {
 297                 b43legacy_phy_write(dev, 0x04AB,
 298                                     (b43legacy_phy_read(dev, 0x04AB)
 299                                     & 0xF0FF) | 0x0600);
 300                 b43legacy_phy_write(dev, 0x048B, 0x005E);
 301                 b43legacy_phy_write(dev, 0x048C,
 302                                     (b43legacy_phy_read(dev, 0x048C) & 0xFF00)
 303                                     | 0x001E);
 304                 b43legacy_phy_write(dev, 0x048D, 0x0002);
 305         }
 306
 307         b43legacy_ilt_write(dev, offset + 0x0800, 0);
 308         b43legacy_ilt_write(dev, offset + 0x0801, 7);
 309         b43legacy_ilt_write(dev, offset + 0x0802, 16);
 310         b43legacy_ilt_write(dev, offset + 0x0803, 28);
 311
 312         if (phy->rev >= 6) {
 313                 b43legacy_phy_write(dev, 0x0426,
 314                                     (b43legacy_phy_read(dev, 0x0426) & 0xFFFC));
 315                 b43legacy_phy_write(dev, 0x0426,
 316                                     (b43legacy_phy_read(dev, 0x0426) & 0xEFFF));
 317         }
 318 }
 319
 320 static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
 321 {
 322         struct b43legacy_phy *phy = &dev->phy;
 323         u16 i;
 324
 325         B43legacy_BUG_ON(phy->type != B43legacy_PHYTYPE_G);
 326         if (phy->rev == 1) {
 327                 b43legacy_phy_write(dev, 0x0406, 0x4F19);
 328                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
 329                                     (b43legacy_phy_read(dev,
 330                                     B43legacy_PHY_G_CRS) & 0xFC3F) | 0x0340);
 331                 b43legacy_phy_write(dev, 0x042C, 0x005A);
 332                 b43legacy_phy_write(dev, 0x0427, 0x001A);
 333
 334                 for (i = 0; i < B43legacy_ILT_FINEFREQG_SIZE; i++)
 335                         b43legacy_ilt_write(dev, 0x5800 + i,
 336                                             b43legacy_ilt_finefreqg[i]);
 337                 for (i = 0; i < B43legacy_ILT_NOISEG1_SIZE; i++)
 338                         b43legacy_ilt_write(dev, 0x1800 + i,
 339                                             b43legacy_ilt_noiseg1[i]);
 340                 for (i = 0; i < B43legacy_ILT_ROTOR_SIZE; i++)
 341                         b43legacy_ilt_write32(dev, 0x2000 + i,
 342                                               b43legacy_ilt_rotor[i]);
 343         } else {
 344                 /* nrssi values are signed 6-bit values. Why 0x7654 here? */
 345                 b43legacy_nrssi_hw_write(dev, 0xBA98, (s16)0x7654);
 346
 347                 if (phy->rev == 2) {
 348                         b43legacy_phy_write(dev, 0x04C0, 0x1861);
 349                         b43legacy_phy_write(dev, 0x04C1, 0x0271);
 350                 } else if (phy->rev > 2) {
 351                         b43legacy_phy_write(dev, 0x04C0, 0x0098);
 352                         b43legacy_phy_write(dev, 0x04C1, 0x0070);
 353                         b43legacy_phy_write(dev, 0x04C9, 0x0080);
 354                 }
 355                 b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev,
 356                                     0x042B) | 0x800);
 357
 358                 for (i = 0; i < 64; i++)
 359                         b43legacy_ilt_write(dev, 0x4000 + i, i);
 360                 for (i = 0; i < B43legacy_ILT_NOISEG2_SIZE; i++)
 361                         b43legacy_ilt_write(dev, 0x1800 + i,
 362                                             b43legacy_ilt_noiseg2[i]);
 363         }
 364
 365         if (phy->rev <= 2)
 366                 for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
 367                         b43legacy_ilt_write(dev, 0x1400 + i,
 368                                             b43legacy_ilt_noisescaleg1[i]);
 369         else if ((phy->rev >= 7) && (b43legacy_phy_read(dev, 0x0449) & 0x0200))
 370                 for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
 371                         b43legacy_ilt_write(dev, 0x1400 + i,
 372                                             b43legacy_ilt_noisescaleg3[i]);
 373         else
 374                 for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
 375                         b43legacy_ilt_write(dev, 0x1400 + i,
 376                                             b43legacy_ilt_noisescaleg2[i]);
 377
 378         if (phy->rev == 2)
 379                 for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
 380                         b43legacy_ilt_write(dev, 0x5000 + i,
 381                                             b43legacy_ilt_sigmasqr1[i]);
 382         else if ((phy->rev > 2) && (phy->rev <= 8))
 383                 for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
 384                         b43legacy_ilt_write(dev, 0x5000 + i,
 385                                             b43legacy_ilt_sigmasqr2[i]);
 386
 387         if (phy->rev == 1) {
 388                 for (i = 0; i < B43legacy_ILT_RETARD_SIZE; i++)
 389                         b43legacy_ilt_write32(dev, 0x2400 + i,
 390                                               b43legacy_ilt_retard[i]);
 391                 for (i = 4; i < 20; i++)
 392                         b43legacy_ilt_write(dev, 0x5400 + i, 0x0020);
 393                 b43legacy_phy_agcsetup(dev);
 394
 395                 if (is_bcm_board_vendor(dev) &&
 396                     (dev->dev->bus->boardinfo.type == 0x0416) &&
 397                     (dev->dev->bus->sprom.board_rev == 0x0017))
 398                         return;
 399
 400                 b43legacy_ilt_write(dev, 0x5001, 0x0002);
 401                 b43legacy_ilt_write(dev, 0x5002, 0x0001);
 402         } else {
 403                 for (i = 0; i <= 0x20; i++)
 404                         b43legacy_ilt_write(dev, 0x1000 + i, 0x0820);
 405                 b43legacy_phy_agcsetup(dev);
 406                 b43legacy_phy_read(dev, 0x0400); /* dummy read */
 407                 b43legacy_phy_write(dev, 0x0403, 0x1000);
 408                 b43legacy_ilt_write(dev, 0x3C02, 0x000F);
 409                 b43legacy_ilt_write(dev, 0x3C03, 0x0014);
 410
 411                 if (is_bcm_board_vendor(dev) &&
 412                     (dev->dev->bus->boardinfo.type == 0x0416) &&
 413                     (dev->dev->bus->sprom.board_rev == 0x0017))
 414                         return;
 415
 416                 b43legacy_ilt_write(dev, 0x0401, 0x0002);
 417                 b43legacy_ilt_write(dev, 0x0402, 0x0001);
 418         }
 419 }
 420
 421 /* Initialize the APHY portion of a GPHY. */
 422 static void b43legacy_phy_inita(struct b43legacy_wldev *dev)
 423 {
 424
 425         might_sleep();
 426
 427         b43legacy_phy_setupg(dev);
 428         if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_PACTRL)
 429                 b43legacy_phy_write(dev, 0x046E, 0x03CF);
 430 }
 431
 432 static void b43legacy_phy_initb2(struct b43legacy_wldev *dev)
 433 {
 434         struct b43legacy_phy *phy = &dev->phy;
 435         u16 offset;
 436         int val;
 437
 438         b43legacy_write16(dev, 0x03EC, 0x3F22);
 439         b43legacy_phy_write(dev, 0x0020, 0x301C);
 440         b43legacy_phy_write(dev, 0x0026, 0x0000);
 441         b43legacy_phy_write(dev, 0x0030, 0x00C6);
 442         b43legacy_phy_write(dev, 0x0088, 0x3E00);
 443         val = 0x3C3D;
 444         for (offset = 0x0089; offset < 0x00A7; offset++) {
 445                 b43legacy_phy_write(dev, offset, val);
 446                 val -= 0x0202;
 447         }
 448         b43legacy_phy_write(dev, 0x03E4, 0x3000);
 449         b43legacy_radio_selectchannel(dev, phy->channel, 0);
 450         if (phy->radio_ver != 0x2050) {
 451                 b43legacy_radio_write16(dev, 0x0075, 0x0080);
 452                 b43legacy_radio_write16(dev, 0x0079, 0x0081);
 453         }
 454         b43legacy_radio_write16(dev, 0x0050, 0x0020);
 455         b43legacy_radio_write16(dev, 0x0050, 0x0023);
 456         if (phy->radio_ver == 0x2050) {
 457                 b43legacy_radio_write16(dev, 0x0050, 0x0020);
 458                 b43legacy_radio_write16(dev, 0x005A, 0x0070);
 459                 b43legacy_radio_write16(dev, 0x005B, 0x007B);
 460                 b43legacy_radio_write16(dev, 0x005C, 0x00B0);
 461                 b43legacy_radio_write16(dev, 0x007A, 0x000F);
 462                 b43legacy_phy_write(dev, 0x0038, 0x0677);
 463                 b43legacy_radio_init2050(dev);
 464         }
 465         b43legacy_phy_write(dev, 0x0014, 0x0080);
 466         b43legacy_phy_write(dev, 0x0032, 0x00CA);
 467         b43legacy_phy_write(dev, 0x0032, 0x00CC);
 468         b43legacy_phy_write(dev, 0x0035, 0x07C2);
 469         b43legacy_phy_lo_b_measure(dev);
 470         b43legacy_phy_write(dev, 0x0026, 0xCC00);
 471         if (phy->radio_ver != 0x2050)
 472                 b43legacy_phy_write(dev, 0x0026, 0xCE00);
 473         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1000);
 474         b43legacy_phy_write(dev, 0x002A, 0x88A3);
 475         if (phy->radio_ver != 0x2050)
 476                 b43legacy_phy_write(dev, 0x002A, 0x88C2);
 477         b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
 478         b43legacy_phy_init_pctl(dev);
 479 }
 480
 481 static void b43legacy_phy_initb4(struct b43legacy_wldev *dev)
 482 {
 483         struct b43legacy_phy *phy = &dev->phy;
 484         u16 offset;
 485         u16 val;
 486
 487         b43legacy_write16(dev, 0x03EC, 0x3F22);
 488         b43legacy_phy_write(dev, 0x0020, 0x301C);
 489         b43legacy_phy_write(dev, 0x0026, 0x0000);
 490         b43legacy_phy_write(dev, 0x0030, 0x00C6);
 491         b43legacy_phy_write(dev, 0x0088, 0x3E00);
 492         val = 0x3C3D;
 493         for (offset = 0x0089; offset < 0x00A7; offset++) {
 494                 b43legacy_phy_write(dev, offset, val);
 495                 val -= 0x0202;
 496         }
 497         b43legacy_phy_write(dev, 0x03E4, 0x3000);
 498         b43legacy_radio_selectchannel(dev, phy->channel, 0);
 499         if (phy->radio_ver != 0x2050) {
 500                 b43legacy_radio_write16(dev, 0x0075, 0x0080);
 501                 b43legacy_radio_write16(dev, 0x0079, 0x0081);
 502         }
 503         b43legacy_radio_write16(dev, 0x0050, 0x0020);
 504         b43legacy_radio_write16(dev, 0x0050, 0x0023);
 505         if (phy->radio_ver == 0x2050) {
 506                 b43legacy_radio_write16(dev, 0x0050, 0x0020);
 507                 b43legacy_radio_write16(dev, 0x005A, 0x0070);
 508                 b43legacy_radio_write16(dev, 0x005B, 0x007B);
 509                 b43legacy_radio_write16(dev, 0x005C, 0x00B0);
 510                 b43legacy_radio_write16(dev, 0x007A, 0x000F);
 511                 b43legacy_phy_write(dev, 0x0038, 0x0677);
 512                 b43legacy_radio_init2050(dev);
 513         }
 514         b43legacy_phy_write(dev, 0x0014, 0x0080);
 515         b43legacy_phy_write(dev, 0x0032, 0x00CA);
 516         if (phy->radio_ver == 0x2050)
 517                 b43legacy_phy_write(dev, 0x0032, 0x00E0);
 518         b43legacy_phy_write(dev, 0x0035, 0x07C2);
 519
 520         b43legacy_phy_lo_b_measure(dev);
 521
 522         b43legacy_phy_write(dev, 0x0026, 0xCC00);
 523         if (phy->radio_ver == 0x2050)
 524                 b43legacy_phy_write(dev, 0x0026, 0xCE00);
 525         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1100);
 526         b43legacy_phy_write(dev, 0x002A, 0x88A3);
 527         if (phy->radio_ver == 0x2050)
 528                 b43legacy_phy_write(dev, 0x002A, 0x88C2);
 529         b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
 530         if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_RSSI) {
 531                 b43legacy_calc_nrssi_slope(dev);
 532                 b43legacy_calc_nrssi_threshold(dev);
 533         }
 534         b43legacy_phy_init_pctl(dev);
 535 }
 536
 537 static void b43legacy_phy_initb5(struct b43legacy_wldev *dev)
 538 {
 539         struct b43legacy_phy *phy = &dev->phy;
 540         u16 offset;
 541         u16 value;
 542         u8 old_channel;
 543
 544         if (phy->analog == 1)
 545                 b43legacy_radio_write16(dev, 0x007A,
 546                                         b43legacy_radio_read16(dev, 0x007A)
 547                                         | 0x0050);
 548         if (!is_bcm_board_vendor(dev) &&
 549             (dev->dev->bus->boardinfo.type != 0x0416)) {
 550                 value = 0x2120;
 551                 for (offset = 0x00A8 ; offset < 0x00C7; offset++) {
 552                         b43legacy_phy_write(dev, offset, value);
 553                         value += 0x0202;
 554                 }
 555         }
 556         b43legacy_phy_write(dev, 0x0035,
 557                             (b43legacy_phy_read(dev, 0x0035) & 0xF0FF)
 558                             | 0x0700);
 559         if (phy->radio_ver == 0x2050)
 560                 b43legacy_phy_write(dev, 0x0038, 0x0667);
 561
 562         if (phy->gmode) {
 563                 if (phy->radio_ver == 0x2050) {
 564                         b43legacy_radio_write16(dev, 0x007A,
 565                                         b43legacy_radio_read16(dev, 0x007A)
 566                                         | 0x0020);
 567                         b43legacy_radio_write16(dev, 0x0051,
 568                                         b43legacy_radio_read16(dev, 0x0051)
 569                                         | 0x0004);
 570                 }
 571                 b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO, 0x0000);
 572
 573                 b43legacy_phy_write(dev, 0x0802, b43legacy_phy_read(dev, 0x0802)
 574                                     | 0x0100);
 575                 b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev, 0x042B)
 576                                     | 0x2000);
 577
 578                 b43legacy_phy_write(dev, 0x001C, 0x186A);
 579
 580                 b43legacy_phy_write(dev, 0x0013, (b43legacy_phy_read(dev,
 581                                     0x0013) & 0x00FF) | 0x1900);
 582                 b43legacy_phy_write(dev, 0x0035, (b43legacy_phy_read(dev,
 583                                     0x0035) & 0xFFC0) | 0x0064);
 584                 b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
 585                                     0x005D) & 0xFF80) | 0x000A);
 586                 b43legacy_phy_write(dev, 0x5B, 0x0000);
 587                 b43legacy_phy_write(dev, 0x5C, 0x0000);
 588         }
 589
 590         if (dev->bad_frames_preempt)
 591                 b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
 592                                     b43legacy_phy_read(dev,
 593                                     B43legacy_PHY_RADIO_BITFIELD) | (1 << 12));
 594
 595         if (phy->analog == 1) {
 596                 b43legacy_phy_write(dev, 0x0026, 0xCE00);
 597                 b43legacy_phy_write(dev, 0x0021, 0x3763);
 598                 b43legacy_phy_write(dev, 0x0022, 0x1BC3);
 599                 b43legacy_phy_write(dev, 0x0023, 0x06F9);
 600                 b43legacy_phy_write(dev, 0x0024, 0x037E);
 601         } else
 602                 b43legacy_phy_write(dev, 0x0026, 0xCC00);
 603         b43legacy_phy_write(dev, 0x0030, 0x00C6);
 604         b43legacy_write16(dev, 0x03EC, 0x3F22);
 605
 606         if (phy->analog == 1)
 607                 b43legacy_phy_write(dev, 0x0020, 0x3E1C);
 608         else
 609                 b43legacy_phy_write(dev, 0x0020, 0x301C);
 610
 611         if (phy->analog == 0)
 612                 b43legacy_write16(dev, 0x03E4, 0x3000);
 613
 614         old_channel = (phy->channel == 0xFF) ? 1 : phy->channel;
 615         /* Force to channel 7, even if not supported. */
 616         b43legacy_radio_selectchannel(dev, 7, 0);
 617
 618         if (phy->radio_ver != 0x2050) {
 619                 b43legacy_radio_write16(dev, 0x0075, 0x0080);
 620                 b43legacy_radio_write16(dev, 0x0079, 0x0081);
 621         }
 622
 623         b43legacy_radio_write16(dev, 0x0050, 0x0020);
 624         b43legacy_radio_write16(dev, 0x0050, 0x0023);
 625
 626         if (phy->radio_ver == 0x2050) {
 627                 b43legacy_radio_write16(dev, 0x0050, 0x0020);
 628                 b43legacy_radio_write16(dev, 0x005A, 0x0070);
 629         }
 630
 631         b43legacy_radio_write16(dev, 0x005B, 0x007B);
 632         b43legacy_radio_write16(dev, 0x005C, 0x00B0);
 633
 634         b43legacy_radio_write16(dev, 0x007A, b43legacy_radio_read16(dev,
 635                                 0x007A) | 0x0007);
 636
 637         b43legacy_radio_selectchannel(dev, old_channel, 0);
 638
 639         b43legacy_phy_write(dev, 0x0014, 0x0080);
 640         b43legacy_phy_write(dev, 0x0032, 0x00CA);
 641         b43legacy_phy_write(dev, 0x002A, 0x88A3);
 642
 643         b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
 644
 645         if (phy->radio_ver == 0x2050)
 646                 b43legacy_radio_write16(dev, 0x005D, 0x000D);
 647
 648         b43legacy_write16(dev, 0x03E4, (b43legacy_read16(dev, 0x03E4) &
 649                           0xFFC0) | 0x0004);
 650 }
 651
 652 static void b43legacy_phy_initb6(struct b43legacy_wldev *dev)
 653 {
 654         struct b43legacy_phy *phy = &dev->phy;
 655         u16 offset;
 656         u16 val;
 657         u8 old_channel;
 658
 659         b43legacy_phy_write(dev, 0x003E, 0x817A);
 660         b43legacy_radio_write16(dev, 0x007A,
 661                                 (b43legacy_radio_read16(dev, 0x007A) | 0x0058));
 662         if (phy->radio_rev == 4 ||
 663              phy->radio_rev == 5) {
 664                 b43legacy_radio_write16(dev, 0x0051, 0x0037);
 665                 b43legacy_radio_write16(dev, 0x0052, 0x0070);
 666                 b43legacy_radio_write16(dev, 0x0053, 0x00B3);
 667                 b43legacy_radio_write16(dev, 0x0054, 0x009B);
 668                 b43legacy_radio_write16(dev, 0x005A, 0x0088);
 669                 b43legacy_radio_write16(dev, 0x005B, 0x0088);
 670                 b43legacy_radio_write16(dev, 0x005D, 0x0088);
 671                 b43legacy_radio_write16(dev, 0x005E, 0x0088);
 672                 b43legacy_radio_write16(dev, 0x007D, 0x0088);
 673                 b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
 674                                       B43legacy_UCODEFLAGS_OFFSET,
 675                                       (b43legacy_shm_read32(dev,
 676                                       B43legacy_SHM_SHARED,
 677                                       B43legacy_UCODEFLAGS_OFFSET)
 678                                       | 0x00000200));
 679         }
 680         if (phy->radio_rev == 8) {
 681                 b43legacy_radio_write16(dev, 0x0051, 0x0000);
 682                 b43legacy_radio_write16(dev, 0x0052, 0x0040);
 683                 b43legacy_radio_write16(dev, 0x0053, 0x00B7);
 684                 b43legacy_radio_write16(dev, 0x0054, 0x0098);
 685                 b43legacy_radio_write16(dev, 0x005A, 0x0088);
 686                 b43legacy_radio_write16(dev, 0x005B, 0x006B);
 687                 b43legacy_radio_write16(dev, 0x005C, 0x000F);
 688                 if (dev->dev->bus->sprom.boardflags_lo & 0x8000) {
 689                         b43legacy_radio_write16(dev, 0x005D, 0x00FA);
 690                         b43legacy_radio_write16(dev, 0x005E, 0x00D8);
 691                 } else {
 692                         b43legacy_radio_write16(dev, 0x005D, 0x00F5);
 693                         b43legacy_radio_write16(dev, 0x005E, 0x00B8);
 694                 }
 695                 b43legacy_radio_write16(dev, 0x0073, 0x0003);
 696                 b43legacy_radio_write16(dev, 0x007D, 0x00A8);
 697                 b43legacy_radio_write16(dev, 0x007C, 0x0001);
 698                 b43legacy_radio_write16(dev, 0x007E, 0x0008);
 699         }
 700         val = 0x1E1F;
 701         for (offset = 0x0088; offset < 0x0098; offset++) {
 702                 b43legacy_phy_write(dev, offset, val);
 703                 val -= 0x0202;
 704         }
 705         val = 0x3E3F;
 706         for (offset = 0x0098; offset < 0x00A8; offset++) {
 707                 b43legacy_phy_write(dev, offset, val);
 708                 val -= 0x0202;
 709         }
 710         val = 0x2120;
 711         for (offset = 0x00A8; offset < 0x00C8; offset++) {
 712                 b43legacy_phy_write(dev, offset, (val & 0x3F3F));
 713                 val += 0x0202;
 714         }
 715         if (phy->type == B43legacy_PHYTYPE_G) {
 716                 b43legacy_radio_write16(dev, 0x007A,
 717                                         b43legacy_radio_read16(dev, 0x007A) |
 718                                         0x0020);
 719                 b43legacy_radio_write16(dev, 0x0051,
 720                                         b43legacy_radio_read16(dev, 0x0051) |
 721                                         0x0004);
 722                 b43legacy_phy_write(dev, 0x0802,
 723                                     b43legacy_phy_read(dev, 0x0802) | 0x0100);
 724                 b43legacy_phy_write(dev, 0x042B,
 725                                     b43legacy_phy_read(dev, 0x042B) | 0x2000);
 726                 b43legacy_phy_write(dev, 0x5B, 0x0000);
 727                 b43legacy_phy_write(dev, 0x5C, 0x0000);
 728         }
 729
 730         old_channel = phy->channel;
 731         if (old_channel >= 8)
 732                 b43legacy_radio_selectchannel(dev, 1, 0);
 733         else
 734                 b43legacy_radio_selectchannel(dev, 13, 0);
 735
 736         b43legacy_radio_write16(dev, 0x0050, 0x0020);
 737         b43legacy_radio_write16(dev, 0x0050, 0x0023);
 738         udelay(40);
 739         if (phy->radio_rev < 6 || phy->radio_rev == 8) {
 740                 b43legacy_radio_write16(dev, 0x007C,
 741                                         (b43legacy_radio_read16(dev, 0x007C)
 742                                         | 0x0002));
 743                 b43legacy_radio_write16(dev, 0x0050, 0x0020);
 744         }
 745         if (phy->radio_rev <= 2) {
 746                 b43legacy_radio_write16(dev, 0x0050, 0x0020);
 747                 b43legacy_radio_write16(dev, 0x005A, 0x0070);
 748                 b43legacy_radio_write16(dev, 0x005B, 0x007B);
 749                 b43legacy_radio_write16(dev, 0x005C, 0x00B0);
 750         }
 751         b43legacy_radio_write16(dev, 0x007A,
 752                                 (b43legacy_radio_read16(dev,
 753                                 0x007A) & 0x00F8) | 0x0007);
 754
 755         b43legacy_radio_selectchannel(dev, old_channel, 0);
 756
 757         b43legacy_phy_write(dev, 0x0014, 0x0200);
 758         if (phy->radio_rev >= 6)
 759                 b43legacy_phy_write(dev, 0x002A, 0x88C2);
 760         else
 761                 b43legacy_phy_write(dev, 0x002A, 0x8AC0);
 762         b43legacy_phy_write(dev, 0x0038, 0x0668);
 763         b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
 764         if (phy->radio_rev == 4 || phy->radio_rev == 5)
 765                 b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
 766                                     0x005D) & 0xFF80) | 0x0003);
 767         if (phy->radio_rev <= 2)
 768                 b43legacy_radio_write16(dev, 0x005D, 0x000D);
 769
 770         if (phy->analog == 4) {
 771                 b43legacy_write16(dev, 0x03E4, 0x0009);
 772                 b43legacy_phy_write(dev, 0x61, b43legacy_phy_read(dev, 0x61)
 773                                     & 0xFFF);
 774         } else
 775                 b43legacy_phy_write(dev, 0x0002, (b43legacy_phy_read(dev,
 776                                     0x0002) & 0xFFC0) | 0x0004);
 777         if (phy->type == B43legacy_PHYTYPE_G)
 778                 b43legacy_write16(dev, 0x03E6, 0x0);
 779         if (phy->type == B43legacy_PHYTYPE_B) {
 780                 b43legacy_write16(dev, 0x03E6, 0x8140);
 781                 b43legacy_phy_write(dev, 0x0016, 0x0410);
 782                 b43legacy_phy_write(dev, 0x0017, 0x0820);
 783                 b43legacy_phy_write(dev, 0x0062, 0x0007);
 784                 b43legacy_radio_init2050(dev);
 785                 b43legacy_phy_lo_g_measure(dev);
 786                 if (dev->dev->bus->sprom.boardflags_lo &
 787                     B43legacy_BFL_RSSI) {
 788                         b43legacy_calc_nrssi_slope(dev);
 789                         b43legacy_calc_nrssi_threshold(dev);
 790                 }
 791                 b43legacy_phy_init_pctl(dev);
 792         }
 793 }
 794
 795 static void b43legacy_calc_loopback_gain(struct b43legacy_wldev *dev)
 796 {
 797         struct b43legacy_phy *phy = &dev->phy;
 798         u16 backup_phy[15] = {0};
 799         u16 backup_radio[3];
 800         u16 backup_bband;
 801         u16 i;
 802         u16 loop1_cnt;
 803         u16 loop1_done;
 804         u16 loop1_omitted;
 805         u16 loop2_done;
 806
 807         backup_phy[0] = b43legacy_phy_read(dev, 0x0429);
 808         backup_phy[1] = b43legacy_phy_read(dev, 0x0001);
 809         backup_phy[2] = b43legacy_phy_read(dev, 0x0811);
 810         backup_phy[3] = b43legacy_phy_read(dev, 0x0812);
 811         if (phy->rev != 1) {
 812                 backup_phy[4] = b43legacy_phy_read(dev, 0x0814);
 813                 backup_phy[5] = b43legacy_phy_read(dev, 0x0815);
 814         }
 815         backup_phy[6] = b43legacy_phy_read(dev, 0x005A);
 816         backup_phy[7] = b43legacy_phy_read(dev, 0x0059);
 817         backup_phy[8] = b43legacy_phy_read(dev, 0x0058);
 818         backup_phy[9] = b43legacy_phy_read(dev, 0x000A);
 819         backup_phy[10] = b43legacy_phy_read(dev, 0x0003);
 820         backup_phy[11] = b43legacy_phy_read(dev, 0x080F);
 821         backup_phy[12] = b43legacy_phy_read(dev, 0x0810);
 822         backup_phy[13] = b43legacy_phy_read(dev, 0x002B);
 823         backup_phy[14] = b43legacy_phy_read(dev, 0x0015);
 824         b43legacy_phy_read(dev, 0x002D); /* dummy read */
 825         backup_bband = phy->bbatt;
 826         backup_radio[0] = b43legacy_radio_read16(dev, 0x0052);
 827         backup_radio[1] = b43legacy_radio_read16(dev, 0x0043);
 828         backup_radio[2] = b43legacy_radio_read16(dev, 0x007A);
 829
 830         b43legacy_phy_write(dev, 0x0429,
 831                             b43legacy_phy_read(dev, 0x0429) & 0x3FFF);
 832         b43legacy_phy_write(dev, 0x0001,
 833                             b43legacy_phy_read(dev, 0x0001) & 0x8000);
 834         b43legacy_phy_write(dev, 0x0811,
 835                             b43legacy_phy_read(dev, 0x0811) | 0x0002);
 836         b43legacy_phy_write(dev, 0x0812,
 837                             b43legacy_phy_read(dev, 0x0812) & 0xFFFD);
 838         b43legacy_phy_write(dev, 0x0811,
 839                             b43legacy_phy_read(dev, 0x0811) | 0x0001);
 840         b43legacy_phy_write(dev, 0x0812,
 841                             b43legacy_phy_read(dev, 0x0812) & 0xFFFE);
 842         if (phy->rev != 1) {
 843                 b43legacy_phy_write(dev, 0x0814,
 844                                     b43legacy_phy_read(dev, 0x0814) | 0x0001);
 845                 b43legacy_phy_write(dev, 0x0815,
 846                                     b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
 847                 b43legacy_phy_write(dev, 0x0814,
 848                                     b43legacy_phy_read(dev, 0x0814) | 0x0002);
 849                 b43legacy_phy_write(dev, 0x0815,
 850                                     b43legacy_phy_read(dev, 0x0815) & 0xFFFD);
 851         }
 852         b43legacy_phy_write(dev, 0x0811, b43legacy_phy_read(dev, 0x0811) |
 853                             0x000C);
 854         b43legacy_phy_write(dev, 0x0812, b43legacy_phy_read(dev, 0x0812) |
 855                             0x000C);
 856
 857         b43legacy_phy_write(dev, 0x0811, (b43legacy_phy_read(dev, 0x0811)
 858                             & 0xFFCF) | 0x0030);
 859         b43legacy_phy_write(dev, 0x0812, (b43legacy_phy_read(dev, 0x0812)
 860                             & 0xFFCF) | 0x0010);
 861
 862         b43legacy_phy_write(dev, 0x005A, 0x0780);
 863         b43legacy_phy_write(dev, 0x0059, 0xC810);
 864         b43legacy_phy_write(dev, 0x0058, 0x000D);
 865         if (phy->analog == 0)
 866                 b43legacy_phy_write(dev, 0x0003, 0x0122);
 867         else
 868                 b43legacy_phy_write(dev, 0x000A,
 869                                     b43legacy_phy_read(dev, 0x000A)
 870                                     | 0x2000);
 871         if (phy->rev != 1) {
 872                 b43legacy_phy_write(dev, 0x0814,
 873                                     b43legacy_phy_read(dev, 0x0814) | 0x0004);
 874                 b43legacy_phy_write(dev, 0x0815,
 875                                     b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
 876         }
 877         b43legacy_phy_write(dev, 0x0003,
 878                             (b43legacy_phy_read(dev, 0x0003)
 879                              & 0xFF9F) | 0x0040);
 880         if (phy->radio_ver == 0x2050 && phy->radio_rev == 2) {
 881                 b43legacy_radio_write16(dev, 0x0052, 0x0000);
 882                 b43legacy_radio_write16(dev, 0x0043,
 883                                         (b43legacy_radio_read16(dev, 0x0043)
 884                                          & 0xFFF0) | 0x0009);
 885                 loop1_cnt = 9;
 886         } else if (phy->radio_rev == 8) {
 887                 b43legacy_radio_write16(dev, 0x0043, 0x000F);
 888                 loop1_cnt = 15;
 889         } else
 890                 loop1_cnt = 0;
 891
 892         b43legacy_phy_set_baseband_attenuation(dev, 11);
 893
 894         if (phy->rev >= 3)
 895                 b43legacy_phy_write(dev, 0x080F, 0xC020);
 896         else
 897                 b43legacy_phy_write(dev, 0x080F, 0x8020);
 898         b43legacy_phy_write(dev, 0x0810, 0x0000);
 899
 900         b43legacy_phy_write(dev, 0x002B,
 901                             (b43legacy_phy_read(dev, 0x002B)
 902                              & 0xFFC0) | 0x0001);
 903         b43legacy_phy_write(dev, 0x002B,
 904                             (b43legacy_phy_read(dev, 0x002B)
 905                              & 0xC0FF) | 0x0800);
 906         b43legacy_phy_write(dev, 0x0811,
 907                             b43legacy_phy_read(dev, 0x0811) | 0x0100);
 908         b43legacy_phy_write(dev, 0x0812,
 909                             b43legacy_phy_read(dev, 0x0812) & 0xCFFF);
 910         if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_EXTLNA) {
 911                 if (phy->rev >= 7) {
 912                         b43legacy_phy_write(dev, 0x0811,
 913                                             b43legacy_phy_read(dev, 0x0811)
 914                                             | 0x0800);
 915                         b43legacy_phy_write(dev, 0x0812,
 916                                             b43legacy_phy_read(dev, 0x0812)
 917                                             | 0x8000);
 918                 }
 919         }
 920         b43legacy_radio_write16(dev, 0x007A,
 921                                 b43legacy_radio_read16(dev, 0x007A)
 922                                 & 0x00F7);
 923
 924         for (i = 0; i < loop1_cnt; i++) {
 925                 b43legacy_radio_write16(dev, 0x0043, loop1_cnt);
 926                 b43legacy_phy_write(dev, 0x0812,
 927                                     (b43legacy_phy_read(dev, 0x0812)
 928                                      & 0xF0FF) | (i << 8));
 929                 b43legacy_phy_write(dev, 0x0015,
 930                                     (b43legacy_phy_read(dev, 0x0015)
 931                                      & 0x0FFF) | 0xA000);
 932                 b43legacy_phy_write(dev, 0x0015,
 933                                     (b43legacy_phy_read(dev, 0x0015)
 934                                      & 0x0FFF) | 0xF000);
 935                 udelay(20);
 936                 if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
 937                         break;
 938         }
 939         loop1_done = i;
 940         loop1_omitted = loop1_cnt - loop1_done;
 941
 942         loop2_done = 0;
 943         if (loop1_done >= 8) {
 944                 b43legacy_phy_write(dev, 0x0812,
 945                                     b43legacy_phy_read(dev, 0x0812)
 946                                     | 0x0030);
 947                 for (i = loop1_done - 8; i < 16; i++) {
 948                         b43legacy_phy_write(dev, 0x0812,
 949                                             (b43legacy_phy_read(dev, 0x0812)
 950                                              & 0xF0FF) | (i << 8));
 951                         b43legacy_phy_write(dev, 0x0015,
 952                                             (b43legacy_phy_read(dev, 0x0015)
 953                                              & 0x0FFF) | 0xA000);
 954                         b43legacy_phy_write(dev, 0x0015,
 955                                             (b43legacy_phy_read(dev, 0x0015)
 956                                              & 0x0FFF) | 0xF000);
 957                         udelay(20);
 958                         if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
 959                                 break;
 960                 }
 961         }
 962
 963         if (phy->rev != 1) {
 964                 b43legacy_phy_write(dev, 0x0814, backup_phy[4]);
 965                 b43legacy_phy_write(dev, 0x0815, backup_phy[5]);
 966         }
 967         b43legacy_phy_write(dev, 0x005A, backup_phy[6]);
 968         b43legacy_phy_write(dev, 0x0059, backup_phy[7]);
 969         b43legacy_phy_write(dev, 0x0058, backup_phy[8]);
 970         b43legacy_phy_write(dev, 0x000A, backup_phy[9]);
 971         b43legacy_phy_write(dev, 0x0003, backup_phy[10]);
 972         b43legacy_phy_write(dev, 0x080F, backup_phy[11]);
 973         b43legacy_phy_write(dev, 0x0810, backup_phy[12]);
 974         b43legacy_phy_write(dev, 0x002B, backup_phy[13]);
 975         b43legacy_phy_write(dev, 0x0015, backup_phy[14]);
 976
 977         b43legacy_phy_set_baseband_attenuation(dev, backup_bband);
 978
 979         b43legacy_radio_write16(dev, 0x0052, backup_radio[0]);
 980         b43legacy_radio_write16(dev, 0x0043, backup_radio[1]);
 981         b43legacy_radio_write16(dev, 0x007A, backup_radio[2]);
 982
 983         b43legacy_phy_write(dev, 0x0811, backup_phy[2] | 0x0003);
 984         udelay(10);
 985         b43legacy_phy_write(dev, 0x0811, backup_phy[2]);
 986         b43legacy_phy_write(dev, 0x0812, backup_phy[3]);
 987         b43legacy_phy_write(dev, 0x0429, backup_phy[0]);
 988         b43legacy_phy_write(dev, 0x0001, backup_phy[1]);
 989
 990         phy->loopback_gain[0] = ((loop1_done * 6) - (loop1_omitted * 4)) - 11;
 991         phy->loopback_gain[1] = (24 - (3 * loop2_done)) * 2;
 992 }
 993
 994 static void b43legacy_phy_initg(struct b43legacy_wldev *dev)
 995 {
 996         struct b43legacy_phy *phy = &dev->phy;
 997         u16 tmp;
 998
 999         if (phy->rev == 1)
1000                 b43legacy_phy_initb5(dev);
1001         else
1002                 b43legacy_phy_initb6(dev);
1003         if (phy->rev >= 2 && phy->gmode)
1004                 b43legacy_phy_inita(dev);
1005
1006         if (phy->rev >= 2) {
1007                 b43legacy_phy_write(dev, 0x0814, 0x0000);
1008                 b43legacy_phy_write(dev, 0x0815, 0x0000);
1009         }
1010         if (phy->rev == 2) {
1011                 b43legacy_phy_write(dev, 0x0811, 0x0000);
1012                 b43legacy_phy_write(dev, 0x0015, 0x00C0);
1013         }
1014         if (phy->rev > 5) {
1015                 b43legacy_phy_write(dev, 0x0811, 0x0400);
1016                 b43legacy_phy_write(dev, 0x0015, 0x00C0);
1017         }
1018         if (phy->gmode) {
1019                 tmp = b43legacy_phy_read(dev, 0x0400) & 0xFF;
1020                 if (tmp == 3) {
1021                         b43legacy_phy_write(dev, 0x04C2, 0x1816);
1022                         b43legacy_phy_write(dev, 0x04C3, 0x8606);
1023                 }
1024                 if (tmp == 4 || tmp == 5) {
1025                         b43legacy_phy_write(dev, 0x04C2, 0x1816);
1026                         b43legacy_phy_write(dev, 0x04C3, 0x8006);
1027                         b43legacy_phy_write(dev, 0x04CC,
1028                                             (b43legacy_phy_read(dev,
1029                                              0x04CC) & 0x00FF) |
1030                                              0x1F00);
1031                 }
1032                 if (phy->rev >= 2)
1033                         b43legacy_phy_write(dev, 0x047E, 0x0078);
1034         }
1035         if (phy->radio_rev == 8) {
1036                 b43legacy_phy_write(dev, 0x0801, b43legacy_phy_read(dev, 0x0801)
1037                                     | 0x0080);
1038                 b43legacy_phy_write(dev, 0x043E, b43legacy_phy_read(dev, 0x043E)
1039                                     | 0x0004);
1040         }
1041         if (phy->rev >= 2 && phy->gmode)
1042                 b43legacy_calc_loopback_gain(dev);
1043         if (phy->radio_rev != 8) {
1044                 if (phy->initval == 0xFFFF)
1045                         phy->initval = b43legacy_radio_init2050(dev);
1046                 else
1047                         b43legacy_radio_write16(dev, 0x0078, phy->initval);
1048         }
1049         if (phy->txctl2 == 0xFFFF)
1050                 b43legacy_phy_lo_g_measure(dev);
1051         else {
1052                 if (phy->radio_ver == 0x2050 && phy->radio_rev == 8)
1053                         b43legacy_radio_write16(dev, 0x0052,
1054                                                 (phy->txctl1 << 4) |
1055                                                 phy->txctl2);
1056                 else
1057                         b43legacy_radio_write16(dev, 0x0052,
1058                                                 (b43legacy_radio_read16(dev,
1059                                                  0x0052) & 0xFFF0) |
1060                                                  phy->txctl1);
1061                 if (phy->rev >= 6)
1062                         b43legacy_phy_write(dev, 0x0036,
1063                                             (b43legacy_phy_read(dev, 0x0036)
1064                                              & 0x0FFF) | (phy->txctl2 << 12));
1065                 if (dev->dev->bus->sprom.boardflags_lo &
1066                     B43legacy_BFL_PACTRL)
1067                         b43legacy_phy_write(dev, 0x002E, 0x8075);
1068                 else
1069                         b43legacy_phy_write(dev, 0x002E, 0x807F);
1070                 if (phy->rev < 2)
1071                         b43legacy_phy_write(dev, 0x002F, 0x0101);
1072                 else
1073                         b43legacy_phy_write(dev, 0x002F, 0x0202);
1074         }
1075         if (phy->gmode) {
1076                 b43legacy_phy_lo_adjust(dev, 0);
1077                 b43legacy_phy_write(dev, 0x080F, 0x8078);
1078         }
1079
1080         if (!(dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_RSSI)) {
1081                 /* The specs state to update the NRSSI LT with
1082                  * the value 0x7FFFFFFF here. I think that is some weird
1083                  * compiler optimization in the original driver.
1084                  * Essentially, what we do here is resetting all NRSSI LT
1085                  * entries to -32 (see the clamp_val() in nrssi_hw_update())
1086                  */
1087                 b43legacy_nrssi_hw_update(dev, 0xFFFF);
1088                 b43legacy_calc_nrssi_threshold(dev);
1089         } else if (phy->gmode || phy->rev >= 2) {
1090                 if (phy->nrssi[0] == -1000) {
1091                         B43legacy_WARN_ON(phy->nrssi[1] != -1000);
1092                         b43legacy_calc_nrssi_slope(dev);
1093                 } else {
1094                         B43legacy_WARN_ON(phy->nrssi[1] == -1000);
1095                         b43legacy_calc_nrssi_threshold(dev);
1096                 }
1097         }
1098         if (phy->radio_rev == 8)
1099                 b43legacy_phy_write(dev, 0x0805, 0x3230);
1100         b43legacy_phy_init_pctl(dev);
1101         if (dev->dev->bus->chip_id == 0x4306
1102             && dev->dev->bus->chip_package == 2) {
1103                 b43legacy_phy_write(dev, 0x0429,
1104                                     b43legacy_phy_read(dev, 0x0429) & 0xBFFF);
1105                 b43legacy_phy_write(dev, 0x04C3,
1106                                     b43legacy_phy_read(dev, 0x04C3) & 0x7FFF);
1107         }
1108 }
1109
1110 static u16 b43legacy_phy_lo_b_r15_loop(struct b43legacy_wldev *dev)
1111 {
1112         int i;
1113         u16 ret = 0;
1114         unsigned long flags;
1115
1116         local_irq_save(flags);
1117         for (i = 0; i < 10; i++) {
1118                 b43legacy_phy_write(dev, 0x0015, 0xAFA0);
1119                 udelay(1);
1120                 b43legacy_phy_write(dev, 0x0015, 0xEFA0);
1121                 udelay(10);
1122                 b43legacy_phy_write(dev, 0x0015, 0xFFA0);
1123                 udelay(40);
1124                 ret += b43legacy_phy_read(dev, 0x002C);
1125         }
1126         local_irq_restore(flags);
1127         b43legacy_voluntary_preempt();
1128
1129         return ret;
1130 }
1131
1132 void b43legacy_phy_lo_b_measure(struct b43legacy_wldev *dev)
1133 {
1134         struct b43legacy_phy *phy = &dev->phy;
1135         u16 regstack[12] = { 0 };
1136         u16 mls;
1137         u16 fval;
1138         int i;
1139         int j;
1140
1141         regstack[0] = b43legacy_phy_read(dev, 0x0015);
1142         regstack[1] = b43legacy_radio_read16(dev, 0x0052) & 0xFFF0;
1143
1144         if (phy->radio_ver == 0x2053) {
1145                 regstack[2] = b43legacy_phy_read(dev, 0x000A);
1146                 regstack[3] = b43legacy_phy_read(dev, 0x002A);
1147                 regstack[4] = b43legacy_phy_read(dev, 0x0035);
1148                 regstack[5] = b43legacy_phy_read(dev, 0x0003);
1149                 regstack[6] = b43legacy_phy_read(dev, 0x0001);
1150                 regstack[7] = b43legacy_phy_read(dev, 0x0030);
1151
1152                 regstack[8] = b43legacy_radio_read16(dev, 0x0043);
1153                 regstack[9] = b43legacy_radio_read16(dev, 0x007A);
1154                 regstack[10] = b43legacy_read16(dev, 0x03EC);
1155                 regstack[11] = b43legacy_radio_read16(dev, 0x0052) & 0x00F0;
1156
1157                 b43legacy_phy_write(dev, 0x0030, 0x00FF);
1158                 b43legacy_write16(dev, 0x03EC, 0x3F3F);
1159                 b43legacy_phy_write(dev, 0x0035, regstack[4] & 0xFF7F);
1160                 b43legacy_radio_write16(dev, 0x007A, regstack[9] & 0xFFF0);
1161         }
1162         b43legacy_phy_write(dev, 0x0015, 0xB000);
1163         b43legacy_phy_write(dev, 0x002B, 0x0004);
1164
1165         if (phy->radio_ver == 0x2053) {
1166                 b43legacy_phy_write(dev, 0x002B, 0x0203);
1167                 b43legacy_phy_write(dev, 0x002A, 0x08A3);
1168         }
1169
1170         phy->minlowsig[0] = 0xFFFF;
1171
1172         for (i = 0; i < 4; i++) {
1173                 b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
1174                 b43legacy_phy_lo_b_r15_loop(dev);
1175         }
1176         for (i = 0; i < 10; i++) {
1177                 b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
1178                 mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
1179                 if (mls < phy->minlowsig[0]) {
1180                         phy->minlowsig[0] = mls;
1181                         phy->minlowsigpos[0] = i;
1182                 }
1183         }
1184         b43legacy_radio_write16(dev, 0x0052, regstack[1]
1185                                 | phy->minlowsigpos[0]);
1186
1187         phy->minlowsig[1] = 0xFFFF;
1188
1189         for (i = -4; i < 5; i += 2) {
1190                 for (j = -4; j < 5; j += 2) {
1191                         if (j < 0)
1192                                 fval = (0x0100 * i) + j + 0x0100;
1193                         else
1194                                 fval = (0x0100 * i) + j;
1195                         b43legacy_phy_write(dev, 0x002F, fval);
1196                         mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
1197                         if (mls < phy->minlowsig[1]) {
1198                                 phy->minlowsig[1] = mls;
1199                                 phy->minlowsigpos[1] = fval;
1200                         }
1201                 }
1202         }
1203         phy->minlowsigpos[1] += 0x0101;
1204
1205         b43legacy_phy_write(dev, 0x002F, phy->minlowsigpos[1]);
1206         if (phy->radio_ver == 0x2053) {
1207                 b43legacy_phy_write(dev, 0x000A, regstack[2]);
1208                 b43legacy_phy_write(dev, 0x002A, regstack[3]);
1209                 b43legacy_phy_write(dev, 0x0035, regstack[4]);
1210                 b43legacy_phy_write(dev, 0x0003, regstack[5]);
1211                 b43legacy_phy_write(dev, 0x0001, regstack[6]);
1212                 b43legacy_phy_write(dev, 0x0030, regstack[7]);
1213
1214                 b43legacy_radio_write16(dev, 0x0043, regstack[8]);
1215                 b43legacy_radio_write16(dev, 0x007A, regstack[9]);
1216
1217                 b43legacy_radio_write16(dev, 0x0052,
1218                                         (b43legacy_radio_read16(dev, 0x0052)
1219                                         & 0x000F) | regstack[11]);
1220
1221                 b43legacy_write16(dev, 0x03EC, regstack[10]);
1222         }
1223         b43legacy_phy_write(dev, 0x0015, regstack[0]);
1224 }
1225
1226 static inline
1227 u16 b43legacy_phy_lo_g_deviation_subval(struct b43legacy_wldev *dev,
1228                                         u16 control)
1229 {
1230         struct b43legacy_phy *phy = &dev->phy;
1231         u16 ret;
1232         unsigned long flags;
1233
1234         local_irq_save(flags);
1235         if (phy->gmode) {
1236                 b43legacy_phy_write(dev, 0x15, 0xE300);
1237                 control <<= 8;
1238                 b43legacy_phy_write(dev, 0x0812, control | 0x00B0);
1239                 udelay(5);
1240                 b43legacy_phy_write(dev, 0x0812, control | 0x00B2);
1241                 udelay(2);
1242                 b43legacy_phy_write(dev, 0x0812, control | 0x00B3);
1243                 udelay(4);
1244                 b43legacy_phy_write(dev, 0x0015, 0xF300);
1245                 udelay(8);
1246         } else {
1247                 b43legacy_phy_write(dev, 0x0015, control | 0xEFA0);
1248                 udelay(2);
1249                 b43legacy_phy_write(dev, 0x0015, control | 0xEFE0);
1250                 udelay(4);
1251                 b43legacy_phy_write(dev, 0x0015, control | 0xFFE0);
1252                 udelay(8);
1253         }
1254         ret = b43legacy_phy_read(dev, 0x002D);
1255         local_irq_restore(flags);
1256         b43legacy_voluntary_preempt();
1257
1258         return ret;
1259 }
1260
1261 static u32 b43legacy_phy_lo_g_singledeviation(struct b43legacy_wldev *dev,
1262                                               u16 control)
1263 {
1264         int i;
1265         u32 ret = 0;
1266
1267         for (i = 0; i < 8; i++)
1268                 ret += b43legacy_phy_lo_g_deviation_subval(dev, control);
1269
1270         return ret;
1271 }
1272
1273 /* Write the LocalOscillator CONTROL */
1274 static inline
1275 void b43legacy_lo_write(struct b43legacy_wldev *dev,
1276                         struct b43legacy_lopair *pair)
1277 {
1278         u16 value;
1279
1280         value = (u8)(pair->low);
1281         value |= ((u8)(pair->high)) << 8;
1282
1283 #ifdef CONFIG_B43LEGACY_DEBUG
1284         /* Sanity check. */
1285         if (pair->low < -8 || pair->low > 8 ||
1286             pair->high < -8 || pair->high > 8) {
1287                 b43legacydbg(dev->wl,
1288                        "WARNING: Writing invalid LOpair "
1289                        "(low: %d, high: %d)\n",
1290                        pair->low, pair->high);
1291                 dump_stack();
1292         }
1293 #endif
1294
1295         b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, value);
1296 }
1297
1298 static inline
1299 struct b43legacy_lopair *b43legacy_find_lopair(struct b43legacy_wldev *dev,
1300                                                u16 bbatt,
1301                                                u16 rfatt,
1302                                                u16 tx)
1303 {
1304         static const u8 dict[10] = { 11, 10, 11, 12, 13, 12, 13, 12, 13, 12 };
1305         struct b43legacy_phy *phy = &dev->phy;
1306
1307         if (bbatt > 6)
1308                 bbatt = 6;
1309         B43legacy_WARN_ON(rfatt >= 10);
1310
1311         if (tx == 3)
1312                 return b43legacy_get_lopair(phy, rfatt, bbatt);
1313         return b43legacy_get_lopair(phy, dict[rfatt], bbatt);
1314 }
1315
1316 static inline
1317 struct b43legacy_lopair *b43legacy_current_lopair(struct b43legacy_wldev *dev)
1318 {
1319         struct b43legacy_phy *phy = &dev->phy;
1320
1321         return b43legacy_find_lopair(dev, phy->bbatt,
1322                                      phy->rfatt, phy->txctl1);
1323 }
1324
1325 /* Adjust B/G LO */
1326 void b43legacy_phy_lo_adjust(struct b43legacy_wldev *dev, int fixed)
1327 {
1328         struct b43legacy_lopair *pair;
1329
1330         if (fixed) {
1331                 /* Use fixed values. Only for initialization. */
1332                 pair = b43legacy_find_lopair(dev, 2, 3, 0);
1333         } else
1334                 pair = b43legacy_current_lopair(dev);
1335         b43legacy_lo_write(dev, pair);
1336 }
1337
1338 static void b43legacy_phy_lo_g_measure_txctl2(struct b43legacy_wldev *dev)
1339 {
1340         struct b43legacy_phy *phy = &dev->phy;
1341         u16 txctl2 = 0;
1342         u16 i;
1343         u32 smallest;
1344         u32 tmp;
1345
1346         b43legacy_radio_write16(dev, 0x0052, 0x0000);
1347         udelay(10);
1348         smallest = b43legacy_phy_lo_g_singledeviation(dev, 0);
1349         for (i = 0; i < 16; i++) {
1350                 b43legacy_radio_write16(dev, 0x0052, i);
1351                 udelay(10);
1352                 tmp = b43legacy_phy_lo_g_singledeviation(dev, 0);
1353                 if (tmp < smallest) {
1354                         smallest = tmp;
1355                         txctl2 = i;
1356                 }
1357         }
1358         phy->txctl2 = txctl2;
1359 }
1360
1361 static
1362 void b43legacy_phy_lo_g_state(struct b43legacy_wldev *dev,
1363                               const struct b43legacy_lopair *in_pair,
1364                               struct b43legacy_lopair *out_pair,
1365                               u16 r27)
1366 {
1367         static const struct b43legacy_lopair transitions[8] = {
1368                 { .high =  1,  .low =  1, },
1369                 { .high =  1,  .low =  0, },
1370                 { .high =  1,  .low = -1, },
1371                 { .high =  0,  .low = -1, },
1372                 { .high = -1,  .low = -1, },
1373                 { .high = -1,  .low =  0, },
1374                 { .high = -1,  .low =  1, },
1375                 { .high =  0,  .low =  1, },
1376         };
1377         struct b43legacy_lopair lowest_transition = {
1378                 .high = in_pair->high,
1379                 .low = in_pair->low,
1380         };
1381         struct b43legacy_lopair tmp_pair;
1382         struct b43legacy_lopair transition;
1383         int i = 12;
1384         int state = 0;
1385         int found_lower;
1386         int j;
1387         int begin;
1388         int end;
1389         u32 lowest_deviation;
1390         u32 tmp;
1391
1392         /* Note that in_pair and out_pair can point to the same pair.
1393          * Be careful. */
1394
1395         b43legacy_lo_write(dev, &lowest_transition);
1396         lowest_deviation = b43legacy_phy_lo_g_singledeviation(dev, r27);
1397         do {
1398                 found_lower = 0;
1399                 B43legacy_WARN_ON(!(state >= 0 && state <= 8));
1400                 if (state == 0) {
1401                         begin = 1;
1402                         end = 8;
1403                 } else if (state % 2 == 0) {
1404                         begin = state - 1;
1405                         end = state + 1;
1406                 } else {
1407                         begin = state - 2;
1408                         end = state + 2;
1409                 }
1410                 if (begin < 1)
1411                         begin += 8;
1412                 if (end > 8)
1413                         end -= 8;
1414
1415                 j = begin;
1416                 tmp_pair.high = lowest_transition.high;
1417                 tmp_pair.low = lowest_transition.low;
1418                 while (1) {
1419                         B43legacy_WARN_ON(!(j >= 1 && j <= 8));
1420                         transition.high = tmp_pair.high +
1421                                           transitions[j - 1].high;
1422                         transition.low = tmp_pair.low + transitions[j - 1].low;
1423                         if ((abs(transition.low) < 9)
1424                              && (abs(transition.high) < 9)) {
1425                                 b43legacy_lo_write(dev, &transition);
1426                                 tmp = b43legacy_phy_lo_g_singledeviation(dev,
1427                                                                        r27);
1428                                 if (tmp < lowest_deviation) {
1429                                         lowest_deviation = tmp;
1430                                         state = j;
1431                                         found_lower = 1;
1432
1433                                         lowest_transition.high =
1434                                                                 transition.high;
1435                                         lowest_transition.low = transition.low;
1436                                 }
1437                         }
1438                         if (j == end)
1439                                 break;
1440                         if (j == 8)
1441                                 j = 1;
1442                         else
1443                                 j++;
1444                 }
1445         } while (i-- && found_lower);
1446
1447         out_pair->high = lowest_transition.high;
1448         out_pair->low = lowest_transition.low;
1449 }
1450
1451 /* Set the baseband attenuation value on chip. */
1452 void b43legacy_phy_set_baseband_attenuation(struct b43legacy_wldev *dev,
1453                                             u16 bbatt)
1454 {
1455         struct b43legacy_phy *phy = &dev->phy;
1456         u16 value;
1457
1458         if (phy->analog == 0) {
1459                 value = (b43legacy_read16(dev, 0x03E6) & 0xFFF0);
1460                 value |= (bbatt & 0x000F);
1461                 b43legacy_write16(dev, 0x03E6, value);
1462                 return;
1463         }
1464
1465         if (phy->analog > 1) {
1466                 value = b43legacy_phy_read(dev, 0x0060) & 0xFFC3;
1467                 value |= (bbatt << 2) & 0x003C;
1468         } else {
1469                 value = b43legacy_phy_read(dev, 0x0060) & 0xFF87;
1470                 value |= (bbatt << 3) & 0x0078;
1471         }
1472         b43legacy_phy_write(dev, 0x0060, value);
1473 }
1474
1475 /* http://bcm-specs.sipsolutions.net/LocalOscillator/Measure */
1476 void b43legacy_phy_lo_g_measure(struct b43legacy_wldev *dev)
1477 {
1478         static const u8 pairorder[10] = { 3, 1, 5, 7, 9, 2, 0, 4, 6, 8 };
1479         const int is_initializing = (b43legacy_status(dev)
1480                                      < B43legacy_STAT_STARTED);
1481         struct b43legacy_phy *phy = &dev->phy;
1482         u16 h;
1483         u16 i;
1484         u16 oldi = 0;
1485         u16 j;
1486         struct b43legacy_lopair control;
1487         struct b43legacy_lopair *tmp_control;
1488         u16 tmp;
1489         u16 regstack[16] = { 0 };
1490         u8 oldchannel;
1491
1492         /* XXX: What are these? */
1493         u8 r27 = 0;
1494         u16 r31;
1495
1496         oldchannel = phy->channel;
1497         /* Setup */
1498         if (phy->gmode) {
1499                 regstack[0] = b43legacy_phy_read(dev, B43legacy_PHY_G_CRS);
1500                 regstack[1] = b43legacy_phy_read(dev, 0x0802);
1501                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
1502                                     & 0x7FFF);
1503                 b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
1504         }
1505         regstack[3] = b43legacy_read16(dev, 0x03E2);
1506         b43legacy_write16(dev, 0x03E2, regstack[3] | 0x8000);
1507         regstack[4] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
1508         regstack[5] = b43legacy_phy_read(dev, 0x15);
1509         regstack[6] = b43legacy_phy_read(dev, 0x2A);
1510         regstack[7] = b43legacy_phy_read(dev, 0x35);
1511         regstack[8] = b43legacy_phy_read(dev, 0x60);
1512         regstack[9] = b43legacy_radio_read16(dev, 0x43);
1513         regstack[10] = b43legacy_radio_read16(dev, 0x7A);
1514         regstack[11] = b43legacy_radio_read16(dev, 0x52);
1515         if (phy->gmode) {
1516                 regstack[12] = b43legacy_phy_read(dev, 0x0811);
1517                 regstack[13] = b43legacy_phy_read(dev, 0x0812);
1518                 regstack[14] = b43legacy_phy_read(dev, 0x0814);
1519                 regstack[15] = b43legacy_phy_read(dev, 0x0815);
1520         }
1521         b43legacy_radio_selectchannel(dev, 6, 0);
1522         if (phy->gmode) {
1523                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
1524                                     & 0x7FFF);
1525                 b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
1526                 b43legacy_dummy_transmission(dev);
1527         }
1528         b43legacy_radio_write16(dev, 0x0043, 0x0006);
1529
1530         b43legacy_phy_set_baseband_attenuation(dev, 2);
1531
1532         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x0000);
1533         b43legacy_phy_write(dev, 0x002E, 0x007F);
1534         b43legacy_phy_write(dev, 0x080F, 0x0078);
1535         b43legacy_phy_write(dev, 0x0035, regstack[7] & ~(1 << 7));
1536         b43legacy_radio_write16(dev, 0x007A, regstack[10] & 0xFFF0);
1537         b43legacy_phy_write(dev, 0x002B, 0x0203);
1538         b43legacy_phy_write(dev, 0x002A, 0x08A3);
1539         if (phy->gmode) {
1540                 b43legacy_phy_write(dev, 0x0814, regstack[14] | 0x0003);
1541                 b43legacy_phy_write(dev, 0x0815, regstack[15] & 0xFFFC);
1542                 b43legacy_phy_write(dev, 0x0811, 0x01B3);
1543                 b43legacy_phy_write(dev, 0x0812, 0x00B2);
1544         }
1545         if (is_initializing)
1546                 b43legacy_phy_lo_g_measure_txctl2(dev);
1547         b43legacy_phy_write(dev, 0x080F, 0x8078);
1548
1549         /* Measure */
1550         control.low = 0;
1551         control.high = 0;
1552         for (h = 0; h < 10; h++) {
1553                 /* Loop over each possible RadioAttenuation (0-9) */
1554                 i = pairorder[h];
1555                 if (is_initializing) {
1556                         if (i == 3) {
1557                                 control.low = 0;
1558                                 control.high = 0;
1559                         } else if (((i % 2 == 1) && (oldi % 2 == 1)) ||
1560                                   ((i % 2 == 0) && (oldi % 2 == 0))) {
1561                                 tmp_control = b43legacy_get_lopair(phy, oldi,
1562                                                                    0);
1563                                 memcpy(&control, tmp_control, sizeof(control));
1564                         } else {
1565                                 tmp_control = b43legacy_get_lopair(phy, 3, 0);
1566                                 memcpy(&control, tmp_control, sizeof(control));
1567                         }
1568                 }
1569                 /* Loop over each possible BasebandAttenuation/2 */
1570                 for (j = 0; j < 4; j++) {
1571                         if (is_initializing) {
1572                                 tmp = i * 2 + j;
1573                                 r27 = 0;
1574                                 r31 = 0;
1575                                 if (tmp > 14) {
1576                                         r31 = 1;
1577                                         if (tmp > 17)
1578                                                 r27 = 1;
1579                                         if (tmp > 19)
1580                                                 r27 = 2;
1581                                 }
1582                         } else {
1583                                 tmp_control = b43legacy_get_lopair(phy, i,
1584                                                                    j * 2);
1585                                 if (!tmp_control->used)
1586                                         continue;
1587                                 memcpy(&control, tmp_control, sizeof(control));
1588                                 r27 = 3;
1589                                 r31 = 0;
1590                         }
1591                         b43legacy_radio_write16(dev, 0x43, i);
1592                         b43legacy_radio_write16(dev, 0x52, phy->txctl2);
1593                         udelay(10);
1594                         b43legacy_voluntary_preempt();
1595
1596                         b43legacy_phy_set_baseband_attenuation(dev, j * 2);
1597
1598                         tmp = (regstack[10] & 0xFFF0);
1599                         if (r31)
1600                                 tmp |= 0x0008;
1601                         b43legacy_radio_write16(dev, 0x007A, tmp);
1602
1603                         tmp_control = b43legacy_get_lopair(phy, i, j * 2);
1604                         b43legacy_phy_lo_g_state(dev, &control, tmp_control,
1605                                                  r27);
1606                 }
1607                 oldi = i;
1608         }
1609         /* Loop over each possible RadioAttenuation (10-13) */
1610         for (i = 10; i < 14; i++) {
1611                 /* Loop over each possible BasebandAttenuation/2 */
1612                 for (j = 0; j < 4; j++) {
1613                         if (is_initializing) {
1614                                 tmp_control = b43legacy_get_lopair(phy, i - 9,
1615                                                                  j * 2);
1616                                 memcpy(&control, tmp_control, sizeof(control));
1617                                 /* FIXME: The next line is wrong, as the
1618                                  * following if statement can never trigger. */
1619                                 tmp = (i - 9) * 2 + j - 5;
1620                                 r27 = 0;
1621                                 r31 = 0;
1622                                 if (tmp > 14) {
1623                                         r31 = 1;
1624                                         if (tmp > 17)
1625                                                 r27 = 1;
1626                                         if (tmp > 19)
1627                                                 r27 = 2;
1628                                 }
1629                         } else {
1630                                 tmp_control = b43legacy_get_lopair(phy, i - 9,
1631                                                                    j * 2);
1632                                 if (!tmp_control->used)
1633                                         continue;
1634                                 memcpy(&control, tmp_control, sizeof(control));
1635                                 r27 = 3;
1636                                 r31 = 0;
1637                         }
1638                         b43legacy_radio_write16(dev, 0x43, i - 9);
1639                         /* FIXME: shouldn't txctl1 be zero in the next line
1640                          * and 3 in the loop above? */
1641                         b43legacy_radio_write16(dev, 0x52,
1642                                               phy->txctl2
1643                                               | (3/*txctl1*/ << 4));
1644                         udelay(10);
1645                         b43legacy_voluntary_preempt();
1646
1647                         b43legacy_phy_set_baseband_attenuation(dev, j * 2);
1648
1649                         tmp = (regstack[10] & 0xFFF0);
1650                         if (r31)
1651                                 tmp |= 0x0008;
1652                         b43legacy_radio_write16(dev, 0x7A, tmp);
1653
1654                         tmp_control = b43legacy_get_lopair(phy, i, j * 2);
1655                         b43legacy_phy_lo_g_state(dev, &control, tmp_control,
1656                                                  r27);
1657                 }
1658         }
1659
1660         /* Restoration */
1661         if (phy->gmode) {
1662                 b43legacy_phy_write(dev, 0x0015, 0xE300);
1663                 b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA0);
1664                 udelay(5);
1665                 b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA2);
1666                 udelay(2);
1667                 b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA3);
1668                 b43legacy_voluntary_preempt();
1669         } else
1670                 b43legacy_phy_write(dev, 0x0015, r27 | 0xEFA0);
1671         b43legacy_phy_lo_adjust(dev, is_initializing);
1672         b43legacy_phy_write(dev, 0x002E, 0x807F);
1673         if (phy->gmode)
1674                 b43legacy_phy_write(dev, 0x002F, 0x0202);
1675         else
1676                 b43legacy_phy_write(dev, 0x002F, 0x0101);
1677         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, regstack[4]);
1678         b43legacy_phy_write(dev, 0x0015, regstack[5]);
1679         b43legacy_phy_write(dev, 0x002A, regstack[6]);
1680         b43legacy_phy_write(dev, 0x0035, regstack[7]);
1681         b43legacy_phy_write(dev, 0x0060, regstack[8]);
1682         b43legacy_radio_write16(dev, 0x0043, regstack[9]);
1683         b43legacy_radio_write16(dev, 0x007A, regstack[10]);
1684         regstack[11] &= 0x00F0;
1685         regstack[11] |= (b43legacy_radio_read16(dev, 0x52) & 0x000F);
1686         b43legacy_radio_write16(dev, 0x52, regstack[11]);
1687         b43legacy_write16(dev, 0x03E2, regstack[3]);
1688         if (phy->gmode) {
1689                 b43legacy_phy_write(dev, 0x0811, regstack[12]);
1690                 b43legacy_phy_write(dev, 0x0812, regstack[13]);
1691                 b43legacy_phy_write(dev, 0x0814, regstack[14]);
1692                 b43legacy_phy_write(dev, 0x0815, regstack[15]);
1693                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]);
1694                 b43legacy_phy_write(dev, 0x0802, regstack[1]);
1695         }
1696         b43legacy_radio_selectchannel(dev, oldchannel, 1);
1697
1698 #ifdef CONFIG_B43LEGACY_DEBUG
1699         {
1700                 /* Sanity check for all lopairs. */
1701                 for (i = 0; i < B43legacy_LO_COUNT; i++) {
1702                         tmp_control = phy->_lo_pairs + i;
1703                         if (tmp_control->low < -8 || tmp_control->low > 8 ||
1704                             tmp_control->high < -8 || tmp_control->high > 8)
1705                                 b43legacywarn(dev->wl,
1706                                        "WARNING: Invalid LOpair (low: %d, high:"
1707                                        " %d, index: %d)\n",
1708                                        tmp_control->low, tmp_control->high, i);
1709                 }
1710         }
1711 #endif /* CONFIG_B43LEGACY_DEBUG */
1712 }
1713
1714 static
1715 void b43legacy_phy_lo_mark_current_used(struct b43legacy_wldev *dev)
1716 {
1717         struct b43legacy_lopair *pair;
1718
1719         pair = b43legacy_current_lopair(dev);
1720         pair->used = 1;
1721 }
1722
1723 void b43legacy_phy_lo_mark_all_unused(struct b43legacy_wldev *dev)
1724 {
1725         struct b43legacy_phy *phy = &dev->phy;
1726         struct b43legacy_lopair *pair;
1727         int i;
1728
1729         for (i = 0; i < B43legacy_LO_COUNT; i++) {
1730                 pair = phy->_lo_pairs + i;
1731                 pair->used = 0;
1732         }
1733 }
1734
1735 /* http://bcm-specs.sipsolutions.net/EstimatePowerOut
1736  * This function converts a TSSI value to dBm in Q5.2
1737  */
1738 static s8 b43legacy_phy_estimate_power_out(struct b43legacy_wldev *dev, s8 tssi)
1739 {
1740         struct b43legacy_phy *phy = &dev->phy;
1741         s8 dbm = 0;
1742         s32 tmp;
1743
1744         tmp = phy->idle_tssi;
1745         tmp += tssi;
1746         tmp -= phy->savedpctlreg;
1747
1748         switch (phy->type) {
1749         case B43legacy_PHYTYPE_B:
1750         case B43legacy_PHYTYPE_G:
1751                 tmp = clamp_val(tmp, 0x00, 0x3F);
1752                 dbm = phy->tssi2dbm[tmp];
1753                 break;
1754         default:
1755                 B43legacy_BUG_ON(1);
1756         }
1757
1758         return dbm;
1759 }
1760
1761 /* http://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
1762 void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
1763 {
1764         struct b43legacy_phy *phy = &dev->phy;
1765         u16 tmp;
1766         u16 txpower;
1767         s8 v0;
1768         s8 v1;
1769         s8 v2;
1770         s8 v3;
1771         s8 average;
1772         int max_pwr;
1773         s16 desired_pwr;
1774         s16 estimated_pwr;
1775         s16 pwr_adjust;
1776         s16 radio_att_delta;
1777         s16 baseband_att_delta;
1778         s16 radio_attenuation;
1779         s16 baseband_attenuation;
1780
1781         if (phy->savedpctlreg == 0xFFFF)
1782                 return;
1783         if ((dev->dev->bus->boardinfo.type == 0x0416) &&
1784             is_bcm_board_vendor(dev))
1785                 return;
1786 #ifdef CONFIG_B43LEGACY_DEBUG
1787         if (phy->manual_txpower_control)
1788                 return;
1789 #endif
1790
1791         B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
1792                          phy->type == B43legacy_PHYTYPE_G));
1793         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0058);
1794         v0 = (s8)(tmp & 0x00FF);
1795         v1 = (s8)((tmp & 0xFF00) >> 8);
1796         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005A);
1797         v2 = (s8)(tmp & 0x00FF);
1798         v3 = (s8)((tmp & 0xFF00) >> 8);
1799         tmp = 0;
1800
1801         if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F) {
1802                 tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
1803                                          0x0070);
1804                 v0 = (s8)(tmp & 0x00FF);
1805                 v1 = (s8)((tmp & 0xFF00) >> 8);
1806                 tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
1807                                          0x0072);
1808                 v2 = (s8)(tmp & 0x00FF);
1809                 v3 = (s8)((tmp & 0xFF00) >> 8);
1810                 if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F)
1811                         return;
1812                 v0 = (v0 + 0x20) & 0x3F;
1813                 v1 = (v1 + 0x20) & 0x3F;
1814                 v2 = (v2 + 0x20) & 0x3F;
1815                 v3 = (v3 + 0x20) & 0x3F;
1816                 tmp = 1;
1817         }
1818         b43legacy_radio_clear_tssi(dev);
1819
1820         average = (v0 + v1 + v2 + v3 + 2) / 4;
1821
1822         if (tmp && (b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005E)
1823             & 0x8))
1824                 average -= 13;
1825
1826         estimated_pwr = b43legacy_phy_estimate_power_out(dev, average);
1827
1828         max_pwr = dev->dev->bus->sprom.maxpwr_bg;
1829
1830         if ((dev->dev->bus->sprom.boardflags_lo
1831              & B43legacy_BFL_PACTRL) &&
1832             (phy->type == B43legacy_PHYTYPE_G))
1833                 max_pwr -= 0x3;
1834         if (unlikely(max_pwr <= 0)) {
1835                 b43legacywarn(dev->wl, "Invalid max-TX-power value in SPROM."
1836                         "\n");
1837                 max_pwr = 74; /* fake it */
1838                 dev->dev->bus->sprom.maxpwr_bg = max_pwr;
1839         }
1840
1841         /* Use regulatory information to get the maximum power.
1842          * In the absence of such data from mac80211, we will use 20 dBm, which
1843          * is the value for the EU, US, Canada, and most of the world.
1844          * The regulatory maximum is reduced by the antenna gain (from sprom)
1845          * and 1.5 dBm (a safety factor??). The result is in Q5.2 format
1846          * which accounts for the factor of 4 */
1847 #define REG_MAX_PWR 20
1848         max_pwr = min(REG_MAX_PWR * 4
1849                       - dev->dev->bus->sprom.antenna_gain.a0
1850                       - 0x6, max_pwr);
1851
1852         /* find the desired power in Q5.2 - power_level is in dBm
1853          * and limit it - max_pwr is already in Q5.2 */
1854         desired_pwr = clamp_val(phy->power_level << 2, 0, max_pwr);
1855         if (b43legacy_debug(dev, B43legacy_DBG_XMITPOWER))
1856                 b43legacydbg(dev->wl, "Current TX power output: " Q52_FMT
1857                        " dBm, Desired TX power output: " Q52_FMT
1858                        " dBm\n", Q52_ARG(estimated_pwr),
1859                        Q52_ARG(desired_pwr));
1860         /* Check if we need to adjust the current power. The factor of 2 is
1861          * for damping */
1862         pwr_adjust = (desired_pwr - estimated_pwr) / 2;
1863         /* RF attenuation delta
1864          * The minus sign is because lower attenuation => more power */
1865         radio_att_delta = -(pwr_adjust + 7) >> 3;
1866         /* Baseband attenuation delta */
1867         baseband_att_delta = -(pwr_adjust >> 1) - (4 * radio_att_delta);
1868         /* Do we need to adjust anything? */
1869         if ((radio_att_delta == 0) && (baseband_att_delta == 0)) {
1870                 b43legacy_phy_lo_mark_current_used(dev);
1871                 return;
1872         }
1873
1874         /* Calculate the new attenuation values. */
1875         baseband_attenuation = phy->bbatt;
1876         baseband_attenuation += baseband_att_delta;
1877         radio_attenuation = phy->rfatt;
1878         radio_attenuation += radio_att_delta;
1879
1880         /* Get baseband and radio attenuation values into permitted ranges.
1881          * baseband 0-11, radio 0-9.
1882          * Radio attenuation affects power level 4 times as much as baseband.
1883          */
1884         if (radio_attenuation < 0) {
1885                 baseband_attenuation -= (4 * -radio_attenuation);
1886                 radio_attenuation = 0;
1887         } else if (radio_attenuation > 9) {
1888                 baseband_attenuation += (4 * (radio_attenuation - 9));
1889                 radio_attenuation = 9;
1890         } else {
1891                 while (baseband_attenuation < 0 && radio_attenuation > 0) {
1892                         baseband_attenuation += 4;
1893                         radio_attenuation--;
1894                 }
1895                 while (baseband_attenuation > 11 && radio_attenuation < 9) {
1896                         baseband_attenuation -= 4;
1897                         radio_attenuation++;
1898                 }
1899         }
1900         baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
1901
1902         txpower = phy->txctl1;
1903         if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
1904                 if (radio_attenuation <= 1) {
1905                         if (txpower == 0) {
1906                                 txpower = 3;
1907                                 radio_attenuation += 2;
1908                                 baseband_attenuation += 2;
1909                         } else if (dev->dev->bus->sprom.boardflags_lo
1910                                    & B43legacy_BFL_PACTRL) {
1911                                 baseband_attenuation += 4 *
1912                                                      (radio_attenuation - 2);
1913                                 radio_attenuation = 2;
1914                         }
1915                 } else if (radio_attenuation > 4 && txpower != 0) {
1916                         txpower = 0;
1917                         if (baseband_attenuation < 3) {
1918                                 radio_attenuation -= 3;
1919                                 baseband_attenuation += 2;
1920                         } else {
1921                                 radio_attenuation -= 2;
1922                                 baseband_attenuation -= 2;
1923                         }
1924                 }
1925         }
1926         /* Save the control values */
1927         phy->txctl1 = txpower;
1928         baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
1929         radio_attenuation = clamp_val(radio_attenuation, 0, 9);
1930         phy->rfatt = radio_attenuation;
1931         phy->bbatt = baseband_attenuation;
1932
1933         /* Adjust the hardware */
1934         b43legacy_phy_lock(dev);
1935         b43legacy_radio_lock(dev);
1936         b43legacy_radio_set_txpower_bg(dev, baseband_attenuation,
1937                                        radio_attenuation, txpower);
1938         b43legacy_phy_lo_mark_current_used(dev);
1939         b43legacy_radio_unlock(dev);
1940         b43legacy_phy_unlock(dev);
1941 }
1942
1943 static inline
1944 s32 b43legacy_tssi2dbm_ad(s32 num, s32 den)
1945 {
1946         if (num < 0)
1947                 return num/den;
1948         else
1949                 return (num+den/2)/den;
1950 }
1951
1952 static inline
1953 s8 b43legacy_tssi2dbm_entry(s8 entry [], u8 index, s16 pab0, s16 pab1, s16 pab2)
1954 {
1955         s32 m1;
1956         s32 m2;
1957         s32 f = 256;
1958         s32 q;
1959         s32 delta;
1960         s8 i = 0;
1961
1962         m1 = b43legacy_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
1963         m2 = max(b43legacy_tssi2dbm_ad(32768 + index * pab2, 256), 1);
1964         do {
1965                 if (i > 15)
1966                         return -EINVAL;
1967                 q = b43legacy_tssi2dbm_ad(f * 4096 -
1968                                           b43legacy_tssi2dbm_ad(m2 * f, 16) *
1969                                           f, 2048);
1970                 delta = abs(q - f);
1971                 f = q;
1972                 i++;
1973         } while (delta >= 2);
1974         entry[index] = clamp_val(b43legacy_tssi2dbm_ad(m1 * f, 8192),
1975                                    -127, 128);
1976         return 0;
1977 }
1978
1979 /* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
1980 int b43legacy_phy_init_tssi2dbm_table(struct b43legacy_wldev *dev)
1981 {
1982         struct b43legacy_phy *phy = &dev->phy;
1983         s16 pab0;
1984         s16 pab1;
1985         s16 pab2;
1986         u8 idx;
1987         s8 *dyn_tssi2dbm;
1988
1989         B43legacy_WARN_ON(!(phy->type == B43legacy_PHYTYPE_B ||
1990                           phy->type == B43legacy_PHYTYPE_G));
1991         pab0 = (s16)(dev->dev->bus->sprom.pa0b0);
1992         pab1 = (s16)(dev->dev->bus->sprom.pa0b1);
1993         pab2 = (s16)(dev->dev->bus->sprom.pa0b2);
1994
1995         if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
1996                 phy->idle_tssi = 0x34;
1997                 phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
1998                 return 0;
1999         }
2000
2001         if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
2002             pab0 != -1 && pab1 != -1 && pab2 != -1) {
2003                 /* The pabX values are set in SPROM. Use them. */
2004                 if ((s8)dev->dev->bus->sprom.itssi_bg != 0 &&
2005                     (s8)dev->dev->bus->sprom.itssi_bg != -1)
2006                         phy->idle_tssi = (s8)(dev->dev->bus->sprom.
2007                                           itssi_bg);
2008                 else
2009                         phy->idle_tssi = 62;
2010                 dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
2011                 if (dyn_tssi2dbm == NULL) {
2012                         b43legacyerr(dev->wl, "Could not allocate memory "
2013                                "for tssi2dbm table\n");
2014                         return -ENOMEM;
2015                 }
2016                 for (idx = 0; idx < 64; idx++)
2017                         if (b43legacy_tssi2dbm_entry(dyn_tssi2dbm, idx, pab0,
2018                                                      pab1, pab2)) {
2019                                 phy->tssi2dbm = NULL;
2020                                 b43legacyerr(dev->wl, "Could not generate "
2021                                        "tssi2dBm table\n");
2022                                 kfree(dyn_tssi2dbm);
2023                                 return -ENODEV;
2024                         }
2025                 phy->tssi2dbm = dyn_tssi2dbm;
2026                 phy->dyn_tssi_tbl = 1;
2027         } else {
2028                 /* pabX values not set in SPROM. */
2029                 switch (phy->type) {
2030                 case B43legacy_PHYTYPE_B:
2031                         phy->idle_tssi = 0x34;
2032                         phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
2033                         break;
2034                 case B43legacy_PHYTYPE_G:
2035                         phy->idle_tssi = 0x34;
2036                         phy->tssi2dbm = b43legacy_tssi2dbm_g_table;
2037                         break;
2038                 }
2039         }
2040
2041         return 0;
2042 }
2043
2044 int b43legacy_phy_init(struct b43legacy_wldev *dev)
2045 {
2046         struct b43legacy_phy *phy = &dev->phy;
2047         int err = -ENODEV;
2048
2049         switch (phy->type) {
2050         case B43legacy_PHYTYPE_B:
2051                 switch (phy->rev) {
2052                 case 2:
2053                         b43legacy_phy_initb2(dev);
2054                         err = 0;
2055                         break;
2056                 case 4:
2057                         b43legacy_phy_initb4(dev);
2058                         err = 0;
2059                         break;
2060                 case 5:
2061                         b43legacy_phy_initb5(dev);
2062                         err = 0;
2063                         break;
2064                 case 6:
2065                         b43legacy_phy_initb6(dev);
2066                         err = 0;
2067                         break;
2068                 }
2069                 break;
2070         case B43legacy_PHYTYPE_G:
2071                 b43legacy_phy_initg(dev);
2072                 err = 0;
2073                 break;
2074         }
2075         if (err)
2076                 b43legacyerr(dev->wl, "Unknown PHYTYPE found\n");
2077
2078         return err;
2079 }
2080
2081 void b43legacy_phy_set_antenna_diversity(struct b43legacy_wldev *dev)
2082 {
2083         struct b43legacy_phy *phy = &dev->phy;
2084         u16 antennadiv;
2085         u16 offset;
2086         u16 value;
2087         u32 ucodeflags;
2088
2089         antennadiv = phy->antenna_diversity;
2090
2091         if (antennadiv == 0xFFFF)
2092                 antennadiv = 3;
2093         B43legacy_WARN_ON(antennadiv > 3);
2094
2095         ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2096                                           B43legacy_UCODEFLAGS_OFFSET);
2097         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
2098                               B43legacy_UCODEFLAGS_OFFSET,
2099                               ucodeflags & ~B43legacy_UCODEFLAG_AUTODIV);
2100
2101         switch (phy->type) {
2102         case B43legacy_PHYTYPE_G:
2103                 offset = 0x0400;
2104
2105                 if (antennadiv == 2)
2106                         value = (3/*automatic*/ << 7);
2107                 else
2108                         value = (antennadiv << 7);
2109                 b43legacy_phy_write(dev, offset + 1,
2110                                     (b43legacy_phy_read(dev, offset + 1)
2111                                     & 0x7E7F) | value);
2112
2113                 if (antennadiv >= 2) {
2114                         if (antennadiv == 2)
2115                                 value = (antennadiv << 7);
2116                         else
2117                                 value = (0/*force0*/ << 7);
2118                         b43legacy_phy_write(dev, offset + 0x2B,
2119                                             (b43legacy_phy_read(dev,
2120                                             offset + 0x2B)
2121                                             & 0xFEFF) | value);
2122                 }
2123
2124                 if (phy->type == B43legacy_PHYTYPE_G) {
2125                         if (antennadiv >= 2)
2126                                 b43legacy_phy_write(dev, 0x048C,
2127                                                     b43legacy_phy_read(dev,
2128                                                     0x048C) | 0x2000);
2129                         else
2130                                 b43legacy_phy_write(dev, 0x048C,
2131                                                     b43legacy_phy_read(dev,
2132                                                     0x048C) & ~0x2000);
2133                         if (phy->rev >= 2) {
2134                                 b43legacy_phy_write(dev, 0x0461,
2135                                                     b43legacy_phy_read(dev,
2136                                                     0x0461) | 0x0010);
2137                                 b43legacy_phy_write(dev, 0x04AD,
2138                                                     (b43legacy_phy_read(dev,
2139                                                     0x04AD)
2140                                                     & 0x00FF) | 0x0015);
2141                                 if (phy->rev == 2)
2142                                         b43legacy_phy_write(dev, 0x0427,
2143                                                             0x0008);
2144                                 else
2145                                         b43legacy_phy_write(dev, 0x0427,
2146                                                 (b43legacy_phy_read(dev, 0x0427)
2147                                                  & 0x00FF) | 0x0008);
2148                         } else if (phy->rev >= 6)
2149                                 b43legacy_phy_write(dev, 0x049B, 0x00DC);
2150                 } else {
2151                         if (phy->rev < 3)
2152                                 b43legacy_phy_write(dev, 0x002B,
2153                                                     (b43legacy_phy_read(dev,
2154                                                     0x002B) & 0x00FF)
2155                                                     | 0x0024);
2156                         else {
2157                                 b43legacy_phy_write(dev, 0x0061,
2158                                                     b43legacy_phy_read(dev,
2159                                                     0x0061) | 0x0010);
2160                                 if (phy->rev == 3) {
2161                                         b43legacy_phy_write(dev, 0x0093,
2162                                                             0x001D);
2163                                         b43legacy_phy_write(dev, 0x0027,
2164                                                             0x0008);
2165                                 } else {
2166                                         b43legacy_phy_write(dev, 0x0093,
2167                                                             0x003A);
2168                                         b43legacy_phy_write(dev, 0x0027,
2169                                                 (b43legacy_phy_read(dev, 0x0027)
2170                                                  & 0x00FF) | 0x0008);
2171                                 }
2172                         }
2173                 }
2174                 break;
2175         case B43legacy_PHYTYPE_B:
2176                 if (dev->dev->id.revision == 2)
2177                         value = (3/*automatic*/ << 7);
2178                 else
2179                         value = (antennadiv << 7);
2180                 b43legacy_phy_write(dev, 0x03E2,
2181                                     (b43legacy_phy_read(dev, 0x03E2)
2182                                     & 0xFE7F) | value);
2183                 break;
2184         default:
2185                 B43legacy_WARN_ON(1);
2186         }
2187
2188         if (antennadiv >= 2) {
2189                 ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2190                                                   B43legacy_UCODEFLAGS_OFFSET);
2191                 b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
2192                                       B43legacy_UCODEFLAGS_OFFSET,
2193                                       ucodeflags | B43legacy_UCODEFLAG_AUTODIV);
2194         }
2195
2196         phy->antenna_diversity = antennadiv;
2197 }
2198
2199 /* Set the PowerSavingControlBits.
2200  * Bitvalues:
2201  *   0  => unset the bit
2202  *   1  => set the bit
2203  *   -1 => calculate the bit
2204  */
2205 void b43legacy_power_saving_ctl_bits(struct b43legacy_wldev *dev,
2206                                      int bit25, int bit26)
2207 {
2208         int i;
2209         u32 status;
2210
2211 /* FIXME: Force 25 to off and 26 to on for now: */
2212 bit25 = 0;
2213 bit26 = 1;
2214
2215         if (bit25 == -1) {
2216                 /* TODO: If powersave is not off and FIXME is not set and we
2217                  *      are not in adhoc and thus is not an AP and we arei
2218                  *      associated, set bit 25 */
2219         }
2220         if (bit26 == -1) {
2221                 /* TODO: If the device is awake or this is an AP, or we are
2222                  *      scanning, or FIXME, or we are associated, or FIXME,
2223                  *      or the latest PS-Poll packet sent was successful,
2224                  *      set bit26  */
2225         }
2226         status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
2227         if (bit25)
2228                 status |= B43legacy_MACCTL_HWPS;
2229         else
2230                 status &= ~B43legacy_MACCTL_HWPS;
2231         if (bit26)
2232                 status |= B43legacy_MACCTL_AWAKE;
2233         else
2234                 status &= ~B43legacy_MACCTL_AWAKE;
2235         b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
2236         if (bit26 && dev->dev->id.revision >= 5) {
2237                 for (i = 0; i < 100; i++) {
2238                         if (b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2239                                                  0x0040) != 4)
2240                                 break;
2241                         udelay(10);
2242                 }
2243         }
2244 }