Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003 
0004   Broadcom B43 wireless driver
0005   IEEE 802.11g PHY driver
0006 
0007   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
0008   Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
0009   Copyright (c) 2005-2008 Michael Buesch <m@bues.ch>
0010   Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
0011   Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
0012 
0013 
0014 */
0015 
0016 #include "b43.h"
0017 #include "phy_g.h"
0018 #include "phy_common.h"
0019 #include "lo.h"
0020 #include "main.h"
0021 #include "wa.h"
0022 
0023 #include <linux/bitrev.h>
0024 #include <linux/slab.h>
0025 
0026 
0027 static const s8 b43_tssi2dbm_g_table[] = {
0028     77, 77, 77, 76,
0029     76, 76, 75, 75,
0030     74, 74, 73, 73,
0031     73, 72, 72, 71,
0032     71, 70, 70, 69,
0033     68, 68, 67, 67,
0034     66, 65, 65, 64,
0035     63, 63, 62, 61,
0036     60, 59, 58, 57,
0037     56, 55, 54, 53,
0038     52, 50, 49, 47,
0039     45, 43, 40, 37,
0040     33, 28, 22, 14,
0041     5, -7, -20, -20,
0042     -20, -20, -20, -20,
0043     -20, -20, -20, -20,
0044 };
0045 
0046 static const u8 b43_radio_channel_codes_bg[] = {
0047     12, 17, 22, 27,
0048     32, 37, 42, 47,
0049     52, 57, 62, 67,
0050     72, 84,
0051 };
0052 
0053 
0054 static void b43_calc_nrssi_threshold(struct b43_wldev *dev);
0055 
0056 
0057 #define bitrev4(tmp) (bitrev8(tmp) >> 4)
0058 
0059 
0060 /* Get the freq, as it has to be written to the device. */
0061 static inline u16 channel2freq_bg(u8 channel)
0062 {
0063     B43_WARN_ON(!(channel >= 1 && channel <= 14));
0064 
0065     return b43_radio_channel_codes_bg[channel - 1];
0066 }
0067 
0068 static void generate_rfatt_list(struct b43_wldev *dev,
0069                 struct b43_rfatt_list *list)
0070 {
0071     struct b43_phy *phy = &dev->phy;
0072 
0073     /* APHY.rev < 5 || GPHY.rev < 6 */
0074     static const struct b43_rfatt rfatt_0[] = {
0075         {.att = 3,.with_padmix = 0,},
0076         {.att = 1,.with_padmix = 0,},
0077         {.att = 5,.with_padmix = 0,},
0078         {.att = 7,.with_padmix = 0,},
0079         {.att = 9,.with_padmix = 0,},
0080         {.att = 2,.with_padmix = 0,},
0081         {.att = 0,.with_padmix = 0,},
0082         {.att = 4,.with_padmix = 0,},
0083         {.att = 6,.with_padmix = 0,},
0084         {.att = 8,.with_padmix = 0,},
0085         {.att = 1,.with_padmix = 1,},
0086         {.att = 2,.with_padmix = 1,},
0087         {.att = 3,.with_padmix = 1,},
0088         {.att = 4,.with_padmix = 1,},
0089     };
0090     /* Radio.rev == 8 && Radio.version == 0x2050 */
0091     static const struct b43_rfatt rfatt_1[] = {
0092         {.att = 2,.with_padmix = 1,},
0093         {.att = 4,.with_padmix = 1,},
0094         {.att = 6,.with_padmix = 1,},
0095         {.att = 8,.with_padmix = 1,},
0096         {.att = 10,.with_padmix = 1,},
0097         {.att = 12,.with_padmix = 1,},
0098         {.att = 14,.with_padmix = 1,},
0099     };
0100     /* Otherwise */
0101     static const struct b43_rfatt rfatt_2[] = {
0102         {.att = 0,.with_padmix = 1,},
0103         {.att = 2,.with_padmix = 1,},
0104         {.att = 4,.with_padmix = 1,},
0105         {.att = 6,.with_padmix = 1,},
0106         {.att = 8,.with_padmix = 1,},
0107         {.att = 9,.with_padmix = 1,},
0108         {.att = 9,.with_padmix = 1,},
0109     };
0110 
0111     if (!b43_has_hardware_pctl(dev)) {
0112         /* Software pctl */
0113         list->list = rfatt_0;
0114         list->len = ARRAY_SIZE(rfatt_0);
0115         list->min_val = 0;
0116         list->max_val = 9;
0117         return;
0118     }
0119     if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
0120         /* Hardware pctl */
0121         list->list = rfatt_1;
0122         list->len = ARRAY_SIZE(rfatt_1);
0123         list->min_val = 0;
0124         list->max_val = 14;
0125         return;
0126     }
0127     /* Hardware pctl */
0128     list->list = rfatt_2;
0129     list->len = ARRAY_SIZE(rfatt_2);
0130     list->min_val = 0;
0131     list->max_val = 9;
0132 }
0133 
0134 static void generate_bbatt_list(struct b43_wldev *dev,
0135                 struct b43_bbatt_list *list)
0136 {
0137     static const struct b43_bbatt bbatt_0[] = {
0138         {.att = 0,},
0139         {.att = 1,},
0140         {.att = 2,},
0141         {.att = 3,},
0142         {.att = 4,},
0143         {.att = 5,},
0144         {.att = 6,},
0145         {.att = 7,},
0146         {.att = 8,},
0147     };
0148 
0149     list->list = bbatt_0;
0150     list->len = ARRAY_SIZE(bbatt_0);
0151     list->min_val = 0;
0152     list->max_val = 8;
0153 }
0154 
0155 static void b43_shm_clear_tssi(struct b43_wldev *dev)
0156 {
0157     b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F);
0158     b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F);
0159     b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F);
0160     b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F);
0161 }
0162 
0163 /* Synthetic PU workaround */
0164 static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
0165 {
0166     struct b43_phy *phy = &dev->phy;
0167 
0168     might_sleep();
0169 
0170     if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
0171         /* We do not need the workaround. */
0172         return;
0173     }
0174 
0175     if (channel <= 10) {
0176         b43_write16(dev, B43_MMIO_CHANNEL,
0177                 channel2freq_bg(channel + 4));
0178     } else {
0179         b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
0180     }
0181     msleep(1);
0182     b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
0183 }
0184 
0185 /* Set the baseband attenuation value on chip. */
0186 void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev,
0187                        u16 baseband_attenuation)
0188 {
0189     struct b43_phy *phy = &dev->phy;
0190 
0191     if (phy->analog == 0) {
0192         b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
0193                          & 0xFFF0) |
0194                 baseband_attenuation);
0195     } else if (phy->analog > 1) {
0196         b43_phy_maskset(dev, B43_PHY_DACCTL, 0xFFC3, (baseband_attenuation << 2));
0197     } else {
0198         b43_phy_maskset(dev, B43_PHY_DACCTL, 0xFF87, (baseband_attenuation << 3));
0199     }
0200 }
0201 
0202 /* Adjust the transmission power output (G-PHY) */
0203 static void b43_set_txpower_g(struct b43_wldev *dev,
0204                   const struct b43_bbatt *bbatt,
0205                   const struct b43_rfatt *rfatt, u8 tx_control)
0206 {
0207     struct b43_phy *phy = &dev->phy;
0208     struct b43_phy_g *gphy = phy->g;
0209     struct b43_txpower_lo_control *lo = gphy->lo_control;
0210     u16 bb, rf;
0211     u16 tx_bias, tx_magn;
0212 
0213     bb = bbatt->att;
0214     rf = rfatt->att;
0215     tx_bias = lo->tx_bias;
0216     tx_magn = lo->tx_magn;
0217     if (unlikely(tx_bias == 0xFF))
0218         tx_bias = 0;
0219 
0220     /* Save the values for later. Use memmove, because it's valid
0221      * to pass &gphy->rfatt as rfatt pointer argument. Same for bbatt. */
0222     gphy->tx_control = tx_control;
0223     memmove(&gphy->rfatt, rfatt, sizeof(*rfatt));
0224     gphy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
0225     memmove(&gphy->bbatt, bbatt, sizeof(*bbatt));
0226 
0227     if (b43_debug(dev, B43_DBG_XMITPOWER)) {
0228         b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
0229                "rfatt(%u), tx_control(0x%02X), "
0230                "tx_bias(0x%02X), tx_magn(0x%02X)\n",
0231                bb, rf, tx_control, tx_bias, tx_magn);
0232     }
0233 
0234     b43_gphy_set_baseband_attenuation(dev, bb);
0235     b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
0236     if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
0237         b43_radio_write16(dev, 0x43,
0238                   (rf & 0x000F) | (tx_control & 0x0070));
0239     } else {
0240         b43_radio_maskset(dev, 0x43, 0xFFF0, (rf & 0x000F));
0241         b43_radio_maskset(dev, 0x52, ~0x0070, (tx_control & 0x0070));
0242     }
0243     if (has_tx_magnification(phy)) {
0244         b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
0245     } else {
0246         b43_radio_maskset(dev, 0x52, 0xFFF0, (tx_bias & 0x000F));
0247     }
0248     b43_lo_g_adjust(dev);
0249 }
0250 
0251 /* GPHY_TSSI_Power_Lookup_Table_Init */
0252 static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
0253 {
0254     struct b43_phy_g *gphy = dev->phy.g;
0255     int i;
0256     u16 value;
0257 
0258     for (i = 0; i < 32; i++)
0259         b43_ofdmtab_write16(dev, 0x3C20, i, gphy->tssi2dbm[i]);
0260     for (i = 32; i < 64; i++)
0261         b43_ofdmtab_write16(dev, 0x3C00, i - 32, gphy->tssi2dbm[i]);
0262     for (i = 0; i < 64; i += 2) {
0263         value = (u16) gphy->tssi2dbm[i];
0264         value |= ((u16) gphy->tssi2dbm[i + 1]) << 8;
0265         b43_phy_write(dev, 0x380 + (i / 2), value);
0266     }
0267 }
0268 
0269 /* GPHY_Gain_Lookup_Table_Init */
0270 static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
0271 {
0272     struct b43_phy *phy = &dev->phy;
0273     struct b43_phy_g *gphy = phy->g;
0274     struct b43_txpower_lo_control *lo = gphy->lo_control;
0275     u16 nr_written = 0;
0276     u16 tmp;
0277     u8 rf, bb;
0278 
0279     for (rf = 0; rf < lo->rfatt_list.len; rf++) {
0280         for (bb = 0; bb < lo->bbatt_list.len; bb++) {
0281             if (nr_written >= 0x40)
0282                 return;
0283             tmp = lo->bbatt_list.list[bb].att;
0284             tmp <<= 8;
0285             if (phy->radio_rev == 8)
0286                 tmp |= 0x50;
0287             else
0288                 tmp |= 0x40;
0289             tmp |= lo->rfatt_list.list[rf].att;
0290             b43_phy_write(dev, 0x3C0 + nr_written, tmp);
0291             nr_written++;
0292         }
0293     }
0294 }
0295 
0296 static void b43_set_all_gains(struct b43_wldev *dev,
0297                   s16 first, s16 second, s16 third)
0298 {
0299     struct b43_phy *phy = &dev->phy;
0300     u16 i;
0301     u16 start = 0x08, end = 0x18;
0302     u16 tmp;
0303     u16 table;
0304 
0305     if (phy->rev <= 1) {
0306         start = 0x10;
0307         end = 0x20;
0308     }
0309 
0310     table = B43_OFDMTAB_GAINX;
0311     if (phy->rev <= 1)
0312         table = B43_OFDMTAB_GAINX_R1;
0313     for (i = 0; i < 4; i++)
0314         b43_ofdmtab_write16(dev, table, i, first);
0315 
0316     for (i = start; i < end; i++)
0317         b43_ofdmtab_write16(dev, table, i, second);
0318 
0319     if (third != -1) {
0320         tmp = ((u16) third << 14) | ((u16) third << 6);
0321         b43_phy_maskset(dev, 0x04A0, 0xBFBF, tmp);
0322         b43_phy_maskset(dev, 0x04A1, 0xBFBF, tmp);
0323         b43_phy_maskset(dev, 0x04A2, 0xBFBF, tmp);
0324     }
0325     b43_dummy_transmission(dev, false, true);
0326 }
0327 
0328 static void b43_set_original_gains(struct b43_wldev *dev)
0329 {
0330     struct b43_phy *phy = &dev->phy;
0331     u16 i, tmp;
0332     u16 table;
0333     u16 start = 0x0008, end = 0x0018;
0334 
0335     if (phy->rev <= 1) {
0336         start = 0x0010;
0337         end = 0x0020;
0338     }
0339 
0340     table = B43_OFDMTAB_GAINX;
0341     if (phy->rev <= 1)
0342         table = B43_OFDMTAB_GAINX_R1;
0343     for (i = 0; i < 4; i++) {
0344         tmp = (i & 0xFFFC);
0345         tmp |= (i & 0x0001) << 1;
0346         tmp |= (i & 0x0002) >> 1;
0347 
0348         b43_ofdmtab_write16(dev, table, i, tmp);
0349     }
0350 
0351     for (i = start; i < end; i++)
0352         b43_ofdmtab_write16(dev, table, i, i - start);
0353 
0354     b43_phy_maskset(dev, 0x04A0, 0xBFBF, 0x4040);
0355     b43_phy_maskset(dev, 0x04A1, 0xBFBF, 0x4040);
0356     b43_phy_maskset(dev, 0x04A2, 0xBFBF, 0x4000);
0357     b43_dummy_transmission(dev, false, true);
0358 }
0359 
0360 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0361 static void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
0362 {
0363     b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
0364     b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
0365 }
0366 
0367 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0368 static s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
0369 {
0370     u16 val;
0371 
0372     b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
0373     val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
0374 
0375     return (s16) val;
0376 }
0377 
0378 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0379 static void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
0380 {
0381     u16 i;
0382     s16 tmp;
0383 
0384     for (i = 0; i < 64; i++) {
0385         tmp = b43_nrssi_hw_read(dev, i);
0386         tmp -= val;
0387         tmp = clamp_val(tmp, -32, 31);
0388         b43_nrssi_hw_write(dev, i, tmp);
0389     }
0390 }
0391 
0392 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0393 static void b43_nrssi_mem_update(struct b43_wldev *dev)
0394 {
0395     struct b43_phy_g *gphy = dev->phy.g;
0396     s16 i, delta;
0397     s32 tmp;
0398 
0399     delta = 0x1F - gphy->nrssi[0];
0400     for (i = 0; i < 64; i++) {
0401         tmp = (i - delta) * gphy->nrssislope;
0402         tmp /= 0x10000;
0403         tmp += 0x3A;
0404         tmp = clamp_val(tmp, 0, 0x3F);
0405         gphy->nrssi_lt[i] = tmp;
0406     }
0407 }
0408 
0409 static void b43_calc_nrssi_offset(struct b43_wldev *dev)
0410 {
0411     struct b43_phy *phy = &dev->phy;
0412     u16 backup[20] = { 0 };
0413     s16 v47F;
0414     u16 i;
0415     u16 saved = 0xFFFF;
0416 
0417     backup[0] = b43_phy_read(dev, 0x0001);
0418     backup[1] = b43_phy_read(dev, 0x0811);
0419     backup[2] = b43_phy_read(dev, 0x0812);
0420     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
0421         backup[3] = b43_phy_read(dev, 0x0814);
0422         backup[4] = b43_phy_read(dev, 0x0815);
0423     }
0424     backup[5] = b43_phy_read(dev, 0x005A);
0425     backup[6] = b43_phy_read(dev, 0x0059);
0426     backup[7] = b43_phy_read(dev, 0x0058);
0427     backup[8] = b43_phy_read(dev, 0x000A);
0428     backup[9] = b43_phy_read(dev, 0x0003);
0429     backup[10] = b43_radio_read16(dev, 0x007A);
0430     backup[11] = b43_radio_read16(dev, 0x0043);
0431 
0432     b43_phy_mask(dev, 0x0429, 0x7FFF);
0433     b43_phy_maskset(dev, 0x0001, 0x3FFF, 0x4000);
0434     b43_phy_set(dev, 0x0811, 0x000C);
0435     b43_phy_maskset(dev, 0x0812, 0xFFF3, 0x0004);
0436     b43_phy_mask(dev, 0x0802, ~(0x1 | 0x2));
0437     if (phy->rev >= 6) {
0438         backup[12] = b43_phy_read(dev, 0x002E);
0439         backup[13] = b43_phy_read(dev, 0x002F);
0440         backup[14] = b43_phy_read(dev, 0x080F);
0441         backup[15] = b43_phy_read(dev, 0x0810);
0442         backup[16] = b43_phy_read(dev, 0x0801);
0443         backup[17] = b43_phy_read(dev, 0x0060);
0444         backup[18] = b43_phy_read(dev, 0x0014);
0445         backup[19] = b43_phy_read(dev, 0x0478);
0446 
0447         b43_phy_write(dev, 0x002E, 0);
0448         b43_phy_write(dev, 0x002F, 0);
0449         b43_phy_write(dev, 0x080F, 0);
0450         b43_phy_write(dev, 0x0810, 0);
0451         b43_phy_set(dev, 0x0478, 0x0100);
0452         b43_phy_set(dev, 0x0801, 0x0040);
0453         b43_phy_set(dev, 0x0060, 0x0040);
0454         b43_phy_set(dev, 0x0014, 0x0200);
0455     }
0456     b43_radio_set(dev, 0x007A, 0x0070);
0457     b43_radio_set(dev, 0x007A, 0x0080);
0458     udelay(30);
0459 
0460     v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
0461     if (v47F >= 0x20)
0462         v47F -= 0x40;
0463     if (v47F == 31) {
0464         for (i = 7; i >= 4; i--) {
0465             b43_radio_write16(dev, 0x007B, i);
0466             udelay(20);
0467             v47F =
0468                 (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
0469             if (v47F >= 0x20)
0470                 v47F -= 0x40;
0471             if (v47F < 31 && saved == 0xFFFF)
0472                 saved = i;
0473         }
0474         if (saved == 0xFFFF)
0475             saved = 4;
0476     } else {
0477         b43_radio_mask(dev, 0x007A, 0x007F);
0478         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
0479             b43_phy_set(dev, 0x0814, 0x0001);
0480             b43_phy_mask(dev, 0x0815, 0xFFFE);
0481         }
0482         b43_phy_set(dev, 0x0811, 0x000C);
0483         b43_phy_set(dev, 0x0812, 0x000C);
0484         b43_phy_set(dev, 0x0811, 0x0030);
0485         b43_phy_set(dev, 0x0812, 0x0030);
0486         b43_phy_write(dev, 0x005A, 0x0480);
0487         b43_phy_write(dev, 0x0059, 0x0810);
0488         b43_phy_write(dev, 0x0058, 0x000D);
0489         if (phy->rev == 0) {
0490             b43_phy_write(dev, 0x0003, 0x0122);
0491         } else {
0492             b43_phy_set(dev, 0x000A, 0x2000);
0493         }
0494         if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
0495             b43_phy_set(dev, 0x0814, 0x0004);
0496             b43_phy_mask(dev, 0x0815, 0xFFFB);
0497         }
0498         b43_phy_maskset(dev, 0x0003, 0xFF9F, 0x0040);
0499         b43_radio_set(dev, 0x007A, 0x000F);
0500         b43_set_all_gains(dev, 3, 0, 1);
0501         b43_radio_maskset(dev, 0x0043, 0x00F0, 0x000F);
0502         udelay(30);
0503         v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
0504         if (v47F >= 0x20)
0505             v47F -= 0x40;
0506         if (v47F == -32) {
0507             for (i = 0; i < 4; i++) {
0508                 b43_radio_write16(dev, 0x007B, i);
0509                 udelay(20);
0510                 v47F =
0511                     (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
0512                        0x003F);
0513                 if (v47F >= 0x20)
0514                     v47F -= 0x40;
0515                 if (v47F > -31 && saved == 0xFFFF)
0516                     saved = i;
0517             }
0518             if (saved == 0xFFFF)
0519                 saved = 3;
0520         } else
0521             saved = 0;
0522     }
0523     b43_radio_write16(dev, 0x007B, saved);
0524 
0525     if (phy->rev >= 6) {
0526         b43_phy_write(dev, 0x002E, backup[12]);
0527         b43_phy_write(dev, 0x002F, backup[13]);
0528         b43_phy_write(dev, 0x080F, backup[14]);
0529         b43_phy_write(dev, 0x0810, backup[15]);
0530     }
0531     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
0532         b43_phy_write(dev, 0x0814, backup[3]);
0533         b43_phy_write(dev, 0x0815, backup[4]);
0534     }
0535     b43_phy_write(dev, 0x005A, backup[5]);
0536     b43_phy_write(dev, 0x0059, backup[6]);
0537     b43_phy_write(dev, 0x0058, backup[7]);
0538     b43_phy_write(dev, 0x000A, backup[8]);
0539     b43_phy_write(dev, 0x0003, backup[9]);
0540     b43_radio_write16(dev, 0x0043, backup[11]);
0541     b43_radio_write16(dev, 0x007A, backup[10]);
0542     b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
0543     b43_phy_set(dev, 0x0429, 0x8000);
0544     b43_set_original_gains(dev);
0545     if (phy->rev >= 6) {
0546         b43_phy_write(dev, 0x0801, backup[16]);
0547         b43_phy_write(dev, 0x0060, backup[17]);
0548         b43_phy_write(dev, 0x0014, backup[18]);
0549         b43_phy_write(dev, 0x0478, backup[19]);
0550     }
0551     b43_phy_write(dev, 0x0001, backup[0]);
0552     b43_phy_write(dev, 0x0812, backup[2]);
0553     b43_phy_write(dev, 0x0811, backup[1]);
0554 }
0555 
0556 static void b43_calc_nrssi_slope(struct b43_wldev *dev)
0557 {
0558     struct b43_phy *phy = &dev->phy;
0559     struct b43_phy_g *gphy = phy->g;
0560     u16 backup[18] = { 0 };
0561     u16 tmp;
0562     s16 nrssi0, nrssi1;
0563 
0564     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
0565 
0566     if (phy->radio_rev >= 9)
0567         return;
0568     if (phy->radio_rev == 8)
0569         b43_calc_nrssi_offset(dev);
0570 
0571     b43_phy_mask(dev, B43_PHY_G_CRS, 0x7FFF);
0572     b43_phy_mask(dev, 0x0802, 0xFFFC);
0573     backup[7] = b43_read16(dev, 0x03E2);
0574     b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
0575     backup[0] = b43_radio_read16(dev, 0x007A);
0576     backup[1] = b43_radio_read16(dev, 0x0052);
0577     backup[2] = b43_radio_read16(dev, 0x0043);
0578     backup[3] = b43_phy_read(dev, 0x0015);
0579     backup[4] = b43_phy_read(dev, 0x005A);
0580     backup[5] = b43_phy_read(dev, 0x0059);
0581     backup[6] = b43_phy_read(dev, 0x0058);
0582     backup[8] = b43_read16(dev, 0x03E6);
0583     backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
0584     if (phy->rev >= 3) {
0585         backup[10] = b43_phy_read(dev, 0x002E);
0586         backup[11] = b43_phy_read(dev, 0x002F);
0587         backup[12] = b43_phy_read(dev, 0x080F);
0588         backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
0589         backup[14] = b43_phy_read(dev, 0x0801);
0590         backup[15] = b43_phy_read(dev, 0x0060);
0591         backup[16] = b43_phy_read(dev, 0x0014);
0592         backup[17] = b43_phy_read(dev, 0x0478);
0593         b43_phy_write(dev, 0x002E, 0);
0594         b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
0595         switch (phy->rev) {
0596         case 4:
0597         case 6:
0598         case 7:
0599             b43_phy_set(dev, 0x0478, 0x0100);
0600             b43_phy_set(dev, 0x0801, 0x0040);
0601             break;
0602         case 3:
0603         case 5:
0604             b43_phy_mask(dev, 0x0801, 0xFFBF);
0605             break;
0606         }
0607         b43_phy_set(dev, 0x0060, 0x0040);
0608         b43_phy_set(dev, 0x0014, 0x0200);
0609     }
0610     b43_radio_set(dev, 0x007A, 0x0070);
0611     b43_set_all_gains(dev, 0, 8, 0);
0612     b43_radio_mask(dev, 0x007A, 0x00F7);
0613     if (phy->rev >= 2) {
0614         b43_phy_maskset(dev, 0x0811, 0xFFCF, 0x0030);
0615         b43_phy_maskset(dev, 0x0812, 0xFFCF, 0x0010);
0616     }
0617     b43_radio_set(dev, 0x007A, 0x0080);
0618     udelay(20);
0619 
0620     nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
0621     if (nrssi0 >= 0x0020)
0622         nrssi0 -= 0x0040;
0623 
0624     b43_radio_mask(dev, 0x007A, 0x007F);
0625     if (phy->rev >= 2) {
0626         b43_phy_maskset(dev, 0x0003, 0xFF9F, 0x0040);
0627     }
0628 
0629     b43_write16(dev, B43_MMIO_CHANNEL_EXT,
0630             b43_read16(dev, B43_MMIO_CHANNEL_EXT)
0631             | 0x2000);
0632     b43_radio_set(dev, 0x007A, 0x000F);
0633     b43_phy_write(dev, 0x0015, 0xF330);
0634     if (phy->rev >= 2) {
0635         b43_phy_maskset(dev, 0x0812, 0xFFCF, 0x0020);
0636         b43_phy_maskset(dev, 0x0811, 0xFFCF, 0x0020);
0637     }
0638 
0639     b43_set_all_gains(dev, 3, 0, 1);
0640     if (phy->radio_rev == 8) {
0641         b43_radio_write16(dev, 0x0043, 0x001F);
0642     } else {
0643         tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
0644         b43_radio_write16(dev, 0x0052, tmp | 0x0060);
0645         tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
0646         b43_radio_write16(dev, 0x0043, tmp | 0x0009);
0647     }
0648     b43_phy_write(dev, 0x005A, 0x0480);
0649     b43_phy_write(dev, 0x0059, 0x0810);
0650     b43_phy_write(dev, 0x0058, 0x000D);
0651     udelay(20);
0652     nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
0653     if (nrssi1 >= 0x0020)
0654         nrssi1 -= 0x0040;
0655     if (nrssi0 == nrssi1)
0656         gphy->nrssislope = 0x00010000;
0657     else
0658         gphy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
0659     if (nrssi0 >= -4) {
0660         gphy->nrssi[0] = nrssi1;
0661         gphy->nrssi[1] = nrssi0;
0662     }
0663     if (phy->rev >= 3) {
0664         b43_phy_write(dev, 0x002E, backup[10]);
0665         b43_phy_write(dev, 0x002F, backup[11]);
0666         b43_phy_write(dev, 0x080F, backup[12]);
0667         b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
0668     }
0669     if (phy->rev >= 2) {
0670         b43_phy_mask(dev, 0x0812, 0xFFCF);
0671         b43_phy_mask(dev, 0x0811, 0xFFCF);
0672     }
0673 
0674     b43_radio_write16(dev, 0x007A, backup[0]);
0675     b43_radio_write16(dev, 0x0052, backup[1]);
0676     b43_radio_write16(dev, 0x0043, backup[2]);
0677     b43_write16(dev, 0x03E2, backup[7]);
0678     b43_write16(dev, 0x03E6, backup[8]);
0679     b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
0680     b43_phy_write(dev, 0x0015, backup[3]);
0681     b43_phy_write(dev, 0x005A, backup[4]);
0682     b43_phy_write(dev, 0x0059, backup[5]);
0683     b43_phy_write(dev, 0x0058, backup[6]);
0684     b43_synth_pu_workaround(dev, phy->channel);
0685     b43_phy_set(dev, 0x0802, (0x0001 | 0x0002));
0686     b43_set_original_gains(dev);
0687     b43_phy_set(dev, B43_PHY_G_CRS, 0x8000);
0688     if (phy->rev >= 3) {
0689         b43_phy_write(dev, 0x0801, backup[14]);
0690         b43_phy_write(dev, 0x0060, backup[15]);
0691         b43_phy_write(dev, 0x0014, backup[16]);
0692         b43_phy_write(dev, 0x0478, backup[17]);
0693     }
0694     b43_nrssi_mem_update(dev);
0695     b43_calc_nrssi_threshold(dev);
0696 }
0697 
0698 static void b43_calc_nrssi_threshold(struct b43_wldev *dev)
0699 {
0700     struct b43_phy *phy = &dev->phy;
0701     struct b43_phy_g *gphy = phy->g;
0702     s32 a, b;
0703     s16 tmp16;
0704     u16 tmp_u16;
0705 
0706     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
0707 
0708     if (!phy->gmode ||
0709         !(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI)) {
0710         tmp16 = b43_nrssi_hw_read(dev, 0x20);
0711         if (tmp16 >= 0x20)
0712             tmp16 -= 0x40;
0713         if (tmp16 < 3) {
0714             b43_phy_maskset(dev, 0x048A, 0xF000, 0x09EB);
0715         } else {
0716             b43_phy_maskset(dev, 0x048A, 0xF000, 0x0AED);
0717         }
0718     } else {
0719         if (gphy->interfmode == B43_INTERFMODE_NONWLAN) {
0720             a = 0xE;
0721             b = 0xA;
0722         } else if (!gphy->aci_wlan_automatic && gphy->aci_enable) {
0723             a = 0x13;
0724             b = 0x12;
0725         } else {
0726             a = 0xE;
0727             b = 0x11;
0728         }
0729 
0730         a = a * (gphy->nrssi[1] - gphy->nrssi[0]);
0731         a += (gphy->nrssi[0] << 6);
0732         if (a < 32)
0733             a += 31;
0734         else
0735             a += 32;
0736         a = a >> 6;
0737         a = clamp_val(a, -31, 31);
0738 
0739         b = b * (gphy->nrssi[1] - gphy->nrssi[0]);
0740         b += (gphy->nrssi[0] << 6);
0741         if (b < 32)
0742             b += 31;
0743         else
0744             b += 32;
0745         b = b >> 6;
0746         b = clamp_val(b, -31, 31);
0747 
0748         tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
0749         tmp_u16 |= ((u32) b & 0x0000003F);
0750         tmp_u16 |= (((u32) a & 0x0000003F) << 6);
0751         b43_phy_write(dev, 0x048A, tmp_u16);
0752     }
0753 }
0754 
0755 /* Stack implementation to save/restore values from the
0756  * interference mitigation code.
0757  * It is save to restore values in random order.
0758  */
0759 static void _stack_save(u32 *_stackptr, size_t *stackidx,
0760             u8 id, u16 offset, u16 value)
0761 {
0762     u32 *stackptr = &(_stackptr[*stackidx]);
0763 
0764     B43_WARN_ON(offset & 0xF000);
0765     B43_WARN_ON(id & 0xF0);
0766     *stackptr = offset;
0767     *stackptr |= ((u32) id) << 12;
0768     *stackptr |= ((u32) value) << 16;
0769     (*stackidx)++;
0770     B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
0771 }
0772 
0773 static u16 _stack_restore(u32 *stackptr, u8 id, u16 offset)
0774 {
0775     size_t i;
0776 
0777     B43_WARN_ON(offset & 0xF000);
0778     B43_WARN_ON(id & 0xF0);
0779     for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
0780         if ((*stackptr & 0x00000FFF) != offset)
0781             continue;
0782         if (((*stackptr & 0x0000F000) >> 12) != id)
0783             continue;
0784         return ((*stackptr & 0xFFFF0000) >> 16);
0785     }
0786     B43_WARN_ON(1);
0787 
0788     return 0;
0789 }
0790 
0791 #define phy_stacksave(offset)                   \
0792     do {                            \
0793         _stack_save(stack, &stackidx, 0x1, (offset),    \
0794                 b43_phy_read(dev, (offset)));   \
0795     } while (0)
0796 #define phy_stackrestore(offset)                \
0797     do {                            \
0798         b43_phy_write(dev, (offset),        \
0799                   _stack_restore(stack, 0x1,    \
0800                          (offset)));    \
0801     } while (0)
0802 #define radio_stacksave(offset)                     \
0803     do {                                \
0804         _stack_save(stack, &stackidx, 0x2, (offset),        \
0805                 b43_radio_read16(dev, (offset)));   \
0806     } while (0)
0807 #define radio_stackrestore(offset)                  \
0808     do {                                \
0809         b43_radio_write16(dev, (offset),            \
0810                       _stack_restore(stack, 0x2,    \
0811                              (offset)));    \
0812     } while (0)
0813 #define ofdmtab_stacksave(table, offset)            \
0814     do {                            \
0815         _stack_save(stack, &stackidx, 0x3, (offset)|(table),    \
0816                 b43_ofdmtab_read16(dev, (table), (offset)));    \
0817     } while (0)
0818 #define ofdmtab_stackrestore(table, offset)         \
0819     do {                            \
0820         b43_ofdmtab_write16(dev, (table),   (offset),   \
0821                   _stack_restore(stack, 0x3,    \
0822                          (offset)|(table)));    \
0823     } while (0)
0824 
0825 static void
0826 b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
0827 {
0828     struct b43_phy *phy = &dev->phy;
0829     struct b43_phy_g *gphy = phy->g;
0830     u16 tmp, flipped;
0831     size_t stackidx = 0;
0832     u32 *stack = gphy->interfstack;
0833 
0834     switch (mode) {
0835     case B43_INTERFMODE_NONWLAN:
0836         if (phy->rev != 1) {
0837             b43_phy_set(dev, 0x042B, 0x0800);
0838             b43_phy_mask(dev, B43_PHY_G_CRS, ~0x4000);
0839             break;
0840         }
0841         radio_stacksave(0x0078);
0842         tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
0843         B43_WARN_ON(tmp > 15);
0844         flipped = bitrev4(tmp);
0845         if (flipped < 10 && flipped >= 8)
0846             flipped = 7;
0847         else if (flipped >= 10)
0848             flipped -= 3;
0849         flipped = (bitrev4(flipped) << 1) | 0x0020;
0850         b43_radio_write16(dev, 0x0078, flipped);
0851 
0852         b43_calc_nrssi_threshold(dev);
0853 
0854         phy_stacksave(0x0406);
0855         b43_phy_write(dev, 0x0406, 0x7E28);
0856 
0857         b43_phy_set(dev, 0x042B, 0x0800);
0858         b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, 0x1000);
0859 
0860         phy_stacksave(0x04A0);
0861         b43_phy_maskset(dev, 0x04A0, 0xC0C0, 0x0008);
0862         phy_stacksave(0x04A1);
0863         b43_phy_maskset(dev, 0x04A1, 0xC0C0, 0x0605);
0864         phy_stacksave(0x04A2);
0865         b43_phy_maskset(dev, 0x04A2, 0xC0C0, 0x0204);
0866         phy_stacksave(0x04A8);
0867         b43_phy_maskset(dev, 0x04A8, 0xC0C0, 0x0803);
0868         phy_stacksave(0x04AB);
0869         b43_phy_maskset(dev, 0x04AB, 0xC0C0, 0x0605);
0870 
0871         phy_stacksave(0x04A7);
0872         b43_phy_write(dev, 0x04A7, 0x0002);
0873         phy_stacksave(0x04A3);
0874         b43_phy_write(dev, 0x04A3, 0x287A);
0875         phy_stacksave(0x04A9);
0876         b43_phy_write(dev, 0x04A9, 0x2027);
0877         phy_stacksave(0x0493);
0878         b43_phy_write(dev, 0x0493, 0x32F5);
0879         phy_stacksave(0x04AA);
0880         b43_phy_write(dev, 0x04AA, 0x2027);
0881         phy_stacksave(0x04AC);
0882         b43_phy_write(dev, 0x04AC, 0x32F5);
0883         break;
0884     case B43_INTERFMODE_MANUALWLAN:
0885         if (b43_phy_read(dev, 0x0033) & 0x0800)
0886             break;
0887 
0888         gphy->aci_enable = true;
0889 
0890         phy_stacksave(B43_PHY_RADIO_BITFIELD);
0891         phy_stacksave(B43_PHY_G_CRS);
0892         if (phy->rev < 2) {
0893             phy_stacksave(0x0406);
0894         } else {
0895             phy_stacksave(0x04C0);
0896             phy_stacksave(0x04C1);
0897         }
0898         phy_stacksave(0x0033);
0899         phy_stacksave(0x04A7);
0900         phy_stacksave(0x04A3);
0901         phy_stacksave(0x04A9);
0902         phy_stacksave(0x04AA);
0903         phy_stacksave(0x04AC);
0904         phy_stacksave(0x0493);
0905         phy_stacksave(0x04A1);
0906         phy_stacksave(0x04A0);
0907         phy_stacksave(0x04A2);
0908         phy_stacksave(0x048A);
0909         phy_stacksave(0x04A8);
0910         phy_stacksave(0x04AB);
0911         if (phy->rev == 2) {
0912             phy_stacksave(0x04AD);
0913             phy_stacksave(0x04AE);
0914         } else if (phy->rev >= 3) {
0915             phy_stacksave(0x04AD);
0916             phy_stacksave(0x0415);
0917             phy_stacksave(0x0416);
0918             phy_stacksave(0x0417);
0919             ofdmtab_stacksave(0x1A00, 0x2);
0920             ofdmtab_stacksave(0x1A00, 0x3);
0921         }
0922         phy_stacksave(0x042B);
0923         phy_stacksave(0x048C);
0924 
0925         b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, ~0x1000);
0926         b43_phy_maskset(dev, B43_PHY_G_CRS, 0xFFFC, 0x0002);
0927 
0928         b43_phy_write(dev, 0x0033, 0x0800);
0929         b43_phy_write(dev, 0x04A3, 0x2027);
0930         b43_phy_write(dev, 0x04A9, 0x1CA8);
0931         b43_phy_write(dev, 0x0493, 0x287A);
0932         b43_phy_write(dev, 0x04AA, 0x1CA8);
0933         b43_phy_write(dev, 0x04AC, 0x287A);
0934 
0935         b43_phy_maskset(dev, 0x04A0, 0xFFC0, 0x001A);
0936         b43_phy_write(dev, 0x04A7, 0x000D);
0937 
0938         if (phy->rev < 2) {
0939             b43_phy_write(dev, 0x0406, 0xFF0D);
0940         } else if (phy->rev == 2) {
0941             b43_phy_write(dev, 0x04C0, 0xFFFF);
0942             b43_phy_write(dev, 0x04C1, 0x00A9);
0943         } else {
0944             b43_phy_write(dev, 0x04C0, 0x00C1);
0945             b43_phy_write(dev, 0x04C1, 0x0059);
0946         }
0947 
0948         b43_phy_maskset(dev, 0x04A1, 0xC0FF, 0x1800);
0949         b43_phy_maskset(dev, 0x04A1, 0xFFC0, 0x0015);
0950         b43_phy_maskset(dev, 0x04A8, 0xCFFF, 0x1000);
0951         b43_phy_maskset(dev, 0x04A8, 0xF0FF, 0x0A00);
0952         b43_phy_maskset(dev, 0x04AB, 0xCFFF, 0x1000);
0953         b43_phy_maskset(dev, 0x04AB, 0xF0FF, 0x0800);
0954         b43_phy_maskset(dev, 0x04AB, 0xFFCF, 0x0010);
0955         b43_phy_maskset(dev, 0x04AB, 0xFFF0, 0x0005);
0956         b43_phy_maskset(dev, 0x04A8, 0xFFCF, 0x0010);
0957         b43_phy_maskset(dev, 0x04A8, 0xFFF0, 0x0006);
0958         b43_phy_maskset(dev, 0x04A2, 0xF0FF, 0x0800);
0959         b43_phy_maskset(dev, 0x04A0, 0xF0FF, 0x0500);
0960         b43_phy_maskset(dev, 0x04A2, 0xFFF0, 0x000B);
0961 
0962         if (phy->rev >= 3) {
0963             b43_phy_mask(dev, 0x048A, 0x7FFF);
0964             b43_phy_maskset(dev, 0x0415, 0x8000, 0x36D8);
0965             b43_phy_maskset(dev, 0x0416, 0x8000, 0x36D8);
0966             b43_phy_maskset(dev, 0x0417, 0xFE00, 0x016D);
0967         } else {
0968             b43_phy_set(dev, 0x048A, 0x1000);
0969             b43_phy_maskset(dev, 0x048A, 0x9FFF, 0x2000);
0970             b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
0971         }
0972         if (phy->rev >= 2) {
0973             b43_phy_set(dev, 0x042B, 0x0800);
0974         }
0975         b43_phy_maskset(dev, 0x048C, 0xF0FF, 0x0200);
0976         if (phy->rev == 2) {
0977             b43_phy_maskset(dev, 0x04AE, 0xFF00, 0x007F);
0978             b43_phy_maskset(dev, 0x04AD, 0x00FF, 0x1300);
0979         } else if (phy->rev >= 6) {
0980             b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
0981             b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
0982             b43_phy_mask(dev, 0x04AD, 0x00FF);
0983         }
0984         b43_calc_nrssi_slope(dev);
0985         break;
0986     default:
0987         B43_WARN_ON(1);
0988     }
0989 }
0990 
0991 static void
0992 b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
0993 {
0994     struct b43_phy *phy = &dev->phy;
0995     struct b43_phy_g *gphy = phy->g;
0996     u32 *stack = gphy->interfstack;
0997 
0998     switch (mode) {
0999     case B43_INTERFMODE_NONWLAN:
1000         if (phy->rev != 1) {
1001             b43_phy_mask(dev, 0x042B, ~0x0800);
1002             b43_phy_set(dev, B43_PHY_G_CRS, 0x4000);
1003             break;
1004         }
1005         radio_stackrestore(0x0078);
1006         b43_calc_nrssi_threshold(dev);
1007         phy_stackrestore(0x0406);
1008         b43_phy_mask(dev, 0x042B, ~0x0800);
1009         if (!dev->bad_frames_preempt) {
1010             b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, ~(1 << 11));
1011         }
1012         b43_phy_set(dev, B43_PHY_G_CRS, 0x4000);
1013         phy_stackrestore(0x04A0);
1014         phy_stackrestore(0x04A1);
1015         phy_stackrestore(0x04A2);
1016         phy_stackrestore(0x04A8);
1017         phy_stackrestore(0x04AB);
1018         phy_stackrestore(0x04A7);
1019         phy_stackrestore(0x04A3);
1020         phy_stackrestore(0x04A9);
1021         phy_stackrestore(0x0493);
1022         phy_stackrestore(0x04AA);
1023         phy_stackrestore(0x04AC);
1024         break;
1025     case B43_INTERFMODE_MANUALWLAN:
1026         if (!(b43_phy_read(dev, 0x0033) & 0x0800))
1027             break;
1028 
1029         gphy->aci_enable = false;
1030 
1031         phy_stackrestore(B43_PHY_RADIO_BITFIELD);
1032         phy_stackrestore(B43_PHY_G_CRS);
1033         phy_stackrestore(0x0033);
1034         phy_stackrestore(0x04A3);
1035         phy_stackrestore(0x04A9);
1036         phy_stackrestore(0x0493);
1037         phy_stackrestore(0x04AA);
1038         phy_stackrestore(0x04AC);
1039         phy_stackrestore(0x04A0);
1040         phy_stackrestore(0x04A7);
1041         if (phy->rev >= 2) {
1042             phy_stackrestore(0x04C0);
1043             phy_stackrestore(0x04C1);
1044         } else
1045             phy_stackrestore(0x0406);
1046         phy_stackrestore(0x04A1);
1047         phy_stackrestore(0x04AB);
1048         phy_stackrestore(0x04A8);
1049         if (phy->rev == 2) {
1050             phy_stackrestore(0x04AD);
1051             phy_stackrestore(0x04AE);
1052         } else if (phy->rev >= 3) {
1053             phy_stackrestore(0x04AD);
1054             phy_stackrestore(0x0415);
1055             phy_stackrestore(0x0416);
1056             phy_stackrestore(0x0417);
1057             ofdmtab_stackrestore(0x1A00, 0x2);
1058             ofdmtab_stackrestore(0x1A00, 0x3);
1059         }
1060         phy_stackrestore(0x04A2);
1061         phy_stackrestore(0x048A);
1062         phy_stackrestore(0x042B);
1063         phy_stackrestore(0x048C);
1064         b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
1065         b43_calc_nrssi_slope(dev);
1066         break;
1067     default:
1068         B43_WARN_ON(1);
1069     }
1070 }
1071 
1072 #undef phy_stacksave
1073 #undef phy_stackrestore
1074 #undef radio_stacksave
1075 #undef radio_stackrestore
1076 #undef ofdmtab_stacksave
1077 #undef ofdmtab_stackrestore
1078 
1079 static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
1080 {
1081     u16 reg, index, ret;
1082 
1083     static const u8 rcc_table[] = {
1084         0x02, 0x03, 0x01, 0x0F,
1085         0x06, 0x07, 0x05, 0x0F,
1086         0x0A, 0x0B, 0x09, 0x0F,
1087         0x0E, 0x0F, 0x0D, 0x0F,
1088     };
1089 
1090     reg = b43_radio_read16(dev, 0x60);
1091     index = (reg & 0x001E) >> 1;
1092     ret = rcc_table[index] << 1;
1093     ret |= (reg & 0x0001);
1094     ret |= 0x0020;
1095 
1096     return ret;
1097 }
1098 
1099 #define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
1100 static u16 radio2050_rfover_val(struct b43_wldev *dev,
1101                 u16 phy_register, unsigned int lpd)
1102 {
1103     struct b43_phy *phy = &dev->phy;
1104     struct b43_phy_g *gphy = phy->g;
1105     struct ssb_sprom *sprom = dev->dev->bus_sprom;
1106 
1107     if (!phy->gmode)
1108         return 0;
1109 
1110     if (has_loopback_gain(phy)) {
1111         int max_lb_gain = gphy->max_lb_gain;
1112         u16 extlna;
1113         u16 i;
1114 
1115         if (phy->radio_rev == 8)
1116             max_lb_gain += 0x3E;
1117         else
1118             max_lb_gain += 0x26;
1119         if (max_lb_gain >= 0x46) {
1120             extlna = 0x3000;
1121             max_lb_gain -= 0x46;
1122         } else if (max_lb_gain >= 0x3A) {
1123             extlna = 0x1000;
1124             max_lb_gain -= 0x3A;
1125         } else if (max_lb_gain >= 0x2E) {
1126             extlna = 0x2000;
1127             max_lb_gain -= 0x2E;
1128         } else {
1129             extlna = 0;
1130             max_lb_gain -= 0x10;
1131         }
1132 
1133         for (i = 0; i < 16; i++) {
1134             max_lb_gain -= (i * 6);
1135             if (max_lb_gain < 6)
1136                 break;
1137         }
1138 
1139         if ((phy->rev < 7) ||
1140             !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
1141             if (phy_register == B43_PHY_RFOVER) {
1142                 return 0x1B3;
1143             } else if (phy_register == B43_PHY_RFOVERVAL) {
1144                 extlna |= (i << 8);
1145                 switch (lpd) {
1146                 case LPD(0, 1, 1):
1147                     return 0x0F92;
1148                 case LPD(0, 0, 1):
1149                 case LPD(1, 0, 1):
1150                     return (0x0092 | extlna);
1151                 case LPD(1, 0, 0):
1152                     return (0x0093 | extlna);
1153                 }
1154                 B43_WARN_ON(1);
1155             }
1156             B43_WARN_ON(1);
1157         } else {
1158             if (phy_register == B43_PHY_RFOVER) {
1159                 return 0x9B3;
1160             } else if (phy_register == B43_PHY_RFOVERVAL) {
1161                 if (extlna)
1162                     extlna |= 0x8000;
1163                 extlna |= (i << 8);
1164                 switch (lpd) {
1165                 case LPD(0, 1, 1):
1166                     return 0x8F92;
1167                 case LPD(0, 0, 1):
1168                     return (0x8092 | extlna);
1169                 case LPD(1, 0, 1):
1170                     return (0x2092 | extlna);
1171                 case LPD(1, 0, 0):
1172                     return (0x2093 | extlna);
1173                 }
1174                 B43_WARN_ON(1);
1175             }
1176             B43_WARN_ON(1);
1177         }
1178     } else {
1179         if ((phy->rev < 7) ||
1180             !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
1181             if (phy_register == B43_PHY_RFOVER) {
1182                 return 0x1B3;
1183             } else if (phy_register == B43_PHY_RFOVERVAL) {
1184                 switch (lpd) {
1185                 case LPD(0, 1, 1):
1186                     return 0x0FB2;
1187                 case LPD(0, 0, 1):
1188                     return 0x00B2;
1189                 case LPD(1, 0, 1):
1190                     return 0x30B2;
1191                 case LPD(1, 0, 0):
1192                     return 0x30B3;
1193                 }
1194                 B43_WARN_ON(1);
1195             }
1196             B43_WARN_ON(1);
1197         } else {
1198             if (phy_register == B43_PHY_RFOVER) {
1199                 return 0x9B3;
1200             } else if (phy_register == B43_PHY_RFOVERVAL) {
1201                 switch (lpd) {
1202                 case LPD(0, 1, 1):
1203                     return 0x8FB2;
1204                 case LPD(0, 0, 1):
1205                     return 0x80B2;
1206                 case LPD(1, 0, 1):
1207                     return 0x20B2;
1208                 case LPD(1, 0, 0):
1209                     return 0x20B3;
1210                 }
1211                 B43_WARN_ON(1);
1212             }
1213             B43_WARN_ON(1);
1214         }
1215     }
1216     return 0;
1217 }
1218 
1219 struct init2050_saved_values {
1220     /* Core registers */
1221     u16 reg_3EC;
1222     u16 reg_3E6;
1223     u16 reg_3F4;
1224     /* Radio registers */
1225     u16 radio_43;
1226     u16 radio_51;
1227     u16 radio_52;
1228     /* PHY registers */
1229     u16 phy_pgactl;
1230     u16 phy_cck_5A;
1231     u16 phy_cck_59;
1232     u16 phy_cck_58;
1233     u16 phy_cck_30;
1234     u16 phy_rfover;
1235     u16 phy_rfoverval;
1236     u16 phy_analogover;
1237     u16 phy_analogoverval;
1238     u16 phy_crs0;
1239     u16 phy_classctl;
1240     u16 phy_lo_mask;
1241     u16 phy_lo_ctl;
1242     u16 phy_syncctl;
1243 };
1244 
1245 static u16 b43_radio_init2050(struct b43_wldev *dev)
1246 {
1247     struct b43_phy *phy = &dev->phy;
1248     struct init2050_saved_values sav;
1249     u16 rcc;
1250     u16 radio78;
1251     u16 ret;
1252     u16 i, j;
1253     u32 tmp1 = 0, tmp2 = 0;
1254 
1255     memset(&sav, 0, sizeof(sav));   /* get rid of "may be used uninitialized..." */
1256 
1257     sav.radio_43 = b43_radio_read16(dev, 0x43);
1258     sav.radio_51 = b43_radio_read16(dev, 0x51);
1259     sav.radio_52 = b43_radio_read16(dev, 0x52);
1260     sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
1261     sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
1262     sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
1263     sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
1264 
1265     if (phy->type == B43_PHYTYPE_B) {
1266         sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
1267         sav.reg_3EC = b43_read16(dev, 0x3EC);
1268 
1269         b43_phy_write(dev, B43_PHY_CCK(0x30), 0xFF);
1270         b43_write16(dev, 0x3EC, 0x3F3F);
1271     } else if (phy->gmode || phy->rev >= 2) {
1272         sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
1273         sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
1274         sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
1275         sav.phy_analogoverval =
1276             b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
1277         sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
1278         sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
1279 
1280         b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0003);
1281         b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFC);
1282         b43_phy_mask(dev, B43_PHY_CRS0, 0x7FFF);
1283         b43_phy_mask(dev, B43_PHY_CLASSCTL, 0xFFFC);
1284         if (has_loopback_gain(phy)) {
1285             sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
1286             sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
1287 
1288             if (phy->rev >= 3)
1289                 b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
1290             else
1291                 b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
1292             b43_phy_write(dev, B43_PHY_LO_CTL, 0);
1293         }
1294 
1295         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1296                   radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1297                            LPD(0, 1, 1)));
1298         b43_phy_write(dev, B43_PHY_RFOVER,
1299                   radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
1300     }
1301     b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
1302 
1303     sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
1304     b43_phy_mask(dev, B43_PHY_SYNCCTL, 0xFF7F);
1305     sav.reg_3E6 = b43_read16(dev, 0x3E6);
1306     sav.reg_3F4 = b43_read16(dev, 0x3F4);
1307 
1308     if (phy->analog == 0) {
1309         b43_write16(dev, 0x03E6, 0x0122);
1310     } else {
1311         if (phy->analog >= 2) {
1312             b43_phy_maskset(dev, B43_PHY_CCK(0x03), 0xFFBF, 0x40);
1313         }
1314         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
1315                 (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
1316     }
1317 
1318     rcc = b43_radio_core_calibration_value(dev);
1319 
1320     if (phy->type == B43_PHYTYPE_B)
1321         b43_radio_write16(dev, 0x78, 0x26);
1322     if (phy->gmode || phy->rev >= 2) {
1323         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1324                   radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1325                            LPD(0, 1, 1)));
1326     }
1327     b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
1328     b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x1403);
1329     if (phy->gmode || phy->rev >= 2) {
1330         b43_phy_write(dev, B43_PHY_RFOVERVAL,
1331                   radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1332                            LPD(0, 0, 1)));
1333     }
1334     b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
1335     b43_radio_set(dev, 0x51, 0x0004);
1336     if (phy->radio_rev == 8) {
1337         b43_radio_write16(dev, 0x43, 0x1F);
1338     } else {
1339         b43_radio_write16(dev, 0x52, 0);
1340         b43_radio_maskset(dev, 0x43, 0xFFF0, 0x0009);
1341     }
1342     b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
1343 
1344     for (i = 0; i < 16; i++) {
1345         b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0480);
1346         b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
1347         b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
1348         if (phy->gmode || phy->rev >= 2) {
1349             b43_phy_write(dev, B43_PHY_RFOVERVAL,
1350                       radio2050_rfover_val(dev,
1351                                B43_PHY_RFOVERVAL,
1352                                LPD(1, 0, 1)));
1353         }
1354         b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
1355         udelay(10);
1356         if (phy->gmode || phy->rev >= 2) {
1357             b43_phy_write(dev, B43_PHY_RFOVERVAL,
1358                       radio2050_rfover_val(dev,
1359                                B43_PHY_RFOVERVAL,
1360                                LPD(1, 0, 1)));
1361         }
1362         b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
1363         udelay(10);
1364         if (phy->gmode || phy->rev >= 2) {
1365             b43_phy_write(dev, B43_PHY_RFOVERVAL,
1366                       radio2050_rfover_val(dev,
1367                                B43_PHY_RFOVERVAL,
1368                                LPD(1, 0, 0)));
1369         }
1370         b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
1371         udelay(20);
1372         tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1373         b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
1374         if (phy->gmode || phy->rev >= 2) {
1375             b43_phy_write(dev, B43_PHY_RFOVERVAL,
1376                       radio2050_rfover_val(dev,
1377                                B43_PHY_RFOVERVAL,
1378                                LPD(1, 0, 1)));
1379         }
1380         b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
1381     }
1382     udelay(10);
1383 
1384     b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
1385     tmp1++;
1386     tmp1 >>= 9;
1387 
1388     for (i = 0; i < 16; i++) {
1389         radio78 = (bitrev4(i) << 1) | 0x0020;
1390         b43_radio_write16(dev, 0x78, radio78);
1391         udelay(10);
1392         for (j = 0; j < 16; j++) {
1393             b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0D80);
1394             b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
1395             b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
1396             if (phy->gmode || phy->rev >= 2) {
1397                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
1398                           radio2050_rfover_val(dev,
1399                                    B43_PHY_RFOVERVAL,
1400                                    LPD(1, 0,
1401                                        1)));
1402             }
1403             b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
1404             udelay(10);
1405             if (phy->gmode || phy->rev >= 2) {
1406                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
1407                           radio2050_rfover_val(dev,
1408                                    B43_PHY_RFOVERVAL,
1409                                    LPD(1, 0,
1410                                        1)));
1411             }
1412             b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
1413             udelay(10);
1414             if (phy->gmode || phy->rev >= 2) {
1415                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
1416                           radio2050_rfover_val(dev,
1417                                    B43_PHY_RFOVERVAL,
1418                                    LPD(1, 0,
1419                                        0)));
1420             }
1421             b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
1422             udelay(10);
1423             tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1424             b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
1425             if (phy->gmode || phy->rev >= 2) {
1426                 b43_phy_write(dev, B43_PHY_RFOVERVAL,
1427                           radio2050_rfover_val(dev,
1428                                    B43_PHY_RFOVERVAL,
1429                                    LPD(1, 0,
1430                                        1)));
1431             }
1432             b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
1433         }
1434         tmp2++;
1435         tmp2 >>= 8;
1436         if (tmp1 < tmp2)
1437             break;
1438     }
1439 
1440     /* Restore the registers */
1441     b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
1442     b43_radio_write16(dev, 0x51, sav.radio_51);
1443     b43_radio_write16(dev, 0x52, sav.radio_52);
1444     b43_radio_write16(dev, 0x43, sav.radio_43);
1445     b43_phy_write(dev, B43_PHY_CCK(0x5A), sav.phy_cck_5A);
1446     b43_phy_write(dev, B43_PHY_CCK(0x59), sav.phy_cck_59);
1447     b43_phy_write(dev, B43_PHY_CCK(0x58), sav.phy_cck_58);
1448     b43_write16(dev, 0x3E6, sav.reg_3E6);
1449     if (phy->analog != 0)
1450         b43_write16(dev, 0x3F4, sav.reg_3F4);
1451     b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
1452     b43_synth_pu_workaround(dev, phy->channel);
1453     if (phy->type == B43_PHYTYPE_B) {
1454         b43_phy_write(dev, B43_PHY_CCK(0x30), sav.phy_cck_30);
1455         b43_write16(dev, 0x3EC, sav.reg_3EC);
1456     } else if (phy->gmode) {
1457         b43_write16(dev, B43_MMIO_PHY_RADIO,
1458                 b43_read16(dev, B43_MMIO_PHY_RADIO)
1459                 & 0x7FFF);
1460         b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
1461         b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
1462         b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
1463         b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
1464                   sav.phy_analogoverval);
1465         b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
1466         b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
1467         if (has_loopback_gain(phy)) {
1468             b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
1469             b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
1470         }
1471     }
1472     if (i > 15)
1473         ret = radio78;
1474     else
1475         ret = rcc;
1476 
1477     return ret;
1478 }
1479 
1480 static void b43_phy_initb5(struct b43_wldev *dev)
1481 {
1482     struct b43_phy *phy = &dev->phy;
1483     struct b43_phy_g *gphy = phy->g;
1484     u16 offset, value;
1485     u8 old_channel;
1486 
1487     if (phy->analog == 1) {
1488         b43_radio_set(dev, 0x007A, 0x0050);
1489     }
1490     if ((dev->dev->board_vendor != SSB_BOARDVENDOR_BCM) &&
1491         (dev->dev->board_type != SSB_BOARD_BU4306)) {
1492         value = 0x2120;
1493         for (offset = 0x00A8; offset < 0x00C7; offset++) {
1494             b43_phy_write(dev, offset, value);
1495             value += 0x202;
1496         }
1497     }
1498     b43_phy_maskset(dev, 0x0035, 0xF0FF, 0x0700);
1499     if (phy->radio_ver == 0x2050)
1500         b43_phy_write(dev, 0x0038, 0x0667);
1501 
1502     if (phy->gmode || phy->rev >= 2) {
1503         if (phy->radio_ver == 0x2050) {
1504             b43_radio_set(dev, 0x007A, 0x0020);
1505             b43_radio_set(dev, 0x0051, 0x0004);
1506         }
1507         b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
1508 
1509         b43_phy_set(dev, 0x0802, 0x0100);
1510         b43_phy_set(dev, 0x042B, 0x2000);
1511 
1512         b43_phy_write(dev, 0x001C, 0x186A);
1513 
1514         b43_phy_maskset(dev, 0x0013, 0x00FF, 0x1900);
1515         b43_phy_maskset(dev, 0x0035, 0xFFC0, 0x0064);
1516         b43_phy_maskset(dev, 0x005D, 0xFF80, 0x000A);
1517     }
1518 
1519     if (dev->bad_frames_preempt) {
1520         b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, (1 << 11));
1521     }
1522 
1523     if (phy->analog == 1) {
1524         b43_phy_write(dev, 0x0026, 0xCE00);
1525         b43_phy_write(dev, 0x0021, 0x3763);
1526         b43_phy_write(dev, 0x0022, 0x1BC3);
1527         b43_phy_write(dev, 0x0023, 0x06F9);
1528         b43_phy_write(dev, 0x0024, 0x037E);
1529     } else
1530         b43_phy_write(dev, 0x0026, 0xCC00);
1531     b43_phy_write(dev, 0x0030, 0x00C6);
1532     b43_write16(dev, 0x03EC, 0x3F22);
1533 
1534     if (phy->analog == 1)
1535         b43_phy_write(dev, 0x0020, 0x3E1C);
1536     else
1537         b43_phy_write(dev, 0x0020, 0x301C);
1538 
1539     if (phy->analog == 0)
1540         b43_write16(dev, 0x03E4, 0x3000);
1541 
1542     old_channel = phy->channel;
1543     /* Force to channel 7, even if not supported. */
1544     b43_gphy_channel_switch(dev, 7, 0);
1545 
1546     if (phy->radio_ver != 0x2050) {
1547         b43_radio_write16(dev, 0x0075, 0x0080);
1548         b43_radio_write16(dev, 0x0079, 0x0081);
1549     }
1550 
1551     b43_radio_write16(dev, 0x0050, 0x0020);
1552     b43_radio_write16(dev, 0x0050, 0x0023);
1553 
1554     if (phy->radio_ver == 0x2050) {
1555         b43_radio_write16(dev, 0x0050, 0x0020);
1556         b43_radio_write16(dev, 0x005A, 0x0070);
1557     }
1558 
1559     b43_radio_write16(dev, 0x005B, 0x007B);
1560     b43_radio_write16(dev, 0x005C, 0x00B0);
1561 
1562     b43_radio_set(dev, 0x007A, 0x0007);
1563 
1564     b43_gphy_channel_switch(dev, old_channel, 0);
1565 
1566     b43_phy_write(dev, 0x0014, 0x0080);
1567     b43_phy_write(dev, 0x0032, 0x00CA);
1568     b43_phy_write(dev, 0x002A, 0x88A3);
1569 
1570     b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
1571 
1572     if (phy->radio_ver == 0x2050)
1573         b43_radio_write16(dev, 0x005D, 0x000D);
1574 
1575     b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
1576 }
1577 
1578 /* https://bcm-v4.sipsolutions.net/802.11/PHY/Init/B6 */
1579 static void b43_phy_initb6(struct b43_wldev *dev)
1580 {
1581     struct b43_phy *phy = &dev->phy;
1582     struct b43_phy_g *gphy = phy->g;
1583     u16 offset, val;
1584     u8 old_channel;
1585 
1586     b43_phy_write(dev, 0x003E, 0x817A);
1587     b43_radio_write16(dev, 0x007A,
1588               (b43_radio_read16(dev, 0x007A) | 0x0058));
1589     if (phy->radio_rev == 4 || phy->radio_rev == 5) {
1590         b43_radio_write16(dev, 0x51, 0x37);
1591         b43_radio_write16(dev, 0x52, 0x70);
1592         b43_radio_write16(dev, 0x53, 0xB3);
1593         b43_radio_write16(dev, 0x54, 0x9B);
1594         b43_radio_write16(dev, 0x5A, 0x88);
1595         b43_radio_write16(dev, 0x5B, 0x88);
1596         b43_radio_write16(dev, 0x5D, 0x88);
1597         b43_radio_write16(dev, 0x5E, 0x88);
1598         b43_radio_write16(dev, 0x7D, 0x88);
1599         b43_hf_write(dev, b43_hf_read(dev)
1600                  | B43_HF_TSSIRPSMW);
1601     }
1602     B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);    /* We had code for these revs here... */
1603     if (phy->radio_rev == 8) {
1604         b43_radio_write16(dev, 0x51, 0);
1605         b43_radio_write16(dev, 0x52, 0x40);
1606         b43_radio_write16(dev, 0x53, 0xB7);
1607         b43_radio_write16(dev, 0x54, 0x98);
1608         b43_radio_write16(dev, 0x5A, 0x88);
1609         b43_radio_write16(dev, 0x5B, 0x6B);
1610         b43_radio_write16(dev, 0x5C, 0x0F);
1611         if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_ALTIQ) {
1612             b43_radio_write16(dev, 0x5D, 0xFA);
1613             b43_radio_write16(dev, 0x5E, 0xD8);
1614         } else {
1615             b43_radio_write16(dev, 0x5D, 0xF5);
1616             b43_radio_write16(dev, 0x5E, 0xB8);
1617         }
1618         b43_radio_write16(dev, 0x0073, 0x0003);
1619         b43_radio_write16(dev, 0x007D, 0x00A8);
1620         b43_radio_write16(dev, 0x007C, 0x0001);
1621         b43_radio_write16(dev, 0x007E, 0x0008);
1622     }
1623     val = 0x1E1F;
1624     for (offset = 0x0088; offset < 0x0098; offset++) {
1625         b43_phy_write(dev, offset, val);
1626         val -= 0x0202;
1627     }
1628     val = 0x3E3F;
1629     for (offset = 0x0098; offset < 0x00A8; offset++) {
1630         b43_phy_write(dev, offset, val);
1631         val -= 0x0202;
1632     }
1633     val = 0x2120;
1634     for (offset = 0x00A8; offset < 0x00C8; offset++) {
1635         b43_phy_write(dev, offset, (val & 0x3F3F));
1636         val += 0x0202;
1637     }
1638     if (phy->type == B43_PHYTYPE_G) {
1639         b43_radio_set(dev, 0x007A, 0x0020);
1640         b43_radio_set(dev, 0x0051, 0x0004);
1641         b43_phy_set(dev, 0x0802, 0x0100);
1642         b43_phy_set(dev, 0x042B, 0x2000);
1643         b43_phy_write(dev, 0x5B, 0);
1644         b43_phy_write(dev, 0x5C, 0);
1645     }
1646 
1647     old_channel = phy->channel;
1648     if (old_channel >= 8)
1649         b43_gphy_channel_switch(dev, 1, 0);
1650     else
1651         b43_gphy_channel_switch(dev, 13, 0);
1652 
1653     b43_radio_write16(dev, 0x0050, 0x0020);
1654     b43_radio_write16(dev, 0x0050, 0x0023);
1655     udelay(40);
1656     if (phy->radio_rev < 6 || phy->radio_rev == 8) {
1657         b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
1658                           | 0x0002));
1659         b43_radio_write16(dev, 0x50, 0x20);
1660     }
1661     if (phy->radio_rev <= 2) {
1662         b43_radio_write16(dev, 0x50, 0x20);
1663         b43_radio_write16(dev, 0x5A, 0x70);
1664         b43_radio_write16(dev, 0x5B, 0x7B);
1665         b43_radio_write16(dev, 0x5C, 0xB0);
1666     }
1667     b43_radio_maskset(dev, 0x007A, 0x00F8, 0x0007);
1668 
1669     b43_gphy_channel_switch(dev, old_channel, 0);
1670 
1671     b43_phy_write(dev, 0x0014, 0x0200);
1672     if (phy->radio_rev >= 6)
1673         b43_phy_write(dev, 0x2A, 0x88C2);
1674     else
1675         b43_phy_write(dev, 0x2A, 0x8AC0);
1676     b43_phy_write(dev, 0x0038, 0x0668);
1677     b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
1678     if (phy->radio_rev == 4 || phy->radio_rev == 5)
1679         b43_phy_maskset(dev, 0x5D, 0xFF80, 0x0003);
1680     if (phy->radio_rev <= 2)
1681         b43_radio_write16(dev, 0x005D, 0x000D);
1682 
1683     if (phy->analog == 4) {
1684         b43_write16(dev, 0x3E4, 9);
1685         b43_phy_mask(dev, 0x61, 0x0FFF);
1686     } else {
1687         b43_phy_maskset(dev, 0x0002, 0xFFC0, 0x0004);
1688     }
1689     if (phy->type == B43_PHYTYPE_B)
1690         B43_WARN_ON(1);
1691     else if (phy->type == B43_PHYTYPE_G)
1692         b43_write16(dev, 0x03E6, 0x0);
1693 }
1694 
1695 static void b43_calc_loopback_gain(struct b43_wldev *dev)
1696 {
1697     struct b43_phy *phy = &dev->phy;
1698     struct b43_phy_g *gphy = phy->g;
1699     u16 backup_phy[16] = { 0 };
1700     u16 backup_radio[3];
1701     u16 backup_bband;
1702     u16 i, j, loop_i_max;
1703     u16 trsw_rx;
1704     u16 loop1_outer_done, loop1_inner_done;
1705 
1706     backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
1707     backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
1708     backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
1709     backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
1710     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1711         backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
1712         backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
1713     }
1714     backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
1715     backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
1716     backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
1717     backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
1718     backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
1719     backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
1720     backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
1721     backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
1722     backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
1723     backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1724     backup_bband = gphy->bbatt.att;
1725     backup_radio[0] = b43_radio_read16(dev, 0x52);
1726     backup_radio[1] = b43_radio_read16(dev, 0x43);
1727     backup_radio[2] = b43_radio_read16(dev, 0x7A);
1728 
1729     b43_phy_mask(dev, B43_PHY_CRS0, 0x3FFF);
1730     b43_phy_set(dev, B43_PHY_CCKBBANDCFG, 0x8000);
1731     b43_phy_set(dev, B43_PHY_RFOVER, 0x0002);
1732     b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xFFFD);
1733     b43_phy_set(dev, B43_PHY_RFOVER, 0x0001);
1734     b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xFFFE);
1735     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1736         b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0001);
1737         b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFE);
1738         b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0002);
1739         b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFD);
1740     }
1741     b43_phy_set(dev, B43_PHY_RFOVER, 0x000C);
1742     b43_phy_set(dev, B43_PHY_RFOVERVAL, 0x000C);
1743     b43_phy_set(dev, B43_PHY_RFOVER, 0x0030);
1744     b43_phy_maskset(dev, B43_PHY_RFOVERVAL, 0xFFCF, 0x10);
1745 
1746     b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0780);
1747     b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
1748     b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
1749 
1750     b43_phy_set(dev, B43_PHY_CCK(0x0A), 0x2000);
1751     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1752         b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0004);
1753         b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFB);
1754     }
1755     b43_phy_maskset(dev, B43_PHY_CCK(0x03), 0xFF9F, 0x40);
1756 
1757     if (phy->radio_rev == 8) {
1758         b43_radio_write16(dev, 0x43, 0x000F);
1759     } else {
1760         b43_radio_write16(dev, 0x52, 0);
1761         b43_radio_maskset(dev, 0x43, 0xFFF0, 0x9);
1762     }
1763     b43_gphy_set_baseband_attenuation(dev, 11);
1764 
1765     if (phy->rev >= 3)
1766         b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
1767     else
1768         b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
1769     b43_phy_write(dev, B43_PHY_LO_CTL, 0);
1770 
1771     b43_phy_maskset(dev, B43_PHY_CCK(0x2B), 0xFFC0, 0x01);
1772     b43_phy_maskset(dev, B43_PHY_CCK(0x2B), 0xC0FF, 0x800);
1773 
1774     b43_phy_set(dev, B43_PHY_RFOVER, 0x0100);
1775     b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xCFFF);
1776 
1777     if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_EXTLNA) {
1778         if (phy->rev >= 7) {
1779             b43_phy_set(dev, B43_PHY_RFOVER, 0x0800);
1780             b43_phy_set(dev, B43_PHY_RFOVERVAL, 0x8000);
1781         }
1782     }
1783     b43_radio_mask(dev, 0x7A, 0x00F7);
1784 
1785     j = 0;
1786     loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
1787     for (i = 0; i < loop_i_max; i++) {
1788         for (j = 0; j < 16; j++) {
1789             b43_radio_write16(dev, 0x43, i);
1790             b43_phy_maskset(dev, B43_PHY_RFOVERVAL, 0xF0FF, (j << 8));
1791             b43_phy_maskset(dev, B43_PHY_PGACTL, 0x0FFF, 0xA000);
1792             b43_phy_set(dev, B43_PHY_PGACTL, 0xF000);
1793             udelay(20);
1794             if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1795                 goto exit_loop1;
1796         }
1797     }
1798       exit_loop1:
1799     loop1_outer_done = i;
1800     loop1_inner_done = j;
1801     if (j >= 8) {
1802         b43_phy_set(dev, B43_PHY_RFOVERVAL, 0x30);
1803         trsw_rx = 0x1B;
1804         for (j = j - 8; j < 16; j++) {
1805             b43_phy_maskset(dev, B43_PHY_RFOVERVAL, 0xF0FF, (j << 8));
1806             b43_phy_maskset(dev, B43_PHY_PGACTL, 0x0FFF, 0xA000);
1807             b43_phy_set(dev, B43_PHY_PGACTL, 0xF000);
1808             udelay(20);
1809             trsw_rx -= 3;
1810             if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1811                 goto exit_loop2;
1812         }
1813     } else
1814         trsw_rx = 0x18;
1815       exit_loop2:
1816 
1817     if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
1818         b43_phy_write(dev, B43_PHY_ANALOGOVER, backup_phy[4]);
1819         b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, backup_phy[5]);
1820     }
1821     b43_phy_write(dev, B43_PHY_CCK(0x5A), backup_phy[6]);
1822     b43_phy_write(dev, B43_PHY_CCK(0x59), backup_phy[7]);
1823     b43_phy_write(dev, B43_PHY_CCK(0x58), backup_phy[8]);
1824     b43_phy_write(dev, B43_PHY_CCK(0x0A), backup_phy[9]);
1825     b43_phy_write(dev, B43_PHY_CCK(0x03), backup_phy[10]);
1826     b43_phy_write(dev, B43_PHY_LO_MASK, backup_phy[11]);
1827     b43_phy_write(dev, B43_PHY_LO_CTL, backup_phy[12]);
1828     b43_phy_write(dev, B43_PHY_CCK(0x2B), backup_phy[13]);
1829     b43_phy_write(dev, B43_PHY_PGACTL, backup_phy[14]);
1830 
1831     b43_gphy_set_baseband_attenuation(dev, backup_bband);
1832 
1833     b43_radio_write16(dev, 0x52, backup_radio[0]);
1834     b43_radio_write16(dev, 0x43, backup_radio[1]);
1835     b43_radio_write16(dev, 0x7A, backup_radio[2]);
1836 
1837     b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2] | 0x0003);
1838     udelay(10);
1839     b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2]);
1840     b43_phy_write(dev, B43_PHY_RFOVERVAL, backup_phy[3]);
1841     b43_phy_write(dev, B43_PHY_CRS0, backup_phy[0]);
1842     b43_phy_write(dev, B43_PHY_CCKBBANDCFG, backup_phy[1]);
1843 
1844     gphy->max_lb_gain =
1845         ((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
1846     gphy->trsw_rx_gain = trsw_rx * 2;
1847 }
1848 
1849 static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
1850 {
1851     struct b43_phy *phy = &dev->phy;
1852 
1853     if (!b43_has_hardware_pctl(dev)) {
1854         b43_phy_write(dev, 0x047A, 0xC111);
1855         return;
1856     }
1857 
1858     b43_phy_mask(dev, 0x0036, 0xFEFF);
1859     b43_phy_write(dev, 0x002F, 0x0202);
1860     b43_phy_set(dev, 0x047C, 0x0002);
1861     b43_phy_set(dev, 0x047A, 0xF000);
1862     if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
1863         b43_phy_maskset(dev, 0x047A, 0xFF0F, 0x0010);
1864         b43_phy_set(dev, 0x005D, 0x8000);
1865         b43_phy_maskset(dev, 0x004E, 0xFFC0, 0x0010);
1866         b43_phy_write(dev, 0x002E, 0xC07F);
1867         b43_phy_set(dev, 0x0036, 0x0400);
1868     } else {
1869         b43_phy_set(dev, 0x0036, 0x0200);
1870         b43_phy_set(dev, 0x0036, 0x0400);
1871         b43_phy_mask(dev, 0x005D, 0x7FFF);
1872         b43_phy_mask(dev, 0x004F, 0xFFFE);
1873         b43_phy_maskset(dev, 0x004E, 0xFFC0, 0x0010);
1874         b43_phy_write(dev, 0x002E, 0xC07F);
1875         b43_phy_maskset(dev, 0x047A, 0xFF0F, 0x0010);
1876     }
1877 }
1878 
1879 /* Hardware power control for G-PHY */
1880 static void b43_hardware_pctl_init_gphy(struct b43_wldev *dev)
1881 {
1882     struct b43_phy *phy = &dev->phy;
1883     struct b43_phy_g *gphy = phy->g;
1884 
1885     if (!b43_has_hardware_pctl(dev)) {
1886         /* No hardware power control */
1887         b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_HWPCTL);
1888         return;
1889     }
1890 
1891     b43_phy_maskset(dev, 0x0036, 0xFFC0, (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
1892     b43_phy_maskset(dev, 0x0478, 0xFF00, (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
1893     b43_gphy_tssi_power_lt_init(dev);
1894     b43_gphy_gain_lt_init(dev);
1895     b43_phy_mask(dev, 0x0060, 0xFFBF);
1896     b43_phy_write(dev, 0x0014, 0x0000);
1897 
1898     B43_WARN_ON(phy->rev < 6);
1899     b43_phy_set(dev, 0x0478, 0x0800);
1900     b43_phy_mask(dev, 0x0478, 0xFEFF);
1901     b43_phy_mask(dev, 0x0801, 0xFFBF);
1902 
1903     b43_gphy_dc_lt_init(dev, 1);
1904 
1905     /* Enable hardware pctl in firmware. */
1906     b43_hf_write(dev, b43_hf_read(dev) | B43_HF_HWPCTL);
1907 }
1908 
1909 /* Initialize B/G PHY power control */
1910 static void b43_phy_init_pctl(struct b43_wldev *dev)
1911 {
1912     struct b43_phy *phy = &dev->phy;
1913     struct b43_phy_g *gphy = phy->g;
1914     struct b43_rfatt old_rfatt;
1915     struct b43_bbatt old_bbatt;
1916     u8 old_tx_control = 0;
1917 
1918     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
1919 
1920     if ((dev->dev->board_vendor == SSB_BOARDVENDOR_BCM) &&
1921         (dev->dev->board_type == SSB_BOARD_BU4306))
1922         return;
1923 
1924     b43_phy_write(dev, 0x0028, 0x8018);
1925 
1926     /* This does something with the Analog... */
1927     b43_write16(dev, B43_MMIO_PHY0, b43_read16(dev, B43_MMIO_PHY0)
1928             & 0xFFDF);
1929 
1930     if (!phy->gmode)
1931         return;
1932     b43_hardware_pctl_early_init(dev);
1933     if (gphy->cur_idle_tssi == 0) {
1934         if (phy->radio_ver == 0x2050 && phy->analog == 0) {
1935             b43_radio_maskset(dev, 0x0076, 0x00F7, 0x0084);
1936         } else {
1937             struct b43_rfatt rfatt;
1938             struct b43_bbatt bbatt;
1939 
1940             memcpy(&old_rfatt, &gphy->rfatt, sizeof(old_rfatt));
1941             memcpy(&old_bbatt, &gphy->bbatt, sizeof(old_bbatt));
1942             old_tx_control = gphy->tx_control;
1943 
1944             bbatt.att = 11;
1945             if (phy->radio_rev == 8) {
1946                 rfatt.att = 15;
1947                 rfatt.with_padmix = true;
1948             } else {
1949                 rfatt.att = 9;
1950                 rfatt.with_padmix = false;
1951             }
1952             b43_set_txpower_g(dev, &bbatt, &rfatt, 0);
1953         }
1954         b43_dummy_transmission(dev, false, true);
1955         gphy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
1956         if (B43_DEBUG) {
1957             /* Current-Idle-TSSI sanity check. */
1958             if (abs(gphy->cur_idle_tssi - gphy->tgt_idle_tssi) >= 20) {
1959                 b43dbg(dev->wl,
1960                        "!WARNING! Idle-TSSI phy->cur_idle_tssi "
1961                        "measuring failed. (cur=%d, tgt=%d). Disabling TX power "
1962                        "adjustment.\n", gphy->cur_idle_tssi,
1963                        gphy->tgt_idle_tssi);
1964                 gphy->cur_idle_tssi = 0;
1965             }
1966         }
1967         if (phy->radio_ver == 0x2050 && phy->analog == 0) {
1968             b43_radio_mask(dev, 0x0076, 0xFF7B);
1969         } else {
1970             b43_set_txpower_g(dev, &old_bbatt,
1971                       &old_rfatt, old_tx_control);
1972         }
1973     }
1974     b43_hardware_pctl_init_gphy(dev);
1975     b43_shm_clear_tssi(dev);
1976 }
1977 
1978 static void b43_phy_inita(struct b43_wldev *dev)
1979 {
1980     struct b43_phy *phy = &dev->phy;
1981 
1982     might_sleep();
1983 
1984     if (phy->rev >= 6) {
1985         if (b43_phy_read(dev, B43_PHY_ENCORE) & B43_PHY_ENCORE_EN)
1986             b43_phy_set(dev, B43_PHY_ENCORE, 0x0010);
1987         else
1988             b43_phy_mask(dev, B43_PHY_ENCORE, ~0x1010);
1989     }
1990 
1991     b43_wa_all(dev);
1992 
1993     if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
1994         b43_phy_maskset(dev, B43_PHY_OFDM(0x6E), 0xE000, 0x3CF);
1995 }
1996 
1997 static void b43_phy_initg(struct b43_wldev *dev)
1998 {
1999     struct b43_phy *phy = &dev->phy;
2000     struct b43_phy_g *gphy = phy->g;
2001     u16 tmp;
2002 
2003     if (phy->rev == 1)
2004         b43_phy_initb5(dev);
2005     else
2006         b43_phy_initb6(dev);
2007 
2008     if (phy->rev >= 2 || phy->gmode)
2009         b43_phy_inita(dev);
2010 
2011     if (phy->rev >= 2) {
2012         b43_phy_write(dev, B43_PHY_ANALOGOVER, 0);
2013         b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, 0);
2014     }
2015     if (phy->rev == 2) {
2016         b43_phy_write(dev, B43_PHY_RFOVER, 0);
2017         b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
2018     }
2019     if (phy->rev > 5) {
2020         b43_phy_write(dev, B43_PHY_RFOVER, 0x400);
2021         b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
2022     }
2023     if (phy->gmode || phy->rev >= 2) {
2024         tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
2025         tmp &= B43_PHYVER_VERSION;
2026         if (tmp == 3 || tmp == 5) {
2027             b43_phy_write(dev, B43_PHY_OFDM(0xC2), 0x1816);
2028             b43_phy_write(dev, B43_PHY_OFDM(0xC3), 0x8006);
2029         }
2030         if (tmp == 5) {
2031             b43_phy_maskset(dev, B43_PHY_OFDM(0xCC), 0x00FF, 0x1F00);
2032         }
2033     }
2034     if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
2035         b43_phy_write(dev, B43_PHY_OFDM(0x7E), 0x78);
2036     if (phy->radio_rev == 8) {
2037         b43_phy_set(dev, B43_PHY_EXTG(0x01), 0x80);
2038         b43_phy_set(dev, B43_PHY_OFDM(0x3E), 0x4);
2039     }
2040     if (has_loopback_gain(phy))
2041         b43_calc_loopback_gain(dev);
2042 
2043     if (phy->radio_rev != 8) {
2044         if (gphy->initval == 0xFFFF)
2045             gphy->initval = b43_radio_init2050(dev);
2046         else
2047             b43_radio_write16(dev, 0x0078, gphy->initval);
2048     }
2049     b43_lo_g_init(dev);
2050     if (has_tx_magnification(phy)) {
2051         b43_radio_write16(dev, 0x52,
2052                   (b43_radio_read16(dev, 0x52) & 0xFF00)
2053                   | gphy->lo_control->tx_bias | gphy->
2054                   lo_control->tx_magn);
2055     } else {
2056         b43_radio_maskset(dev, 0x52, 0xFFF0, gphy->lo_control->tx_bias);
2057     }
2058     if (phy->rev >= 6) {
2059         b43_phy_maskset(dev, B43_PHY_CCK(0x36), 0x0FFF, (gphy->lo_control->tx_bias << 12));
2060     }
2061     if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
2062         b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
2063     else
2064         b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
2065     if (phy->rev < 2)
2066         b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
2067     else
2068         b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
2069     if (phy->gmode || phy->rev >= 2) {
2070         b43_lo_g_adjust(dev);
2071         b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
2072     }
2073 
2074     if (!(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI)) {
2075         /* The specs state to update the NRSSI LT with
2076          * the value 0x7FFFFFFF here. I think that is some weird
2077          * compiler optimization in the original driver.
2078          * Essentially, what we do here is resetting all NRSSI LT
2079          * entries to -32 (see the clamp_val() in nrssi_hw_update())
2080          */
2081         b43_nrssi_hw_update(dev, 0xFFFF);   //FIXME?
2082         b43_calc_nrssi_threshold(dev);
2083     } else if (phy->gmode || phy->rev >= 2) {
2084         if (gphy->nrssi[0] == -1000) {
2085             B43_WARN_ON(gphy->nrssi[1] != -1000);
2086             b43_calc_nrssi_slope(dev);
2087         } else
2088             b43_calc_nrssi_threshold(dev);
2089     }
2090     if (phy->radio_rev == 8)
2091         b43_phy_write(dev, B43_PHY_EXTG(0x05), 0x3230);
2092     b43_phy_init_pctl(dev);
2093     /* FIXME: The spec says in the following if, the 0 should be replaced
2094        'if OFDM may not be used in the current locale'
2095        but OFDM is legal everywhere */
2096     if ((dev->dev->chip_id == 0x4306
2097          && dev->dev->chip_pkg == 2) || 0) {
2098         b43_phy_mask(dev, B43_PHY_CRS0, 0xBFFF);
2099         b43_phy_mask(dev, B43_PHY_OFDM(0xC3), 0x7FFF);
2100     }
2101 }
2102 
2103 void b43_gphy_channel_switch(struct b43_wldev *dev,
2104                  unsigned int channel,
2105                  bool synthetic_pu_workaround)
2106 {
2107     if (synthetic_pu_workaround)
2108         b43_synth_pu_workaround(dev, channel);
2109 
2110     b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
2111 
2112     if (channel == 14) {
2113         if (dev->dev->bus_sprom->country_code ==
2114             SSB_SPROM1CCODE_JAPAN)
2115             b43_hf_write(dev,
2116                      b43_hf_read(dev) & ~B43_HF_ACPR);
2117         else
2118             b43_hf_write(dev,
2119                      b43_hf_read(dev) | B43_HF_ACPR);
2120         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2121                 b43_read16(dev, B43_MMIO_CHANNEL_EXT)
2122                 | (1 << 11));
2123     } else {
2124         b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2125                 b43_read16(dev, B43_MMIO_CHANNEL_EXT)
2126                 & 0xF7BF);
2127     }
2128 }
2129 
2130 static void default_baseband_attenuation(struct b43_wldev *dev,
2131                      struct b43_bbatt *bb)
2132 {
2133     struct b43_phy *phy = &dev->phy;
2134 
2135     if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
2136         bb->att = 0;
2137     else
2138         bb->att = 2;
2139 }
2140 
2141 static void default_radio_attenuation(struct b43_wldev *dev,
2142                       struct b43_rfatt *rf)
2143 {
2144     struct b43_bus_dev *bdev = dev->dev;
2145     struct b43_phy *phy = &dev->phy;
2146 
2147     rf->with_padmix = false;
2148 
2149     if (dev->dev->board_vendor == SSB_BOARDVENDOR_BCM &&
2150         dev->dev->board_type == SSB_BOARD_BCM4309G) {
2151         if (dev->dev->board_rev < 0x43) {
2152             rf->att = 2;
2153             return;
2154         } else if (dev->dev->board_rev < 0x51) {
2155             rf->att = 3;
2156             return;
2157         }
2158     }
2159 
2160     switch (phy->radio_ver) {
2161     case 0x2053:
2162         switch (phy->radio_rev) {
2163         case 1:
2164             rf->att = 6;
2165             return;
2166         }
2167         break;
2168     case 0x2050:
2169         switch (phy->radio_rev) {
2170         case 0:
2171             rf->att = 5;
2172             return;
2173         case 1:
2174             if (phy->type == B43_PHYTYPE_G) {
2175                 if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2176                     && bdev->board_type == SSB_BOARD_BCM4309G
2177                     && bdev->board_rev >= 30)
2178                     rf->att = 3;
2179                 else if (bdev->board_vendor ==
2180                      SSB_BOARDVENDOR_BCM
2181                      && bdev->board_type ==
2182                      SSB_BOARD_BU4306)
2183                     rf->att = 3;
2184                 else
2185                     rf->att = 1;
2186             } else {
2187                 if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2188                     && bdev->board_type == SSB_BOARD_BCM4309G
2189                     && bdev->board_rev >= 30)
2190                     rf->att = 7;
2191                 else
2192                     rf->att = 6;
2193             }
2194             return;
2195         case 2:
2196             if (phy->type == B43_PHYTYPE_G) {
2197                 if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2198                     && bdev->board_type == SSB_BOARD_BCM4309G
2199                     && bdev->board_rev >= 30)
2200                     rf->att = 3;
2201                 else if (bdev->board_vendor ==
2202                      SSB_BOARDVENDOR_BCM
2203                      && bdev->board_type ==
2204                      SSB_BOARD_BU4306)
2205                     rf->att = 5;
2206                 else if (bdev->chip_id == 0x4320)
2207                     rf->att = 4;
2208                 else
2209                     rf->att = 3;
2210             } else
2211                 rf->att = 6;
2212             return;
2213         case 3:
2214             rf->att = 5;
2215             return;
2216         case 4:
2217         case 5:
2218             rf->att = 1;
2219             return;
2220         case 6:
2221         case 7:
2222             rf->att = 5;
2223             return;
2224         case 8:
2225             rf->att = 0xA;
2226             rf->with_padmix = true;
2227             return;
2228         case 9:
2229         default:
2230             rf->att = 5;
2231             return;
2232         }
2233     }
2234     rf->att = 5;
2235 }
2236 
2237 static u16 default_tx_control(struct b43_wldev *dev)
2238 {
2239     struct b43_phy *phy = &dev->phy;
2240 
2241     if (phy->radio_ver != 0x2050)
2242         return 0;
2243     if (phy->radio_rev == 1)
2244         return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
2245     if (phy->radio_rev < 6)
2246         return B43_TXCTL_PA2DB;
2247     if (phy->radio_rev == 8)
2248         return B43_TXCTL_TXMIX;
2249     return 0;
2250 }
2251 
2252 static u8 b43_gphy_aci_detect(struct b43_wldev *dev, u8 channel)
2253 {
2254     struct b43_phy *phy = &dev->phy;
2255     struct b43_phy_g *gphy = phy->g;
2256     u8 ret = 0;
2257     u16 saved, rssi, temp;
2258     int i, j = 0;
2259 
2260     saved = b43_phy_read(dev, 0x0403);
2261     b43_switch_channel(dev, channel);
2262     b43_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
2263     if (gphy->aci_hw_rssi)
2264         rssi = b43_phy_read(dev, 0x048A) & 0x3F;
2265     else
2266         rssi = saved & 0x3F;
2267     /* clamp temp to signed 5bit */
2268     if (rssi > 32)
2269         rssi -= 64;
2270     for (i = 0; i < 100; i++) {
2271         temp = (b43_phy_read(dev, 0x047F) >> 8) & 0x3F;
2272         if (temp > 32)
2273             temp -= 64;
2274         if (temp < rssi)
2275             j++;
2276         if (j >= 20)
2277             ret = 1;
2278     }
2279     b43_phy_write(dev, 0x0403, saved);
2280 
2281     return ret;
2282 }
2283 
2284 static u8 b43_gphy_aci_scan(struct b43_wldev *dev)
2285 {
2286     struct b43_phy *phy = &dev->phy;
2287     u8 ret[13] = { 0 };
2288     unsigned int channel = phy->channel;
2289     unsigned int i, j, start, end;
2290 
2291     if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
2292         return 0;
2293 
2294     b43_phy_lock(dev);
2295     b43_radio_lock(dev);
2296     b43_phy_mask(dev, 0x0802, 0xFFFC);
2297     b43_phy_mask(dev, B43_PHY_G_CRS, 0x7FFF);
2298     b43_set_all_gains(dev, 3, 8, 1);
2299 
2300     start = (channel > 5) ? channel - 5 : 1;
2301     end = (channel + 5 < 14) ? channel + 5 : 13;
2302 
2303     for (i = start; i <= end; i++) {
2304         if (abs(channel - i) > 2)
2305             ret[i - 1] = b43_gphy_aci_detect(dev, i);
2306     }
2307     b43_switch_channel(dev, channel);
2308     b43_phy_maskset(dev, 0x0802, 0xFFFC, 0x0003);
2309     b43_phy_mask(dev, 0x0403, 0xFFF8);
2310     b43_phy_set(dev, B43_PHY_G_CRS, 0x8000);
2311     b43_set_original_gains(dev);
2312     for (i = 0; i < 13; i++) {
2313         if (!ret[i])
2314             continue;
2315         end = (i + 5 < 13) ? i + 5 : 13;
2316         for (j = i; j < end; j++)
2317             ret[j] = 1;
2318     }
2319     b43_radio_unlock(dev);
2320     b43_phy_unlock(dev);
2321 
2322     return ret[channel - 1];
2323 }
2324 
2325 static s32 b43_tssi2dbm_ad(s32 num, s32 den)
2326 {
2327     if (num < 0)
2328         return num / den;
2329     else
2330         return (num + den / 2) / den;
2331 }
2332 
2333 static s8 b43_tssi2dbm_entry(s8 entry[], u8 index,
2334                  s16 pab0, s16 pab1, s16 pab2)
2335 {
2336     s32 m1, m2, f = 256, q, delta;
2337     s8 i = 0;
2338 
2339     m1 = b43_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
2340     m2 = max(b43_tssi2dbm_ad(32768 + index * pab2, 256), 1);
2341     do {
2342         if (i > 15)
2343             return -EINVAL;
2344         q = b43_tssi2dbm_ad(f * 4096 -
2345                     b43_tssi2dbm_ad(m2 * f, 16) * f, 2048);
2346         delta = abs(q - f);
2347         f = q;
2348         i++;
2349     } while (delta >= 2);
2350     entry[index] = clamp_val(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
2351     return 0;
2352 }
2353 
2354 u8 *b43_generate_dyn_tssi2dbm_tab(struct b43_wldev *dev,
2355                   s16 pab0, s16 pab1, s16 pab2)
2356 {
2357     unsigned int i;
2358     u8 *tab;
2359     int err;
2360 
2361     tab = kmalloc(64, GFP_KERNEL);
2362     if (!tab) {
2363         b43err(dev->wl, "Could not allocate memory "
2364                "for tssi2dbm table\n");
2365         return NULL;
2366     }
2367     for (i = 0; i < 64; i++) {
2368         err = b43_tssi2dbm_entry(tab, i, pab0, pab1, pab2);
2369         if (err) {
2370             b43err(dev->wl, "Could not generate "
2371                    "tssi2dBm table\n");
2372             kfree(tab);
2373             return NULL;
2374         }
2375     }
2376 
2377     return tab;
2378 }
2379 
2380 /* Initialise the TSSI->dBm lookup table */
2381 static int b43_gphy_init_tssi2dbm_table(struct b43_wldev *dev)
2382 {
2383     struct b43_phy *phy = &dev->phy;
2384     struct b43_phy_g *gphy = phy->g;
2385     s16 pab0, pab1, pab2;
2386 
2387     pab0 = (s16) (dev->dev->bus_sprom->pa0b0);
2388     pab1 = (s16) (dev->dev->bus_sprom->pa0b1);
2389     pab2 = (s16) (dev->dev->bus_sprom->pa0b2);
2390 
2391     B43_WARN_ON((dev->dev->chip_id == 0x4301) &&
2392             (phy->radio_ver != 0x2050)); /* Not supported anymore */
2393 
2394     gphy->dyn_tssi_tbl = false;
2395 
2396     if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
2397         pab0 != -1 && pab1 != -1 && pab2 != -1) {
2398         /* The pabX values are set in SPROM. Use them. */
2399         if ((s8) dev->dev->bus_sprom->itssi_bg != 0 &&
2400             (s8) dev->dev->bus_sprom->itssi_bg != -1) {
2401             gphy->tgt_idle_tssi =
2402                 (s8) (dev->dev->bus_sprom->itssi_bg);
2403         } else
2404             gphy->tgt_idle_tssi = 62;
2405         gphy->tssi2dbm = b43_generate_dyn_tssi2dbm_tab(dev, pab0,
2406                                    pab1, pab2);
2407         if (!gphy->tssi2dbm)
2408             return -ENOMEM;
2409         gphy->dyn_tssi_tbl = true;
2410     } else {
2411         /* pabX values not set in SPROM. */
2412         gphy->tgt_idle_tssi = 52;
2413         gphy->tssi2dbm = b43_tssi2dbm_g_table;
2414     }
2415 
2416     return 0;
2417 }
2418 
2419 static int b43_gphy_op_allocate(struct b43_wldev *dev)
2420 {
2421     struct b43_phy_g *gphy;
2422     struct b43_txpower_lo_control *lo;
2423     int err;
2424 
2425     gphy = kzalloc(sizeof(*gphy), GFP_KERNEL);
2426     if (!gphy) {
2427         err = -ENOMEM;
2428         goto error;
2429     }
2430     dev->phy.g = gphy;
2431 
2432     lo = kzalloc(sizeof(*lo), GFP_KERNEL);
2433     if (!lo) {
2434         err = -ENOMEM;
2435         goto err_free_gphy;
2436     }
2437     gphy->lo_control = lo;
2438 
2439     err = b43_gphy_init_tssi2dbm_table(dev);
2440     if (err)
2441         goto err_free_lo;
2442 
2443     return 0;
2444 
2445 err_free_lo:
2446     kfree(lo);
2447 err_free_gphy:
2448     kfree(gphy);
2449 error:
2450     return err;
2451 }
2452 
2453 static void b43_gphy_op_prepare_structs(struct b43_wldev *dev)
2454 {
2455     struct b43_phy *phy = &dev->phy;
2456     struct b43_phy_g *gphy = phy->g;
2457     const void *tssi2dbm;
2458     int tgt_idle_tssi;
2459     struct b43_txpower_lo_control *lo;
2460     unsigned int i;
2461 
2462     /* tssi2dbm table is constant, so it is initialized at alloc time.
2463      * Save a copy of the pointer. */
2464     tssi2dbm = gphy->tssi2dbm;
2465     tgt_idle_tssi = gphy->tgt_idle_tssi;
2466     /* Save the LO pointer. */
2467     lo = gphy->lo_control;
2468 
2469     /* Zero out the whole PHY structure. */
2470     memset(gphy, 0, sizeof(*gphy));
2471 
2472     /* Restore pointers. */
2473     gphy->tssi2dbm = tssi2dbm;
2474     gphy->tgt_idle_tssi = tgt_idle_tssi;
2475     gphy->lo_control = lo;
2476 
2477     memset(gphy->minlowsig, 0xFF, sizeof(gphy->minlowsig));
2478 
2479     /* NRSSI */
2480     for (i = 0; i < ARRAY_SIZE(gphy->nrssi); i++)
2481         gphy->nrssi[i] = -1000;
2482     for (i = 0; i < ARRAY_SIZE(gphy->nrssi_lt); i++)
2483         gphy->nrssi_lt[i] = i;
2484 
2485     gphy->lofcal = 0xFFFF;
2486     gphy->initval = 0xFFFF;
2487 
2488     gphy->interfmode = B43_INTERFMODE_NONE;
2489 
2490     /* OFDM-table address caching. */
2491     gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
2492 
2493     gphy->average_tssi = 0xFF;
2494 
2495     /* Local Osciallator structure */
2496     lo->tx_bias = 0xFF;
2497     INIT_LIST_HEAD(&lo->calib_list);
2498 }
2499 
2500 static void b43_gphy_op_free(struct b43_wldev *dev)
2501 {
2502     struct b43_phy *phy = &dev->phy;
2503     struct b43_phy_g *gphy = phy->g;
2504 
2505     kfree(gphy->lo_control);
2506 
2507     if (gphy->dyn_tssi_tbl)
2508         kfree(gphy->tssi2dbm);
2509     gphy->dyn_tssi_tbl = false;
2510     gphy->tssi2dbm = NULL;
2511 
2512     kfree(gphy);
2513     dev->phy.g = NULL;
2514 }
2515 
2516 static int b43_gphy_op_prepare_hardware(struct b43_wldev *dev)
2517 {
2518     struct b43_phy *phy = &dev->phy;
2519     struct b43_phy_g *gphy = phy->g;
2520     struct b43_txpower_lo_control *lo = gphy->lo_control;
2521 
2522     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2523 
2524     default_baseband_attenuation(dev, &gphy->bbatt);
2525     default_radio_attenuation(dev, &gphy->rfatt);
2526     gphy->tx_control = (default_tx_control(dev) << 4);
2527     generate_rfatt_list(dev, &lo->rfatt_list);
2528     generate_bbatt_list(dev, &lo->bbatt_list);
2529 
2530     /* Commit previous writes */
2531     b43_read32(dev, B43_MMIO_MACCTL);
2532 
2533     if (phy->rev == 1) {
2534         /* Workaround: Temporarly disable gmode through the early init
2535          * phase, as the gmode stuff is not needed for phy rev 1 */
2536         phy->gmode = false;
2537         b43_wireless_core_reset(dev, 0);
2538         b43_phy_initg(dev);
2539         phy->gmode = true;
2540         b43_wireless_core_reset(dev, 1);
2541     }
2542 
2543     return 0;
2544 }
2545 
2546 static int b43_gphy_op_init(struct b43_wldev *dev)
2547 {
2548     b43_phy_initg(dev);
2549 
2550     return 0;
2551 }
2552 
2553 static void b43_gphy_op_exit(struct b43_wldev *dev)
2554 {
2555     b43_lo_g_cleanup(dev);
2556 }
2557 
2558 static u16 b43_gphy_op_read(struct b43_wldev *dev, u16 reg)
2559 {
2560     b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
2561     return b43_read16(dev, B43_MMIO_PHY_DATA);
2562 }
2563 
2564 static void b43_gphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
2565 {
2566     b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
2567     b43_write16(dev, B43_MMIO_PHY_DATA, value);
2568 }
2569 
2570 static u16 b43_gphy_op_radio_read(struct b43_wldev *dev, u16 reg)
2571 {
2572     /* Register 1 is a 32-bit register. */
2573     B43_WARN_ON(reg == 1);
2574     /* G-PHY needs 0x80 for read access. */
2575     reg |= 0x80;
2576 
2577     b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
2578     return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
2579 }
2580 
2581 static void b43_gphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
2582 {
2583     /* Register 1 is a 32-bit register. */
2584     B43_WARN_ON(reg == 1);
2585 
2586     b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
2587     b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
2588 }
2589 
2590 static bool b43_gphy_op_supports_hwpctl(struct b43_wldev *dev)
2591 {
2592     return (dev->phy.rev >= 6);
2593 }
2594 
2595 static void b43_gphy_op_software_rfkill(struct b43_wldev *dev,
2596                     bool blocked)
2597 {
2598     struct b43_phy *phy = &dev->phy;
2599     struct b43_phy_g *gphy = phy->g;
2600     unsigned int channel;
2601 
2602     might_sleep();
2603 
2604     if (!blocked) {
2605         /* Turn radio ON */
2606         if (phy->radio_on)
2607             return;
2608 
2609         b43_phy_write(dev, 0x0015, 0x8000);
2610         b43_phy_write(dev, 0x0015, 0xCC00);
2611         b43_phy_write(dev, 0x0015, (phy->gmode ? 0x00C0 : 0x0000));
2612         if (gphy->radio_off_context.valid) {
2613             /* Restore the RFover values. */
2614             b43_phy_write(dev, B43_PHY_RFOVER,
2615                       gphy->radio_off_context.rfover);
2616             b43_phy_write(dev, B43_PHY_RFOVERVAL,
2617                       gphy->radio_off_context.rfoverval);
2618             gphy->radio_off_context.valid = false;
2619         }
2620         channel = phy->channel;
2621         b43_gphy_channel_switch(dev, 6, 1);
2622         b43_gphy_channel_switch(dev, channel, 0);
2623     } else {
2624         /* Turn radio OFF */
2625         u16 rfover, rfoverval;
2626 
2627         rfover = b43_phy_read(dev, B43_PHY_RFOVER);
2628         rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
2629         gphy->radio_off_context.rfover = rfover;
2630         gphy->radio_off_context.rfoverval = rfoverval;
2631         gphy->radio_off_context.valid = true;
2632         b43_phy_write(dev, B43_PHY_RFOVER, rfover | 0x008C);
2633         b43_phy_write(dev, B43_PHY_RFOVERVAL, rfoverval & 0xFF73);
2634     }
2635 }
2636 
2637 static int b43_gphy_op_switch_channel(struct b43_wldev *dev,
2638                       unsigned int new_channel)
2639 {
2640     if ((new_channel < 1) || (new_channel > 14))
2641         return -EINVAL;
2642     b43_gphy_channel_switch(dev, new_channel, 0);
2643 
2644     return 0;
2645 }
2646 
2647 static unsigned int b43_gphy_op_get_default_chan(struct b43_wldev *dev)
2648 {
2649     return 1; /* Default to channel 1 */
2650 }
2651 
2652 static void b43_gphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
2653 {
2654     struct b43_phy *phy = &dev->phy;
2655     u16 tmp;
2656     int autodiv = 0;
2657 
2658     if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
2659         autodiv = 1;
2660 
2661     b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ANTDIVHELP);
2662 
2663     b43_phy_maskset(dev, B43_PHY_BBANDCFG, ~B43_PHY_BBANDCFG_RXANT,
2664             (autodiv ? B43_ANTENNA_AUTO1 : antenna) <<
2665             B43_PHY_BBANDCFG_RXANT_SHIFT);
2666 
2667     if (autodiv) {
2668         tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
2669         if (antenna == B43_ANTENNA_AUTO1)
2670             tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
2671         else
2672             tmp |= B43_PHY_ANTDWELL_AUTODIV1;
2673         b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
2674     }
2675 
2676     tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
2677     if (autodiv)
2678         tmp |= B43_PHY_ANTWRSETT_ARXDIV;
2679     else
2680         tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
2681     b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
2682 
2683     if (autodiv)
2684         b43_phy_set(dev, B43_PHY_ANTWRSETT, B43_PHY_ANTWRSETT_ARXDIV);
2685     else {
2686         b43_phy_mask(dev, B43_PHY_ANTWRSETT,
2687                  B43_PHY_ANTWRSETT_ARXDIV);
2688     }
2689 
2690     if (phy->rev >= 2) {
2691         b43_phy_set(dev, B43_PHY_OFDM61, B43_PHY_OFDM61_10);
2692         b43_phy_maskset(dev, B43_PHY_DIVSRCHGAINBACK, 0xFF00, 0x15);
2693 
2694         if (phy->rev == 2)
2695             b43_phy_write(dev, B43_PHY_ADIVRELATED, 8);
2696         else
2697             b43_phy_maskset(dev, B43_PHY_ADIVRELATED, 0xFF00, 8);
2698     }
2699     if (phy->rev >= 6)
2700         b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
2701 
2702     b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ANTDIVHELP);
2703 }
2704 
2705 static int b43_gphy_op_interf_mitigation(struct b43_wldev *dev,
2706                      enum b43_interference_mitigation mode)
2707 {
2708     struct b43_phy *phy = &dev->phy;
2709     struct b43_phy_g *gphy = phy->g;
2710     int currentmode;
2711 
2712     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2713     if ((phy->rev == 0) || (!phy->gmode))
2714         return -ENODEV;
2715 
2716     gphy->aci_wlan_automatic = false;
2717     switch (mode) {
2718     case B43_INTERFMODE_AUTOWLAN:
2719         gphy->aci_wlan_automatic = true;
2720         if (gphy->aci_enable)
2721             mode = B43_INTERFMODE_MANUALWLAN;
2722         else
2723             mode = B43_INTERFMODE_NONE;
2724         break;
2725     case B43_INTERFMODE_NONE:
2726     case B43_INTERFMODE_NONWLAN:
2727     case B43_INTERFMODE_MANUALWLAN:
2728         break;
2729     default:
2730         return -EINVAL;
2731     }
2732 
2733     currentmode = gphy->interfmode;
2734     if (currentmode == mode)
2735         return 0;
2736     if (currentmode != B43_INTERFMODE_NONE)
2737         b43_radio_interference_mitigation_disable(dev, currentmode);
2738 
2739     if (mode == B43_INTERFMODE_NONE) {
2740         gphy->aci_enable = false;
2741         gphy->aci_hw_rssi = false;
2742     } else
2743         b43_radio_interference_mitigation_enable(dev, mode);
2744     gphy->interfmode = mode;
2745 
2746     return 0;
2747 }
2748 
2749 /* https://bcm-specs.sipsolutions.net/EstimatePowerOut
2750  * This function converts a TSSI value to dBm in Q5.2
2751  */
2752 static s8 b43_gphy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
2753 {
2754     struct b43_phy_g *gphy = dev->phy.g;
2755     s8 dbm;
2756     s32 tmp;
2757 
2758     tmp = (gphy->tgt_idle_tssi - gphy->cur_idle_tssi + tssi);
2759     tmp = clamp_val(tmp, 0x00, 0x3F);
2760     dbm = gphy->tssi2dbm[tmp];
2761 
2762     return dbm;
2763 }
2764 
2765 static void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
2766                         int *_bbatt, int *_rfatt)
2767 {
2768     int rfatt = *_rfatt;
2769     int bbatt = *_bbatt;
2770     struct b43_txpower_lo_control *lo = dev->phy.g->lo_control;
2771 
2772     /* Get baseband and radio attenuation values into their permitted ranges.
2773      * Radio attenuation affects power level 4 times as much as baseband. */
2774 
2775     /* Range constants */
2776     const int rf_min = lo->rfatt_list.min_val;
2777     const int rf_max = lo->rfatt_list.max_val;
2778     const int bb_min = lo->bbatt_list.min_val;
2779     const int bb_max = lo->bbatt_list.max_val;
2780 
2781     while (1) {
2782         if (rfatt > rf_max && bbatt > bb_max - 4)
2783             break;  /* Can not get it into ranges */
2784         if (rfatt < rf_min && bbatt < bb_min + 4)
2785             break;  /* Can not get it into ranges */
2786         if (bbatt > bb_max && rfatt > rf_max - 1)
2787             break;  /* Can not get it into ranges */
2788         if (bbatt < bb_min && rfatt < rf_min + 1)
2789             break;  /* Can not get it into ranges */
2790 
2791         if (bbatt > bb_max) {
2792             bbatt -= 4;
2793             rfatt += 1;
2794             continue;
2795         }
2796         if (bbatt < bb_min) {
2797             bbatt += 4;
2798             rfatt -= 1;
2799             continue;
2800         }
2801         if (rfatt > rf_max) {
2802             rfatt -= 1;
2803             bbatt += 4;
2804             continue;
2805         }
2806         if (rfatt < rf_min) {
2807             rfatt += 1;
2808             bbatt -= 4;
2809             continue;
2810         }
2811         break;
2812     }
2813 
2814     *_rfatt = clamp_val(rfatt, rf_min, rf_max);
2815     *_bbatt = clamp_val(bbatt, bb_min, bb_max);
2816 }
2817 
2818 static void b43_gphy_op_adjust_txpower(struct b43_wldev *dev)
2819 {
2820     struct b43_phy *phy = &dev->phy;
2821     struct b43_phy_g *gphy = phy->g;
2822     int rfatt, bbatt;
2823     u8 tx_control;
2824 
2825     b43_mac_suspend(dev);
2826 
2827     /* Calculate the new attenuation values. */
2828     bbatt = gphy->bbatt.att;
2829     bbatt += gphy->bbatt_delta;
2830     rfatt = gphy->rfatt.att;
2831     rfatt += gphy->rfatt_delta;
2832 
2833     b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
2834     tx_control = gphy->tx_control;
2835     if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
2836         if (rfatt <= 1) {
2837             if (tx_control == 0) {
2838                 tx_control =
2839                     B43_TXCTL_PA2DB |
2840                     B43_TXCTL_TXMIX;
2841                 rfatt += 2;
2842                 bbatt += 2;
2843             } else if (dev->dev->bus_sprom->
2844                    boardflags_lo &
2845                    B43_BFL_PACTRL) {
2846                 bbatt += 4 * (rfatt - 2);
2847                 rfatt = 2;
2848             }
2849         } else if (rfatt > 4 && tx_control) {
2850             tx_control = 0;
2851             if (bbatt < 3) {
2852                 rfatt -= 3;
2853                 bbatt += 2;
2854             } else {
2855                 rfatt -= 2;
2856                 bbatt -= 2;
2857             }
2858         }
2859     }
2860     /* Save the control values */
2861     gphy->tx_control = tx_control;
2862     b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
2863     gphy->rfatt.att = rfatt;
2864     gphy->bbatt.att = bbatt;
2865 
2866     if (b43_debug(dev, B43_DBG_XMITPOWER))
2867         b43dbg(dev->wl, "Adjusting TX power\n");
2868 
2869     /* Adjust the hardware */
2870     b43_phy_lock(dev);
2871     b43_radio_lock(dev);
2872     b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt,
2873               gphy->tx_control);
2874     b43_radio_unlock(dev);
2875     b43_phy_unlock(dev);
2876 
2877     b43_mac_enable(dev);
2878 }
2879 
2880 static enum b43_txpwr_result b43_gphy_op_recalc_txpower(struct b43_wldev *dev,
2881                             bool ignore_tssi)
2882 {
2883     struct b43_phy *phy = &dev->phy;
2884     struct b43_phy_g *gphy = phy->g;
2885     unsigned int average_tssi;
2886     int cck_result, ofdm_result;
2887     int estimated_pwr, desired_pwr, pwr_adjust;
2888     int rfatt_delta, bbatt_delta;
2889     unsigned int max_pwr;
2890 
2891     /* First get the average TSSI */
2892     cck_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_CCK);
2893     ofdm_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_OFDM_G);
2894     if ((cck_result < 0) && (ofdm_result < 0)) {
2895         /* No TSSI information available */
2896         if (!ignore_tssi)
2897             goto no_adjustment_needed;
2898         cck_result = 0;
2899         ofdm_result = 0;
2900     }
2901     if (cck_result < 0)
2902         average_tssi = ofdm_result;
2903     else if (ofdm_result < 0)
2904         average_tssi = cck_result;
2905     else
2906         average_tssi = (cck_result + ofdm_result) / 2;
2907     /* Merge the average with the stored value. */
2908     if (likely(gphy->average_tssi != 0xFF))
2909         average_tssi = (average_tssi + gphy->average_tssi) / 2;
2910     gphy->average_tssi = average_tssi;
2911     B43_WARN_ON(average_tssi >= B43_TSSI_MAX);
2912 
2913     /* Estimate the TX power emission based on the TSSI */
2914     estimated_pwr = b43_gphy_estimate_power_out(dev, average_tssi);
2915 
2916     B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2917     max_pwr = dev->dev->bus_sprom->maxpwr_bg;
2918     if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
2919         max_pwr -= 3; /* minus 0.75 */
2920     if (unlikely(max_pwr >= INT_TO_Q52(30/*dBm*/))) {
2921         b43warn(dev->wl,
2922             "Invalid max-TX-power value in SPROM.\n");
2923         max_pwr = INT_TO_Q52(20); /* fake it */
2924         dev->dev->bus_sprom->maxpwr_bg = max_pwr;
2925     }
2926 
2927     /* Get desired power (in Q5.2) */
2928     if (phy->desired_txpower < 0)
2929         desired_pwr = INT_TO_Q52(0);
2930     else
2931         desired_pwr = INT_TO_Q52(phy->desired_txpower);
2932     /* And limit it. max_pwr already is Q5.2 */
2933     desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
2934     if (b43_debug(dev, B43_DBG_XMITPOWER)) {
2935         b43dbg(dev->wl,
2936                "[TX power]  current = " Q52_FMT
2937                " dBm,  desired = " Q52_FMT
2938                " dBm,  max = " Q52_FMT "\n",
2939                Q52_ARG(estimated_pwr),
2940                Q52_ARG(desired_pwr),
2941                Q52_ARG(max_pwr));
2942     }
2943 
2944     /* Calculate the adjustment delta. */
2945     pwr_adjust = desired_pwr - estimated_pwr;
2946     if (pwr_adjust == 0)
2947         goto no_adjustment_needed;
2948 
2949     /* RF attenuation delta. */
2950     rfatt_delta = ((pwr_adjust + 7) / 8);
2951     /* Lower attenuation => Bigger power output. Negate it. */
2952     rfatt_delta = -rfatt_delta;
2953 
2954     /* Baseband attenuation delta. */
2955     bbatt_delta = pwr_adjust / 2;
2956     /* Lower attenuation => Bigger power output. Negate it. */
2957     bbatt_delta = -bbatt_delta;
2958     /* RF att affects power level 4 times as much as
2959      * Baseband attennuation. Subtract it. */
2960     bbatt_delta -= 4 * rfatt_delta;
2961 
2962 #if B43_DEBUG
2963     if (b43_debug(dev, B43_DBG_XMITPOWER)) {
2964         int dbm = pwr_adjust < 0 ? -pwr_adjust : pwr_adjust;
2965         b43dbg(dev->wl,
2966                "[TX power deltas]  %s" Q52_FMT " dBm   =>   "
2967                "bbatt-delta = %d,  rfatt-delta = %d\n",
2968                (pwr_adjust < 0 ? "-" : ""), Q52_ARG(dbm),
2969                bbatt_delta, rfatt_delta);
2970     }
2971 #endif /* DEBUG */
2972 
2973     /* So do we finally need to adjust something in hardware? */
2974     if ((rfatt_delta == 0) && (bbatt_delta == 0))
2975         goto no_adjustment_needed;
2976 
2977     /* Save the deltas for later when we adjust the power. */
2978     gphy->bbatt_delta = bbatt_delta;
2979     gphy->rfatt_delta = rfatt_delta;
2980 
2981     /* We need to adjust the TX power on the device. */
2982     return B43_TXPWR_RES_NEED_ADJUST;
2983 
2984 no_adjustment_needed:
2985     return B43_TXPWR_RES_DONE;
2986 }
2987 
2988 static void b43_gphy_op_pwork_15sec(struct b43_wldev *dev)
2989 {
2990     struct b43_phy *phy = &dev->phy;
2991     struct b43_phy_g *gphy = phy->g;
2992 
2993     b43_mac_suspend(dev);
2994     //TODO: update_aci_moving_average
2995     if (gphy->aci_enable && gphy->aci_wlan_automatic) {
2996         if (!gphy->aci_enable && 1 /*TODO: not scanning? */ ) {
2997             if (0 /*TODO: bunch of conditions */ ) {
2998                 phy->ops->interf_mitigation(dev,
2999                     B43_INTERFMODE_MANUALWLAN);
3000             }
3001         } else if (0 /*TODO*/) {
3002                if (/*(aci_average > 1000) &&*/ !b43_gphy_aci_scan(dev))
3003                 phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
3004         }
3005     } else if (gphy->interfmode == B43_INTERFMODE_NONWLAN &&
3006            phy->rev == 1) {
3007         //TODO: implement rev1 workaround
3008     }
3009     b43_lo_g_maintenance_work(dev);
3010     b43_mac_enable(dev);
3011 }
3012 
3013 static void b43_gphy_op_pwork_60sec(struct b43_wldev *dev)
3014 {
3015     struct b43_phy *phy = &dev->phy;
3016 
3017     if (!(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI))
3018         return;
3019 
3020     b43_mac_suspend(dev);
3021     b43_calc_nrssi_slope(dev);
3022     if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
3023         u8 old_chan = phy->channel;
3024 
3025         /* VCO Calibration */
3026         if (old_chan >= 8)
3027             b43_switch_channel(dev, 1);
3028         else
3029             b43_switch_channel(dev, 13);
3030         b43_switch_channel(dev, old_chan);
3031     }
3032     b43_mac_enable(dev);
3033 }
3034 
3035 const struct b43_phy_operations b43_phyops_g = {
3036     .allocate       = b43_gphy_op_allocate,
3037     .free           = b43_gphy_op_free,
3038     .prepare_structs    = b43_gphy_op_prepare_structs,
3039     .prepare_hardware   = b43_gphy_op_prepare_hardware,
3040     .init           = b43_gphy_op_init,
3041     .exit           = b43_gphy_op_exit,
3042     .phy_read       = b43_gphy_op_read,
3043     .phy_write      = b43_gphy_op_write,
3044     .radio_read     = b43_gphy_op_radio_read,
3045     .radio_write        = b43_gphy_op_radio_write,
3046     .supports_hwpctl    = b43_gphy_op_supports_hwpctl,
3047     .software_rfkill    = b43_gphy_op_software_rfkill,
3048     .switch_analog      = b43_phyop_switch_analog_generic,
3049     .switch_channel     = b43_gphy_op_switch_channel,
3050     .get_default_chan   = b43_gphy_op_get_default_chan,
3051     .set_rx_antenna     = b43_gphy_op_set_rx_antenna,
3052     .interf_mitigation  = b43_gphy_op_interf_mitigation,
3053     .recalc_txpower     = b43_gphy_op_recalc_txpower,
3054     .adjust_txpower     = b43_gphy_op_adjust_txpower,
3055     .pwork_15sec        = b43_gphy_op_pwork_15sec,
3056     .pwork_60sec        = b43_gphy_op_pwork_60sec,
3057 };
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!