broadcom/b43legacy/phy.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003
0004   Broadcom B43legacy wireless driver
0005
0006   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
0007              Stefano Brivio <stefano.brivio@polimi.it>
0008              Michael Buesch <m@bues.ch>
0009              Danny van Dyk <kugelfang@gentoo.org>
0010      Andreas Jaggi <andreas.jaggi@waterwave.ch>
0011   Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
0012
0013   Some parts of the code in this file are derived from the ipw2200
0014   driver  Copyright(c) 2003 - 2004 Intel Corporation.
0015
0016
0017 */
0018
0019 #include <linux/delay.h>
0020 #include <linux/pci.h>
0021 #include <linux/sched.h>
0022 #include <linux/slab.h>
0023 #include <linux/types.h>
0024
0025 #include "b43legacy.h"
0026 #include "phy.h"
0027 #include "main.h"
0028 #include "radio.h"
0029 #include "ilt.h"
0030
0031
0032 static const s8 b43legacy_tssi2dbm_b_table[] = {
0033     0x4D, 0x4C, 0x4B, 0x4A,
0034     0x4A, 0x49, 0x48, 0x47,
0035     0x47, 0x46, 0x45, 0x45,
0036     0x44, 0x43, 0x42, 0x42,
0037     0x41, 0x40, 0x3F, 0x3E,
0038     0x3D, 0x3C, 0x3B, 0x3A,
0039     0x39, 0x38, 0x37, 0x36,
0040     0x35, 0x34, 0x32, 0x31,
0041     0x30, 0x2F, 0x2D, 0x2C,
0042     0x2B, 0x29, 0x28, 0x26,
0043     0x25, 0x23, 0x21, 0x1F,
0044     0x1D, 0x1A, 0x17, 0x14,
0045     0x10, 0x0C, 0x06, 0x00,
0046       -7,   -7,   -7,   -7,
0047       -7,   -7,   -7,   -7,
0048       -7,   -7,   -7,   -7,
0049 };
0050
0051 static const s8 b43legacy_tssi2dbm_g_table[] = {
0052      77,  77,  77,  76,
0053      76,  76,  75,  75,
0054      74,  74,  73,  73,
0055      73,  72,  72,  71,
0056      71,  70,  70,  69,
0057      68,  68,  67,  67,
0058      66,  65,  65,  64,
0059      63,  63,  62,  61,
0060      60,  59,  58,  57,
0061      56,  55,  54,  53,
0062      52,  50,  49,  47,
0063      45,  43,  40,  37,
0064      33,  28,  22,  14,
0065       5,  -7, -20, -20,
0066     -20, -20, -20, -20,
0067     -20, -20, -20, -20,
0068 };
0069
0070 static void b43legacy_phy_initg(struct b43legacy_wldev *dev);
0071
0072 /* Lock the PHY registers against concurrent access from the microcode.
0073  * This lock is nonrecursive. */
0074 void b43legacy_phy_lock(struct b43legacy_wldev *dev)
0075 {
0076 #if B43legacy_DEBUG
0077     B43legacy_WARN_ON(dev->phy.phy_locked);
0078     dev->phy.phy_locked = 1;
0079 #endif
0080
0081     if (dev->dev->id.revision < 3) {
0082         b43legacy_mac_suspend(dev);
0083     } else {
0084         if (!b43legacy_is_mode(dev->wl, NL80211_IFTYPE_AP))
0085             b43legacy_power_saving_ctl_bits(dev, -1, 1);
0086     }
0087 }
0088
0089 void b43legacy_phy_unlock(struct b43legacy_wldev *dev)
0090 {
0091 #if B43legacy_DEBUG
0092     B43legacy_WARN_ON(!dev->phy.phy_locked);
0093     dev->phy.phy_locked = 0;
0094 #endif
0095
0096     if (dev->dev->id.revision < 3) {
0097         b43legacy_mac_enable(dev);
0098     } else {
0099         if (!b43legacy_is_mode(dev->wl, NL80211_IFTYPE_AP))
0100             b43legacy_power_saving_ctl_bits(dev, -1, -1);
0101     }
0102 }
0103
0104 u16 b43legacy_phy_read(struct b43legacy_wldev *dev, u16 offset)
0105 {
0106     b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
0107     return b43legacy_read16(dev, B43legacy_MMIO_PHY_DATA);
0108 }
0109
0110 void b43legacy_phy_write(struct b43legacy_wldev *dev, u16 offset, u16 val)
0111 {
0112     b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
0113     b43legacy_write16(dev, B43legacy_MMIO_PHY_DATA, val);
0114 }
0115
0116 void b43legacy_phy_calibrate(struct b43legacy_wldev *dev)
0117 {
0118     struct b43legacy_phy *phy = &dev->phy;
0119
0120     b43legacy_read32(dev, B43legacy_MMIO_MACCTL); /* Dummy read. */
0121     if (phy->calibrated)
0122         return;
0123     if (phy->type == B43legacy_PHYTYPE_G && phy->rev == 1) {
0124         b43legacy_wireless_core_reset(dev, 0);
0125         b43legacy_phy_initg(dev);
0126         b43legacy_wireless_core_reset(dev, B43legacy_TMSLOW_GMODE);
0127     }
0128     phy->calibrated = 1;
0129 }
0130
0131 /* initialize B PHY power control
0132  * as described in https://bcm-specs.sipsolutions.net/InitPowerControl
0133  */
0134 static void b43legacy_phy_init_pctl(struct b43legacy_wldev *dev)
0135 {
0136     struct b43legacy_phy *phy = &dev->phy;
0137     u16 saved_batt = 0;
0138     u16 saved_ratt = 0;
0139     u16 saved_txctl1 = 0;
0140     int must_reset_txpower = 0;
0141
0142     B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
0143               phy->type == B43legacy_PHYTYPE_G));
0144     if (is_bcm_board_vendor(dev) &&
0145         (dev->dev->bus->boardinfo.type == 0x0416))
0146         return;
0147
0148     b43legacy_phy_write(dev, 0x0028, 0x8018);
0149     b43legacy_write16(dev, 0x03E6, b43legacy_read16(dev, 0x03E6) & 0xFFDF);
0150
0151     if (phy->type == B43legacy_PHYTYPE_G) {
0152         if (!phy->gmode)
0153             return;
0154         b43legacy_phy_write(dev, 0x047A, 0xC111);
0155     }
0156     if (phy->savedpctlreg != 0xFFFF)
0157         return;
0158 #ifdef CONFIG_B43LEGACY_DEBUG
0159     if (phy->manual_txpower_control)
0160         return;
0161 #endif
0162
0163     if (phy->type == B43legacy_PHYTYPE_B &&
0164         phy->rev >= 2 &&
0165         phy->radio_ver == 0x2050)
0166         b43legacy_radio_write16(dev, 0x0076,
0167                     b43legacy_radio_read16(dev, 0x0076)
0168                     | 0x0084);
0169     else {
0170         saved_batt = phy->bbatt;
0171         saved_ratt = phy->rfatt;
0172         saved_txctl1 = phy->txctl1;
0173         if ((phy->radio_rev >= 6) && (phy->radio_rev <= 8)
0174             && /*FIXME: incomplete specs for 5 < revision < 9 */ 0)
0175             b43legacy_radio_set_txpower_bg(dev, 0xB, 0x1F, 0);
0176         else
0177             b43legacy_radio_set_txpower_bg(dev, 0xB, 9, 0);
0178         must_reset_txpower = 1;
0179     }
0180     b43legacy_dummy_transmission(dev);
0181
0182     phy->savedpctlreg = b43legacy_phy_read(dev, B43legacy_PHY_G_PCTL);
0183
0184     if (must_reset_txpower)
0185         b43legacy_radio_set_txpower_bg(dev, saved_batt, saved_ratt,
0186                            saved_txctl1);
0187     else
0188         b43legacy_radio_write16(dev, 0x0076, b43legacy_radio_read16(dev,
0189                     0x0076) & 0xFF7B);
0190     b43legacy_radio_clear_tssi(dev);
0191 }
0192
0193 static void b43legacy_phy_agcsetup(struct b43legacy_wldev *dev)
0194 {
0195     struct b43legacy_phy *phy = &dev->phy;
0196     u16 offset = 0x0000;
0197
0198     if (phy->rev == 1)
0199         offset = 0x4C00;
0200
0201     b43legacy_ilt_write(dev, offset, 0x00FE);
0202     b43legacy_ilt_write(dev, offset + 1, 0x000D);
0203     b43legacy_ilt_write(dev, offset + 2, 0x0013);
0204     b43legacy_ilt_write(dev, offset + 3, 0x0019);
0205
0206     if (phy->rev == 1) {
0207         b43legacy_ilt_write(dev, 0x1800, 0x2710);
0208         b43legacy_ilt_write(dev, 0x1801, 0x9B83);
0209         b43legacy_ilt_write(dev, 0x1802, 0x9B83);
0210         b43legacy_ilt_write(dev, 0x1803, 0x0F8D);
0211         b43legacy_phy_write(dev, 0x0455, 0x0004);
0212     }
0213
0214     b43legacy_phy_write(dev, 0x04A5, (b43legacy_phy_read(dev, 0x04A5)
0215                       & 0x00FF) | 0x5700);
0216     b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
0217                       & 0xFF80) | 0x000F);
0218     b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
0219                       & 0xC07F) | 0x2B80);
0220     b43legacy_phy_write(dev, 0x048C, (b43legacy_phy_read(dev, 0x048C)
0221                       & 0xF0FF) | 0x0300);
0222
0223     b43legacy_radio_write16(dev, 0x007A,
0224                 b43legacy_radio_read16(dev, 0x007A)
0225                 | 0x0008);
0226
0227     b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
0228                 & 0xFFF0) | 0x0008);
0229     b43legacy_phy_write(dev, 0x04A1, (b43legacy_phy_read(dev, 0x04A1)
0230                 & 0xF0FF) | 0x0600);
0231     b43legacy_phy_write(dev, 0x04A2, (b43legacy_phy_read(dev, 0x04A2)
0232                 & 0xF0FF) | 0x0700);
0233     b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
0234                 & 0xF0FF) | 0x0100);
0235
0236     if (phy->rev == 1)
0237         b43legacy_phy_write(dev, 0x04A2,
0238                     (b43legacy_phy_read(dev, 0x04A2)
0239                     & 0xFFF0) | 0x0007);
0240
0241     b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
0242                 & 0xFF00) | 0x001C);
0243     b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
0244                 & 0xC0FF) | 0x0200);
0245     b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
0246                 & 0xFF00) | 0x001C);
0247     b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
0248                 & 0xFF00) | 0x0020);
0249     b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
0250                 & 0xC0FF) | 0x0200);
0251     b43legacy_phy_write(dev, 0x0482, (b43legacy_phy_read(dev, 0x0482)
0252                 & 0xFF00) | 0x002E);
0253     b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
0254                 & 0x00FF) | 0x1A00);
0255     b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
0256                 & 0xFF00) | 0x0028);
0257     b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
0258                 & 0x00FF) | 0x2C00);
0259
0260     if (phy->rev == 1) {
0261         b43legacy_phy_write(dev, 0x0430, 0x092B);
0262         b43legacy_phy_write(dev, 0x041B,
0263                     (b43legacy_phy_read(dev, 0x041B)
0264                     & 0xFFE1) | 0x0002);
0265     } else {
0266         b43legacy_phy_write(dev, 0x041B,
0267                     b43legacy_phy_read(dev, 0x041B) & 0xFFE1);
0268         b43legacy_phy_write(dev, 0x041F, 0x287A);
0269         b43legacy_phy_write(dev, 0x0420,
0270                     (b43legacy_phy_read(dev, 0x0420)
0271                     & 0xFFF0) | 0x0004);
0272     }
0273
0274     if (phy->rev > 2) {
0275         b43legacy_phy_write(dev, 0x0422, 0x287A);
0276         b43legacy_phy_write(dev, 0x0420,
0277                     (b43legacy_phy_read(dev, 0x0420)
0278                     & 0x0FFF) | 0x3000);
0279     }
0280
0281     b43legacy_phy_write(dev, 0x04A8, (b43legacy_phy_read(dev, 0x04A8)
0282                 & 0x8080) | 0x7874);
0283     b43legacy_phy_write(dev, 0x048E, 0x1C00);
0284
0285     if (phy->rev == 1) {
0286         b43legacy_phy_write(dev, 0x04AB,
0287                     (b43legacy_phy_read(dev, 0x04AB)
0288                     & 0xF0FF) | 0x0600);
0289         b43legacy_phy_write(dev, 0x048B, 0x005E);
0290         b43legacy_phy_write(dev, 0x048C,
0291                     (b43legacy_phy_read(dev, 0x048C) & 0xFF00)
0292                     | 0x001E);
0293         b43legacy_phy_write(dev, 0x048D, 0x0002);
0294     }
0295
0296     b43legacy_ilt_write(dev, offset + 0x0800, 0);
0297     b43legacy_ilt_write(dev, offset + 0x0801, 7);
0298     b43legacy_ilt_write(dev, offset + 0x0802, 16);
0299     b43legacy_ilt_write(dev, offset + 0x0803, 28);
0300
0301     if (phy->rev >= 6) {
0302         b43legacy_phy_write(dev, 0x0426,
0303                     (b43legacy_phy_read(dev, 0x0426) & 0xFFFC));
0304         b43legacy_phy_write(dev, 0x0426,
0305                     (b43legacy_phy_read(dev, 0x0426) & 0xEFFF));
0306     }
0307 }
0308
0309 static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
0310 {
0311     struct b43legacy_phy *phy = &dev->phy;
0312     u16 i;
0313
0314     B43legacy_BUG_ON(phy->type != B43legacy_PHYTYPE_G);
0315     if (phy->rev == 1) {
0316         b43legacy_phy_write(dev, 0x0406, 0x4F19);
0317         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0318                     (b43legacy_phy_read(dev,
0319                     B43legacy_PHY_G_CRS) & 0xFC3F) | 0x0340);
0320         b43legacy_phy_write(dev, 0x042C, 0x005A);
0321         b43legacy_phy_write(dev, 0x0427, 0x001A);
0322
0323         for (i = 0; i < B43legacy_ILT_FINEFREQG_SIZE; i++)
0324             b43legacy_ilt_write(dev, 0x5800 + i,
0325                         b43legacy_ilt_finefreqg[i]);
0326         for (i = 0; i < B43legacy_ILT_NOISEG1_SIZE; i++)
0327             b43legacy_ilt_write(dev, 0x1800 + i,
0328                         b43legacy_ilt_noiseg1[i]);
0329         for (i = 0; i < B43legacy_ILT_ROTOR_SIZE; i++)
0330             b43legacy_ilt_write32(dev, 0x2000 + i,
0331                           b43legacy_ilt_rotor[i]);
0332     } else {
0333         /* nrssi values are signed 6-bit values. Why 0x7654 here? */
0334         b43legacy_nrssi_hw_write(dev, 0xBA98, (s16)0x7654);
0335
0336         if (phy->rev == 2) {
0337             b43legacy_phy_write(dev, 0x04C0, 0x1861);
0338             b43legacy_phy_write(dev, 0x04C1, 0x0271);
0339         } else if (phy->rev > 2) {
0340             b43legacy_phy_write(dev, 0x04C0, 0x0098);
0341             b43legacy_phy_write(dev, 0x04C1, 0x0070);
0342             b43legacy_phy_write(dev, 0x04C9, 0x0080);
0343         }
0344         b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev,
0345                     0x042B) | 0x800);
0346
0347         for (i = 0; i < 64; i++)
0348             b43legacy_ilt_write(dev, 0x4000 + i, i);
0349         for (i = 0; i < B43legacy_ILT_NOISEG2_SIZE; i++)
0350             b43legacy_ilt_write(dev, 0x1800 + i,
0351                         b43legacy_ilt_noiseg2[i]);
0352     }
0353
0354     if (phy->rev <= 2)
0355         for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
0356             b43legacy_ilt_write(dev, 0x1400 + i,
0357                         b43legacy_ilt_noisescaleg1[i]);
0358     else if ((phy->rev >= 7) && (b43legacy_phy_read(dev, 0x0449) & 0x0200))
0359         for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
0360             b43legacy_ilt_write(dev, 0x1400 + i,
0361                         b43legacy_ilt_noisescaleg3[i]);
0362     else
0363         for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
0364             b43legacy_ilt_write(dev, 0x1400 + i,
0365                         b43legacy_ilt_noisescaleg2[i]);
0366
0367     if (phy->rev == 2)
0368         for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
0369             b43legacy_ilt_write(dev, 0x5000 + i,
0370                         b43legacy_ilt_sigmasqr1[i]);
0371     else if ((phy->rev > 2) && (phy->rev <= 8))
0372         for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
0373             b43legacy_ilt_write(dev, 0x5000 + i,
0374                         b43legacy_ilt_sigmasqr2[i]);
0375
0376     if (phy->rev == 1) {
0377         for (i = 0; i < B43legacy_ILT_RETARD_SIZE; i++)
0378             b43legacy_ilt_write32(dev, 0x2400 + i,
0379                           b43legacy_ilt_retard[i]);
0380         for (i = 4; i < 20; i++)
0381             b43legacy_ilt_write(dev, 0x5400 + i, 0x0020);
0382         b43legacy_phy_agcsetup(dev);
0383
0384         if (is_bcm_board_vendor(dev) &&
0385             (dev->dev->bus->boardinfo.type == 0x0416) &&
0386             (dev->dev->bus->sprom.board_rev == 0x0017))
0387             return;
0388
0389         b43legacy_ilt_write(dev, 0x5001, 0x0002);
0390         b43legacy_ilt_write(dev, 0x5002, 0x0001);
0391     } else {
0392         for (i = 0; i <= 0x20; i++)
0393             b43legacy_ilt_write(dev, 0x1000 + i, 0x0820);
0394         b43legacy_phy_agcsetup(dev);
0395         b43legacy_phy_read(dev, 0x0400); /* dummy read */
0396         b43legacy_phy_write(dev, 0x0403, 0x1000);
0397         b43legacy_ilt_write(dev, 0x3C02, 0x000F);
0398         b43legacy_ilt_write(dev, 0x3C03, 0x0014);
0399
0400         if (is_bcm_board_vendor(dev) &&
0401             (dev->dev->bus->boardinfo.type == 0x0416) &&
0402             (dev->dev->bus->sprom.board_rev == 0x0017))
0403             return;
0404
0405         b43legacy_ilt_write(dev, 0x0401, 0x0002);
0406         b43legacy_ilt_write(dev, 0x0402, 0x0001);
0407     }
0408 }
0409
0410 /* Initialize the APHY portion of a GPHY. */
0411 static void b43legacy_phy_inita(struct b43legacy_wldev *dev)
0412 {
0413
0414     might_sleep();
0415
0416     b43legacy_phy_setupg(dev);
0417     if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_PACTRL)
0418         b43legacy_phy_write(dev, 0x046E, 0x03CF);
0419 }
0420
0421 static void b43legacy_phy_initb2(struct b43legacy_wldev *dev)
0422 {
0423     struct b43legacy_phy *phy = &dev->phy;
0424     u16 offset;
0425     int val;
0426
0427     b43legacy_write16(dev, 0x03EC, 0x3F22);
0428     b43legacy_phy_write(dev, 0x0020, 0x301C);
0429     b43legacy_phy_write(dev, 0x0026, 0x0000);
0430     b43legacy_phy_write(dev, 0x0030, 0x00C6);
0431     b43legacy_phy_write(dev, 0x0088, 0x3E00);
0432     val = 0x3C3D;
0433     for (offset = 0x0089; offset < 0x00A7; offset++) {
0434         b43legacy_phy_write(dev, offset, val);
0435         val -= 0x0202;
0436     }
0437     b43legacy_phy_write(dev, 0x03E4, 0x3000);
0438     b43legacy_radio_selectchannel(dev, phy->channel, 0);
0439     if (phy->radio_ver != 0x2050) {
0440         b43legacy_radio_write16(dev, 0x0075, 0x0080);
0441         b43legacy_radio_write16(dev, 0x0079, 0x0081);
0442     }
0443     b43legacy_radio_write16(dev, 0x0050, 0x0020);
0444     b43legacy_radio_write16(dev, 0x0050, 0x0023);
0445     if (phy->radio_ver == 0x2050) {
0446         b43legacy_radio_write16(dev, 0x0050, 0x0020);
0447         b43legacy_radio_write16(dev, 0x005A, 0x0070);
0448         b43legacy_radio_write16(dev, 0x005B, 0x007B);
0449         b43legacy_radio_write16(dev, 0x005C, 0x00B0);
0450         b43legacy_radio_write16(dev, 0x007A, 0x000F);
0451         b43legacy_phy_write(dev, 0x0038, 0x0677);
0452         b43legacy_radio_init2050(dev);
0453     }
0454     b43legacy_phy_write(dev, 0x0014, 0x0080);
0455     b43legacy_phy_write(dev, 0x0032, 0x00CA);
0456     b43legacy_phy_write(dev, 0x0032, 0x00CC);
0457     b43legacy_phy_write(dev, 0x0035, 0x07C2);
0458     b43legacy_phy_lo_b_measure(dev);
0459     b43legacy_phy_write(dev, 0x0026, 0xCC00);
0460     if (phy->radio_ver != 0x2050)
0461         b43legacy_phy_write(dev, 0x0026, 0xCE00);
0462     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1000);
0463     b43legacy_phy_write(dev, 0x002A, 0x88A3);
0464     if (phy->radio_ver != 0x2050)
0465         b43legacy_phy_write(dev, 0x002A, 0x88C2);
0466     b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
0467     b43legacy_phy_init_pctl(dev);
0468 }
0469
0470 static void b43legacy_phy_initb4(struct b43legacy_wldev *dev)
0471 {
0472     struct b43legacy_phy *phy = &dev->phy;
0473     u16 offset;
0474     u16 val;
0475
0476     b43legacy_write16(dev, 0x03EC, 0x3F22);
0477     b43legacy_phy_write(dev, 0x0020, 0x301C);
0478     b43legacy_phy_write(dev, 0x0026, 0x0000);
0479     b43legacy_phy_write(dev, 0x0030, 0x00C6);
0480     b43legacy_phy_write(dev, 0x0088, 0x3E00);
0481     val = 0x3C3D;
0482     for (offset = 0x0089; offset < 0x00A7; offset++) {
0483         b43legacy_phy_write(dev, offset, val);
0484         val -= 0x0202;
0485     }
0486     b43legacy_phy_write(dev, 0x03E4, 0x3000);
0487     b43legacy_radio_selectchannel(dev, phy->channel, 0);
0488     if (phy->radio_ver != 0x2050) {
0489         b43legacy_radio_write16(dev, 0x0075, 0x0080);
0490         b43legacy_radio_write16(dev, 0x0079, 0x0081);
0491     }
0492     b43legacy_radio_write16(dev, 0x0050, 0x0020);
0493     b43legacy_radio_write16(dev, 0x0050, 0x0023);
0494     if (phy->radio_ver == 0x2050) {
0495         b43legacy_radio_write16(dev, 0x0050, 0x0020);
0496         b43legacy_radio_write16(dev, 0x005A, 0x0070);
0497         b43legacy_radio_write16(dev, 0x005B, 0x007B);
0498         b43legacy_radio_write16(dev, 0x005C, 0x00B0);
0499         b43legacy_radio_write16(dev, 0x007A, 0x000F);
0500         b43legacy_phy_write(dev, 0x0038, 0x0677);
0501         b43legacy_radio_init2050(dev);
0502     }
0503     b43legacy_phy_write(dev, 0x0014, 0x0080);
0504     b43legacy_phy_write(dev, 0x0032, 0x00CA);
0505     if (phy->radio_ver == 0x2050)
0506         b43legacy_phy_write(dev, 0x0032, 0x00E0);
0507     b43legacy_phy_write(dev, 0x0035, 0x07C2);
0508
0509     b43legacy_phy_lo_b_measure(dev);
0510
0511     b43legacy_phy_write(dev, 0x0026, 0xCC00);
0512     if (phy->radio_ver == 0x2050)
0513         b43legacy_phy_write(dev, 0x0026, 0xCE00);
0514     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1100);
0515     b43legacy_phy_write(dev, 0x002A, 0x88A3);
0516     if (phy->radio_ver == 0x2050)
0517         b43legacy_phy_write(dev, 0x002A, 0x88C2);
0518     b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
0519     if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_RSSI) {
0520         b43legacy_calc_nrssi_slope(dev);
0521         b43legacy_calc_nrssi_threshold(dev);
0522     }
0523     b43legacy_phy_init_pctl(dev);
0524 }
0525
0526 static void b43legacy_phy_initb5(struct b43legacy_wldev *dev)
0527 {
0528     struct b43legacy_phy *phy = &dev->phy;
0529     u16 offset;
0530     u16 value;
0531     u8 old_channel;
0532
0533     if (phy->analog == 1)
0534         b43legacy_radio_write16(dev, 0x007A,
0535                     b43legacy_radio_read16(dev, 0x007A)
0536                     | 0x0050);
0537     if (!is_bcm_board_vendor(dev) &&
0538         (dev->dev->bus->boardinfo.type != 0x0416)) {
0539         value = 0x2120;
0540         for (offset = 0x00A8 ; offset < 0x00C7; offset++) {
0541             b43legacy_phy_write(dev, offset, value);
0542             value += 0x0202;
0543         }
0544     }
0545     b43legacy_phy_write(dev, 0x0035,
0546                 (b43legacy_phy_read(dev, 0x0035) & 0xF0FF)
0547                 | 0x0700);
0548     if (phy->radio_ver == 0x2050)
0549         b43legacy_phy_write(dev, 0x0038, 0x0667);
0550
0551     if (phy->gmode) {
0552         if (phy->radio_ver == 0x2050) {
0553             b43legacy_radio_write16(dev, 0x007A,
0554                     b43legacy_radio_read16(dev, 0x007A)
0555                     | 0x0020);
0556             b43legacy_radio_write16(dev, 0x0051,
0557                     b43legacy_radio_read16(dev, 0x0051)
0558                     | 0x0004);
0559         }
0560         b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO, 0x0000);
0561
0562         b43legacy_phy_write(dev, 0x0802, b43legacy_phy_read(dev, 0x0802)
0563                     | 0x0100);
0564         b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev, 0x042B)
0565                     | 0x2000);
0566
0567         b43legacy_phy_write(dev, 0x001C, 0x186A);
0568
0569         b43legacy_phy_write(dev, 0x0013, (b43legacy_phy_read(dev,
0570                     0x0013) & 0x00FF) | 0x1900);
0571         b43legacy_phy_write(dev, 0x0035, (b43legacy_phy_read(dev,
0572                     0x0035) & 0xFFC0) | 0x0064);
0573         b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
0574                     0x005D) & 0xFF80) | 0x000A);
0575         b43legacy_phy_write(dev, 0x5B, 0x0000);
0576         b43legacy_phy_write(dev, 0x5C, 0x0000);
0577     }
0578
0579     if (dev->bad_frames_preempt)
0580         b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
0581                     b43legacy_phy_read(dev,
0582                     B43legacy_PHY_RADIO_BITFIELD) | (1 << 12));
0583
0584     if (phy->analog == 1) {
0585         b43legacy_phy_write(dev, 0x0026, 0xCE00);
0586         b43legacy_phy_write(dev, 0x0021, 0x3763);
0587         b43legacy_phy_write(dev, 0x0022, 0x1BC3);
0588         b43legacy_phy_write(dev, 0x0023, 0x06F9);
0589         b43legacy_phy_write(dev, 0x0024, 0x037E);
0590     } else
0591         b43legacy_phy_write(dev, 0x0026, 0xCC00);
0592     b43legacy_phy_write(dev, 0x0030, 0x00C6);
0593     b43legacy_write16(dev, 0x03EC, 0x3F22);
0594
0595     if (phy->analog == 1)
0596         b43legacy_phy_write(dev, 0x0020, 0x3E1C);
0597     else
0598         b43legacy_phy_write(dev, 0x0020, 0x301C);
0599
0600     if (phy->analog == 0)
0601         b43legacy_write16(dev, 0x03E4, 0x3000);
0602
0603     old_channel = (phy->channel == 0xFF) ? 1 : phy->channel;
0604     /* Force to channel 7, even if not supported. */
0605     b43legacy_radio_selectchannel(dev, 7, 0);
0606
0607     if (phy->radio_ver != 0x2050) {
0608         b43legacy_radio_write16(dev, 0x0075, 0x0080);
0609         b43legacy_radio_write16(dev, 0x0079, 0x0081);
0610     }
0611
0612     b43legacy_radio_write16(dev, 0x0050, 0x0020);
0613     b43legacy_radio_write16(dev, 0x0050, 0x0023);
0614
0615     if (phy->radio_ver == 0x2050) {
0616         b43legacy_radio_write16(dev, 0x0050, 0x0020);
0617         b43legacy_radio_write16(dev, 0x005A, 0x0070);
0618     }
0619
0620     b43legacy_radio_write16(dev, 0x005B, 0x007B);
0621     b43legacy_radio_write16(dev, 0x005C, 0x00B0);
0622
0623     b43legacy_radio_write16(dev, 0x007A, b43legacy_radio_read16(dev,
0624                 0x007A) | 0x0007);
0625
0626     b43legacy_radio_selectchannel(dev, old_channel, 0);
0627
0628     b43legacy_phy_write(dev, 0x0014, 0x0080);
0629     b43legacy_phy_write(dev, 0x0032, 0x00CA);
0630     b43legacy_phy_write(dev, 0x002A, 0x88A3);
0631
0632     b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
0633
0634     if (phy->radio_ver == 0x2050)
0635         b43legacy_radio_write16(dev, 0x005D, 0x000D);
0636
0637     b43legacy_write16(dev, 0x03E4, (b43legacy_read16(dev, 0x03E4) &
0638               0xFFC0) | 0x0004);
0639 }
0640
0641 static void b43legacy_phy_initb6(struct b43legacy_wldev *dev)
0642 {
0643     struct b43legacy_phy *phy = &dev->phy;
0644     u16 offset;
0645     u16 val;
0646     u8 old_channel;
0647
0648     b43legacy_phy_write(dev, 0x003E, 0x817A);
0649     b43legacy_radio_write16(dev, 0x007A,
0650                 (b43legacy_radio_read16(dev, 0x007A) | 0x0058));
0651     if (phy->radio_rev == 4 ||
0652          phy->radio_rev == 5) {
0653         b43legacy_radio_write16(dev, 0x0051, 0x0037);
0654         b43legacy_radio_write16(dev, 0x0052, 0x0070);
0655         b43legacy_radio_write16(dev, 0x0053, 0x00B3);
0656         b43legacy_radio_write16(dev, 0x0054, 0x009B);
0657         b43legacy_radio_write16(dev, 0x005A, 0x0088);
0658         b43legacy_radio_write16(dev, 0x005B, 0x0088);
0659         b43legacy_radio_write16(dev, 0x005D, 0x0088);
0660         b43legacy_radio_write16(dev, 0x005E, 0x0088);
0661         b43legacy_radio_write16(dev, 0x007D, 0x0088);
0662         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
0663                       B43legacy_UCODEFLAGS_OFFSET,
0664                       (b43legacy_shm_read32(dev,
0665                       B43legacy_SHM_SHARED,
0666                       B43legacy_UCODEFLAGS_OFFSET)
0667                       | 0x00000200));
0668     }
0669     if (phy->radio_rev == 8) {
0670         b43legacy_radio_write16(dev, 0x0051, 0x0000);
0671         b43legacy_radio_write16(dev, 0x0052, 0x0040);
0672         b43legacy_radio_write16(dev, 0x0053, 0x00B7);
0673         b43legacy_radio_write16(dev, 0x0054, 0x0098);
0674         b43legacy_radio_write16(dev, 0x005A, 0x0088);
0675         b43legacy_radio_write16(dev, 0x005B, 0x006B);
0676         b43legacy_radio_write16(dev, 0x005C, 0x000F);
0677         if (dev->dev->bus->sprom.boardflags_lo & 0x8000) {
0678             b43legacy_radio_write16(dev, 0x005D, 0x00FA);
0679             b43legacy_radio_write16(dev, 0x005E, 0x00D8);
0680         } else {
0681             b43legacy_radio_write16(dev, 0x005D, 0x00F5);
0682             b43legacy_radio_write16(dev, 0x005E, 0x00B8);
0683         }
0684         b43legacy_radio_write16(dev, 0x0073, 0x0003);
0685         b43legacy_radio_write16(dev, 0x007D, 0x00A8);
0686         b43legacy_radio_write16(dev, 0x007C, 0x0001);
0687         b43legacy_radio_write16(dev, 0x007E, 0x0008);
0688     }
0689     val = 0x1E1F;
0690     for (offset = 0x0088; offset < 0x0098; offset++) {
0691         b43legacy_phy_write(dev, offset, val);
0692         val -= 0x0202;
0693     }
0694     val = 0x3E3F;
0695     for (offset = 0x0098; offset < 0x00A8; offset++) {
0696         b43legacy_phy_write(dev, offset, val);
0697         val -= 0x0202;
0698     }
0699     val = 0x2120;
0700     for (offset = 0x00A8; offset < 0x00C8; offset++) {
0701         b43legacy_phy_write(dev, offset, (val & 0x3F3F));
0702         val += 0x0202;
0703     }
0704     if (phy->type == B43legacy_PHYTYPE_G) {
0705         b43legacy_radio_write16(dev, 0x007A,
0706                     b43legacy_radio_read16(dev, 0x007A) |
0707                     0x0020);
0708         b43legacy_radio_write16(dev, 0x0051,
0709                     b43legacy_radio_read16(dev, 0x0051) |
0710                     0x0004);
0711         b43legacy_phy_write(dev, 0x0802,
0712                     b43legacy_phy_read(dev, 0x0802) | 0x0100);
0713         b43legacy_phy_write(dev, 0x042B,
0714                     b43legacy_phy_read(dev, 0x042B) | 0x2000);
0715         b43legacy_phy_write(dev, 0x5B, 0x0000);
0716         b43legacy_phy_write(dev, 0x5C, 0x0000);
0717     }
0718
0719     old_channel = phy->channel;
0720     if (old_channel >= 8)
0721         b43legacy_radio_selectchannel(dev, 1, 0);
0722     else
0723         b43legacy_radio_selectchannel(dev, 13, 0);
0724
0725     b43legacy_radio_write16(dev, 0x0050, 0x0020);
0726     b43legacy_radio_write16(dev, 0x0050, 0x0023);
0727     udelay(40);
0728     if (phy->radio_rev < 6 || phy->radio_rev == 8) {
0729         b43legacy_radio_write16(dev, 0x007C,
0730                     (b43legacy_radio_read16(dev, 0x007C)
0731                     | 0x0002));
0732         b43legacy_radio_write16(dev, 0x0050, 0x0020);
0733     }
0734     if (phy->radio_rev <= 2) {
0735         b43legacy_radio_write16(dev, 0x0050, 0x0020);
0736         b43legacy_radio_write16(dev, 0x005A, 0x0070);
0737         b43legacy_radio_write16(dev, 0x005B, 0x007B);
0738         b43legacy_radio_write16(dev, 0x005C, 0x00B0);
0739     }
0740     b43legacy_radio_write16(dev, 0x007A,
0741                 (b43legacy_radio_read16(dev,
0742                 0x007A) & 0x00F8) | 0x0007);
0743
0744     b43legacy_radio_selectchannel(dev, old_channel, 0);
0745
0746     b43legacy_phy_write(dev, 0x0014, 0x0200);
0747     if (phy->radio_rev >= 6)
0748         b43legacy_phy_write(dev, 0x002A, 0x88C2);
0749     else
0750         b43legacy_phy_write(dev, 0x002A, 0x8AC0);
0751     b43legacy_phy_write(dev, 0x0038, 0x0668);
0752     b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
0753     if (phy->radio_rev == 4 || phy->radio_rev == 5)
0754         b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
0755                     0x005D) & 0xFF80) | 0x0003);
0756     if (phy->radio_rev <= 2)
0757         b43legacy_radio_write16(dev, 0x005D, 0x000D);
0758
0759     if (phy->analog == 4) {
0760         b43legacy_write16(dev, 0x03E4, 0x0009);
0761         b43legacy_phy_write(dev, 0x61, b43legacy_phy_read(dev, 0x61)
0762                     & 0xFFF);
0763     } else
0764         b43legacy_phy_write(dev, 0x0002, (b43legacy_phy_read(dev,
0765                     0x0002) & 0xFFC0) | 0x0004);
0766     if (phy->type == B43legacy_PHYTYPE_G)
0767         b43legacy_write16(dev, 0x03E6, 0x0);
0768     if (phy->type == B43legacy_PHYTYPE_B) {
0769         b43legacy_write16(dev, 0x03E6, 0x8140);
0770         b43legacy_phy_write(dev, 0x0016, 0x0410);
0771         b43legacy_phy_write(dev, 0x0017, 0x0820);
0772         b43legacy_phy_write(dev, 0x0062, 0x0007);
0773         b43legacy_radio_init2050(dev);
0774         b43legacy_phy_lo_g_measure(dev);
0775         if (dev->dev->bus->sprom.boardflags_lo &
0776             B43legacy_BFL_RSSI) {
0777             b43legacy_calc_nrssi_slope(dev);
0778             b43legacy_calc_nrssi_threshold(dev);
0779         }
0780         b43legacy_phy_init_pctl(dev);
0781     }
0782 }
0783
0784 static void b43legacy_calc_loopback_gain(struct b43legacy_wldev *dev)
0785 {
0786     struct b43legacy_phy *phy = &dev->phy;
0787     u16 backup_phy[15] = {0};
0788     u16 backup_radio[3];
0789     u16 backup_bband;
0790     u16 i;
0791     u16 loop1_cnt;
0792     u16 loop1_done;
0793     u16 loop1_omitted;
0794     u16 loop2_done;
0795
0796     backup_phy[0] = b43legacy_phy_read(dev, 0x0429);
0797     backup_phy[1] = b43legacy_phy_read(dev, 0x0001);
0798     backup_phy[2] = b43legacy_phy_read(dev, 0x0811);
0799     backup_phy[3] = b43legacy_phy_read(dev, 0x0812);
0800     if (phy->rev != 1) {
0801         backup_phy[4] = b43legacy_phy_read(dev, 0x0814);
0802         backup_phy[5] = b43legacy_phy_read(dev, 0x0815);
0803     }
0804     backup_phy[6] = b43legacy_phy_read(dev, 0x005A);
0805     backup_phy[7] = b43legacy_phy_read(dev, 0x0059);
0806     backup_phy[8] = b43legacy_phy_read(dev, 0x0058);
0807     backup_phy[9] = b43legacy_phy_read(dev, 0x000A);
0808     backup_phy[10] = b43legacy_phy_read(dev, 0x0003);
0809     backup_phy[11] = b43legacy_phy_read(dev, 0x080F);
0810     backup_phy[12] = b43legacy_phy_read(dev, 0x0810);
0811     backup_phy[13] = b43legacy_phy_read(dev, 0x002B);
0812     backup_phy[14] = b43legacy_phy_read(dev, 0x0015);
0813     b43legacy_phy_read(dev, 0x002D); /* dummy read */
0814     backup_bband = phy->bbatt;
0815     backup_radio[0] = b43legacy_radio_read16(dev, 0x0052);
0816     backup_radio[1] = b43legacy_radio_read16(dev, 0x0043);
0817     backup_radio[2] = b43legacy_radio_read16(dev, 0x007A);
0818
0819     b43legacy_phy_write(dev, 0x0429,
0820                 b43legacy_phy_read(dev, 0x0429) & 0x3FFF);
0821     b43legacy_phy_write(dev, 0x0001,
0822                 b43legacy_phy_read(dev, 0x0001) & 0x8000);
0823     b43legacy_phy_write(dev, 0x0811,
0824                 b43legacy_phy_read(dev, 0x0811) | 0x0002);
0825     b43legacy_phy_write(dev, 0x0812,
0826                 b43legacy_phy_read(dev, 0x0812) & 0xFFFD);
0827     b43legacy_phy_write(dev, 0x0811,
0828                 b43legacy_phy_read(dev, 0x0811) | 0x0001);
0829     b43legacy_phy_write(dev, 0x0812,
0830                 b43legacy_phy_read(dev, 0x0812) & 0xFFFE);
0831     if (phy->rev != 1) {
0832         b43legacy_phy_write(dev, 0x0814,
0833                     b43legacy_phy_read(dev, 0x0814) | 0x0001);
0834         b43legacy_phy_write(dev, 0x0815,
0835                     b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
0836         b43legacy_phy_write(dev, 0x0814,
0837                     b43legacy_phy_read(dev, 0x0814) | 0x0002);
0838         b43legacy_phy_write(dev, 0x0815,
0839                     b43legacy_phy_read(dev, 0x0815) & 0xFFFD);
0840     }
0841     b43legacy_phy_write(dev, 0x0811, b43legacy_phy_read(dev, 0x0811) |
0842                 0x000C);
0843     b43legacy_phy_write(dev, 0x0812, b43legacy_phy_read(dev, 0x0812) |
0844                 0x000C);
0845
0846     b43legacy_phy_write(dev, 0x0811, (b43legacy_phy_read(dev, 0x0811)
0847                 & 0xFFCF) | 0x0030);
0848     b43legacy_phy_write(dev, 0x0812, (b43legacy_phy_read(dev, 0x0812)
0849                 & 0xFFCF) | 0x0010);
0850
0851     b43legacy_phy_write(dev, 0x005A, 0x0780);
0852     b43legacy_phy_write(dev, 0x0059, 0xC810);
0853     b43legacy_phy_write(dev, 0x0058, 0x000D);
0854     if (phy->analog == 0)
0855         b43legacy_phy_write(dev, 0x0003, 0x0122);
0856     else
0857         b43legacy_phy_write(dev, 0x000A,
0858                     b43legacy_phy_read(dev, 0x000A)
0859                     | 0x2000);
0860     if (phy->rev != 1) {
0861         b43legacy_phy_write(dev, 0x0814,
0862                     b43legacy_phy_read(dev, 0x0814) | 0x0004);
0863         b43legacy_phy_write(dev, 0x0815,
0864                     b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
0865     }
0866     b43legacy_phy_write(dev, 0x0003,
0867                 (b43legacy_phy_read(dev, 0x0003)
0868                  & 0xFF9F) | 0x0040);
0869     if (phy->radio_ver == 0x2050 && phy->radio_rev == 2) {
0870         b43legacy_radio_write16(dev, 0x0052, 0x0000);
0871         b43legacy_radio_write16(dev, 0x0043,
0872                     (b43legacy_radio_read16(dev, 0x0043)
0873                      & 0xFFF0) | 0x0009);
0874         loop1_cnt = 9;
0875     } else if (phy->radio_rev == 8) {
0876         b43legacy_radio_write16(dev, 0x0043, 0x000F);
0877         loop1_cnt = 15;
0878     } else
0879         loop1_cnt = 0;
0880
0881     b43legacy_phy_set_baseband_attenuation(dev, 11);
0882
0883     if (phy->rev >= 3)
0884         b43legacy_phy_write(dev, 0x080F, 0xC020);
0885     else
0886         b43legacy_phy_write(dev, 0x080F, 0x8020);
0887     b43legacy_phy_write(dev, 0x0810, 0x0000);
0888
0889     b43legacy_phy_write(dev, 0x002B,
0890                 (b43legacy_phy_read(dev, 0x002B)
0891                  & 0xFFC0) | 0x0001);
0892     b43legacy_phy_write(dev, 0x002B,
0893                 (b43legacy_phy_read(dev, 0x002B)
0894                  & 0xC0FF) | 0x0800);
0895     b43legacy_phy_write(dev, 0x0811,
0896                 b43legacy_phy_read(dev, 0x0811) | 0x0100);
0897     b43legacy_phy_write(dev, 0x0812,
0898                 b43legacy_phy_read(dev, 0x0812) & 0xCFFF);
0899     if (dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_EXTLNA) {
0900         if (phy->rev >= 7) {
0901             b43legacy_phy_write(dev, 0x0811,
0902                         b43legacy_phy_read(dev, 0x0811)
0903                         | 0x0800);
0904             b43legacy_phy_write(dev, 0x0812,
0905                         b43legacy_phy_read(dev, 0x0812)
0906                         | 0x8000);
0907         }
0908     }
0909     b43legacy_radio_write16(dev, 0x007A,
0910                 b43legacy_radio_read16(dev, 0x007A)
0911                 & 0x00F7);
0912
0913     for (i = 0; i < loop1_cnt; i++) {
0914         b43legacy_radio_write16(dev, 0x0043, loop1_cnt);
0915         b43legacy_phy_write(dev, 0x0812,
0916                     (b43legacy_phy_read(dev, 0x0812)
0917                      & 0xF0FF) | (i << 8));
0918         b43legacy_phy_write(dev, 0x0015,
0919                     (b43legacy_phy_read(dev, 0x0015)
0920                      & 0x0FFF) | 0xA000);
0921         b43legacy_phy_write(dev, 0x0015,
0922                     (b43legacy_phy_read(dev, 0x0015)
0923                      & 0x0FFF) | 0xF000);
0924         udelay(20);
0925         if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
0926             break;
0927     }
0928     loop1_done = i;
0929     loop1_omitted = loop1_cnt - loop1_done;
0930
0931     loop2_done = 0;
0932     if (loop1_done >= 8) {
0933         b43legacy_phy_write(dev, 0x0812,
0934                     b43legacy_phy_read(dev, 0x0812)
0935                     | 0x0030);
0936         for (i = loop1_done - 8; i < 16; i++) {
0937             b43legacy_phy_write(dev, 0x0812,
0938                         (b43legacy_phy_read(dev, 0x0812)
0939                          & 0xF0FF) | (i << 8));
0940             b43legacy_phy_write(dev, 0x0015,
0941                         (b43legacy_phy_read(dev, 0x0015)
0942                          & 0x0FFF) | 0xA000);
0943             b43legacy_phy_write(dev, 0x0015,
0944                         (b43legacy_phy_read(dev, 0x0015)
0945                          & 0x0FFF) | 0xF000);
0946             udelay(20);
0947             if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
0948                 break;
0949         }
0950     }
0951
0952     if (phy->rev != 1) {
0953         b43legacy_phy_write(dev, 0x0814, backup_phy[4]);
0954         b43legacy_phy_write(dev, 0x0815, backup_phy[5]);
0955     }
0956     b43legacy_phy_write(dev, 0x005A, backup_phy[6]);
0957     b43legacy_phy_write(dev, 0x0059, backup_phy[7]);
0958     b43legacy_phy_write(dev, 0x0058, backup_phy[8]);
0959     b43legacy_phy_write(dev, 0x000A, backup_phy[9]);
0960     b43legacy_phy_write(dev, 0x0003, backup_phy[10]);
0961     b43legacy_phy_write(dev, 0x080F, backup_phy[11]);
0962     b43legacy_phy_write(dev, 0x0810, backup_phy[12]);
0963     b43legacy_phy_write(dev, 0x002B, backup_phy[13]);
0964     b43legacy_phy_write(dev, 0x0015, backup_phy[14]);
0965
0966     b43legacy_phy_set_baseband_attenuation(dev, backup_bband);
0967
0968     b43legacy_radio_write16(dev, 0x0052, backup_radio[0]);
0969     b43legacy_radio_write16(dev, 0x0043, backup_radio[1]);
0970     b43legacy_radio_write16(dev, 0x007A, backup_radio[2]);
0971
0972     b43legacy_phy_write(dev, 0x0811, backup_phy[2] | 0x0003);
0973     udelay(10);
0974     b43legacy_phy_write(dev, 0x0811, backup_phy[2]);
0975     b43legacy_phy_write(dev, 0x0812, backup_phy[3]);
0976     b43legacy_phy_write(dev, 0x0429, backup_phy[0]);
0977     b43legacy_phy_write(dev, 0x0001, backup_phy[1]);
0978
0979     phy->loopback_gain[0] = ((loop1_done * 6) - (loop1_omitted * 4)) - 11;
0980     phy->loopback_gain[1] = (24 - (3 * loop2_done)) * 2;
0981 }
0982
0983 static void b43legacy_phy_initg(struct b43legacy_wldev *dev)
0984 {
0985     struct b43legacy_phy *phy = &dev->phy;
0986     u16 tmp;
0987
0988     if (phy->rev == 1)
0989         b43legacy_phy_initb5(dev);
0990     else
0991         b43legacy_phy_initb6(dev);
0992     if (phy->rev >= 2 && phy->gmode)
0993         b43legacy_phy_inita(dev);
0994
0995     if (phy->rev >= 2) {
0996         b43legacy_phy_write(dev, 0x0814, 0x0000);
0997         b43legacy_phy_write(dev, 0x0815, 0x0000);
0998     }
0999     if (phy->rev == 2) {
1000         b43legacy_phy_write(dev, 0x0811, 0x0000);
1001         b43legacy_phy_write(dev, 0x0015, 0x00C0);
1002     }
1003     if (phy->rev > 5) {
1004         b43legacy_phy_write(dev, 0x0811, 0x0400);
1005         b43legacy_phy_write(dev, 0x0015, 0x00C0);
1006     }
1007     if (phy->gmode) {
1008         tmp = b43legacy_phy_read(dev, 0x0400) & 0xFF;
1009         if (tmp == 3) {
1010             b43legacy_phy_write(dev, 0x04C2, 0x1816);
1011             b43legacy_phy_write(dev, 0x04C3, 0x8606);
1012         }
1013         if (tmp == 4 || tmp == 5) {
1014             b43legacy_phy_write(dev, 0x04C2, 0x1816);
1015             b43legacy_phy_write(dev, 0x04C3, 0x8006);
1016             b43legacy_phy_write(dev, 0x04CC,
1017                         (b43legacy_phy_read(dev,
1018                          0x04CC) & 0x00FF) |
1019                          0x1F00);
1020         }
1021         if (phy->rev >= 2)
1022             b43legacy_phy_write(dev, 0x047E, 0x0078);
1023     }
1024     if (phy->radio_rev == 8) {
1025         b43legacy_phy_write(dev, 0x0801, b43legacy_phy_read(dev, 0x0801)
1026                     | 0x0080);
1027         b43legacy_phy_write(dev, 0x043E, b43legacy_phy_read(dev, 0x043E)
1028                     | 0x0004);
1029     }
1030     if (phy->rev >= 2 && phy->gmode)
1031         b43legacy_calc_loopback_gain(dev);
1032     if (phy->radio_rev != 8) {
1033         if (phy->initval == 0xFFFF)
1034             phy->initval = b43legacy_radio_init2050(dev);
1035         else
1036             b43legacy_radio_write16(dev, 0x0078, phy->initval);
1037     }
1038     if (phy->txctl2 == 0xFFFF)
1039         b43legacy_phy_lo_g_measure(dev);
1040     else {
1041         if (phy->radio_ver == 0x2050 && phy->radio_rev == 8)
1042             b43legacy_radio_write16(dev, 0x0052,
1043                         (phy->txctl1 << 4) |
1044                         phy->txctl2);
1045         else
1046             b43legacy_radio_write16(dev, 0x0052,
1047                         (b43legacy_radio_read16(dev,
1048                          0x0052) & 0xFFF0) |
1049                          phy->txctl1);
1050         if (phy->rev >= 6)
1051             b43legacy_phy_write(dev, 0x0036,
1052                         (b43legacy_phy_read(dev, 0x0036)
1053                          & 0x0FFF) | (phy->txctl2 << 12));
1054         if (dev->dev->bus->sprom.boardflags_lo &
1055             B43legacy_BFL_PACTRL)
1056             b43legacy_phy_write(dev, 0x002E, 0x8075);
1057         else
1058             b43legacy_phy_write(dev, 0x002E, 0x807F);
1059         if (phy->rev < 2)
1060             b43legacy_phy_write(dev, 0x002F, 0x0101);
1061         else
1062             b43legacy_phy_write(dev, 0x002F, 0x0202);
1063     }
1064     if (phy->gmode) {
1065         b43legacy_phy_lo_adjust(dev, 0);
1066         b43legacy_phy_write(dev, 0x080F, 0x8078);
1067     }
1068
1069     if (!(dev->dev->bus->sprom.boardflags_lo & B43legacy_BFL_RSSI)) {
1070         /* The specs state to update the NRSSI LT with
1071          * the value 0x7FFFFFFF here. I think that is some weird
1072          * compiler optimization in the original driver.
1073          * Essentially, what we do here is resetting all NRSSI LT
1074          * entries to -32 (see the clamp_val() in nrssi_hw_update())
1075          */
1076         b43legacy_nrssi_hw_update(dev, 0xFFFF);
1077         b43legacy_calc_nrssi_threshold(dev);
1078     } else if (phy->gmode || phy->rev >= 2) {
1079         if (phy->nrssi[0] == -1000) {
1080             B43legacy_WARN_ON(phy->nrssi[1] != -1000);
1081             b43legacy_calc_nrssi_slope(dev);
1082         } else {
1083             B43legacy_WARN_ON(phy->nrssi[1] == -1000);
1084             b43legacy_calc_nrssi_threshold(dev);
1085         }
1086     }
1087     if (phy->radio_rev == 8)
1088         b43legacy_phy_write(dev, 0x0805, 0x3230);
1089     b43legacy_phy_init_pctl(dev);
1090     if (dev->dev->bus->chip_id == 0x4306
1091         && dev->dev->bus->chip_package == 2) {
1092         b43legacy_phy_write(dev, 0x0429,
1093                     b43legacy_phy_read(dev, 0x0429) & 0xBFFF);
1094         b43legacy_phy_write(dev, 0x04C3,
1095                     b43legacy_phy_read(dev, 0x04C3) & 0x7FFF);
1096     }
1097 }
1098
1099 static u16 b43legacy_phy_lo_b_r15_loop(struct b43legacy_wldev *dev)
1100 {
1101     int i;
1102     u16 ret = 0;
1103     unsigned long flags;
1104
1105     local_irq_save(flags);
1106     for (i = 0; i < 10; i++) {
1107         b43legacy_phy_write(dev, 0x0015, 0xAFA0);
1108         udelay(1);
1109         b43legacy_phy_write(dev, 0x0015, 0xEFA0);
1110         udelay(10);
1111         b43legacy_phy_write(dev, 0x0015, 0xFFA0);
1112         udelay(40);
1113         ret += b43legacy_phy_read(dev, 0x002C);
1114     }
1115     local_irq_restore(flags);
1116     cond_resched();
1117
1118     return ret;
1119 }
1120
1121 void b43legacy_phy_lo_b_measure(struct b43legacy_wldev *dev)
1122 {
1123     struct b43legacy_phy *phy = &dev->phy;
1124     u16 regstack[12] = { 0 };
1125     u16 mls;
1126     s16 fval;
1127     int i;
1128     int j;
1129
1130     regstack[0] = b43legacy_phy_read(dev, 0x0015);
1131     regstack[1] = b43legacy_radio_read16(dev, 0x0052) & 0xFFF0;
1132
1133     if (phy->radio_ver == 0x2053) {
1134         regstack[2] = b43legacy_phy_read(dev, 0x000A);
1135         regstack[3] = b43legacy_phy_read(dev, 0x002A);
1136         regstack[4] = b43legacy_phy_read(dev, 0x0035);
1137         regstack[5] = b43legacy_phy_read(dev, 0x0003);
1138         regstack[6] = b43legacy_phy_read(dev, 0x0001);
1139         regstack[7] = b43legacy_phy_read(dev, 0x0030);
1140
1141         regstack[8] = b43legacy_radio_read16(dev, 0x0043);
1142         regstack[9] = b43legacy_radio_read16(dev, 0x007A);
1143         regstack[10] = b43legacy_read16(dev, 0x03EC);
1144         regstack[11] = b43legacy_radio_read16(dev, 0x0052) & 0x00F0;
1145
1146         b43legacy_phy_write(dev, 0x0030, 0x00FF);
1147         b43legacy_write16(dev, 0x03EC, 0x3F3F);
1148         b43legacy_phy_write(dev, 0x0035, regstack[4] & 0xFF7F);
1149         b43legacy_radio_write16(dev, 0x007A, regstack[9] & 0xFFF0);
1150     }
1151     b43legacy_phy_write(dev, 0x0015, 0xB000);
1152     b43legacy_phy_write(dev, 0x002B, 0x0004);
1153
1154     if (phy->radio_ver == 0x2053) {
1155         b43legacy_phy_write(dev, 0x002B, 0x0203);
1156         b43legacy_phy_write(dev, 0x002A, 0x08A3);
1157     }
1158
1159     phy->minlowsig[0] = 0xFFFF;
1160
1161     for (i = 0; i < 4; i++) {
1162         b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
1163         b43legacy_phy_lo_b_r15_loop(dev);
1164     }
1165     for (i = 0; i < 10; i++) {
1166         b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
1167         mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
1168         if (mls < phy->minlowsig[0]) {
1169             phy->minlowsig[0] = mls;
1170             phy->minlowsigpos[0] = i;
1171         }
1172     }
1173     b43legacy_radio_write16(dev, 0x0052, regstack[1]
1174                 | phy->minlowsigpos[0]);
1175
1176     phy->minlowsig[1] = 0xFFFF;
1177
1178     for (i = -4; i < 5; i += 2) {
1179         for (j = -4; j < 5; j += 2) {
1180             if (j < 0)
1181                 fval = (0x0100 * i) + j + 0x0100;
1182             else
1183                 fval = (0x0100 * i) + j;
1184             b43legacy_phy_write(dev, 0x002F, fval);
1185             mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
1186             if (mls < phy->minlowsig[1]) {
1187                 phy->minlowsig[1] = mls;
1188                 phy->minlowsigpos[1] = fval;
1189             }
1190         }
1191     }
1192     phy->minlowsigpos[1] += 0x0101;
1193
1194     b43legacy_phy_write(dev, 0x002F, phy->minlowsigpos[1]);
1195     if (phy->radio_ver == 0x2053) {
1196         b43legacy_phy_write(dev, 0x000A, regstack[2]);
1197         b43legacy_phy_write(dev, 0x002A, regstack[3]);
1198         b43legacy_phy_write(dev, 0x0035, regstack[4]);
1199         b43legacy_phy_write(dev, 0x0003, regstack[5]);
1200         b43legacy_phy_write(dev, 0x0001, regstack[6]);
1201         b43legacy_phy_write(dev, 0x0030, regstack[7]);
1202
1203         b43legacy_radio_write16(dev, 0x0043, regstack[8]);
1204         b43legacy_radio_write16(dev, 0x007A, regstack[9]);
1205
1206         b43legacy_radio_write16(dev, 0x0052,
1207                     (b43legacy_radio_read16(dev, 0x0052)
1208                     & 0x000F) | regstack[11]);
1209
1210         b43legacy_write16(dev, 0x03EC, regstack[10]);
1211     }
1212     b43legacy_phy_write(dev, 0x0015, regstack[0]);
1213 }
1214
1215 static inline
1216 u16 b43legacy_phy_lo_g_deviation_subval(struct b43legacy_wldev *dev,
1217                     u16 control)
1218 {
1219     struct b43legacy_phy *phy = &dev->phy;
1220     u16 ret;
1221     unsigned long flags;
1222
1223     local_irq_save(flags);
1224     if (phy->gmode) {
1225         b43legacy_phy_write(dev, 0x15, 0xE300);
1226         control <<= 8;
1227         b43legacy_phy_write(dev, 0x0812, control | 0x00B0);
1228         udelay(5);
1229         b43legacy_phy_write(dev, 0x0812, control | 0x00B2);
1230         udelay(2);
1231         b43legacy_phy_write(dev, 0x0812, control | 0x00B3);
1232         udelay(4);
1233         b43legacy_phy_write(dev, 0x0015, 0xF300);
1234         udelay(8);
1235     } else {
1236         b43legacy_phy_write(dev, 0x0015, control | 0xEFA0);
1237         udelay(2);
1238         b43legacy_phy_write(dev, 0x0015, control | 0xEFE0);
1239         udelay(4);
1240         b43legacy_phy_write(dev, 0x0015, control | 0xFFE0);
1241         udelay(8);
1242     }
1243     ret = b43legacy_phy_read(dev, 0x002D);
1244     local_irq_restore(flags);
1245     cond_resched();
1246
1247     return ret;
1248 }
1249
1250 static u32 b43legacy_phy_lo_g_singledeviation(struct b43legacy_wldev *dev,
1251                           u16 control)
1252 {
1253     int i;
1254     u32 ret = 0;
1255
1256     for (i = 0; i < 8; i++)
1257         ret += b43legacy_phy_lo_g_deviation_subval(dev, control);
1258
1259     return ret;
1260 }
1261
1262 /* Write the LocalOscillator CONTROL */
1263 static inline
1264 void b43legacy_lo_write(struct b43legacy_wldev *dev,
1265             struct b43legacy_lopair *pair)
1266 {
1267     u16 value;
1268
1269     value = (u8)(pair->low);
1270     value |= ((u8)(pair->high)) << 8;
1271
1272 #ifdef CONFIG_B43LEGACY_DEBUG
1273     /* Sanity check. */
1274     if (pair->low < -8 || pair->low > 8 ||
1275         pair->high < -8 || pair->high > 8) {
1276         b43legacydbg(dev->wl,
1277                "WARNING: Writing invalid LOpair "
1278                "(low: %d, high: %d)\n",
1279                pair->low, pair->high);
1280         dump_stack();
1281     }
1282 #endif
1283
1284     b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, value);
1285 }
1286
1287 static inline
1288 struct b43legacy_lopair *b43legacy_find_lopair(struct b43legacy_wldev *dev,
1289                            u16 bbatt,
1290                            u16 rfatt,
1291                            u16 tx)
1292 {
1293     static const u8 dict[10] = { 11, 10, 11, 12, 13, 12, 13, 12, 13, 12 };
1294     struct b43legacy_phy *phy = &dev->phy;
1295
1296     if (bbatt > 6)
1297         bbatt = 6;
1298     B43legacy_WARN_ON(rfatt >= 10);
1299
1300     if (tx == 3)
1301         return b43legacy_get_lopair(phy, rfatt, bbatt);
1302     return b43legacy_get_lopair(phy, dict[rfatt], bbatt);
1303 }
1304
1305 static inline
1306 struct b43legacy_lopair *b43legacy_current_lopair(struct b43legacy_wldev *dev)
1307 {
1308     struct b43legacy_phy *phy = &dev->phy;
1309
1310     return b43legacy_find_lopair(dev, phy->bbatt,
1311                      phy->rfatt, phy->txctl1);
1312 }
1313
1314 /* Adjust B/G LO */
1315 void b43legacy_phy_lo_adjust(struct b43legacy_wldev *dev, int fixed)
1316 {
1317     struct b43legacy_lopair *pair;
1318
1319     if (fixed) {
1320         /* Use fixed values. Only for initialization. */
1321         pair = b43legacy_find_lopair(dev, 2, 3, 0);
1322     } else
1323         pair = b43legacy_current_lopair(dev);
1324     b43legacy_lo_write(dev, pair);
1325 }
1326
1327 static void b43legacy_phy_lo_g_measure_txctl2(struct b43legacy_wldev *dev)
1328 {
1329     struct b43legacy_phy *phy = &dev->phy;
1330     u16 txctl2 = 0;
1331     u16 i;
1332     u32 smallest;
1333     u32 tmp;
1334
1335     b43legacy_radio_write16(dev, 0x0052, 0x0000);
1336     udelay(10);
1337     smallest = b43legacy_phy_lo_g_singledeviation(dev, 0);
1338     for (i = 0; i < 16; i++) {
1339         b43legacy_radio_write16(dev, 0x0052, i);
1340         udelay(10);
1341         tmp = b43legacy_phy_lo_g_singledeviation(dev, 0);
1342         if (tmp < smallest) {
1343             smallest = tmp;
1344             txctl2 = i;
1345         }
1346     }
1347     phy->txctl2 = txctl2;
1348 }
1349
1350 static
1351 void b43legacy_phy_lo_g_state(struct b43legacy_wldev *dev,
1352                   const struct b43legacy_lopair *in_pair,
1353                   struct b43legacy_lopair *out_pair,
1354                   u16 r27)
1355 {
1356     static const struct b43legacy_lopair transitions[8] = {
1357         { .high =  1,  .low =  1, },
1358         { .high =  1,  .low =  0, },
1359         { .high =  1,  .low = -1, },
1360         { .high =  0,  .low = -1, },
1361         { .high = -1,  .low = -1, },
1362         { .high = -1,  .low =  0, },
1363         { .high = -1,  .low =  1, },
1364         { .high =  0,  .low =  1, },
1365     };
1366     struct b43legacy_lopair lowest_transition = {
1367         .high = in_pair->high,
1368         .low = in_pair->low,
1369     };
1370     struct b43legacy_lopair tmp_pair;
1371     struct b43legacy_lopair transition;
1372     int i = 12;
1373     int state = 0;
1374     int found_lower;
1375     int j;
1376     int begin;
1377     int end;
1378     u32 lowest_deviation;
1379     u32 tmp;
1380
1381     /* Note that in_pair and out_pair can point to the same pair.
1382      * Be careful. */
1383
1384     b43legacy_lo_write(dev, &lowest_transition);
1385     lowest_deviation = b43legacy_phy_lo_g_singledeviation(dev, r27);
1386     do {
1387         found_lower = 0;
1388         B43legacy_WARN_ON(!(state >= 0 && state <= 8));
1389         if (state == 0) {
1390             begin = 1;
1391             end = 8;
1392         } else if (state % 2 == 0) {
1393             begin = state - 1;
1394             end = state + 1;
1395         } else {
1396             begin = state - 2;
1397             end = state + 2;
1398         }
1399         if (begin < 1)
1400             begin += 8;
1401         if (end > 8)
1402             end -= 8;
1403
1404         j = begin;
1405         tmp_pair.high = lowest_transition.high;
1406         tmp_pair.low = lowest_transition.low;
1407         while (1) {
1408             B43legacy_WARN_ON(!(j >= 1 && j <= 8));
1409             transition.high = tmp_pair.high +
1410                       transitions[j - 1].high;
1411             transition.low = tmp_pair.low + transitions[j - 1].low;
1412             if ((abs(transition.low) < 9)
1413                  && (abs(transition.high) < 9)) {
1414                 b43legacy_lo_write(dev, &transition);
1415                 tmp = b43legacy_phy_lo_g_singledeviation(dev,
1416                                        r27);
1417                 if (tmp < lowest_deviation) {
1418                     lowest_deviation = tmp;
1419                     state = j;
1420                     found_lower = 1;
1421
1422                     lowest_transition.high =
1423                                 transition.high;
1424                     lowest_transition.low = transition.low;
1425                 }
1426             }
1427             if (j == end)
1428                 break;
1429             if (j == 8)
1430                 j = 1;
1431             else
1432                 j++;
1433         }
1434     } while (i-- && found_lower);
1435
1436     out_pair->high = lowest_transition.high;
1437     out_pair->low = lowest_transition.low;
1438 }
1439
1440 /* Set the baseband attenuation value on chip. */
1441 void b43legacy_phy_set_baseband_attenuation(struct b43legacy_wldev *dev,
1442                         u16 bbatt)
1443 {
1444     struct b43legacy_phy *phy = &dev->phy;
1445     u16 value;
1446
1447     if (phy->analog == 0) {
1448         value = (b43legacy_read16(dev, 0x03E6) & 0xFFF0);
1449         value |= (bbatt & 0x000F);
1450         b43legacy_write16(dev, 0x03E6, value);
1451         return;
1452     }
1453
1454     if (phy->analog > 1) {
1455         value = b43legacy_phy_read(dev, 0x0060) & 0xFFC3;
1456         value |= (bbatt << 2) & 0x003C;
1457     } else {
1458         value = b43legacy_phy_read(dev, 0x0060) & 0xFF87;
1459         value |= (bbatt << 3) & 0x0078;
1460     }
1461     b43legacy_phy_write(dev, 0x0060, value);
1462 }
1463
1464 /* https://bcm-specs.sipsolutions.net/LocalOscillator/Measure */
1465 void b43legacy_phy_lo_g_measure(struct b43legacy_wldev *dev)
1466 {
1467     static const u8 pairorder[10] = { 3, 1, 5, 7, 9, 2, 0, 4, 6, 8 };
1468     const int is_initializing = (b43legacy_status(dev)
1469                      < B43legacy_STAT_STARTED);
1470     struct b43legacy_phy *phy = &dev->phy;
1471     u16 h;
1472     u16 i;
1473     u16 oldi = 0;
1474     u16 j;
1475     struct b43legacy_lopair control;
1476     struct b43legacy_lopair *tmp_control;
1477     u16 tmp;
1478     u16 regstack[16] = { 0 };
1479     u8 oldchannel;
1480
1481     /* XXX: What are these? */
1482     u8 r27 = 0;
1483     u16 r31;
1484
1485     oldchannel = phy->channel;
1486     /* Setup */
1487     if (phy->gmode) {
1488         regstack[0] = b43legacy_phy_read(dev, B43legacy_PHY_G_CRS);
1489         regstack[1] = b43legacy_phy_read(dev, 0x0802);
1490         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
1491                     & 0x7FFF);
1492         b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
1493     }
1494     regstack[3] = b43legacy_read16(dev, 0x03E2);
1495     b43legacy_write16(dev, 0x03E2, regstack[3] | 0x8000);
1496     regstack[4] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
1497     regstack[5] = b43legacy_phy_read(dev, 0x15);
1498     regstack[6] = b43legacy_phy_read(dev, 0x2A);
1499     regstack[7] = b43legacy_phy_read(dev, 0x35);
1500     regstack[8] = b43legacy_phy_read(dev, 0x60);
1501     regstack[9] = b43legacy_radio_read16(dev, 0x43);
1502     regstack[10] = b43legacy_radio_read16(dev, 0x7A);
1503     regstack[11] = b43legacy_radio_read16(dev, 0x52);
1504     if (phy->gmode) {
1505         regstack[12] = b43legacy_phy_read(dev, 0x0811);
1506         regstack[13] = b43legacy_phy_read(dev, 0x0812);
1507         regstack[14] = b43legacy_phy_read(dev, 0x0814);
1508         regstack[15] = b43legacy_phy_read(dev, 0x0815);
1509     }
1510     b43legacy_radio_selectchannel(dev, 6, 0);
1511     if (phy->gmode) {
1512         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
1513                     & 0x7FFF);
1514         b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
1515         b43legacy_dummy_transmission(dev);
1516     }
1517     b43legacy_radio_write16(dev, 0x0043, 0x0006);
1518
1519     b43legacy_phy_set_baseband_attenuation(dev, 2);
1520
1521     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x0000);
1522     b43legacy_phy_write(dev, 0x002E, 0x007F);
1523     b43legacy_phy_write(dev, 0x080F, 0x0078);
1524     b43legacy_phy_write(dev, 0x0035, regstack[7] & ~(1 << 7));
1525     b43legacy_radio_write16(dev, 0x007A, regstack[10] & 0xFFF0);
1526     b43legacy_phy_write(dev, 0x002B, 0x0203);
1527     b43legacy_phy_write(dev, 0x002A, 0x08A3);
1528     if (phy->gmode) {
1529         b43legacy_phy_write(dev, 0x0814, regstack[14] | 0x0003);
1530         b43legacy_phy_write(dev, 0x0815, regstack[15] & 0xFFFC);
1531         b43legacy_phy_write(dev, 0x0811, 0x01B3);
1532         b43legacy_phy_write(dev, 0x0812, 0x00B2);
1533     }
1534     if (is_initializing)
1535         b43legacy_phy_lo_g_measure_txctl2(dev);
1536     b43legacy_phy_write(dev, 0x080F, 0x8078);
1537
1538     /* Measure */
1539     control.low = 0;
1540     control.high = 0;
1541     for (h = 0; h < 10; h++) {
1542         /* Loop over each possible RadioAttenuation (0-9) */
1543         i = pairorder[h];
1544         if (is_initializing) {
1545             if (i == 3) {
1546                 control.low = 0;
1547                 control.high = 0;
1548             } else if (((i % 2 == 1) && (oldi % 2 == 1)) ||
1549                   ((i % 2 == 0) && (oldi % 2 == 0))) {
1550                 tmp_control = b43legacy_get_lopair(phy, oldi,
1551                                    0);
1552                 memcpy(&control, tmp_control, sizeof(control));
1553             } else {
1554                 tmp_control = b43legacy_get_lopair(phy, 3, 0);
1555                 memcpy(&control, tmp_control, sizeof(control));
1556             }
1557         }
1558         /* Loop over each possible BasebandAttenuation/2 */
1559         for (j = 0; j < 4; j++) {
1560             if (is_initializing) {
1561                 tmp = i * 2 + j;
1562                 r27 = 0;
1563                 r31 = 0;
1564                 if (tmp > 14) {
1565                     r31 = 1;
1566                     if (tmp > 17)
1567                         r27 = 1;
1568                     if (tmp > 19)
1569                         r27 = 2;
1570                 }
1571             } else {
1572                 tmp_control = b43legacy_get_lopair(phy, i,
1573                                    j * 2);
1574                 if (!tmp_control->used)
1575                     continue;
1576                 memcpy(&control, tmp_control, sizeof(control));
1577                 r27 = 3;
1578                 r31 = 0;
1579             }
1580             b43legacy_radio_write16(dev, 0x43, i);
1581             b43legacy_radio_write16(dev, 0x52, phy->txctl2);
1582             udelay(10);
1583             cond_resched();
1584
1585             b43legacy_phy_set_baseband_attenuation(dev, j * 2);
1586
1587             tmp = (regstack[10] & 0xFFF0);
1588             if (r31)
1589                 tmp |= 0x0008;
1590             b43legacy_radio_write16(dev, 0x007A, tmp);
1591
1592             tmp_control = b43legacy_get_lopair(phy, i, j * 2);
1593             b43legacy_phy_lo_g_state(dev, &control, tmp_control,
1594                          r27);
1595         }
1596         oldi = i;
1597     }
1598     /* Loop over each possible RadioAttenuation (10-13) */
1599     for (i = 10; i < 14; i++) {
1600         /* Loop over each possible BasebandAttenuation/2 */
1601         for (j = 0; j < 4; j++) {
1602             if (is_initializing) {
1603                 tmp_control = b43legacy_get_lopair(phy, i - 9,
1604                                  j * 2);
1605                 memcpy(&control, tmp_control, sizeof(control));
1606                 /* FIXME: The next line is wrong, as the
1607                  * following if statement can never trigger. */
1608                 tmp = (i - 9) * 2 + j - 5;
1609                 r27 = 0;
1610                 r31 = 0;
1611                 if (tmp > 14) {
1612                     r31 = 1;
1613                     if (tmp > 17)
1614                         r27 = 1;
1615                     if (tmp > 19)
1616                         r27 = 2;
1617                 }
1618             } else {
1619                 tmp_control = b43legacy_get_lopair(phy, i - 9,
1620                                    j * 2);
1621                 if (!tmp_control->used)
1622                     continue;
1623                 memcpy(&control, tmp_control, sizeof(control));
1624                 r27 = 3;
1625                 r31 = 0;
1626             }
1627             b43legacy_radio_write16(dev, 0x43, i - 9);
1628             /* FIXME: shouldn't txctl1 be zero in the next line
1629              * and 3 in the loop above? */
1630             b43legacy_radio_write16(dev, 0x52,
1631                           phy->txctl2
1632                           | (3/*txctl1*/ << 4));
1633             udelay(10);
1634             cond_resched();
1635
1636             b43legacy_phy_set_baseband_attenuation(dev, j * 2);
1637
1638             tmp = (regstack[10] & 0xFFF0);
1639             if (r31)
1640                 tmp |= 0x0008;
1641             b43legacy_radio_write16(dev, 0x7A, tmp);
1642
1643             tmp_control = b43legacy_get_lopair(phy, i, j * 2);
1644             b43legacy_phy_lo_g_state(dev, &control, tmp_control,
1645                          r27);
1646         }
1647     }
1648
1649     /* Restoration */
1650     if (phy->gmode) {
1651         b43legacy_phy_write(dev, 0x0015, 0xE300);
1652         b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA0);
1653         udelay(5);
1654         b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA2);
1655         udelay(2);
1656         b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA3);
1657         cond_resched();
1658     } else
1659         b43legacy_phy_write(dev, 0x0015, r27 | 0xEFA0);
1660     b43legacy_phy_lo_adjust(dev, is_initializing);
1661     b43legacy_phy_write(dev, 0x002E, 0x807F);
1662     if (phy->gmode)
1663         b43legacy_phy_write(dev, 0x002F, 0x0202);
1664     else
1665         b43legacy_phy_write(dev, 0x002F, 0x0101);
1666     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, regstack[4]);
1667     b43legacy_phy_write(dev, 0x0015, regstack[5]);
1668     b43legacy_phy_write(dev, 0x002A, regstack[6]);
1669     b43legacy_phy_write(dev, 0x0035, regstack[7]);
1670     b43legacy_phy_write(dev, 0x0060, regstack[8]);
1671     b43legacy_radio_write16(dev, 0x0043, regstack[9]);
1672     b43legacy_radio_write16(dev, 0x007A, regstack[10]);
1673     regstack[11] &= 0x00F0;
1674     regstack[11] |= (b43legacy_radio_read16(dev, 0x52) & 0x000F);
1675     b43legacy_radio_write16(dev, 0x52, regstack[11]);
1676     b43legacy_write16(dev, 0x03E2, regstack[3]);
1677     if (phy->gmode) {
1678         b43legacy_phy_write(dev, 0x0811, regstack[12]);
1679         b43legacy_phy_write(dev, 0x0812, regstack[13]);
1680         b43legacy_phy_write(dev, 0x0814, regstack[14]);
1681         b43legacy_phy_write(dev, 0x0815, regstack[15]);
1682         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]);
1683         b43legacy_phy_write(dev, 0x0802, regstack[1]);
1684     }
1685     b43legacy_radio_selectchannel(dev, oldchannel, 1);
1686
1687 #ifdef CONFIG_B43LEGACY_DEBUG
1688     {
1689         /* Sanity check for all lopairs. */
1690         for (i = 0; i < B43legacy_LO_COUNT; i++) {
1691             tmp_control = phy->_lo_pairs + i;
1692             if (tmp_control->low < -8 || tmp_control->low > 8 ||
1693                 tmp_control->high < -8 || tmp_control->high > 8)
1694                 b43legacywarn(dev->wl,
1695                        "WARNING: Invalid LOpair (low: %d, high:"
1696                        " %d, index: %d)\n",
1697                        tmp_control->low, tmp_control->high, i);
1698         }
1699     }
1700 #endif /* CONFIG_B43LEGACY_DEBUG */
1701 }
1702
1703 static
1704 void b43legacy_phy_lo_mark_current_used(struct b43legacy_wldev *dev)
1705 {
1706     struct b43legacy_lopair *pair;
1707
1708     pair = b43legacy_current_lopair(dev);
1709     pair->used = 1;
1710 }
1711
1712 void b43legacy_phy_lo_mark_all_unused(struct b43legacy_wldev *dev)
1713 {
1714     struct b43legacy_phy *phy = &dev->phy;
1715     struct b43legacy_lopair *pair;
1716     int i;
1717
1718     for (i = 0; i < B43legacy_LO_COUNT; i++) {
1719         pair = phy->_lo_pairs + i;
1720         pair->used = 0;
1721     }
1722 }
1723
1724 /* https://bcm-specs.sipsolutions.net/EstimatePowerOut
1725  * This function converts a TSSI value to dBm in Q5.2
1726  */
1727 static s8 b43legacy_phy_estimate_power_out(struct b43legacy_wldev *dev, s8 tssi)
1728 {
1729     struct b43legacy_phy *phy = &dev->phy;
1730     s8 dbm = 0;
1731     s32 tmp;
1732
1733     tmp = phy->idle_tssi;
1734     tmp += tssi;
1735     tmp -= phy->savedpctlreg;
1736
1737     switch (phy->type) {
1738     case B43legacy_PHYTYPE_B:
1739     case B43legacy_PHYTYPE_G:
1740         tmp = clamp_val(tmp, 0x00, 0x3F);
1741         dbm = phy->tssi2dbm[tmp];
1742         break;
1743     default:
1744         B43legacy_BUG_ON(1);
1745     }
1746
1747     return dbm;
1748 }
1749
1750 /* https://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
1751 void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
1752 {
1753     struct b43legacy_phy *phy = &dev->phy;
1754     u16 tmp;
1755     u16 txpower;
1756     s8 v0;
1757     s8 v1;
1758     s8 v2;
1759     s8 v3;
1760     s8 average;
1761     int max_pwr;
1762     s16 desired_pwr;
1763     s16 estimated_pwr;
1764     s16 pwr_adjust;
1765     s16 radio_att_delta;
1766     s16 baseband_att_delta;
1767     s16 radio_attenuation;
1768     s16 baseband_attenuation;
1769
1770     if (phy->savedpctlreg == 0xFFFF)
1771         return;
1772     if ((dev->dev->bus->boardinfo.type == 0x0416) &&
1773         is_bcm_board_vendor(dev))
1774         return;
1775 #ifdef CONFIG_B43LEGACY_DEBUG
1776     if (phy->manual_txpower_control)
1777         return;
1778 #endif
1779
1780     B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
1781              phy->type == B43legacy_PHYTYPE_G));
1782     tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0058);
1783     v0 = (s8)(tmp & 0x00FF);
1784     v1 = (s8)((tmp & 0xFF00) >> 8);
1785     tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005A);
1786     v2 = (s8)(tmp & 0x00FF);
1787     v3 = (s8)((tmp & 0xFF00) >> 8);
1788     tmp = 0;
1789
1790     if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F) {
1791         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
1792                      0x0070);
1793         v0 = (s8)(tmp & 0x00FF);
1794         v1 = (s8)((tmp & 0xFF00) >> 8);
1795         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
1796                      0x0072);
1797         v2 = (s8)(tmp & 0x00FF);
1798         v3 = (s8)((tmp & 0xFF00) >> 8);
1799         if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F)
1800             return;
1801         v0 = (v0 + 0x20) & 0x3F;
1802         v1 = (v1 + 0x20) & 0x3F;
1803         v2 = (v2 + 0x20) & 0x3F;
1804         v3 = (v3 + 0x20) & 0x3F;
1805         tmp = 1;
1806     }
1807     b43legacy_radio_clear_tssi(dev);
1808
1809     average = (v0 + v1 + v2 + v3 + 2) / 4;
1810
1811     if (tmp && (b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005E)
1812         & 0x8))
1813         average -= 13;
1814
1815     estimated_pwr = b43legacy_phy_estimate_power_out(dev, average);
1816
1817     max_pwr = dev->dev->bus->sprom.maxpwr_bg;
1818
1819     if ((dev->dev->bus->sprom.boardflags_lo
1820          & B43legacy_BFL_PACTRL) &&
1821         (phy->type == B43legacy_PHYTYPE_G))
1822         max_pwr -= 0x3;
1823     if (unlikely(max_pwr <= 0)) {
1824         b43legacywarn(dev->wl, "Invalid max-TX-power value in SPROM."
1825             "\n");
1826         max_pwr = 74; /* fake it */
1827         dev->dev->bus->sprom.maxpwr_bg = max_pwr;
1828     }
1829
1830     /* Use regulatory information to get the maximum power.
1831      * In the absence of such data from mac80211, we will use 20 dBm, which
1832      * is the value for the EU, US, Canada, and most of the world.
1833      * The regulatory maximum is reduced by the antenna gain (from sprom)
1834      * and 1.5 dBm (a safety factor??). The result is in Q5.2 format
1835      * which accounts for the factor of 4 */
1836 #define REG_MAX_PWR 20
1837     max_pwr = min(REG_MAX_PWR * 4
1838               - dev->dev->bus->sprom.antenna_gain.a0
1839               - 0x6, max_pwr);
1840
1841     /* find the desired power in Q5.2 - power_level is in dBm
1842      * and limit it - max_pwr is already in Q5.2 */
1843     desired_pwr = clamp_val(phy->power_level << 2, 0, max_pwr);
1844     if (b43legacy_debug(dev, B43legacy_DBG_XMITPOWER))
1845         b43legacydbg(dev->wl, "Current TX power output: " Q52_FMT
1846                " dBm, Desired TX power output: " Q52_FMT
1847                " dBm\n", Q52_ARG(estimated_pwr),
1848                Q52_ARG(desired_pwr));
1849     /* Check if we need to adjust the current power. The factor of 2 is
1850      * for damping */
1851     pwr_adjust = (desired_pwr - estimated_pwr) / 2;
1852     /* RF attenuation delta
1853      * The minus sign is because lower attenuation => more power */
1854     radio_att_delta = -(pwr_adjust + 7) >> 3;
1855     /* Baseband attenuation delta */
1856     baseband_att_delta = -(pwr_adjust >> 1) - (4 * radio_att_delta);
1857     /* Do we need to adjust anything? */
1858     if ((radio_att_delta == 0) && (baseband_att_delta == 0)) {
1859         b43legacy_phy_lo_mark_current_used(dev);
1860         return;
1861     }
1862
1863     /* Calculate the new attenuation values. */
1864     baseband_attenuation = phy->bbatt;
1865     baseband_attenuation += baseband_att_delta;
1866     radio_attenuation = phy->rfatt;
1867     radio_attenuation += radio_att_delta;
1868
1869     /* Get baseband and radio attenuation values into permitted ranges.
1870      * baseband 0-11, radio 0-9.
1871      * Radio attenuation affects power level 4 times as much as baseband.
1872      */
1873     if (radio_attenuation < 0) {
1874         baseband_attenuation -= (4 * -radio_attenuation);
1875         radio_attenuation = 0;
1876     } else if (radio_attenuation > 9) {
1877         baseband_attenuation += (4 * (radio_attenuation - 9));
1878         radio_attenuation = 9;
1879     } else {
1880         while (baseband_attenuation < 0 && radio_attenuation > 0) {
1881             baseband_attenuation += 4;
1882             radio_attenuation--;
1883         }
1884         while (baseband_attenuation > 11 && radio_attenuation < 9) {
1885             baseband_attenuation -= 4;
1886             radio_attenuation++;
1887         }
1888     }
1889     baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
1890
1891     txpower = phy->txctl1;
1892     if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
1893         if (radio_attenuation <= 1) {
1894             if (txpower == 0) {
1895                 txpower = 3;
1896                 radio_attenuation += 2;
1897                 baseband_attenuation += 2;
1898             } else if (dev->dev->bus->sprom.boardflags_lo
1899                    & B43legacy_BFL_PACTRL) {
1900                 baseband_attenuation += 4 *
1901                              (radio_attenuation - 2);
1902                 radio_attenuation = 2;
1903             }
1904         } else if (radio_attenuation > 4 && txpower != 0) {
1905             txpower = 0;
1906             if (baseband_attenuation < 3) {
1907                 radio_attenuation -= 3;
1908                 baseband_attenuation += 2;
1909             } else {
1910                 radio_attenuation -= 2;
1911                 baseband_attenuation -= 2;
1912             }
1913         }
1914     }
1915     /* Save the control values */
1916     phy->txctl1 = txpower;
1917     baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
1918     radio_attenuation = clamp_val(radio_attenuation, 0, 9);
1919     phy->rfatt = radio_attenuation;
1920     phy->bbatt = baseband_attenuation;
1921
1922     /* Adjust the hardware */
1923     b43legacy_phy_lock(dev);
1924     b43legacy_radio_lock(dev);
1925     b43legacy_radio_set_txpower_bg(dev, baseband_attenuation,
1926                        radio_attenuation, txpower);
1927     b43legacy_phy_lo_mark_current_used(dev);
1928     b43legacy_radio_unlock(dev);
1929     b43legacy_phy_unlock(dev);
1930 }
1931
1932 static inline
1933 s32 b43legacy_tssi2dbm_ad(s32 num, s32 den)
1934 {
1935     if (num < 0)
1936         return num/den;
1937     else
1938         return (num+den/2)/den;
1939 }
1940
1941 static inline
1942 s8 b43legacy_tssi2dbm_entry(s8 entry [], u8 index, s16 pab0, s16 pab1, s16 pab2)
1943 {
1944     s32 m1;
1945     s32 m2;
1946     s32 f = 256;
1947     s32 q;
1948     s32 delta;
1949     s8 i = 0;
1950
1951     m1 = b43legacy_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
1952     m2 = max(b43legacy_tssi2dbm_ad(32768 + index * pab2, 256), 1);
1953     do {
1954         if (i > 15)
1955             return -EINVAL;
1956         q = b43legacy_tssi2dbm_ad(f * 4096 -
1957                       b43legacy_tssi2dbm_ad(m2 * f, 16) *
1958                       f, 2048);
1959         delta = abs(q - f);
1960         f = q;
1961         i++;
1962     } while (delta >= 2);
1963     entry[index] = clamp_val(b43legacy_tssi2dbm_ad(m1 * f, 8192),
1964                    -127, 128);
1965     return 0;
1966 }
1967
1968 /* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
1969 int b43legacy_phy_init_tssi2dbm_table(struct b43legacy_wldev *dev)
1970 {
1971     struct b43legacy_phy *phy = &dev->phy;
1972     s16 pab0;
1973     s16 pab1;
1974     s16 pab2;
1975     u8 idx;
1976     s8 *dyn_tssi2dbm;
1977
1978     B43legacy_WARN_ON(!(phy->type == B43legacy_PHYTYPE_B ||
1979               phy->type == B43legacy_PHYTYPE_G));
1980     pab0 = (s16)(dev->dev->bus->sprom.pa0b0);
1981     pab1 = (s16)(dev->dev->bus->sprom.pa0b1);
1982     pab2 = (s16)(dev->dev->bus->sprom.pa0b2);
1983
1984     if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
1985         phy->idle_tssi = 0x34;
1986         phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
1987         return 0;
1988     }
1989
1990     if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
1991         pab0 != -1 && pab1 != -1 && pab2 != -1) {
1992         /* The pabX values are set in SPROM. Use them. */
1993         if ((s8)dev->dev->bus->sprom.itssi_bg != 0 &&
1994             (s8)dev->dev->bus->sprom.itssi_bg != -1)
1995             phy->idle_tssi = (s8)(dev->dev->bus->sprom.
1996                       itssi_bg);
1997         else
1998             phy->idle_tssi = 62;
1999         dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
2000         if (dyn_tssi2dbm == NULL) {
2001             b43legacyerr(dev->wl, "Could not allocate memory "
2002                    "for tssi2dbm table\n");
2003             return -ENOMEM;
2004         }
2005         for (idx = 0; idx < 64; idx++)
2006             if (b43legacy_tssi2dbm_entry(dyn_tssi2dbm, idx, pab0,
2007                              pab1, pab2)) {
2008                 phy->tssi2dbm = NULL;
2009                 b43legacyerr(dev->wl, "Could not generate "
2010                        "tssi2dBm table\n");
2011                 kfree(dyn_tssi2dbm);
2012                 return -ENODEV;
2013             }
2014         phy->tssi2dbm = dyn_tssi2dbm;
2015         phy->dyn_tssi_tbl = 1;
2016     } else {
2017         /* pabX values not set in SPROM. */
2018         switch (phy->type) {
2019         case B43legacy_PHYTYPE_B:
2020             phy->idle_tssi = 0x34;
2021             phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
2022             break;
2023         case B43legacy_PHYTYPE_G:
2024             phy->idle_tssi = 0x34;
2025             phy->tssi2dbm = b43legacy_tssi2dbm_g_table;
2026             break;
2027         }
2028     }
2029
2030     return 0;
2031 }
2032
2033 int b43legacy_phy_init(struct b43legacy_wldev *dev)
2034 {
2035     struct b43legacy_phy *phy = &dev->phy;
2036     int err = -ENODEV;
2037
2038     switch (phy->type) {
2039     case B43legacy_PHYTYPE_B:
2040         switch (phy->rev) {
2041         case 2:
2042             b43legacy_phy_initb2(dev);
2043             err = 0;
2044             break;
2045         case 4:
2046             b43legacy_phy_initb4(dev);
2047             err = 0;
2048             break;
2049         case 5:
2050             b43legacy_phy_initb5(dev);
2051             err = 0;
2052             break;
2053         case 6:
2054             b43legacy_phy_initb6(dev);
2055             err = 0;
2056             break;
2057         }
2058         break;
2059     case B43legacy_PHYTYPE_G:
2060         b43legacy_phy_initg(dev);
2061         err = 0;
2062         break;
2063     }
2064     if (err)
2065         b43legacyerr(dev->wl, "Unknown PHYTYPE found\n");
2066
2067     return err;
2068 }
2069
2070 void b43legacy_phy_set_antenna_diversity(struct b43legacy_wldev *dev)
2071 {
2072     struct b43legacy_phy *phy = &dev->phy;
2073     u16 antennadiv;
2074     u16 offset;
2075     u16 value;
2076     u32 ucodeflags;
2077
2078     antennadiv = phy->antenna_diversity;
2079
2080     if (antennadiv == 0xFFFF)
2081         antennadiv = 3;
2082     B43legacy_WARN_ON(antennadiv > 3);
2083
2084     ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2085                       B43legacy_UCODEFLAGS_OFFSET);
2086     b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
2087                   B43legacy_UCODEFLAGS_OFFSET,
2088                   ucodeflags & ~B43legacy_UCODEFLAG_AUTODIV);
2089
2090     switch (phy->type) {
2091     case B43legacy_PHYTYPE_G:
2092         offset = 0x0400;
2093
2094         if (antennadiv == 2)
2095             value = (3/*automatic*/ << 7);
2096         else
2097             value = (antennadiv << 7);
2098         b43legacy_phy_write(dev, offset + 1,
2099                     (b43legacy_phy_read(dev, offset + 1)
2100                     & 0x7E7F) | value);
2101
2102         if (antennadiv >= 2) {
2103             if (antennadiv == 2)
2104                 value = (antennadiv << 7);
2105             else
2106                 value = (0/*force0*/ << 7);
2107             b43legacy_phy_write(dev, offset + 0x2B,
2108                         (b43legacy_phy_read(dev,
2109                         offset + 0x2B)
2110                         & 0xFEFF) | value);
2111         }
2112
2113         if (phy->type == B43legacy_PHYTYPE_G) {
2114             if (antennadiv >= 2)
2115                 b43legacy_phy_write(dev, 0x048C,
2116                             b43legacy_phy_read(dev,
2117                             0x048C) | 0x2000);
2118             else
2119                 b43legacy_phy_write(dev, 0x048C,
2120                             b43legacy_phy_read(dev,
2121                             0x048C) & ~0x2000);
2122             if (phy->rev >= 2) {
2123                 b43legacy_phy_write(dev, 0x0461,
2124                             b43legacy_phy_read(dev,
2125                             0x0461) | 0x0010);
2126                 b43legacy_phy_write(dev, 0x04AD,
2127                             (b43legacy_phy_read(dev,
2128                             0x04AD)
2129                             & 0x00FF) | 0x0015);
2130                 if (phy->rev == 2)
2131                     b43legacy_phy_write(dev, 0x0427,
2132                                 0x0008);
2133                 else
2134                     b43legacy_phy_write(dev, 0x0427,
2135                         (b43legacy_phy_read(dev, 0x0427)
2136                          & 0x00FF) | 0x0008);
2137             } else if (phy->rev >= 6)
2138                 b43legacy_phy_write(dev, 0x049B, 0x00DC);
2139         } else {
2140             if (phy->rev < 3)
2141                 b43legacy_phy_write(dev, 0x002B,
2142                             (b43legacy_phy_read(dev,
2143                             0x002B) & 0x00FF)
2144                             | 0x0024);
2145             else {
2146                 b43legacy_phy_write(dev, 0x0061,
2147                             b43legacy_phy_read(dev,
2148                             0x0061) | 0x0010);
2149                 if (phy->rev == 3) {
2150                     b43legacy_phy_write(dev, 0x0093,
2151                                 0x001D);
2152                     b43legacy_phy_write(dev, 0x0027,
2153                                 0x0008);
2154                 } else {
2155                     b43legacy_phy_write(dev, 0x0093,
2156                                 0x003A);
2157                     b43legacy_phy_write(dev, 0x0027,
2158                         (b43legacy_phy_read(dev, 0x0027)
2159                          & 0x00FF) | 0x0008);
2160                 }
2161             }
2162         }
2163         break;
2164     case B43legacy_PHYTYPE_B:
2165         if (dev->dev->id.revision == 2)
2166             value = (3/*automatic*/ << 7);
2167         else
2168             value = (antennadiv << 7);
2169         b43legacy_phy_write(dev, 0x03E2,
2170                     (b43legacy_phy_read(dev, 0x03E2)
2171                     & 0xFE7F) | value);
2172         break;
2173     default:
2174         B43legacy_WARN_ON(1);
2175     }
2176
2177     if (antennadiv >= 2) {
2178         ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2179                           B43legacy_UCODEFLAGS_OFFSET);
2180         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
2181                       B43legacy_UCODEFLAGS_OFFSET,
2182                       ucodeflags | B43legacy_UCODEFLAG_AUTODIV);
2183     }
2184
2185     phy->antenna_diversity = antennadiv;
2186 }
2187
2188 /* Set the PowerSavingControlBits.
2189  * Bitvalues:
2190  *   0  => unset the bit
2191  *   1  => set the bit
2192  *   -1 => calculate the bit
2193  */
2194 void b43legacy_power_saving_ctl_bits(struct b43legacy_wldev *dev,
2195                      int bit25, int bit26)
2196 {
2197     int i;
2198     u32 status;
2199
2200 /* FIXME: Force 25 to off and 26 to on for now: */
2201 bit25 = 0;
2202 bit26 = 1;
2203
2204     if (bit25 == -1) {
2205         /* TODO: If powersave is not off and FIXME is not set and we
2206          *  are not in adhoc and thus is not an AP and we arei
2207          *  associated, set bit 25 */
2208     }
2209     if (bit26 == -1) {
2210         /* TODO: If the device is awake or this is an AP, or we are
2211          *  scanning, or FIXME, or we are associated, or FIXME,
2212          *  or the latest PS-Poll packet sent was successful,
2213          *  set bit26  */
2214     }
2215     status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
2216     if (bit25)
2217         status |= B43legacy_MACCTL_HWPS;
2218     else
2219         status &= ~B43legacy_MACCTL_HWPS;
2220     if (bit26)
2221         status |= B43legacy_MACCTL_AWAKE;
2222     else
2223         status &= ~B43legacy_MACCTL_AWAKE;
2224     b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
2225     if (bit26 && dev->dev->id.revision >= 5) {
2226         for (i = 0; i < 100; i++) {
2227             if (b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
2228                          0x0040) != 4)
2229                 break;
2230             udelay(10);
2231         }
2232     }
2233 }