diff options
Diffstat (limited to 'drivers/net/wireless/b43/phy_g.c')
| -rw-r--r-- | drivers/net/wireless/b43/phy_g.c | 3282 |
1 files changed, 3282 insertions, 0 deletions
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/b43/phy_g.c new file mode 100644 index 00000000000..232181f6333 --- /dev/null +++ b/drivers/net/wireless/b43/phy_g.c @@ -0,0 +1,3282 @@ +/* + + Broadcom B43 wireless driver + IEEE 802.11g PHY driver + + Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>, + Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it> + Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de> + Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org> + Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch> + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; see the file COPYING. If not, write to + the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor, + Boston, MA 02110-1301, USA. + +*/ + +#include "b43.h" +#include "phy_g.h" +#include "phy_common.h" +#include "lo.h" +#include "main.h" + +#include <linux/bitrev.h> + + +static const s8 b43_tssi2dbm_g_table[] = { + 77, 77, 77, 76, + 76, 76, 75, 75, + 74, 74, 73, 73, + 73, 72, 72, 71, + 71, 70, 70, 69, + 68, 68, 67, 67, + 66, 65, 65, 64, + 63, 63, 62, 61, + 60, 59, 58, 57, + 56, 55, 54, 53, + 52, 50, 49, 47, + 45, 43, 40, 37, + 33, 28, 22, 14, + 5, -7, -20, -20, + -20, -20, -20, -20, + -20, -20, -20, -20, +}; + +const u8 b43_radio_channel_codes_bg[] = { + 12, 17, 22, 27, + 32, 37, 42, 47, + 52, 57, 62, 67, + 72, 84, +}; + + +static void b43_calc_nrssi_threshold(struct b43_wldev *dev); + + +#define bitrev4(tmp) (bitrev8(tmp) >> 4) + + +/* Get the freq, as it has to be written to the device. */ +static inline u16 channel2freq_bg(u8 channel) +{ + B43_WARN_ON(!(channel >= 1 && channel <= 14)); + + return b43_radio_channel_codes_bg[channel - 1]; +} + +static void generate_rfatt_list(struct b43_wldev *dev, + struct b43_rfatt_list *list) +{ + struct b43_phy *phy = &dev->phy; + + /* APHY.rev < 5 || GPHY.rev < 6 */ + static const struct b43_rfatt rfatt_0[] = { + {.att = 3,.with_padmix = 0,}, + {.att = 1,.with_padmix = 0,}, + {.att = 5,.with_padmix = 0,}, + {.att = 7,.with_padmix = 0,}, + {.att = 9,.with_padmix = 0,}, + {.att = 2,.with_padmix = 0,}, + {.att = 0,.with_padmix = 0,}, + {.att = 4,.with_padmix = 0,}, + {.att = 6,.with_padmix = 0,}, + {.att = 8,.with_padmix = 0,}, + {.att = 1,.with_padmix = 1,}, + {.att = 2,.with_padmix = 1,}, + {.att = 3,.with_padmix = 1,}, + {.att = 4,.with_padmix = 1,}, + }; + /* Radio.rev == 8 && Radio.version == 0x2050 */ + static const struct b43_rfatt rfatt_1[] = { + {.att = 2,.with_padmix = 1,}, + {.att = 4,.with_padmix = 1,}, + {.att = 6,.with_padmix = 1,}, + {.att = 8,.with_padmix = 1,}, + {.att = 10,.with_padmix = 1,}, + {.att = 12,.with_padmix = 1,}, + {.att = 14,.with_padmix = 1,}, + }; + /* Otherwise */ + static const struct b43_rfatt rfatt_2[] = { + {.att = 0,.with_padmix = 1,}, + {.att = 2,.with_padmix = 1,}, + {.att = 4,.with_padmix = 1,}, + {.att = 6,.with_padmix = 1,}, + {.att = 8,.with_padmix = 1,}, + {.att = 9,.with_padmix = 1,}, + {.att = 9,.with_padmix = 1,}, + }; + + if (!b43_has_hardware_pctl(dev)) { + /* Software pctl */ + list->list = rfatt_0; + list->len = ARRAY_SIZE(rfatt_0); + list->min_val = 0; + list->max_val = 9; + return; + } + if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) { + /* Hardware pctl */ + list->list = rfatt_1; + list->len = ARRAY_SIZE(rfatt_1); + list->min_val = 0; + list->max_val = 14; + return; + } + /* Hardware pctl */ + list->list = rfatt_2; + list->len = ARRAY_SIZE(rfatt_2); + list->min_val = 0; + list->max_val = 9; +} + +static void generate_bbatt_list(struct b43_wldev *dev, + struct b43_bbatt_list *list) +{ + static const struct b43_bbatt bbatt_0[] = { + {.att = 0,}, + {.att = 1,}, + {.att = 2,}, + {.att = 3,}, + {.att = 4,}, + {.att = 5,}, + {.att = 6,}, + {.att = 7,}, + {.att = 8,}, + }; + + list->list = bbatt_0; + list->len = ARRAY_SIZE(bbatt_0); + list->min_val = 0; + list->max_val = 8; +} + +static void b43_shm_clear_tssi(struct b43_wldev *dev) +{ + b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F); + b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F); + b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F); + b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F); +} + +/* Synthetic PU workaround */ +static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel) +{ + struct b43_phy *phy = &dev->phy; + + might_sleep(); + + if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) { + /* We do not need the workaround. */ + return; + } + + if (channel <= 10) { + b43_write16(dev, B43_MMIO_CHANNEL, + channel2freq_bg(channel + 4)); + } else { + b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1)); + } + msleep(1); + b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel)); +} + +/* Set the baseband attenuation value on chip. */ +void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev, + u16 baseband_attenuation) +{ + struct b43_phy *phy = &dev->phy; + + if (phy->analog == 0) { + b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0) + & 0xFFF0) | + baseband_attenuation); + } else if (phy->analog > 1) { + b43_phy_write(dev, B43_PHY_DACCTL, + (b43_phy_read(dev, B43_PHY_DACCTL) + & 0xFFC3) | (baseband_attenuation << 2)); + } else { + b43_phy_write(dev, B43_PHY_DACCTL, + (b43_phy_read(dev, B43_PHY_DACCTL) + & 0xFF87) | (baseband_attenuation << 3)); + } +} + +/* Adjust the transmission power output (G-PHY) */ +void b43_set_txpower_g(struct b43_wldev *dev, + const struct b43_bbatt *bbatt, + const struct b43_rfatt *rfatt, u8 tx_control) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + struct b43_txpower_lo_control *lo = gphy->lo_control; + u16 bb, rf; + u16 tx_bias, tx_magn; + + bb = bbatt->att; + rf = rfatt->att; + tx_bias = lo->tx_bias; + tx_magn = lo->tx_magn; + if (unlikely(tx_bias == 0xFF)) + tx_bias = 0; + + /* Save the values for later. Use memmove, because it's valid + * to pass &gphy->rfatt as rfatt pointer argument. Same for bbatt. */ + gphy->tx_control = tx_control; + memmove(&gphy->rfatt, rfatt, sizeof(*rfatt)); + gphy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX); + memmove(&gphy->bbatt, bbatt, sizeof(*bbatt)); + + if (b43_debug(dev, B43_DBG_XMITPOWER)) { + b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), " + "rfatt(%u), tx_control(0x%02X), " + "tx_bias(0x%02X), tx_magn(0x%02X)\n", + bb, rf, tx_control, tx_bias, tx_magn); + } + + b43_gphy_set_baseband_attenuation(dev, bb); + b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf); + if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) { + b43_radio_write16(dev, 0x43, + (rf & 0x000F) | (tx_control & 0x0070)); + } else { + b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43) + & 0xFFF0) | (rf & 0x000F)); + b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52) + & ~0x0070) | (tx_control & + 0x0070)); + } + if (has_tx_magnification(phy)) { + b43_radio_write16(dev, 0x52, tx_magn | tx_bias); + } else { + b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52) + & 0xFFF0) | (tx_bias & 0x000F)); + } + b43_lo_g_adjust(dev); +} + +/* GPHY_TSSI_Power_Lookup_Table_Init */ +static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev) +{ + struct b43_phy_g *gphy = dev->phy.g; + int i; + u16 value; + + for (i = 0; i < 32; i++) + b43_ofdmtab_write16(dev, 0x3C20, i, gphy->tssi2dbm[i]); + for (i = 32; i < 64; i++) + b43_ofdmtab_write16(dev, 0x3C00, i - 32, gphy->tssi2dbm[i]); + for (i = 0; i < 64; i += 2) { + value = (u16) gphy->tssi2dbm[i]; + value |= ((u16) gphy->tssi2dbm[i + 1]) << 8; + b43_phy_write(dev, 0x380 + (i / 2), value); + } +} + +/* GPHY_Gain_Lookup_Table_Init */ +static void b43_gphy_gain_lt_init(struct b43_wldev *dev) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + struct b43_txpower_lo_control *lo = gphy->lo_control; + u16 nr_written = 0; + u16 tmp; + u8 rf, bb; + + for (rf = 0; rf < lo->rfatt_list.len; rf++) { + for (bb = 0; bb < lo->bbatt_list.len; bb++) { + if (nr_written >= 0x40) + return; + tmp = lo->bbatt_list.list[bb].att; + tmp <<= 8; + if (phy->radio_rev == 8) + tmp |= 0x50; + else + tmp |= 0x40; + tmp |= lo->rfatt_list.list[rf].att; + b43_phy_write(dev, 0x3C0 + nr_written, tmp); + nr_written++; + } + } +} + +static void b43_set_all_gains(struct b43_wldev *dev, + s16 first, s16 second, s16 third) +{ + struct b43_phy *phy = &dev->phy; + u16 i; + u16 start = 0x08, end = 0x18; + u16 tmp; + u16 table; + + if (phy->rev <= 1) { + start = 0x10; + end = 0x20; + } + + table = B43_OFDMTAB_GAINX; + if (phy->rev <= 1) + table = B43_OFDMTAB_GAINX_R1; + for (i = 0; i < 4; i++) + b43_ofdmtab_write16(dev, table, i, first); + + for (i = start; i < end; i++) + b43_ofdmtab_write16(dev, table, i, second); + + if (third != -1) { + tmp = ((u16) third << 14) | ((u16) third << 6); + b43_phy_write(dev, 0x04A0, + (b43_phy_read(dev, 0x04A0) & 0xBFBF) | tmp); + b43_phy_write(dev, 0x04A1, + (b43_phy_read(dev, 0x04A1) & 0xBFBF) | tmp); + b43_phy_write(dev, 0x04A2, + (b43_phy_read(dev, 0x04A2) & 0xBFBF) | tmp); + } + b43_dummy_transmission(dev); +} + +static void b43_set_original_gains(struct b43_wldev *dev) +{ + struct b43_phy *phy = &dev->phy; + u16 i, tmp; + u16 table; + u16 start = 0x0008, end = 0x0018; + + if (phy->rev <= 1) { + start = 0x0010; + end = 0x0020; + } + + table = B43_OFDMTAB_GAINX; + if (phy->rev <= 1) + table = B43_OFDMTAB_GAINX_R1; + for (i = 0; i < 4; i++) { + tmp = (i & 0xFFFC); + tmp |= (i & 0x0001) << 1; + tmp |= (i & 0x0002) >> 1; + + b43_ofdmtab_write16(dev, table, i, tmp); + } + + for (i = start; i < end; i++) + b43_ofdmtab_write16(dev, table, i, i - start); + + b43_phy_write(dev, 0x04A0, + (b43_phy_read(dev, 0x04A0) & 0xBFBF) | 0x4040); + b43_phy_write(dev, 0x04A1, + (b43_phy_read(dev, 0x04A1) & 0xBFBF) | 0x4040); + b43_phy_write(dev, 0x04A2, + (b43_phy_read(dev, 0x04A2) & 0xBFBF) | 0x4000); + b43_dummy_transmission(dev); +} + +/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */ +void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val) +{ + b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset); + b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val); +} + +/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */ +s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset) +{ + u16 val; + + b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset); + val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA); + + return (s16) val; +} + +/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */ +void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val) +{ + u16 i; + s16 tmp; + + for (i = 0; i < 64; i++) { + tmp = b43_nrssi_hw_read(dev, i); + tmp -= val; + tmp = clamp_val(tmp, -32, 31); + b43_nrssi_hw_write(dev, i, tmp); + } +} + +/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */ +void b43_nrssi_mem_update(struct b43_wldev *dev) +{ + struct b43_phy_g *gphy = dev->phy.g; + s16 i, delta; + s32 tmp; + + delta = 0x1F - gphy->nrssi[0]; + for (i = 0; i < 64; i++) { + tmp = (i - delta) * gphy->nrssislope; + tmp /= 0x10000; + tmp += 0x3A; + tmp = clamp_val(tmp, 0, 0x3F); + gphy->nrssi_lt[i] = tmp; + } +} + +static void b43_calc_nrssi_offset(struct b43_wldev *dev) +{ + struct b43_phy *phy = &dev->phy; + u16 backup[20] = { 0 }; + s16 v47F; + u16 i; + u16 saved = 0xFFFF; + + backup[0] = b43_phy_read(dev, 0x0001); + backup[1] = b43_phy_read(dev, 0x0811); + backup[2] = b43_phy_read(dev, 0x0812); + if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */ + backup[3] = b43_phy_read(dev, 0x0814); + backup[4] = b43_phy_read(dev, 0x0815); + } + backup[5] = b43_phy_read(dev, 0x005A); + backup[6] = b43_phy_read(dev, 0x0059); + backup[7] = b43_phy_read(dev, 0x0058); + backup[8] = b43_phy_read(dev, 0x000A); + backup[9] = b43_phy_read(dev, 0x0003); + backup[10] = b43_radio_read16(dev, 0x007A); + backup[11] = b43_radio_read16(dev, 0x0043); + + b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) & 0x7FFF); + b43_phy_write(dev, 0x0001, + (b43_phy_read(dev, 0x0001) & 0x3FFF) | 0x4000); + b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C); + b43_phy_write(dev, 0x0812, + (b43_phy_read(dev, 0x0812) & 0xFFF3) | 0x0004); + b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & ~(0x1 | 0x2)); + if (phy->rev >= 6) { + backup[12] = b43_phy_read(dev, 0x002E); + backup[13] = b43_phy_read(dev, 0x002F); + backup[14] = b43_phy_read(dev, 0x080F); + backup[15] = b43_phy_read(dev, 0x0810); + backup[16] = b43_phy_read(dev, 0x0801); + backup[17] = b43_phy_read(dev, 0x0060); + backup[18] = b43_phy_read(dev, 0x0014); + backup[19] = b43_phy_read(dev, 0x0478); + + b43_phy_write(dev, 0x002E, 0); + b43_phy_write(dev, 0x002F, 0); + b43_phy_write(dev, 0x080F, 0); + b43_phy_write(dev, 0x0810, 0); + b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478) | 0x0100); + b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801) | 0x0040); + b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) | 0x0040); + b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) | 0x0200); + } + b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0070); + b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0080); + udelay(30); + + v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F); + if (v47F >= 0x20) + v47F -= 0x40; + if (v47F == 31) { + for (i = 7; i >= 4; i--) { + b43_radio_write16(dev, 0x007B, i); + udelay(20); + v47F = + (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F); + if (v47F >= 0x20) + v47F -= 0x40; + if (v47F < 31 && saved == 0xFFFF) + saved = i; + } + if (saved == 0xFFFF) + saved = 4; + } else { + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) & 0x007F); + if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */ + b43_phy_write(dev, 0x0814, + b43_phy_read(dev, 0x0814) | 0x0001); + b43_phy_write(dev, 0x0815, + b43_phy_read(dev, 0x0815) & 0xFFFE); + } + b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C); + b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x000C); + b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x0030); + b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x0030); + b43_phy_write(dev, 0x005A, 0x0480); + b43_phy_write(dev, 0x0059, 0x0810); + b43_phy_write(dev, 0x0058, 0x000D); + if (phy->rev == 0) { + b43_phy_write(dev, 0x0003, 0x0122); + } else { + b43_phy_write(dev, 0x000A, b43_phy_read(dev, 0x000A) + | 0x2000); + } + if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */ + b43_phy_write(dev, 0x0814, + b43_phy_read(dev, 0x0814) | 0x0004); + b43_phy_write(dev, 0x0815, + b43_phy_read(dev, 0x0815) & 0xFFFB); + } + b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) & 0xFF9F) + | 0x0040); + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) | 0x000F); + b43_set_all_gains(dev, 3, 0, 1); + b43_radio_write16(dev, 0x0043, (b43_radio_read16(dev, 0x0043) + & 0x00F0) | 0x000F); + udelay(30); + v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F); + if (v47F >= 0x20) + v47F -= 0x40; + if (v47F == -32) { + for (i = 0; i < 4; i++) { + b43_radio_write16(dev, 0x007B, i); + udelay(20); + v47F = + (s16) ((b43_phy_read(dev, 0x047F) >> 8) & + 0x003F); + if (v47F >= 0x20) + v47F -= 0x40; + if (v47F > -31 && saved == 0xFFFF) + saved = i; + } + if (saved == 0xFFFF) + saved = 3; + } else + saved = 0; + } + b43_radio_write16(dev, 0x007B, saved); + + if (phy->rev >= 6) { + b43_phy_write(dev, 0x002E, backup[12]); + b43_phy_write(dev, 0x002F, backup[13]); + b43_phy_write(dev, 0x080F, backup[14]); + b43_phy_write(dev, 0x0810, backup[15]); + } + if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */ + b43_phy_write(dev, 0x0814, backup[3]); + b43_phy_write(dev, 0x0815, backup[4]); + } + b43_phy_write(dev, 0x005A, backup[5]); + b43_phy_write(dev, 0x0059, backup[6]); + b43_phy_write(dev, 0x0058, backup[7]); + b43_phy_write(dev, 0x000A, backup[8]); + b43_phy_write(dev, 0x0003, backup[9]); + b43_radio_write16(dev, 0x0043, backup[11]); + b43_radio_write16(dev, 0x007A, backup[10]); + b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2); + b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) | 0x8000); + b43_set_original_gains(dev); + if (phy->rev >= 6) { + b43_phy_write(dev, 0x0801, backup[16]); + b43_phy_write(dev, 0x0060, backup[17]); + b43_phy_write(dev, 0x0014, backup[18]); + b43_phy_write(dev, 0x0478, backup[19]); + } + b43_phy_write(dev, 0x0001, backup[0]); + b43_phy_write(dev, 0x0812, backup[2]); + b43_phy_write(dev, 0x0811, backup[1]); +} + +void b43_calc_nrssi_slope(struct b43_wldev *dev) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + u16 backup[18] = { 0 }; + u16 tmp; + s16 nrssi0, nrssi1; + + B43_WARN_ON(phy->type != B43_PHYTYPE_G); + + if (phy->radio_rev >= 9) + return; + if (phy->radio_rev == 8) + b43_calc_nrssi_offset(dev); + + b43_phy_write(dev, B43_PHY_G_CRS, + b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF); + b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC); + backup[7] = b43_read16(dev, 0x03E2); + b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000); + backup[0] = b43_radio_read16(dev, 0x007A); + backup[1] = b43_radio_read16(dev, 0x0052); + backup[2] = b43_radio_read16(dev, 0x0043); + backup[3] = b43_phy_read(dev, 0x0015); + backup[4] = b43_phy_read(dev, 0x005A); + backup[5] = b43_phy_read(dev, 0x0059); + backup[6] = b43_phy_read(dev, 0x0058); + backup[8] = b43_read16(dev, 0x03E6); + backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT); + if (phy->rev >= 3) { + backup[10] = b43_phy_read(dev, 0x002E); + backup[11] = b43_phy_read(dev, 0x002F); + backup[12] = b43_phy_read(dev, 0x080F); + backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL); + backup[14] = b43_phy_read(dev, 0x0801); + backup[15] = b43_phy_read(dev, 0x0060); + backup[16] = b43_phy_read(dev, 0x0014); + backup[17] = b43_phy_read(dev, 0x0478); + b43_phy_write(dev, 0x002E, 0); + b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0); + switch (phy->rev) { + case 4: + case 6: + case 7: + b43_phy_write(dev, 0x0478, + b43_phy_read(dev, 0x0478) + | 0x0100); + b43_phy_write(dev, 0x0801, + b43_phy_read(dev, 0x0801) + | 0x0040); + break; + case 3: + case 5: + b43_phy_write(dev, 0x0801, + b43_phy_read(dev, 0x0801) + & 0xFFBF); + break; + } + b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) + | 0x0040); + b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) + | 0x0200); + } + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) | 0x0070); + b43_set_all_gains(dev, 0, 8, 0); + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) & 0x00F7); + if (phy->rev >= 2) { + b43_phy_write(dev, 0x0811, + (b43_phy_read(dev, 0x0811) & 0xFFCF) | + 0x0030); + b43_phy_write(dev, 0x0812, + (b43_phy_read(dev, 0x0812) & 0xFFCF) | + 0x0010); + } + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) | 0x0080); + udelay(20); + + nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F); + if (nrssi0 >= 0x0020) + nrssi0 -= 0x0040; + + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) & 0x007F); + if (phy->rev >= 2) { + b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) + & 0xFF9F) | 0x0040); + } + + b43_write16(dev, B43_MMIO_CHANNEL_EXT, + b43_read16(dev, B43_MMIO_CHANNEL_EXT) + | 0x2000); + b43_radio_write16(dev, 0x007A, + b43_radio_read16(dev, 0x007A) | 0x000F); + b43_phy_write(dev, 0x0015, 0xF330); + if (phy->rev >= 2) { + b43_phy_write(dev, 0x0812, + (b43_phy_read(dev, 0x0812) & 0xFFCF) | + 0x0020); + b43_phy_write(dev, 0x0811, + (b43_phy_read(dev, 0x0811) & 0xFFCF) | + 0x0020); + } + + b43_set_all_gains(dev, 3, 0, 1); + if (phy->radio_rev == 8) { + b43_radio_write16(dev, 0x0043, 0x001F); + } else { + tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F; + b43_radio_write16(dev, 0x0052, tmp | 0x0060); + tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0; + b43_radio_write16(dev, 0x0043, tmp | 0x0009); + } + b43_phy_write(dev, 0x005A, 0x0480); + b43_phy_write(dev, 0x0059, 0x0810); + b43_phy_write(dev, 0x0058, 0x000D); + udelay(20); + nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F); + if (nrssi1 >= 0x0020) + nrssi1 -= 0x0040; + if (nrssi0 == nrssi1) + gphy->nrssislope = 0x00010000; + else + gphy->nrssislope = 0x00400000 / (nrssi0 - nrssi1); + if (nrssi0 >= -4) { + gphy->nrssi[0] = nrssi1; + gphy->nrssi[1] = nrssi0; + } + if (phy->rev >= 3) { + b43_phy_write(dev, 0x002E, backup[10]); + b43_phy_write(dev, 0x002F, backup[11]); + b43_phy_write(dev, 0x080F, backup[12]); + b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]); + } + if (phy->rev >= 2) { + b43_phy_write(dev, 0x0812, + b43_phy_read(dev, 0x0812) & 0xFFCF); + b43_phy_write(dev, 0x0811, + b43_phy_read(dev, 0x0811) & 0xFFCF); + } + + b43_radio_write16(dev, 0x007A, backup[0]); + b43_radio_write16(dev, 0x0052, backup[1]); + b43_radio_write16(dev, 0x0043, backup[2]); + b43_write16(dev, 0x03E2, backup[7]); + b43_write16(dev, 0x03E6, backup[8]); + b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]); + b43_phy_write(dev, 0x0015, backup[3]); + b43_phy_write(dev, 0x005A, backup[4]); + b43_phy_write(dev, 0x0059, backup[5]); + b43_phy_write(dev, 0x0058, backup[6]); + b43_synth_pu_workaround(dev, phy->channel); + b43_phy_write(dev, 0x0802, + b43_phy_read(dev, 0x0802) | (0x0001 | 0x0002)); + b43_set_original_gains(dev); + b43_phy_write(dev, B43_PHY_G_CRS, + b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000); + if (phy->rev >= 3) { + b43_phy_write(dev, 0x0801, backup[14]); + b43_phy_write(dev, 0x0060, backup[15]); + b43_phy_write(dev, 0x0014, backup[16]); + b43_phy_write(dev, 0x0478, backup[17]); + } + b43_nrssi_mem_update(dev); + b43_calc_nrssi_threshold(dev); +} + +static void b43_calc_nrssi_threshold(struct b43_wldev *dev) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + s32 a, b; + s16 tmp16; + u16 tmp_u16; + + B43_WARN_ON(phy->type != B43_PHYTYPE_G); + + if (!phy->gmode || + !(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) { + tmp16 = b43_nrssi_hw_read(dev, 0x20); + if (tmp16 >= 0x20) + tmp16 -= 0x40; + if (tmp16 < 3) { + b43_phy_write(dev, 0x048A, + (b43_phy_read(dev, 0x048A) + & 0xF000) | 0x09EB); + } else { + b43_phy_write(dev, 0x048A, + (b43_phy_read(dev, 0x048A) + & 0xF000) | 0x0AED); + } + } else { + if (gphy->interfmode == B43_INTERFMODE_NONWLAN) { + a = 0xE; + b = 0xA; + } else if (!gphy->aci_wlan_automatic && gphy->aci_enable) { + a = 0x13; + b = 0x12; + } else { + a = 0xE; + b = 0x11; + } + + a = a * (gphy->nrssi[1] - gphy->nrssi[0]); + a += (gphy->nrssi[0] << 6); + if (a < 32) + a += 31; + else + a += 32; + a = a >> 6; + a = clamp_val(a, -31, 31); + + b = b * (gphy->nrssi[1] - gphy->nrssi[0]); + b += (gphy->nrssi[0] << 6); + if (b < 32) + b += 31; + else + b += 32; + b = b >> 6; + b = clamp_val(b, -31, 31); + + tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000; + tmp_u16 |= ((u32) b & 0x0000003F); + tmp_u16 |= (((u32) a & 0x0000003F) << 6); + b43_phy_write(dev, 0x048A, tmp_u16); + } +} + +/* Stack implementation to save/restore values from the + * interference mitigation code. + * It is save to restore values in random order. + */ +static void _stack_save(u32 * _stackptr, size_t * stackidx, + u8 id, u16 offset, u16 value) +{ + u32 *stackptr = &(_stackptr[*stackidx]); + + B43_WARN_ON(offset & 0xF000); + B43_WARN_ON(id & 0xF0); + *stackptr = offset; + *stackptr |= ((u32) id) << 12; + *stackptr |= ((u32) value) << 16; + (*stackidx)++; + B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE); +} + +static u16 _stack_restore(u32 * stackptr, u8 id, u16 offset) +{ + size_t i; + + B43_WARN_ON(offset & 0xF000); + B43_WARN_ON(id & 0xF0); + for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) { + if ((*stackptr & 0x00000FFF) != offset) + continue; + if (((*stackptr & 0x0000F000) >> 12) != id) + continue; + return ((*stackptr & 0xFFFF0000) >> 16); + } + B43_WARN_ON(1); + + return 0; +} + +#define phy_stacksave(offset) \ + do { \ + _stack_save(stack, &stackidx, 0x1, (offset), \ + b43_phy_read(dev, (offset))); \ + } while (0) +#define phy_stackrestore(offset) \ + do { \ + b43_phy_write(dev, (offset), \ + _stack_restore(stack, 0x1, \ + (offset))); \ + } while (0) +#define radio_stacksave(offset) \ + do { \ + _stack_save(stack, &stackidx, 0x2, (offset), \ + b43_radio_read16(dev, (offset))); \ + } while (0) +#define radio_stackrestore(offset) \ + do { \ + b43_radio_write16(dev, (offset), \ + _stack_restore(stack, 0x2, \ + (offset))); \ + } while (0) +#define ofdmtab_stacksave(table, offset) \ + do { \ + _stack_save(stack, &stackidx, 0x3, (offset)|(table), \ + b43_ofdmtab_read16(dev, (table), (offset))); \ + } while (0) +#define ofdmtab_stackrestore(table, offset) \ + do { \ + b43_ofdmtab_write16(dev, (table), (offset), \ + _stack_restore(stack, 0x3, \ + (offset)|(table))); \ + } while (0) + +static void +b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + u16 tmp, flipped; + size_t stackidx = 0; + u32 *stack = gphy->interfstack; + + switch (mode) { + case B43_INTERFMODE_NONWLAN: + if (phy->rev != 1) { + b43_phy_write(dev, 0x042B, + b43_phy_read(dev, 0x042B) | 0x0800); + b43_phy_write(dev, B43_PHY_G_CRS, + b43_phy_read(dev, + B43_PHY_G_CRS) & ~0x4000); + break; + } + radio_stacksave(0x0078); + tmp = (b43_radio_read16(dev, 0x0078) & 0x001E); + B43_WARN_ON(tmp > 15); + flipped = bitrev4(tmp); + if (flipped < 10 && flipped >= 8) + flipped = 7; + else if (flipped >= 10) + flipped -= 3; + flipped = (bitrev4(flipped) << 1) | 0x0020; + b43_radio_write16(dev, 0x0078, flipped); + + b43_calc_nrssi_threshold(dev); + + phy_stacksave(0x0406); + b43_phy_write(dev, 0x0406, 0x7E28); + + b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x0800); + b43_phy_write(dev, B43_PHY_RADIO_BITFIELD, + b43_phy_read(dev, + B43_PHY_RADIO_BITFIELD) | 0x1000); + + phy_stacksave(0x04A0); + b43_phy_write(dev, 0x04A0, + (b43_phy_read(dev, 0x04A0) & 0xC0C0) | 0x0008); + phy_stacksave(0x04A1); + b43_phy_write(dev, 0x04A1, + (b43_phy_read(dev, 0x04A1) & 0xC0C0) | 0x0605); + phy_stacksave(0x04A2); + b43_phy_write(dev, 0x04A2, + (b43_phy_read(dev, 0x04A2) & 0xC0C0) | 0x0204); + phy_stacksave(0x04A8); + b43_phy_write(dev, 0x04A8, + (b43_phy_read(dev, 0x04A8) & 0xC0C0) | 0x0803); + phy_stacksave(0x04AB); + b43_phy_write(dev, 0x04AB, + (b43_phy_read(dev, 0x04AB) & 0xC0C0) | 0x0605); + + phy_stacksave(0x04A7); + b43_phy_write(dev, 0x04A7, 0x0002); + phy_stacksave(0x04A3); + b43_phy_write(dev, 0x04A3, 0x287A); + phy_stacksave(0x04A9); + b43_phy_write(dev, 0x04A9, 0x2027); + phy_stacksave(0x0493); + b43_phy_write(dev, 0x0493, 0x32F5); + phy_stacksave(0x04AA); + b43_phy_write(dev, 0x04AA, 0x2027); + phy_stacksave(0x04AC); + b43_phy_write(dev, 0x04AC, 0x32F5); + break; + case B43_INTERFMODE_MANUALWLAN: + if (b43_phy_read(dev, 0x0033) & 0x0800) + break; + + gphy->aci_enable = 1; + + phy_stacksave(B43_PHY_RADIO_BITFIELD); + phy_stacksave(B43_PHY_G_CRS); + if (phy->rev < 2) { + phy_stacksave(0x0406); + } else { + phy_stacksave(0x04C0); + phy_stacksave(0x04C1); + } + phy_stacksave(0x0033); + phy_stacksave(0x04A7); + phy_stacksave(0x04A3); + phy_stacksave(0x04A9); + phy_stacksave(0x04AA); + phy_stacksave(0x04AC); + phy_stacksave(0x0493); + phy_stacksave(0x04A1); + phy_stacksave(0x04A0); + phy_stacksave(0x04A2); + phy_stacksave(0x048A); + phy_stacksave(0x04A8); + phy_stacksave(0x04AB); + if (phy->rev == 2) { + phy_stacksave(0x04AD); + phy_stacksave(0x04AE); + } else if (phy->rev >= 3) { + phy_stacksave(0x04AD); + phy_stacksave(0x0415); + phy_stacksave(0x0416); + phy_stacksave(0x0417); + ofdmtab_stacksave(0x1A00, 0x2); + ofdmtab_stacksave(0x1A00, 0x3); + } + phy_stacksave(0x042B); + phy_stacksave(0x048C); + + b43_phy_write(dev, B43_PHY_RADIO_BITFIELD, + b43_phy_read(dev, B43_PHY_RADIO_BITFIELD) + & ~0x1000); + b43_phy_write(dev, B43_PHY_G_CRS, + (b43_phy_read(dev, B43_PHY_G_CRS) + & 0xFFFC) | 0x0002); + + b43_phy_write(dev, 0x0033, 0x0800); + b43_phy_write(dev, 0x04A3, 0x2027); + b43_phy_write(dev, 0x04A9, 0x1CA8); + b43_phy_write(dev, 0x0493, 0x287A); + b43_phy_write(dev, 0x04AA, 0x1CA8); + b43_phy_write(dev, 0x04AC, 0x287A); + + b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0) + & 0xFFC0) | 0x001A); + b43_phy_write(dev, 0x04A7, 0x000D); + + if (phy->rev < 2) { + b43_phy_write(dev, 0x0406, 0xFF0D); + } else if (phy->rev == 2) { + b43_phy_write(dev, 0x04C0, 0xFFFF); + b43_phy_write(dev, 0x04C1, 0x00A9); + } else { + b43_phy_write(dev, 0x04C0, 0x00C1); + b43_phy_write(dev, 0x04C1, 0x0059); + } + + b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1) + & 0xC0FF) | 0x1800); + b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1) + & 0xFFC0) | 0x0015); + b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8) + & 0xCFFF) | 0x1000); + b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8) + & 0xF0FF) | 0x0A00); + b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB) + & 0xCFFF) | 0x1000); + b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB) + & 0xF0FF) | 0x0800); + b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB) + & 0xFFCF) | 0x0010); + b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB) + & 0xFFF0) | 0x0005); + b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8) + & 0xFFCF) | 0x0010); + b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8) + & 0xFFF0) | 0x0006); + b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2) + & 0xF0FF) | 0x0800); + b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0) + & 0xF0FF) | 0x0500); + b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2) + & 0xFFF0) | 0x000B); + + if (phy->rev >= 3) { + b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A) + & ~0x8000); + b43_phy_write(dev, 0x0415, (b43_phy_read(dev, 0x0415) + & 0x8000) | 0x36D8); + b43_phy_write(dev, 0x0416, (b43_phy_read(dev, 0x0416) + & 0x8000) | 0x36D8); + b43_phy_write(dev, 0x0417, (b43_phy_read(dev, 0x0417) + & 0xFE00) | 0x016D); + } else { + b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A) + | 0x1000); + b43_phy_write(dev, 0x048A, (b43_phy_read(dev, 0x048A) + & 0x9FFF) | 0x2000); + b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW); + } + if (phy->rev >= 2) { + b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) + | 0x0800); + } + b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C) + & 0xF0FF) | 0x0200); + if (phy->rev == 2) { + b43_phy_write(dev, 0x04AE, (b43_phy_read(dev, 0x04AE) + & 0xFF00) | 0x007F); + b43_phy_write(dev, 0x04AD, (b43_phy_read(dev, 0x04AD) + & 0x00FF) | 0x1300); + } else if (phy->rev >= 6) { + b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F); + b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F); + b43_phy_write(dev, 0x04AD, b43_phy_read(dev, 0x04AD) + & 0x00FF); + } + b43_calc_nrssi_slope(dev); + break; + default: + B43_WARN_ON(1); + } +} + +static void +b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode) +{ + struct b43_phy *phy = &dev->phy; + struct b43_phy_g *gphy = phy->g; + u32 *stack = gphy->interfstack; + + switch (mode) { + case B43_INTERFMODE_NONWLAN: + if (phy->rev != 1) { + b43_phy_write(dev, 0x042B, + b43_phy_read(dev, 0x042B) & ~0x0800); + b43_phy_write(dev, B43_PHY_G_CRS, + b43_phy_read(dev, + B43_PHY_G_CRS) | 0x4000); + break; + } + radio_stackrestore(0x0078); + b43_calc_nrssi_threshold(dev); + phy_stackrestore(0x0406); + b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) & ~0x0800); + if (!dev->bad_frames_preempt) { + b43_phy_write(dev, B43_PHY_RADIO_BITFIELD, + b43_phy_read(dev, B43_PHY_RADIO_BITFIELD) + & ~(1 << 11)); + } + b43_phy_write(dev, B43_PHY_G_CRS, + b43_phy_read(dev, B43_PHY_G_CRS) | 0x4000); + phy_stackrestore(0x04A0); + phy_stackrestore(0x04A1); + phy_stackrestore(0x04A2); + phy_stackrestore(0x04A8); + phy_stackrestore(0x04AB); + phy_stackrestore(0x04A7); + phy_stackrestore(0x04A3); + phy_stackrestore(0x04A9) |